Skip to content

Commit

Permalink
Merge pull request #662 from robotology/refactorMK5Coupling
Browse files Browse the repository at this point in the history 
HandMk5CouplingHandler: refactor it in order to make it configurable
  • Loading branch information
@traversaro
traversaro authored Sep 13, 2023
2 parents 0fd9300 + 9de8b0e commit ca16bd0
Show file tree
Hide file tree
Showing 5 changed files with 82 additions and 168 deletions.
3 changes: 3 additions & 0 deletions CHANGELOG.md
View file
Original file line number Diff line number Diff line change
Expand Up @@ -5,6 +5,9 @@ The format of this document is based on [Keep a Changelog](https://keepachangelo

## [Unreleased]

### Changed
- The `icub_hand_mk5` coupling handler has been refactored, it now requires the group `COUPLING_PARAMS` that allows to differentiate between the coupling mk5.0 and mk5.1(https://github.com/robotology/gazebo-yarp-plugins/pull/662).

## [4.8.0] - 2023-07-31

### Added
Expand Down
91 changes: 5 additions & 86 deletions plugins/controlboard/include/yarp/dev/ControlBoardDriverCoupling.h
View file
Original file line number Diff line number Diff line change
Expand Up @@ -230,6 +230,8 @@ class HandMk5CouplingHandler : public BaseCouplingHandler
HandMk5CouplingHandler (gazebo::physics::Model* model, yarp::sig::VectorOf<int> coupled_joints, std::vector<std::string> coupled_joint_names, std::vector<Range> coupled_joint_limits);

public:

bool parseFingerParameters(yarp::os::Bottle& hand_params);
bool decouplePos (yarp::sig::Vector& current_pos);
bool decoupleVel (yarp::sig::Vector& current_vel);
bool decoupleAcc (yarp::sig::Vector& current_acc);
Expand All @@ -247,8 +249,6 @@ class HandMk5CouplingHandler : public BaseCouplingHandler
{
double L0x;
double L0y;
double L1x;
double L1y;
double q2bias;
double q1off;
double k;
Expand All @@ -257,41 +257,7 @@ class HandMk5CouplingHandler : public BaseCouplingHandler
double b;
};

const FingerParameters mParamsThumb =
{
.L0x = -0.00555,
.L0y = 0.00285,
.q2bias = 180.0,
.q1off = 4.29,
.k = 0.0171,
.d = 0.02006,
.l = 0.0085,
.b = 0.00624
};

const FingerParameters mParamsIndex =
{
.L0x = -0.0050,
.L0y = 0.0040,
.q2bias = 173.35,
.q1off = 2.86,
.k = 0.02918,
.d = 0.03004,
.l = 0.00604,
.b = 0.0064
};

const FingerParameters mParamsPinky =
{
.L0x = -0.0050,
.L0y = 0.0040,
.q2bias = 170.54,
.q1off = 3.43,
.k = 0.02425,
.d = 0.02504,
.l = 0.00608,
.b = 0.0064
};
std::map<std::string, FingerParameters> mFingerParameters;

/*
* This method implements the law q2 = q2(q1) from
Expand All @@ -300,29 +266,7 @@ class HandMk5CouplingHandler : public BaseCouplingHandler
*
* The inputs q1 and the return value of the function are in degrees.
*/
double evaluateCoupledJoint(const double& q1, const FingerParameters& parameters);

/*
* This method evalutes the relative angle between the proximal and distal joints of the thumb finger.
*
* The input q1 and the return value of the function are in degrees.
*/
double evaluateCoupledJointThumb(const double& q1);

/*
* This method evalutes the relative angle between the proximal and distal joints of the index finger.
* This is also valid for the middle and ring fingers.
*
* The input q1 and the return value of the function are in degrees.
*/
double evaluateCoupledJointIndex(const double& q1);

/*
* This method evalutes the relative angle between the proximal and distal joints of the pinky finger.
*
* The input q1 and the return value of the function are in degrees.
*/
double evaluateCoupledJointPinky(const double& q1);
double evaluateCoupledJoint(const double& q1, const std::string& finger_name);

/*
* This method implements the law \frac{\partial{q2}}{\partial{q1}} from
Expand All @@ -332,33 +276,8 @@ class HandMk5CouplingHandler : public BaseCouplingHandler
*
* The input q1 is in degrees.
*/
double evaluateCoupledJointJacobian(const double& q1, const FingerParameters& parameters);

/*
* This method evalutes the jacobian of the relative angle between the proximal and distal joints of the thumb finger
* with respect to the proximal joint.
*
* The input q1 is in degrees.
*/
double evaluateCoupledJointJacobianThumb(const double& q1);

/*
* This method evalutes the jacobian of the relative angle between the proximal and distal joints of the index finger
* with respect to the proximal joint.
*
* This is also valid for the middle and ring fingers.
*
* The input q1 is in degrees.
*/
double evaluateCoupledJointJacobianIndex(const double& q1);
double evaluateCoupledJointJacobian(const double& q1, const std::string& finger_name);

/*
* This method evalutes the jacobian of the relative angle between the proximal and distal joints of the pinky finger
* with respect to the proximal joint.
*
* The input q1 is in degrees.
*/
double evaluateCoupledJointJacobianPinky(const double& q1);
};

#endif //GAZEBOYARP_COUPLING_H
26 changes: 13 additions & 13 deletions plugins/controlboard/src/ControlBoard.cc
View file
Original file line number Diff line number Diff line change
Expand Up @@ -43,12 +43,12 @@ GZ_REGISTER_MODEL_PLUGIN(GazeboYarpControlBoard)
m_wrapper.close();
}

if (m_iVirtAnalogSensorWrap)
if (m_iVirtAnalogSensorWrap)
{
m_iVirtAnalogSensorWrap->detachAll();
m_iVirtAnalogSensorWrap = nullptr;
}

if (m_virtAnalogSensorWrapper.isValid())
{
m_virtAnalogSensorWrapper.close();
Expand Down Expand Up @@ -110,15 +110,15 @@ GZ_REGISTER_MODEL_PLUGIN(GazeboYarpControlBoard)
}
#ifndef GAZEBO_YARP_PLUGINS_DISABLE_IMPLICIT_NETWORK_WRAPPERS
bool disable_wrapper = m_parameters.check("disableImplicitNetworkWrapper");

if (disable_wrapper && !m_parameters.check("yarpDeviceName"))
{
yError() << "GazeboYarpControlBoard : missing yarpDeviceName parameter for one device in robot " << m_robotName;
return;
}
if (!disable_wrapper)


if (!disable_wrapper)
{
yarp::os::Bottle wrapper_group = m_parameters.findGroup("WRAPPER");
if(wrapper_group.isNull())
Expand Down Expand Up @@ -166,18 +166,18 @@ GZ_REGISTER_MODEL_PLUGIN(GazeboYarpControlBoard)
virt_group.append(yarp::os::Bottle(networks));
}



yarp::os::Bottle *netList = wrapper_group.find("networks").asList();

if (netList->isNull()) {
yCError(GAZEBOCONTROLBOARD) <<"net list to attach to was not found, load failed.";
m_wrapper.close();
return;
}





for (int n = 0; n < netList->size(); n++)
{
yarp::dev::PolyDriverDescriptor newPoly;
Expand All @@ -201,9 +201,9 @@ GZ_REGISTER_MODEL_PLUGIN(GazeboYarpControlBoard)
scopedDeviceName = m_robotName + "::" + m_parameters.find("yarpDeviceName").asString();
m_scopedDeviceName = scopedDeviceName;
}

newPoly.poly = GazeboYarpPlugins::Handler::getHandler()->getDevice(scopedDeviceName);

if( newPoly.poly != NULL)
{
// device already exists, use it, setting it againg to increment the usage counter.
Expand Down Expand Up @@ -276,7 +276,7 @@ GZ_REGISTER_MODEL_PLUGIN(GazeboYarpControlBoard)
return;
}
}

if (!m_iWrap || !m_iWrap->attachAll(m_controlBoards))
{
yCError(GAZEBOCONTROLBOARD) << "error while attaching wrapper to device.";
Expand Down
13 changes: 13 additions & 0 deletions plugins/controlboard/src/ControlBoardDriver.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -339,7 +339,20 @@ bool GazeboYarpControlBoardDriver::gazebo_init()
}
else if (coupling_bottle->get(0).asString()=="icub_hand_mk5")
{
yarp::os::Bottle& coupling_group_bottle = m_pluginParameters.findGroup("COUPLING_PARAMS");

if(coupling_group_bottle.isNull())
{
yCError(GAZEBOCONTROLBOARD) << "Missing param in COUPLING_PARAMS section";
return false;
}

BaseCouplingHandler* cpl = new HandMk5CouplingHandler(m_robot,coupled_joints, coupled_joint_names, coupled_joint_limits);
if (! static_cast<HandMk5CouplingHandler*>(cpl)->parseFingerParameters(coupling_group_bottle))
{
yCError(GAZEBOCONTROLBOARD) << "Error parsing coupling parameter, wrong size of the finger parameters list";
return false;
}
m_coupling_handler.push_back(cpl);
yCInfo(GAZEBOCONTROLBOARD) << "using icub_hand_mk5";
}
Expand Down
Loading

0 comments on commit ca16bd0

Please sign in to comment.