Skip to content

Commit

Permalink
ControlBoardDriver: support use of IJointCoupling devices in gazebo_y…
Browse files Browse the repository at this point in the history
…arp_controlboard and drop support for hardcoded HandMK5 coupling (#671)

Co-authored-by: Silvio Traversaro <silvio@traversaro.it>
  • Loading branch information
Nicogene and traversaro authored Dec 21, 2023
1 parent b57b299 commit c892802
Showing 10 changed files with 300 additions and 475 deletions.
7 changes: 7 additions & 0 deletions CHANGELOG.md
Original file line number Diff line number Diff line change
@@ -5,6 +5,13 @@ The format of this document is based on [Keep a Changelog](https://keepachangelo

## [Unreleased]

### Added
- The `gazebo_yarp_controlboard` plugin gained support to load at runtime arbitrary couplings specified by a yarp device that exposes the [`yarp::dev::IJointCoupling`](https://github.com/robotology/yarp/blob/v3.9.0/src/libYARP_dev/src/yarp/dev/IJointCoupling.h#L16) interface. The name of the yarp device used to load the coupling is passed via the `device` parameter in the `COUPLING` group. For an example of PR that uses this new feature, check https://github.com/icub-tech-iit/ergocub-software/pull/178 .

### Removed

- The support for passing the `icub_hand_mk5` parameter in the `COUPLING` the `gazebo_yarp_controlboard` plugin was removed. This breaks compatibility with models contained in ergocub-software <= 0.6.0, to use those models use a newer version of ergocub-software.

## [4.9.0] - 2023-10-31

### Changed
4 changes: 2 additions & 2 deletions CMakeLists.txt
Original file line number Diff line number Diff line change
@@ -46,7 +46,7 @@ if(GAZEBO_YARP_PLUGINS_HAS_YARP_ROBOTINTERFACE)
list(APPEND YARP_ADDITIONAL_COMPONENTS_REQUIRED "robotinterface")
endif()

find_package(YARP 3.6 REQUIRED COMPONENTS os sig dev math idl_tools ${YARP_ADDITIONAL_COMPONENTS_REQUIRED})
find_package(YARP 3.9 REQUIRED COMPONENTS os sig dev math idl_tools ${YARP_ADDITIONAL_COMPONENTS_REQUIRED})
find_package(Gazebo REQUIRED)
if (Gazebo_VERSION_MAJOR LESS 11.0)
message(status "Gazebo version : " ${Gazebo_VERSION_MAJOR}.${Gazebo_VERSION_MINOR}.${Gazebo_VERSION_PATCH})
@@ -60,7 +60,7 @@ if(MSVC)
# It is tipically better to use target_include_directories in place of
# global include_directories, but in this case it is more compact to use include_directories
include_directories(${OGRE_Paging_INCLUDE_DIRS})

# Workaround for https://github.com/robotology/gazebo-yarp-plugins/issues/482
add_definitions(-D__TBB_NO_IMPLICIT_LINKAGE=1)
endif()
9 changes: 8 additions & 1 deletion plugins/controlboard/include/yarp/dev/ControlBoardDriver.h
Original file line number Diff line number Diff line change
@@ -18,6 +18,7 @@
#include <yarp/dev/ControlBoardInterfaces.h>
#include <yarp/dev/IControlMode.h>
#include <yarp/dev/IInteractionMode.h>
#include <yarp/dev/IJointCoupling.h>
#include <yarp/dev/IRemoteVariables.h>
#include <yarp/dev/IVirtualAnalogSensor.h>
#include <yarp/sig/Vector.h>
@@ -372,6 +373,9 @@ class yarp::dev::GazeboYarpControlBoardDriver:
std::vector<Range> m_jointPosLimits;
std::vector<Range> m_jointVelLimits;

std::vector<Range> m_actuatedAxesPosLimits;
std::vector<Range> m_actuatedAxesVelLimits;

/**
* The zero position is the position of the GAZEBO joint that will be read as the starting one
* i.e. getEncoder(j)=m_zeroPosition+gazebo.getEncoder(j);
@@ -410,11 +414,14 @@ class yarp::dev::GazeboYarpControlBoardDriver:

//trajectory generator
std::vector<TrajectoryGenerator*> m_trajectory_generator;
std::vector<BaseCouplingHandler*> m_coupling_handler;
BaseCouplingHandler* m_coupling_handler{nullptr};
std::vector<RampFilter*> m_speed_ramp_handler;
std::vector<Watchdog*> m_velocity_watchdog;
std::vector<Watchdog*> m_velocityControl;

yarp::dev::IJointCoupling* m_ijointcoupling{nullptr};
yarp::dev::PolyDriver m_coupling_driver;


yarp::sig::Vector m_trajectoryGenerationReferencePosition; /**< reference position for trajectory generation in position mode [Degrees] */
yarp::sig::Vector m_trajectoryGenerationReferenceSpeed; /**< reference speed for trajectory generation in position mode [Degrees/Seconds]*/
57 changes: 0 additions & 57 deletions plugins/controlboard/include/yarp/dev/ControlBoardDriverCoupling.h
Original file line number Diff line number Diff line change
@@ -223,61 +223,4 @@ class HandMk4CouplingHandler : public BaseCouplingHandler
std::vector<double> pinkie_lut;
};

class HandMk5CouplingHandler : public BaseCouplingHandler
{

public:
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);
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, const yarp::sig::Vector& pos_feedback);
yarp::sig::Vector decoupleRefTrq (yarp::sig::Vector& trq_ref);

private:
/**
* Parameters from https://icub-tech-iit.github.io/documentation/hands/hands_mk5_coupling
*/
struct FingerParameters
{
double L0x;
double L0y;
double q2bias;
double q1off;
double k;
double d;
double l;
double b;
};

std::map<std::string, FingerParameters> mFingerParameters;

/*
* 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 std::string& finger_name);

/*
* 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 std::string& finger_name);

};

#endif //GAZEBOYARP_COUPLING_H
Loading

0 comments on commit c892802

Please sign in to comment.