Joint can't recover from position limits in servo mode #1683
Description
Bug Report
- I checked the documentation and the forum but found no answer.
- I found similar issue Servo joint doesn't respect position limits #1583.
Environment
DART version: 6.13 - main branch built from source on commit 189e24d
OS name and version name(or number): Ubuntu 22.04
Compiler name and version number: GCC 11.3.0
Description
I still experience similar problem to #1583 when the axis of rotation is z (e.g. <xyz>0 0 1</xyz>).
The problem is that the joint, once hits the limit and then can't recover from it.
Related issues and PRs:
- Joint Controllers not working properly when joint limits are specified gazebosim/gz-sim#1684
- Mimic Joint Improvement ros-controls/gz_ros2_control#86 (workaround)
- Renamed ign to gz ros-controls/gz_ros2_control#67 (comment) (another perspective)
- Servo joint doesn't respect position limits #1583
I changed the sdf a bit (/home/lovro/tmp/dart/data/sdf/test/test.sdf):
<sdf version="1.5">
<model name="simple_joint_test">
<pose>10 10 2 0 0 0</pose>
<link name="base">
<inertial>
<mass>100</mass>
</inertial>
<visual name="visual">
<pose>0 0 0 -1.57 0 0</pose>
<geometry>
<cylinder>
<radius>0.1</radius>
<length>0.2</length>
</cylinder>
</geometry>
</visual>
</link>
<link name="bar">
<pose>0 0 -1 0 0 0</pose>
<inertial>
<mass>1</mass>
</inertial>
<visual name="visual">
<geometry>
<box>
<size>0.1 0.1 0.1</size>
</box>
</geometry>
</visual>
</link>
<joint name="j0" type="fixed">
<parent>world</parent>
<child>base</child>
</joint>
<joint name="j1" type="revolute">
<pose>0 0 1 0 0 0</pose>
<parent>base</parent>
<child>bar</child>
<axis>
<xyz>0 0 1</xyz>
<limit>
<upper>0.85</upper>
<lower>-0.75</lower>
<effort>100</effort>
<velocity>10</velocity>
</limit>
</axis>
</joint>
</model>
</sdf>
Testing example:
#include <dart/dart.hpp>
#include <dart/utils/sdf/SdfParser.hpp>
using namespace dart;
int main()
{
// Create a world
dart::simulation::WorldPtr world(new dart::simulation::World);
auto sdf_model = dart::utils::SdfParser::readSkeleton(
"/home/lovro/tmp/dart/data/sdf/test/test.sdf");
world->addSkeleton(sdf_model);
auto skeleton = world->getSkeleton("simple_joint_test");
auto *dartJoint = skeleton->getJoint("j1");
dartJoint->setLimitEnforcement(true);
std::cout<<"getPositionLowerLimit = "<<dartJoint->getPositionLowerLimit(0)<<"\n";
std::cout<<"getPositionUpperLimit = "<<dartJoint->getPositionUpperLimit(0)<<"\n";
std::cout<<"getForceLowerLimit = "<<dartJoint->getForceLowerLimit(0)<<"\n";
std::cout<<"getForceUpperLimit = "<<dartJoint->getForceUpperLimit(0)<<"\n";
std::cout<<"getVelocityUpperLimit = "<<dartJoint->getVelocityUpperLimit(0)<<"\n";
std::cout<<"getVelocityLowerLimit = "<<dartJoint->getVelocityLowerLimit(0)<<"\n";
std::cout<<"areLimitsEnforced = "<<dartJoint->areLimitsEnforced()<<"\n";
std::cout<<"getCoulombFriction = "<<dartJoint->getCoulombFriction(0)<<"\n";
std::cout<<"world->getGravity() = "<<world->getGravity()<<"\n";
dartJoint->setActuatorType(dart::dynamics::Joint::SERVO);
// move in positive direction
for (std::size_t i = 0; i < 1000; ++i)
{
dartJoint->setCommand(0, 1.0);
world->step();
}
std::cout<<"State after 1000 steps with 1.0 rad/sec: \n";
std::cout<<dartJoint->getPosition(0)<<"\n";
std::cout<<dartJoint->getVelocity(0)<<"\n";
// move in negative direction
for (std::size_t i = 0; i < 3000; ++i)
{
dartJoint->setCommand(0, -1.0);
world->step();
}
std::cout<<"State after 3000 steps with -1.0 rad/sec: \n";
std::cout<<dartJoint->getPosition(0)<<"\n";
std::cout<<dartJoint->getVelocity(0)<<"\n";
return 0;
}
If I change the axis of the revolute joint to <xyz>0 1 0</xyz> or <xyz>1 0 0</xyz> I get good results:
getPositionLowerLimit = -0.75
getPositionUpperLimit = 0.85
getForceLowerLimit = -inf
getForceUpperLimit = inf
getVelocityUpperLimit = inf
getVelocityLowerLimit = -inf
areLimitsEnforced = 1
getCoulombFriction = 0
world->getGravity() = 0
0
-9.81
State after 1000 steps with 1.0 rad/sec:
0.85
-2.12946e-14
State after 3000 steps with -1.0 rad/sec:
-0.75
1.27693e-14
When I use <xyz>0 0 1</xyz> z axis as rotation axis of the joint I get following:
getPositionLowerLimit = -0.75
getPositionUpperLimit = 0.85
getForceLowerLimit = -inf
getForceUpperLimit = inf
getVelocityUpperLimit = inf
getVelocityLowerLimit = -inf
areLimitsEnforced = 1
getCoulombFriction = 0
world->getGravity() = 0
0
-9.81
State after 1000 steps with 1.0 rad/sec:
0.85
0
State after 3000 steps with -1.0 rad/sec:
0.85
0
I also played with changing the gravity direction, and as I move gravity from z to y axis, both of them stop working. If I move gravity to x axis, then the example can't recover from the joint limits on any axis.