Skip to content

Commit

Permalink
Merge pull request #650 from xEnVrE/fix/hand_coupling
Browse files Browse the repository at this point in the history 
Update coupling handlers of ergoCub Hand MK5
  • Loading branch information
@traversaro
traversaro committed May 23, 2023
2 parents ea7a48e + 27de5f4 commit 830ab76
Show file tree
Hide file tree
Showing 5 changed files with 368 additions and 274 deletions.
3 changes: 3 additions & 0 deletions CHANGELOG.md
View file
Original file line number Diff line number Diff line change
Expand Up @@ -8,10 +8,13 @@ The format of this document is based on [Keep a Changelog](https://keepachangelo
### Changed

- The `gazebo_imu` plugin used to output orientation measurements as if the sensor was zero-aligned with the world frame, regardless of the initial orientation of the part the sensor is attached to. To match most real-world IMUs, the default behavior was changed to allow measuring orientation with regard to the world frame at all times. The old behavior can be restored by setting the `useInitialSensorOrientationAsReference` config option (https://github.com/robotology/gazebo-yarp-plugins/pull/639).
- The `gazebo_yarp_controlboard` plugin is compiled using C++ 20 compiler features (https://github.com/robotology/gazebo-yarp-plugins/pull/650).
- The `BaseCouplingHandler::decoupleVelRef()` methods within `gazebo_yarp_controlboard` provides an input argument containing the full joints position feedback, to be used to implement velocity decoupling laws that depend on the joints position.

### Fixed

- Fix wrong install include for gazebo_yarp_lib_common library (https://github.com/robotology/gazebo-yarp-plugins/pull/644).
- Fix wrong implementation of coupling handlers of ergoCub MK5 hands within the `gazebo_yarp_controlboard` plugin (https://github.com/robotology/gazebo-yarp-plugins/pull/650).

## [4.6.0] - 2023-01-13

Expand Down
1 change: 1 addition & 0 deletions plugins/controlboard/CMakeLists.txt
View file
Original file line number Diff line number Diff line change
Expand Up @@ -47,3 +47,4 @@ add_gazebo_yarp_plugin_target(LIBRARY_NAME controlboard
LINKED_LIBRARIES gazebo_yarp_lib_common gazebo_yarp_singleton ${YARP_LIBRARIES} ${GAZEBO_LIBRARIES} ${Boost_LIBRARIES}
HEADERS ${controlBoard_headers}
SOURCES ${controlBoard_source})
target_compile_features(gazebo_yarp_controlboard PUBLIC cxx_std_20)
150 changes: 131 additions & 19 deletions plugins/controlboard/include/yarp/dev/ControlBoardDriverCoupling.h
View file
Original file line number Diff line number Diff line change
Expand Up @@ -46,7 +46,7 @@ class BaseCouplingHandler
virtual bool checkJointIsCoupled(int joint);

virtual yarp::sig::Vector decoupleRefPos (yarp::sig::Vector& pos_ref) = 0;
virtual yarp::sig::Vector decoupleRefVel (yarp::sig::Vector& vel_ref) = 0;
virtual yarp::sig::Vector decoupleRefVel (yarp::sig::Vector& vel_ref, const yarp::sig::Vector& pos_feedback) = 0;
virtual yarp::sig::Vector decoupleRefTrq (yarp::sig::Vector& trq_ref) = 0;

virtual void setCoupledJointLimit(int joint, const double& min, const double& max);
Expand All @@ -66,7 +66,7 @@ class EyesCouplingHandler : public BaseCouplingHandler
bool decoupleTrq (yarp::sig::Vector& current_trq);

yarp::sig::Vector decoupleRefPos (yarp::sig::Vector& pos_ref);
yarp::sig::Vector decoupleRefVel (yarp::sig::Vector& vel_ref);
yarp::sig::Vector decoupleRefVel (yarp::sig::Vector& vel_ref, const yarp::sig::Vector& pos_feedback);
yarp::sig::Vector decoupleRefTrq (yarp::sig::Vector& trq_ref);
};

Expand All @@ -83,7 +83,7 @@ class ThumbCouplingHandler : public BaseCouplingHandler
bool decoupleTrq (yarp::sig::Vector& current_trq);

yarp::sig::Vector decoupleRefPos (yarp::sig::Vector& pos_ref);
yarp::sig::Vector decoupleRefVel (yarp::sig::Vector& vel_ref);
yarp::sig::Vector decoupleRefVel (yarp::sig::Vector& vel_ref, const yarp::sig::Vector& pos_feedback);
yarp::sig::Vector decoupleRefTrq (yarp::sig::Vector& trq_ref);
};

Expand All @@ -100,7 +100,7 @@ class IndexCouplingHandler : public BaseCouplingHandler
bool decoupleTrq (yarp::sig::Vector& current_trq);

yarp::sig::Vector decoupleRefPos (yarp::sig::Vector& pos_ref);
yarp::sig::Vector decoupleRefVel (yarp::sig::Vector& vel_ref);
yarp::sig::Vector decoupleRefVel (yarp::sig::Vector& vel_ref, const yarp::sig::Vector& pos_feedback);
yarp::sig::Vector decoupleRefTrq (yarp::sig::Vector& trq_ref);
};

Expand All @@ -117,7 +117,7 @@ class MiddleCouplingHandler : public BaseCouplingHandler
bool decoupleTrq (yarp::sig::Vector& current_trq);

yarp::sig::Vector decoupleRefPos (yarp::sig::Vector& pos_ref);
yarp::sig::Vector decoupleRefVel (yarp::sig::Vector& vel_ref);
yarp::sig::Vector decoupleRefVel (yarp::sig::Vector& vel_ref, const yarp::sig::Vector& pos_feedback);
yarp::sig::Vector decoupleRefTrq (yarp::sig::Vector& trq_ref);
};

Expand All @@ -134,7 +134,7 @@ class PinkyCouplingHandler : public BaseCouplingHandler
bool decoupleTrq (yarp::sig::Vector& current_trq);

yarp::sig::Vector decoupleRefPos (yarp::sig::Vector& pos_ref);
yarp::sig::Vector decoupleRefVel (yarp::sig::Vector& vel_ref);
yarp::sig::Vector decoupleRefVel (yarp::sig::Vector& vel_ref, const yarp::sig::Vector& pos_feedback);
yarp::sig::Vector decoupleRefTrq (yarp::sig::Vector& trq_ref);
};

Expand All @@ -151,7 +151,7 @@ class FingersAbductionCouplingHandler : public BaseCouplingHandler
bool decoupleTrq (yarp::sig::Vector& current_trq);

yarp::sig::Vector decoupleRefPos (yarp::sig::Vector& pos_ref);
yarp::sig::Vector decoupleRefVel (yarp::sig::Vector& vel_ref);
yarp::sig::Vector decoupleRefVel (yarp::sig::Vector& vel_ref, const yarp::sig::Vector& pos_feedback);
yarp::sig::Vector decoupleRefTrq (yarp::sig::Vector& trq_ref);
};

Expand All @@ -168,7 +168,7 @@ class CerHandCouplingHandler : public BaseCouplingHandler
bool decoupleTrq (yarp::sig::Vector& current_trq);

yarp::sig::Vector decoupleRefPos (yarp::sig::Vector& pos_ref);
yarp::sig::Vector decoupleRefVel (yarp::sig::Vector& vel_ref);
yarp::sig::Vector decoupleRefVel (yarp::sig::Vector& vel_ref, const yarp::sig::Vector& pos_feedback);
yarp::sig::Vector decoupleRefTrq (yarp::sig::Vector& trq_ref);
};

Expand All @@ -185,7 +185,7 @@ class HandMk3CouplingHandler : public BaseCouplingHandler
bool decoupleTrq (yarp::sig::Vector& current_trq);

yarp::sig::Vector decoupleRefPos (yarp::sig::Vector& pos_ref);
yarp::sig::Vector decoupleRefVel (yarp::sig::Vector& vel_ref);
yarp::sig::Vector decoupleRefVel (yarp::sig::Vector& vel_ref, const yarp::sig::Vector& pos_feedback);
yarp::sig::Vector decoupleRefTrq (yarp::sig::Vector& trq_ref);

protected:
Expand All @@ -210,7 +210,7 @@ class HandMk4CouplingHandler : public BaseCouplingHandler
bool decoupleTrq (yarp::sig::Vector& current_trq);

yarp::sig::Vector decoupleRefPos (yarp::sig::Vector& pos_ref);
yarp::sig::Vector decoupleRefVel (yarp::sig::Vector& vel_ref);
yarp::sig::Vector decoupleRefVel (yarp::sig::Vector& vel_ref, const yarp::sig::Vector& pos_feedback);
yarp::sig::Vector decoupleRefTrq (yarp::sig::Vector& trq_ref);

protected:
Expand All @@ -236,17 +236,129 @@ class HandMk5CouplingHandler : public BaseCouplingHandler
bool decoupleTrq (yarp::sig::Vector& current_trq);

yarp::sig::Vector decoupleRefPos (yarp::sig::Vector& pos_ref);
yarp::sig::Vector decoupleRefVel (yarp::sig::Vector& vel_ref);
yarp::sig::Vector decoupleRefVel (yarp::sig::Vector& vel_ref, const yarp::sig::Vector& pos_feedback);
yarp::sig::Vector decoupleRefTrq (yarp::sig::Vector& trq_ref);

protected:
double decouple (double q2, std::vector<double>& lut);

const int LUTSIZE;

std::vector<double> thumb_lut;
std::vector<double> index_lut;
std::vector<double> pinkie_lut;
private:
/**
* Parameters from https://icub-tech-iit.github.io/documentation/hands/hands_mk5_coupling
*/
struct FingerParameters
{
double L0x;
double L0y;
double L1x;
double L1y;
double q2bias;
double q1off;
double k;
double d;
double l;
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
};

/*
* This method implements the law q2 = q2(q1) from
* https://icub-tech-iit.github.io/documentation/hands/hands_mk5_coupling,
* i.e., the absolute angle of the distal joint q2 with respect to the palm.
*
* 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);

/*
* This method implements the law \frac{\partial{q2}}{\partial{q1}} from
* https://icub-tech-iit.github.io/documentation/hands/hands_mk5_coupling,
* i.e., the jacobian of the absolute angle of the distal joint q2 measured from the palm,
* with respect to the proximal joint q1.
*
* 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);

/*
* 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
2 changes: 1 addition & 1 deletion plugins/controlboard/src/ControlBoardDriver.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -646,7 +646,7 @@ void GazeboYarpControlBoardDriver::onUpdate(const gazebo::common::UpdateInfo& _i
if (m_coupling_handler[cpl_cnt])
{
m_motReferencePositions = m_coupling_handler[cpl_cnt]->decoupleRefPos(m_jntReferencePositions);
m_motReferenceVelocities = m_coupling_handler[cpl_cnt]->decoupleRefVel(m_jntReferenceVelocities);
m_motReferenceVelocities = m_coupling_handler[cpl_cnt]->decoupleRefVel(m_jntReferenceVelocities, m_motPositions);
m_motReferenceTorques = m_coupling_handler[cpl_cnt]->decoupleRefTrq(m_jntReferenceTorques);
}
}
Expand Down
Loading

0 comments on commit 830ab76

Please sign in to comment.