Skip to content

Commit

Permalink
Merge pull request #146 from roboticslab-uc3m/fix-145-solver-limits
Browse files Browse the repository at this point in the history 
Delete ICartesianSolver::setLimits, load joint limits upon device initialization [ @PeterBowman THANKS!!!!! ]
  • Loading branch information
@jgvictores
jgvictores authored Feb 23, 2018
2 parents 92daeb5 + 91fec27 commit 7ea960b
Show file tree
Hide file tree
Showing 16 changed files with 164 additions and 228 deletions.
76 changes: 50 additions & 26 deletions libraries/YarpPlugins/AmorCartesianControl/DeviceDriverImpl.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -4,11 +4,14 @@

#include <string>

#include <yarp/os/Bottle.h>
#include <yarp/os/Property.h>
#include <yarp/os/Value.h>

#include <ColorDebug.hpp>

#include "KinematicRepresentation.hpp"

// ------------------- DeviceDriver Related ------------------------------------

bool roboticslab::AmorCartesianControl::open(yarp::os::Searchable& config)
Expand Down Expand Up @@ -41,32 +44,6 @@ bool roboticslab::AmorCartesianControl::open(yarp::os::Searchable& config)
return false;
}

std::string kinematicsFile = config.check("kinematics", yarp::os::Value(""),
"AMOR kinematics description").asString();

yarp::os::Property cartesianDeviceOptions;

if (!cartesianDeviceOptions.fromConfigFile(kinematicsFile))
{
CD_ERROR("Cannot read from --kinematics \"%s\".\n", kinematicsFile.c_str());
return false;
}

cartesianDeviceOptions.put("device", "KdlSolver");

if (!cartesianDevice.open(cartesianDeviceOptions))
{
CD_ERROR("Solver device not valid.\n");
return false;
}

if (!cartesianDevice.view(iCartesianSolver))
{
CD_ERROR("Could not view iCartesianSolver.\n");
close();
return false;
}

yarp::os::Value vHandle = config.find("handle");

if (vHandle.isNull())
Expand Down Expand Up @@ -97,6 +74,53 @@ bool roboticslab::AmorCartesianControl::open(yarp::os::Searchable& config)

CD_SUCCESS("Acquired AMOR handle!\n");

yarp::os::Bottle qMin, qMax;

for (int i = 0; i < AMOR_NUM_JOINTS; i++)
{
AMOR_JOINT_INFO jointInfo;

if (amor_get_joint_info(handle, i, &jointInfo) != AMOR_SUCCESS)
{
CD_ERROR("%s\n", amor_error());
close();
return false;
}

qMin.addDouble(KinRepresentation::radToDeg(jointInfo.lowerJointLimit));
qMax.addDouble(KinRepresentation::radToDeg(jointInfo.upperJointLimit));
}

std::string kinematicsFile = config.check("kinematics", yarp::os::Value(""),
"AMOR kinematics description").asString();

yarp::os::Property cartesianDeviceOptions;

if (!cartesianDeviceOptions.fromConfigFile(kinematicsFile))
{
CD_ERROR("Cannot read from --kinematics \"%s\".\n", kinematicsFile.c_str());
return false;
}

cartesianDeviceOptions.put("device", "KdlSolver");
cartesianDeviceOptions.put("mins", yarp::os::Value::makeList(qMin.toString().c_str()));
cartesianDeviceOptions.put("maxs", yarp::os::Value::makeList(qMax.toString().c_str()));

if (!cartesianDevice.open(cartesianDeviceOptions))
{
CD_ERROR("Solver device not valid.\n");
return false;
}

if (!cartesianDevice.view(iCartesianSolver))
{
CD_ERROR("Could not view iCartesianSolver.\n");
close();
return false;
}

CD_SUCCESS("Created solver device!\n");

return true;
}

Expand Down
3 changes: 0 additions & 3 deletions libraries/YarpPlugins/AsibotSolver/AsibotSolver.hpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -81,9 +81,6 @@ class AsibotSolver : public yarp::dev::DeviceDriver, public ICartesianSolver
// Perform inverse dynamics.
virtual bool invDyn(const std::vector<double> &q,const std::vector<double> &qdot,const std::vector<double> &qdotdot, const std::vector< std::vector<double> > &fexts, std::vector<double> &t);

// Set joint limits.
virtual bool setLimits(const std::vector<double> &qMin, const std::vector<double> &qMax);

// -------- DeviceDriver declarations. Implementation in IDeviceImpl.cpp --------

/**
Expand Down
42 changes: 23 additions & 19 deletions libraries/YarpPlugins/AsibotSolver/DeviceDriverImpl.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -42,37 +42,41 @@ bool roboticslab::AsibotSolver::open(yarp::os::Searchable& config)

CD_INFO("AsibotSolver using A0: %f, A1: %f, A2: %f, A3: %f.\n", A0, A1, A2, A3);

yarp::os::Value mins = config.check("mins", yarp::os::Value::getNullValue(), "minimum joint limits");

if (!mins.isNull())
if (!config.check("mins") || !config.check("maxs"))
{
yarp::os::Bottle * b = mins.asList();
CD_ERROR("Missing 'mins' and/or 'maxs' option(s).\n");
return false;
}

for (int i = 0; i < b->size(); i++)
{
qMin.push_back(b->get(i).asDouble());
}
yarp::os::Bottle *mins = config.findGroup("mins", "joint lower limits").get(1).asList();
yarp::os::Bottle *maxs = config.findGroup("maxs", "joint upper limits").get(1).asList();

if (mins == YARP_NULLPTR || maxs == YARP_NULLPTR)
{
CD_ERROR("Empty 'mins' and/or 'maxs' option(s)\n");
return false;
}
else

if (mins->size() != NUM_MOTORS || maxs->size() != NUM_MOTORS)
{
qMin.resize(NUM_MOTORS, -90);
CD_ERROR("mins.size(), maxs.size() (%d, %d) != NUM_MOTORS (%d)\n", mins->size(), maxs->size(), NUM_MOTORS);
return false;
}

yarp::os::Value maxs = config.check("maxs", yarp::os::Value::getNullValue(), "maximum joint limits");
qMin.resize(NUM_MOTORS);
qMax.resize(NUM_MOTORS);

if (!maxs.isNull())
for (int i = 0; i < NUM_MOTORS; i++)
{
yarp::os::Bottle * b = maxs.asList();
qMin[i] = mins->get(i).asDouble();
qMax[i] = maxs->get(i).asDouble();

for (int i = 0; i < b->size(); i++)
if (qMin[i] >= qMax[i])
{
qMax.push_back(b->get(i).asDouble());
CD_ERROR("qMin >= qMax (%f >= %f) at joint %d\n", qMin[i], qMax[i], i);
return false;
}
}
else
{
qMax.resize(NUM_MOTORS, 90);
}

std::string strategy = config.check("invKinStrategy", yarp::os::Value(DEFAULT_STRATEGY), "IK configuration strategy").asString();

Expand Down
23 changes: 0 additions & 23 deletions libraries/YarpPlugins/AsibotSolver/ICartesianSolverImpl.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -497,26 +497,3 @@ bool roboticslab::AsibotSolver::invDyn(const std::vector<double> &q,const std::v
}

// -----------------------------------------------------------------------------

bool roboticslab::AsibotSolver::setLimits(const std::vector<double> &qMin, const std::vector<double> &qMax)
{
for (int motor = 0; motor < NUM_MOTORS; motor++)
{
if (qMin[motor] > qMax[motor])
{
CD_ERROR("qMin > qMax at joint q%d (%f vs %f).\n", motor + 1, qMin[motor], qMax[motor]);
return false;
}
else if (qMin[motor] == qMax[motor])
{
CD_WARNING("qMin = qMax at joint q%d.\n", motor + 1);
}

this->qMax[motor] = qMax[motor];
this->qMin[motor] = qMin[motor];
}

return true;
}

// -----------------------------------------------------------------------------
56 changes: 25 additions & 31 deletions libraries/YarpPlugins/BasicCartesianControl/DeviceDriverImpl.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -33,22 +33,8 @@ bool roboticslab::BasicCartesianControl::open(yarp::os::Searchable& config) {
return false;
}

std::string solverStr = config.check("solver",yarp::os::Value(DEFAULT_SOLVER),"cartesian solver").asString();
std::string robotStr = config.check("robot",yarp::os::Value(DEFAULT_ROBOT),"robot device").asString();

yarp::os::Property solverOptions;
solverOptions.fromString( config.toString() );
solverOptions.put("device",solverStr);

solverDevice.open(solverOptions);
if( ! solverDevice.isValid() ) {
CD_ERROR("solver device not valid: %s.\n",solverStr.c_str());
return false;
}
if( ! solverDevice.view(iCartesianSolver) ) {
CD_ERROR("Could not view iCartesianSolver in: %s.\n",solverStr.c_str());
return false;
}
std::string solverStr = config.check("solver",yarp::os::Value(DEFAULT_SOLVER),"cartesian solver").asString();

yarp::os::Property robotOptions;
robotOptions.fromString( config.toString() );
Expand Down Expand Up @@ -86,30 +72,38 @@ bool roboticslab::BasicCartesianControl::open(yarp::os::Searchable& config) {
iEncoders->getAxes(&numRobotJoints);
CD_INFO("numRobotJoints: %d.\n",numRobotJoints);

iCartesianSolver->getNumJoints( &numSolverJoints );
CD_INFO("numSolverJoints: %d.\n",numSolverJoints);

if( numRobotJoints != numSolverJoints )
{
CD_WARNING("numRobotJoints(%d) != numSolverJoints(%d) !!!\n",numRobotJoints,numSolverJoints);
}

std::vector<double> qMin, qMax;
yarp::os::Bottle qMin, qMax;
for(unsigned int joint=0;joint<numRobotJoints;joint++)
{
double min, max;
iControlLimits->getLimits(joint,&min,&max);
qMin.push_back(min);
qMax.push_back(max);
qMin.addDouble(min);
qMax.addDouble(max);
CD_INFO("Joint %d limits: [%f,%f]\n",joint,min,max);
}
if( qMin[0] == qMax[0] )
{
CD_WARNING("Not setting joint limits on solver, because qMin[0] == qMax[0].\n");

yarp::os::Property solverOptions;
solverOptions.fromString( config.toString() );
solverOptions.put("device",solverStr);
solverOptions.put("mins", yarp::os::Value::makeList(qMin.toString().c_str()));
solverOptions.put("maxs", yarp::os::Value::makeList(qMax.toString().c_str()));

solverDevice.open(solverOptions);
if( ! solverDevice.isValid() ) {
CD_ERROR("solver device not valid: %s.\n",solverStr.c_str());
return false;
}
else
if( ! solverDevice.view(iCartesianSolver) ) {
CD_ERROR("Could not view iCartesianSolver in: %s.\n",solverStr.c_str());
return false;
}

iCartesianSolver->getNumJoints( &numSolverJoints );
CD_INFO("numSolverJoints: %d.\n",numSolverJoints);

if( numRobotJoints != numSolverJoints )
{
iCartesianSolver->setLimits(qMin,qMax);
CD_WARNING("numRobotJoints(%d) != numSolverJoints(%d) !!!\n",numRobotJoints,numSolverJoints);
}

return this->start();
Expand Down
10 changes: 0 additions & 10 deletions libraries/YarpPlugins/ICartesianSolver.h
View file
Original file line number Diff line number Diff line change
Expand Up @@ -173,16 +173,6 @@ class ICartesianSolver
*/
virtual bool invDyn(const std::vector<double> &q,const std::vector<double> &qdot, const std::vector<double> &qdotdot, const std::vector< std::vector<double> > &fexts, std::vector<double> &t) = 0;

/**
* @brief Set joint limits
*
* @param qMin Vector of minimum joint values expressed in meters or degrees.
* @param qMax Vector of maximum joint values expressed in meters or degrees.
*
* @return true on success, false otherwise
*/
virtual bool setLimits(const std::vector<double> &qMin, const std::vector<double> &qMax) = 0;

};

} // namespace roboticslab
Expand Down
34 changes: 31 additions & 3 deletions libraries/YarpPlugins/KdlSolver/DeviceDriverImpl.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -172,11 +172,39 @@ bool roboticslab::KdlSolver::open(yarp::os::Searchable& config)

qMax.resize(chain.getNrOfJoints());
qMin.resize(chain.getNrOfJoints());
//-- Limits [ -pi , pi ].

//-- Joint limits
if (!fullConfig.check("mins") || !fullConfig.check("maxs"))
{
CD_ERROR("Missing 'mins' and/or 'maxs' option(s).\n");
return false;
}

yarp::os::Bottle *maxs = fullConfig.findGroup("maxs", "joint upper limits").get(1).asList();
yarp::os::Bottle *mins = fullConfig.findGroup("mins", "joint lower limits").get(1).asList();

if (maxs == YARP_NULLPTR || mins == YARP_NULLPTR)
{
CD_ERROR("Empty 'mins' and/or 'maxs' option(s)\n");
return false;
}

if (maxs->size() != chain.getNrOfJoints() || mins->size() != chain.getNrOfJoints())
{
CD_ERROR("chain.getNrOfJoints (%d) != maxs.size(), mins.size() (%d, %d)\n", chain.getNrOfJoints(), maxs->size(), mins->size());
return false;
}

for (int motor=0; motor<chain.getNrOfJoints(); motor++)
{
qMax(motor) = M_PI;
qMin(motor) = -M_PI;
qMax(motor) = maxs->get(motor).asDouble();
qMin(motor) = mins->get(motor).asDouble();

if (qMin(motor) >= qMax(motor))
{
CD_ERROR("qMin >= qMax (%f >= %f) at joint %d\n", qMin(motor), qMax(motor), motor);
return false;
}
}

//-- Precision and max iterations for IK solver.
Expand Down
12 changes: 0 additions & 12 deletions libraries/YarpPlugins/KdlSolver/ICartesianSolverImpl.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -331,15 +331,3 @@ bool roboticslab::KdlSolver::invDyn(const std::vector<double> &q,const std::vect
}

// -----------------------------------------------------------------------------

bool roboticslab::KdlSolver::setLimits(const std::vector<double> &qMin, const std::vector<double> &qMax)
{
for (int motor = 0; motor < getChain().getNrOfJoints(); motor++)
{
this->qMax(motor) = KinRepresentation::degToRad(qMax[motor]);
this->qMin(motor) = KinRepresentation::degToRad(qMin[motor]);
}
return true;
}

// -----------------------------------------------------------------------------
3 changes: 0 additions & 3 deletions libraries/YarpPlugins/KdlSolver/KdlSolver.hpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -85,9 +85,6 @@ class KdlSolver : public yarp::dev::DeviceDriver, public ICartesianSolver
// Perform inverse dynamics.
virtual bool invDyn(const std::vector<double> &q,const std::vector<double> &qdot,const std::vector<double> &qdotdot, const std::vector< std::vector<double> > &fexts, std::vector<double> &t);

// Set joint limits.
virtual bool setLimits(const std::vector<double> &qMin, const std::vector<double> &qMax);

// -------- DeviceDriver declarations. Implementation in IDeviceImpl.cpp --------

/**
Expand Down
Loading

0 comments on commit 7ea960b

Please sign in to comment.