Skip to content

Commit

Permalink
ControlBoardDriver: start using IJointCoupling for handMk5
Browse files Browse the repository at this point in the history
  • Loading branch information
@Nicogene
Nicogene committed Dec 13, 2023
1 parent b57b299 commit 84f63a9
Show file tree
Hide file tree
Showing 2 changed files with 49 additions and 25 deletions.
4 changes: 4 additions & 0 deletions plugins/controlboard/include/yarp/dev/ControlBoardDriver.h
View file
Original file line number Diff line number Diff line change
Expand Up @@ -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>
Expand Down Expand Up @@ -415,6 +416,9 @@ class yarp::dev::GazeboYarpControlBoardDriver:
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]*/
Expand Down
70 changes: 45 additions & 25 deletions plugins/controlboard/src/ControlBoardDriver.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -337,24 +337,26 @@ bool GazeboYarpControlBoardDriver::gazebo_init()
m_coupling_handler.push_back(cpl);
yCInfo(GAZEBOCONTROLBOARD) << "using icub_left_hand_mk4";
}
else if (coupling_bottle->get(0).asString()=="icub_hand_mk5")
else if (coupling_bottle->check("device") && coupling_bottle->find("device").asString()=="couplingXCubHandMk5")
{
yarp::os::Bottle& coupling_group_bottle = m_pluginParameters.findGroup("COUPLING_PARAMS");
yarp::os::Property coupling_handler_conf;

if(coupling_group_bottle.isNull())
coupling_handler_conf.put("device", "couplingXCubHandMk5");
coupling_handler_conf.put("options_as_str", m_pluginParameters.toString());

if(!m_coupling_driver.open(coupling_handler_conf))
{
yCError(GAZEBOCONTROLBOARD) << "Missing param in COUPLING_PARAMS section";
yCError(GAZEBOCONTROLBOARD) << "Failed to open coupling device";
yCDebug(GAZEBOCONTROLBOARD) << "CONF:" << coupling_handler_conf.toString();
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))
if(!m_coupling_driver.view(m_ijointcoupling))
{
yCError(GAZEBOCONTROLBOARD) << "Error parsing coupling parameter, wrong size of the finger parameters list";
yCError(GAZEBOCONTROLBOARD) << "Failed to view IJointCoupling interface";
return false;
}
m_coupling_handler.push_back(cpl);
yCInfo(GAZEBOCONTROLBOARD) << "using icub_hand_mk5";
yCInfo(GAZEBOCONTROLBOARD) << "using coupling_xcub_hand_mk5";
}
else if (coupling_bottle->get(0).asString()=="none")
{
Expand Down Expand Up @@ -479,11 +481,18 @@ void GazeboYarpControlBoardDriver::resetPositionsAndTrajectoryGenerators()
initial_positions.push_back(convertGazeboToUser(i, gazeboPos));
}

for (size_t cpl_cnt = 0; cpl_cnt < m_coupling_handler.size(); cpl_cnt++) {
if (m_coupling_handler[cpl_cnt])
m_coupling_handler[cpl_cnt]->decouplePos(initial_positions);
if(m_ijointcoupling){
// TODO
}
else {
for (size_t cpl_cnt = 0; cpl_cnt < m_coupling_handler.size(); cpl_cnt++) {
if (m_coupling_handler[cpl_cnt])
m_coupling_handler[cpl_cnt]->decouplePos(initial_positions);
}
}



for (unsigned int i = 0; i < m_numberOfJoints; ++i) {
if (isValidUserDOF(i)) {

Expand Down Expand Up @@ -576,14 +585,19 @@ void GazeboYarpControlBoardDriver::onUpdate(const gazebo::common::UpdateInfo& _i
m_motPositions=m_positions;
m_motVelocities=m_velocities;
//measurements decoupling
for (size_t cpl_cnt = 0; cpl_cnt < m_coupling_handler.size(); cpl_cnt++)
{
if (m_coupling_handler[cpl_cnt])
if(m_ijointcoupling){
// TODO
}
else {
for (size_t cpl_cnt = 0; cpl_cnt < m_coupling_handler.size(); cpl_cnt++)
{
m_coupling_handler[cpl_cnt]->decouplePos(m_positions);
m_coupling_handler[cpl_cnt]->decoupleVel(m_velocities);
//m_coupling_handler[cpl_cnt]->decoupleAcc(m_accelerations); //missing
m_coupling_handler[cpl_cnt]->decoupleTrq(m_torques);
if (m_coupling_handler[cpl_cnt])
{
m_coupling_handler[cpl_cnt]->decouplePos(m_positions);
m_coupling_handler[cpl_cnt]->decoupleVel(m_velocities);
//m_coupling_handler[cpl_cnt]->decoupleAcc(m_accelerations); //missing
m_coupling_handler[cpl_cnt]->decoupleTrq(m_torques);
}
}
}

Expand Down Expand Up @@ -654,13 +668,19 @@ void GazeboYarpControlBoardDriver::onUpdate(const gazebo::common::UpdateInfo& _i
m_motReferencePositions=m_jntReferencePositions;
m_motReferenceVelocities=m_jntReferenceVelocities;
m_motReferenceTorques=m_jntReferenceTorques;
for (size_t cpl_cnt = 0; cpl_cnt < m_coupling_handler.size(); cpl_cnt++)
{
if (m_coupling_handler[cpl_cnt])

if(m_ijointcoupling){
// TODO
}
else {
for (size_t cpl_cnt = 0; cpl_cnt < m_coupling_handler.size(); cpl_cnt++)
{
m_motReferencePositions = m_coupling_handler[cpl_cnt]->decoupleRefPos(m_jntReferencePositions);
m_motReferenceVelocities = m_coupling_handler[cpl_cnt]->decoupleRefVel(m_jntReferenceVelocities, m_motPositions);
m_motReferenceTorques = m_coupling_handler[cpl_cnt]->decoupleRefTrq(m_jntReferenceTorques);
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_motPositions);
m_motReferenceTorques = m_coupling_handler[cpl_cnt]->decoupleRefTrq(m_jntReferenceTorques);
}
}
}

Expand Down

0 comments on commit 84f63a9

Please sign in to comment.