143 #include <yarp/os/LogStream.h>
144 #include <yarp/os/Searchable.h>
145 #include <yarp/os/Property.h>
146 #include <yarp/os/Value.h>
147 #include <yarp/os/Bottle.h>
148 #include <yarp/os/Network.h>
149 #include <yarp/os/ResourceFinder.h>
150 #include <yarp/os/RFModule.h>
151 #include <yarp/sig/Vector.h>
152 #include <yarp/sig/Matrix.h>
153 #include <yarp/math/Math.h>
158 using namespace yarp::os;
159 using namespace yarp::sig;
160 using namespace yarp::math;
169 bool configured{
false};
177 iKinLinIneqConstr::clone(obj);
182 C_orig = ptr->C_orig;
183 lB_orig = ptr->lB_orig;
184 uB_orig = ptr->uB_orig;
191 if (options.check(
"LIC_num")) {
192 auto LIC_num = options.find(
"LIC_num").asInt32();
194 for (
auto i = 0; i < LIC_num; i++) {
197 Bottle& group = options.findGroup(tag.str());
198 if (group.isNull()) {
199 yError() <<
"Unable to find group \"" << tag.str() <<
"\"";
202 Bottle* row = group.find(
"C").asList();
203 if (row ==
nullptr) {
204 yError() <<
"Unable to find option \"C\" for group \"" << tag.str() <<
"\"";
207 if (row->size() != chain->getN()) {
208 yError() <<
"Option \"C\" of group \"" << tag.str() <<
"\" has wrong size";
211 C_orig = pile(C_orig,
zeros(chain->getN()));
212 for (
auto i = 0; i < row->size(); i++) {
213 C_orig(C_orig.rows() - 1, i) = row->get(i).asFloat64();
215 lB_orig = cat(lB_orig, group.check(
"lB", Value(lowerBoundInf)).asFloat64());
216 uB_orig = cat(uB_orig, group.check(
"uB", Value(upperBoundInf)).asFloat64());
219 yInfo() <<
"Detected generic linear inequalities constraints";
230 getC().resize(C_orig.rows(), 0);
233 for (
auto i = 0U; i < chain->getN(); i++) {
234 if (chain->isLinkBlocked(i)) {
235 auto v = chain->getAng(i) * C_orig.getCol(i);
239 getC() = cat(getC(), C_orig.getCol(i));
250 shared_ptr<iKinLimb> limb{
nullptr};
251 shared_ptr<GenericLinIneqConstr> cns{
nullptr};
252 shared_ptr<PartDescriptor> desc{
nullptr};
257 if (!options.check(
"robot")) {
258 yError() <<
"\"robot\" option is missing!";
262 if (!options.check(
"NumberOfDrivers")) {
263 yError() <<
"\"NumberOfDrivers\" option is missing!";
267 string robot = options.find(
"robot").asString();
268 yInfo() <<
"Configuring solver for " << robot <<
" ...";
270 Property linksOptions;
272 limb = shared_ptr<iKinLimb>(
new iKinLimb(linksOptions));
273 if (!limb->isValid()) {
274 yError() <<
"invalid links parameters!";
280 desc->lmb = limb.get();
281 desc->chn = limb->asChain();
282 desc->cns = cns.get();
284 bool failure =
false;
285 desc->num = options.find(
"NumberOfDrivers").asInt32();
286 for (
int cnt = 0; cnt < desc->num; cnt++) {
288 str <<
"driver_" << cnt;
289 Bottle& driver = options.findGroup(str.str());
290 if (driver.isNull()) {
291 yError() <<
"\"" << str.str() <<
"\" option is missing!";
296 if (!driver.check(
"Key")) {
297 yError() <<
"\"Key\" option is missing!";
302 if (!driver.check(
"JointsOrder")) {
303 yError() <<
"\"JointsOrder\" option is missing!";
308 string part = driver.find(
"Key").asString();
309 bool directOrder = (driver.find(
"JointsOrder").asString() ==
"direct");
312 optPart.put(
"device",
"remote_controlboard");
313 optPart.put(
"remote",
"/" + robot +
"/" + part);
314 optPart.put(
"local",
"/" + slvName +
"/" + part);
315 optPart.put(
"robot", robot);
316 optPart.put(
"part", part);
317 desc->prp.push_back(optPart);
318 desc->rvs.push_back(!directOrder);
321 return (failure ?
nullptr : desc.get());
334 shared_ptr<CartesianSolver> slv{
nullptr};
339 string part, slvName;
340 if (rf.check(
"part")) {
341 part = rf.find(
"part").asString();
343 yError() <<
"part option is not specified";
347 Bottle& group = rf.findGroup(part);
348 if (group.isNull()) {
349 yError() <<
"unable to locate " << part <<
" definition";
353 if (group.check(
"name")) {
354 slvName = group.find(
"name").asString();
356 yError() <<
"name option is missing";
360 if (group.check(
"CustomKinFile")) {
361 yInfo() <<
"Custom Cartesian Solver detected!";
363 ResourceFinder rf_kin;
364 rf_kin.setDefaultContext(rf.getContext());
365 rf_kin.configure(0,
nullptr);
369 }
else if ((part ==
"left_arm") || (part ==
"right_arm")) {
371 }
else if ((part ==
"left_leg") || (part ==
"right_leg")) {
374 yError() << part <<
" is invalid";
378 return slv->open(group);
397 if (slv->isClosed()) {
401 if (slv->getTimeoutFlag()) {
402 slv->getTimeoutFlag() =
false;
414 if (!
yarp.checkNetwork()) {
415 yError() <<
"YARP server not available!";
420 rf.setDefaultContext(
"cartesianSolver");
421 rf.setDefaultConfigFile(
"cartesianSolver.ini");
422 rf.configure(
argc, argv);
425 return mod.runModule(rf);
PartDescriptor * getPartDesc(Searchable &options)
CustomCartesianSolver(const string &name)
void clone(const iKinLinIneqConstr *obj) override
void update(void *) override
Updates internal state.
GenericLinIneqConstr(iKinChain *chain_, Searchable &options)
bool configure(ResourceFinder &rf)
Abstract class defining the core of on-line solvers.
Derived class which implements the on-line solver for the chain torso+arm.
Derived class which implements the on-line solver for the leg chain.
A Base class for defining a Serial Link Chain.
A class for defining generic Limb.
Class for defining Linear Inequality Constraints of the form lB <= C*q <= uB for the nonlinear proble...
int main(int argc, char *argv[])
static string pathToCustomKinFile
Copyright (C) 2008 RobotCub Consortium.