Skip to content
New issue

Have a question about this project? Sign up for a free GitHub account to open an issue and contact its maintainers and the community.

Sign up for GitHub

By clicking “Sign up for GitHub”, you agree to our terms of service and privacy statement. We’ll occasionally send you account related emails.

Already on GitHub? Sign in to your account

Fix Control Board wrong joint limits for coupled joints #472

Closed
wants to merge 4 commits into from
Closed

Fix Control Board wrong joint limits for coupled joints #472

Show file tree
Hide file tree
Changes from all commits
Commits
Show all changes
4 commits
Select commit Hold shift + click to select a range
4613608
Move private struct yarp::dev::GazeboYarpControlBoardriver::Range in …
xEnVrE Feb 28, 2018
01796a2
Implement method decouplePosLimits within decoupling handlers
xEnVrE Feb 28, 2018
8d43622
Take into account coupling while evaluating joint limits in GazeboYar…
xEnVrE Feb 28, 2018
f55cb7a
Update CHANGELOG.md
xEnVrE Mar 26, 2020
File filter

Filter by extension

Filter by extension
Conversations
Failed to load comments.
Loading
Jump to
Jump to file
Failed to load files.
Loading
Diff view
Diff view
4 changes: 4 additions & 0 deletions CHANGELOG.md
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -7,6 +7,10 @@ The format of this document is based on [Keep a Changelog](https://keepachangelo

### Added
- Add the possibility to create different types of joints with the `linkattacher` plugin (https://github.com/robotology/gazebo-yarp-plugins/pull/461).
- Add the possibility to decouple joint limits using method BaseCouplingHandler::decouplePosLimits(), within the 'controlboard' plugin (https://github.com/robotology/gazebo-yarp-plugins/pull/472).

### Fixed
- Fix evaluation of joints position limits for physically coupled DoFs in method GazeboYarpControlBoardDriver::setMinMaxPos(), within the 'controlboard' plugin (https://github.com/robotology/gazebo-yarp-plugins/pull/472).

## [3.3.0] - 2019-12-13

Expand Down
3 changes: 2 additions & 1 deletion plugins/controlboard/CMakeLists.txt
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -34,7 +34,8 @@ set(controlBoard_source src/ControlBoard.cc
set(controlBoard_headers include/gazebo/ControlBoard.hh
include/yarp/dev/ControlBoardDriver.h
include/yarp/dev/ControlBoardDriverTrajectory.h
include/yarp/dev/ControlBoardDriverCoupling.h)
include/yarp/dev/ControlBoardDriverCoupling.h
include/yarp/dev/ControlBoardDriverRange.h)



Expand Down
7 changes: 1 addition & 6 deletions plugins/controlboard/include/yarp/dev/ControlBoardDriver.h
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -27,6 +27,7 @@
#include <boost/shared_ptr.hpp>
#include <ControlBoardDriverTrajectory.h>
#include <ControlBoardDriverCoupling.h>
#include <ControlBoardDriverRange.h>

#include <gazebo/common/PID.hh>
#include <gazebo/common/Time.hh>
Expand Down Expand Up @@ -349,12 +350,6 @@ class yarp::dev::GazeboYarpControlBoardDriver:
JointType_Prismatic
};

struct Range {
Range() : min(0), max(0){}
double min;
double max;
};

std::string m_deviceName;
gazebo::physics::Model* m_robot;

Expand Down
18 changes: 18 additions & 0 deletions plugins/controlboard/include/yarp/dev/ControlBoardDriverCoupling.h
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -9,6 +9,8 @@

#include <gazebo/physics/Model.hh>

#include <ControlBoardDriverRange.h>

namespace yarp {
namespace dev {
enum CouplingType
Expand Down Expand Up @@ -36,6 +38,8 @@ class BaseCouplingHandler
virtual bool decoupleAcc(yarp::sig::Vector& current_acc) = 0;
virtual bool decoupleTrq(yarp::sig::Vector& current_trq) = 0;

virtual bool decouplePosLimits(std::vector<Range>& pos_limits) = 0;

virtual yarp::sig::VectorOf<int> getCoupledJoints();
virtual std::string getCoupledJointName (int joint);
virtual bool checkJointIsCoupled(int joint);
Expand All @@ -57,6 +61,8 @@ class EyesCouplingHandler : public BaseCouplingHandler
bool decoupleAcc (yarp::sig::Vector& current_acc);
bool decoupleTrq (yarp::sig::Vector& current_trq);

bool decouplePosLimits(std::vector<Range>& pos_limits);

yarp::sig::Vector decoupleRefPos (yarp::sig::Vector& pos_ref);
yarp::sig::Vector decoupleRefVel (yarp::sig::Vector& vel_ref);
yarp::sig::Vector decoupleRefTrq (yarp::sig::Vector& trq_ref);
Expand All @@ -74,6 +80,8 @@ class ThumbCouplingHandler : public BaseCouplingHandler
bool decoupleAcc (yarp::sig::Vector& current_acc);
bool decoupleTrq (yarp::sig::Vector& current_trq);

bool decouplePosLimits(std::vector<Range>& pos_limits);

yarp::sig::Vector decoupleRefPos (yarp::sig::Vector& pos_ref);
yarp::sig::Vector decoupleRefVel (yarp::sig::Vector& vel_ref);
yarp::sig::Vector decoupleRefTrq (yarp::sig::Vector& trq_ref);
Expand All @@ -91,6 +99,8 @@ class IndexCouplingHandler : public BaseCouplingHandler
bool decoupleAcc (yarp::sig::Vector& current_acc);
bool decoupleTrq (yarp::sig::Vector& current_trq);

bool decouplePosLimits(std::vector<Range>& pos_limits);

yarp::sig::Vector decoupleRefPos (yarp::sig::Vector& pos_ref);
yarp::sig::Vector decoupleRefVel (yarp::sig::Vector& vel_ref);
yarp::sig::Vector decoupleRefTrq (yarp::sig::Vector& trq_ref);
Expand All @@ -108,6 +118,8 @@ class MiddleCouplingHandler : public BaseCouplingHandler
bool decoupleAcc (yarp::sig::Vector& current_acc);
bool decoupleTrq (yarp::sig::Vector& current_trq);

bool decouplePosLimits(std::vector<Range>& pos_limits);

yarp::sig::Vector decoupleRefPos (yarp::sig::Vector& pos_ref);
yarp::sig::Vector decoupleRefVel (yarp::sig::Vector& vel_ref);
yarp::sig::Vector decoupleRefTrq (yarp::sig::Vector& trq_ref);
Expand All @@ -125,6 +137,8 @@ class PinkyCouplingHandler : public BaseCouplingHandler
bool decoupleAcc (yarp::sig::Vector& current_acc);
bool decoupleTrq (yarp::sig::Vector& current_trq);

bool decouplePosLimits(std::vector<Range>& pos_limits);

yarp::sig::Vector decoupleRefPos (yarp::sig::Vector& pos_ref);
yarp::sig::Vector decoupleRefVel (yarp::sig::Vector& vel_ref);
yarp::sig::Vector decoupleRefTrq (yarp::sig::Vector& trq_ref);
Expand All @@ -142,6 +156,8 @@ class FingersAbductionCouplingHandler : public BaseCouplingHandler
bool decoupleAcc (yarp::sig::Vector& current_acc);
bool decoupleTrq (yarp::sig::Vector& current_trq);

bool decouplePosLimits(std::vector<Range>& pos_limits);

yarp::sig::Vector decoupleRefPos (yarp::sig::Vector& pos_ref);
yarp::sig::Vector decoupleRefVel (yarp::sig::Vector& vel_ref);
yarp::sig::Vector decoupleRefTrq (yarp::sig::Vector& trq_ref);
Expand All @@ -159,6 +175,8 @@ class CerHandCouplingHandler : public BaseCouplingHandler
bool decoupleAcc (yarp::sig::Vector& current_acc);
bool decoupleTrq (yarp::sig::Vector& current_trq);

bool decouplePosLimits(std::vector<Range>& pos_limits);

yarp::sig::Vector decoupleRefPos (yarp::sig::Vector& pos_ref);
yarp::sig::Vector decoupleRefVel (yarp::sig::Vector& vel_ref);
yarp::sig::Vector decoupleRefTrq (yarp::sig::Vector& trq_ref);
Expand Down
16 changes: 16 additions & 0 deletions plugins/controlboard/include/yarp/dev/ControlBoardDriverRange.h
View file
Open in desktop
Original file line number Diff line number Diff line change
@@ -0,0 +1,16 @@
/*
* Copyright (C) 2013-2015 Fondazione Istituto Italiano di Tecnologia RBCS & iCub Facility & ADVR
* Authors: see AUTHORS file.
* CopyPolicy: Released under the terms of the LGPLv2.1 or any later version, see LGPL.TXT or LGPL3.TXT
*/

#ifndef GAZEBOYARP_CONTROLBOARDDRIVERRANGE_HH
#define GAZEBOYARP_CONTROLBOARDDRIVERRANGE_HH

struct Range {
Range() : min(0), max(0){}
double min;
double max;
};

#endif
9 changes: 9 additions & 0 deletions plugins/controlboard/src/ControlBoardDriver.cpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -686,6 +686,15 @@ bool GazeboYarpControlBoardDriver::setMinMaxPos()
yWarning() << "Missing LIMITS section";
}

// handle coupling
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]->decouplePosLimits(m_jointPosLimits);
}
}

return true;
}

Expand Down
47 changes: 47 additions & 0 deletions plugins/controlboard/src/ControlBoardDriverCoupling.cpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -107,6 +107,11 @@ bool EyesCouplingHandler::decoupleTrq (yarp::sig::Vector& current_trq)
return false;
}

bool EyesCouplingHandler::decouplePosLimits (std::vector<Range>& pos_limits) // NOT IMPLEMENTED
{
return false;
}

yarp::sig::Vector EyesCouplingHandler::decoupleRefPos (yarp::sig::Vector& pos_ref)
{
yarp::sig::Vector out = pos_ref;
Expand Down Expand Up @@ -171,6 +176,14 @@ bool ThumbCouplingHandler::decoupleTrq (yarp::sig::Vector& current_trq)
return false;
}

bool ThumbCouplingHandler::decouplePosLimits (std::vector<Range>& pos_limits)
{
if (m_coupledJoints.size() != m_couplingSize) return false;
pos_limits[m_coupledJoints[2]].min = pos_limits[m_coupledJoints[2]].min + pos_limits[m_coupledJoints[3]].min;
pos_limits[m_coupledJoints[2]].max = pos_limits[m_coupledJoints[2]].max + pos_limits[m_coupledJoints[3]].max;
return true;
}

yarp::sig::Vector ThumbCouplingHandler::decoupleRefPos (yarp::sig::Vector& pos_ref)
{
yarp::sig::Vector out = pos_ref;
Expand Down Expand Up @@ -241,6 +254,14 @@ bool IndexCouplingHandler::decoupleTrq (yarp::sig::Vector& current_trq)
return false;
}

bool IndexCouplingHandler::decouplePosLimits (std::vector<Range>& pos_limits)
{
if (m_coupledJoints.size() != m_couplingSize) return false;
pos_limits[m_coupledJoints[1]].min = pos_limits[m_coupledJoints[1]].min + pos_limits[m_coupledJoints[2]].min;
pos_limits[m_coupledJoints[1]].max = pos_limits[m_coupledJoints[1]].max + pos_limits[m_coupledJoints[2]].max;
return true;
}

yarp::sig::Vector IndexCouplingHandler::decoupleRefPos (yarp::sig::Vector& pos_ref)
{
yarp::sig::Vector out = pos_ref;
Expand Down Expand Up @@ -308,6 +329,14 @@ bool MiddleCouplingHandler::decoupleTrq (yarp::sig::Vector& current_trq)
return false;
}

bool MiddleCouplingHandler::decouplePosLimits (std::vector<Range>& pos_limits)
{
if (m_coupledJoints.size() != m_couplingSize) return false;
pos_limits[m_coupledJoints[1]].min = pos_limits[m_coupledJoints[1]].min + pos_limits[m_coupledJoints[2]].min;
pos_limits[m_coupledJoints[1]].max = pos_limits[m_coupledJoints[1]].max + pos_limits[m_coupledJoints[2]].max;
return true;
}

yarp::sig::Vector MiddleCouplingHandler::decoupleRefPos (yarp::sig::Vector& pos_ref)
{
yarp::sig::Vector out = pos_ref;
Expand Down Expand Up @@ -375,6 +404,14 @@ bool PinkyCouplingHandler::decoupleTrq (yarp::sig::Vector& current_trq)
return false;
}

bool PinkyCouplingHandler::decouplePosLimits (std::vector<Range>& pos_limits)
{
if (m_coupledJoints.size() != m_couplingSize) return false;
pos_limits[m_coupledJoints[0]].min = pos_limits[m_coupledJoints[0]].min + pos_limits[m_coupledJoints[1]].min + pos_limits[m_coupledJoints[2]].min;
pos_limits[m_coupledJoints[0]].max = pos_limits[m_coupledJoints[0]].max + pos_limits[m_coupledJoints[1]].max + pos_limits[m_coupledJoints[2]].max;
return true;
}

yarp::sig::Vector PinkyCouplingHandler::decoupleRefPos (yarp::sig::Vector& pos_ref)
{
yarp::sig::Vector out = pos_ref;
Expand Down Expand Up @@ -452,6 +489,11 @@ bool FingersAbductionCouplingHandler::decoupleTrq (yarp::sig::Vector& current_tr
return false;
}

bool FingersAbductionCouplingHandler::decouplePosLimits (std::vector<Range>& pos_limits) // NOT IMPLEMENTED
{
return false;
}

yarp::sig::Vector FingersAbductionCouplingHandler::decoupleRefPos (yarp::sig::Vector& pos_ref)
{
yarp::sig::Vector out = pos_ref;
Expand Down Expand Up @@ -532,6 +574,11 @@ bool CerHandCouplingHandler::decoupleTrq (yarp::sig::Vector& current_trq)
return false;
}

bool CerHandCouplingHandler::decouplePosLimits (std::vector<Range>& pos_limits) // NOT IMPLEMENTED
{
return false;
}

yarp::sig::Vector CerHandCouplingHandler::decoupleRefPos (yarp::sig::Vector& pos_ref)
{
yarp::sig::Vector out = pos_ref;
Expand Down