Skip to content

Commit

Permalink
Merge pull request #126 from robotology/fix/joint-limits-conversions
Browse files Browse the repository at this point in the history 
Fix initialization of joint position limits from yarp configuration
  • Loading branch information
@xela-95
xela-95 authored Mar 19, 2024
2 parents 954f5ca + 24b2f7f commit 6b25488
Show file tree
Hide file tree
Showing 4 changed files with 260 additions and 9 deletions.
20 changes: 11 additions & 9 deletions plugins/controlboard/src/ControlBoard.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -646,18 +646,20 @@ bool ControlBoard::initializeJointPositionLimits(const gz::sim::EntityComponentM
size_t numberOfJoints = m_controlBoardData.joints.size();
Bottle limitMinGroup, limitMaxGroup;

for (auto& joint : m_controlBoardData.joints)
if (!(tryGetGroup(limitsGroup, limitMinGroup, "jntPosMin", "", numberOfJoints + 1)
&& tryGetGroup(limitsGroup, limitMaxGroup, "jntPosMax", "", numberOfJoints + 1)))
{
yError() << "Error while reading joint position limits from plugin configuration";
return false;
}

for (size_t i = 0; i < numberOfJoints; ++i)
{
// TODO: access gazebo joint position limits and use them to check if software limits
// ([LIMITS] group) are consistent. In case they are not defined set them as sw limits.
if (!(tryGetGroup(limitsGroup, limitMinGroup, "jntPosMin", "", numberOfJoints + 1)
&& tryGetGroup(limitsGroup, limitMaxGroup, "jntPosMax", "", numberOfJoints + 1)))
{
yError() << "Error while reading joint position limits from plugin parameters";
return false;
}
joint.positionLimitMin = limitMinGroup.get(1).asFloat64();
joint.positionLimitMax = limitMaxGroup.get(1).asFloat64();
auto& joint = m_controlBoardData.joints[i];
joint.positionLimitMin = limitMinGroup.get(i + 1).asFloat64();
joint.positionLimitMax = limitMaxGroup.get(i + 1).asFloat64();
}

return true;
Expand Down
43 changes: 43 additions & 0 deletions tests/controlboard/ControlBoardCommonsTest.cc
View file
Original file line number Diff line number Diff line change
Expand Up @@ -10,6 +10,8 @@
#include <gz/sim/TestFixture.hh>
#include <gz/sim/World.hh>

#include <vector>
#include <yarp/dev/IControlLimits.h>
#include <yarp/dev/IControlMode.h>
#include <yarp/dev/IEncoders.h>
#include <yarp/dev/IPositionControl.h>
Expand Down Expand Up @@ -72,5 +74,46 @@ TEST(ControlBoardCommonsTest, ConfigureMultipleControlBoards)
ASSERT_TRUE(std::find(keys.begin(), keys.end(), cb2Key) != keys.end());
}

// Check that joint position limits are read correctly from yarp configuration
TEST(ControlBoardCommonsTest, JointPositionLimitsForMultipleJoints)
{
std::string modelSdfName = "coupled_pendulum_two_joints_single_controlboard.sdf";
std::string sdfPath = std::string("../../../tests/controlboard/") + modelSdfName;
std::string deviceScopedName = "model/coupled_pendulum/controlboard_plugin_device";
yarp::dev::PolyDriver* driver;
yarp::dev::IControlLimits* iControlLimits;

gz::sim::TestFixture testFixture{sdfPath};
gz::common::Console::SetVerbosity(4);

testFixture.Finalize();

driver = gzyarp::Handler::getHandler()->getDevice(deviceScopedName);
ASSERT_TRUE(driver != nullptr);
ASSERT_TRUE(driver->view(iControlLimits));

auto singleton = ControlBoardDataSingleton::getControlBoardHandler();
auto keys = singleton->getControlBoardKeys();

std::cerr << "ControlBoard singleton keys vector size: " << keys.size() << std::endl;
ASSERT_EQ(keys.size(), 1);

std::cerr << "ControlBoard ids: " << keys[0] << std::endl;
ASSERT_TRUE(std::find(keys.begin(), keys.end(), deviceScopedName) != keys.end());

auto controlBoardData = singleton->getControlBoardData(deviceScopedName);

auto expectedJointMaxLimits = std::vector<double>{200.0, 10.0};
auto expectedJointMinLimits = std::vector<double>{-200.0, -10.0};

for (int i = 0; i < expectedJointMaxLimits.size(); i++)
{
double maxLimit, minLimit;
iControlLimits->getLimits(i, &minLimit, &maxLimit);
EXPECT_DOUBLE_EQ(maxLimit, expectedJointMaxLimits[i]);
EXPECT_DOUBLE_EQ(minLimit, expectedJointMinLimits[i]);
}
}

} // namespace test
} // namespace gzyarp
25 changes: 25 additions & 0 deletions tests/controlboard/conf/gazebo_controlboard_multiple_joints.ini
View file
Original file line number Diff line number Diff line change
@@ -0,0 +1,25 @@
yarpDeviceName controlboard_plugin_device
jointNames upper_joint lower_joint

#PIDs:
# this information is used to set the PID values in simulation for GAZEBO, we need only the first three values
[POSITION_CONTROL]
controlUnits metric_units
controlLaw joint_pid_gazebo_v1
kp 3000.0 3000.0
kd 2.0 2.0
ki 0.1 0.1
maxInt 99999 99999
maxOutput 99999 99999
shift 0.0 0.0
ko 0.0 0.0
stictionUp 0.0 0.0
stictionDwn 0.0 0.0

[GAZEBO_VELOCITY_PIDS]
#Torso
Pid0 500.0 2.0 0.1 9999 9999 9 9

[LIMITS]
jntPosMax 200.0 10.0
jntPosMin -200.0 -10.0
181 changes: 181 additions & 0 deletions tests/controlboard/coupled_pendulum_two_joints_single_controlboard.sdf
View file
Original file line number Diff line number Diff line change
@@ -0,0 +1,181 @@
<?xml version="1.0"?>

<sdf version='1.11'>

<world name="tutorial_controlboard">
<!-- <gravity>0 0 0</gravity> -->
<light type="directional" name="sun">
<cast_shadows>true</cast_shadows>
<pose>0 0 10 0 0 0</pose>
<diffuse>0.8 0.8 0.8 1</diffuse>
<specular>0.2 0.2 0.2 1</specular>
<attenuation>
<range>1000</range>
<constant>0.9</constant>
<linear>0.01</linear>
<quadratic>0.001</quadratic>
</attenuation>
<direction>-0.5 0.1 -0.9</direction>
</light>

<model name="ground_plane">
<static>true</static>
<link name="link">
<collision name="collision">
<geometry>
<plane>
<normal>0 0 1</normal>
<size>100 100</size>
</plane>
</geometry>
</collision>
<visual name="visual">
<geometry>
<plane>
<normal>0 0 1</normal>
<size>100 100</size>
</plane>
</geometry>
<material>
<ambient>0.8 0.8 0.8 1</ambient>
<diffuse>0.8 0.8 0.8 1</diffuse>
<specular>0.8 0.8 0.8 1</specular>
</material>
</visual>
</link>
</model>

<model name="coupled_pendulum">
<link name='base_link'>
<inertial>
<pose>0 0 0 0 0 0</pose>
<mass>1000</mass>
<inertia>
<ixx>1</ixx>
<ixy>0</ixy>
<ixz>0</ixz>
<iyy>1</iyy>
<iyz>0</iyz>
<izz>1</izz>
</inertia>
</inertial>
<collision name='base_link_fixed_joint_lump__base_collision'>
<pose>0 0 1 0 0 0</pose>
<geometry>
<box>
<size>0.14999999999999999 0.14999999999999999 2.1499999999999999</size>
</box>
</geometry>
</collision>
<visual name='base_link_fixed_joint_lump__base_visual'>
<pose>0 0 1 0 0 0</pose>
<geometry>
<box>
<size>0.14999999999999999 0.14999999999999999 2.1499999999999999</size>
</box>
</geometry>
</visual>
</link>
<joint name='lower_joint' type='revolute'>
<pose relative_to='base_link'>0.14999999999999999 0 1.75 3.1415000000000002 0 0</pose>
<parent>base_link</parent>
<child>link2</child>
<axis>
<xyz>1 0 0</xyz>
<limit>
<lower>-5</lower>
<upper>5</upper>
<effort>100</effort>
<velocity>100</velocity>
</limit>
<dynamics>
<spring_reference>0</spring_reference>
<spring_stiffness>0</spring_stiffness>
</dynamics>
</axis>
</joint>
<link name='link2'>
<pose relative_to='lower_joint'>0 0 0 0 0 0</pose>
<inertial>
<pose>0 0 -0.90000000000000002 -0.30000000000000004 0 0</pose>
<mass>1</mass>
<inertia>
<ixx>1</ixx>
<ixy>0</ixy>
<ixz>0</ixz>
<iyy>1</iyy>
<iyz>0</iyz>
<izz>1</izz>
</inertia>
</inertial>
<visual name='link2_visual'>
<pose>0 0 -0.5 0 0 0</pose>
<geometry>
<box>
<size>0.14999999999999999 0.14999999999999999 1</size>
</box>
</geometry>
</visual>
</link>
<joint name='upper_joint' type='revolute'>
<pose relative_to='base_link'>-0.14999999999999999 0 1.75 3.1415000000000002 0 0</pose>
<parent>base_link</parent>
<child>link1</child>
<axis>
<xyz>1 0 0</xyz>
<limit>
<lower>-5</lower>
<upper>5</upper>
<effort>100</effort>
<velocity>100</velocity>
</limit>
<dynamics>
<spring_reference>0</spring_reference>
<spring_stiffness>0</spring_stiffness>
</dynamics>
</axis>
</joint>
<link name='link1'>
<pose relative_to='upper_joint'>0 0 0 0 0 0</pose>
<inertial>
<pose>0 0 -0.90000000000000002 0 0 0</pose>
<mass>1</mass>
<inertia>
<ixx>1</ixx>
<ixy>0</ixy>
<ixz>0</ixz>
<iyy>1</iyy>
<iyz>0</iyz>
<izz>1</izz>
</inertia>
</inertial>
<visual name='link1_visual'>
<pose>0 0 -0.5 0 0 0</pose>
<geometry>
<box>
<size>0.14999999999999999 0.14999999999999999 1</size>
</box>
</geometry>
</visual>
</link>

<plugin name="gzyarp::ControlBoard" filename="gz-sim-yarp-controlboard-system">
<yarpConfigurationFile>
../../../tests/controlboard/conf/gazebo_controlboard_multiple_joints.ini
</yarpConfigurationFile>
<initialConfiguration>0.0</initialConfiguration>
</plugin>
<!-- <plugin name='robotinterface' filename='libgazebo_yarp_robotinterface.so'>
<yarpRobotInterfaceConfigurationFile>model://coupled_pendulum/conf/coupled_pendulum_nws.xml</yarpRobotInterfaceConfigurationFile>
</plugin> -->

<frame name='fake_base_fixed_joint' attached_to='base_link'>
<pose>0 0 0 0 0 0</pose>
</frame>
<frame name='base' attached_to='fake_base_fixed_joint'>
<pose>0 0 0 0 0 0</pose>
</frame>
</model>

</world>
</sdf>

0 comments on commit 6b25488

Please sign in to comment.