I am trying to used dart to write a controller for my gazebo simulation for 6DOF robot. The PD controller works great but when I add compensation for Coriolis the effort value skyrockets to absurd amounts. The system becomes very unstable. Furthermore, the gravity forces also don’t cause the body to remain at rest if it starts from zero position. Can someone explain how they have implemented Computed torque control in the past using DART?
I saw this Gazebo plugin in operation a few years ago, and it seemed to work quite well.
I have a second question. I have a couple dummy links, specifically a world link and tool link.There also are dummy joints associated with these. I saw that when I use getPositions function of Skeleton Class it automatically ignores these fixed joints. Am I correct in assuming that they are ignored when setting or getting positions ?
That’s right, fixed joints (called
WeldJoints in DART) have no degrees of freedom, so there are no joint positions to set or get for them.
Sorry the continuous replies. Thanks for quick replies. I am still running into a gravity compensation problem. I compared my code to the plugin and they seem to do the exact same thing, except I am trying to write a separate controller that publishes effort values to joint_effort_controller topic used by ros_control. I have copied subscriber code below. Hopefully, you can see something I am missing.
MyRobot(int argc, char **argv, std::string& robot_description, ros::NodeHandle node);
// Callback function used for subscriber
void IDCallBack(const sensor_msgs::JointState::ConstPtr& msg);
// Function to remove the world link and tool link as with them there are two extra joints
dart::dynamics::SkeletonPtr TrimMyRobot(dart::dynamics::SkeletonPtr manipulator);
private : // The following object is used to load the URDF into dart kinematic tree and parse it
// A pointer to the Skeleton Object/ the robot object
// unsigned int to store the number of joints
// Vector to save the gravity acceleration from gazebo
// Current joint position acquired from Gazebo
// Desired joint position for the next time step
// Current joint velocity acquired from Gazebo
// Desired joint velocity for the next time step
// Current joint acceleration acquired from Gazebo
// Desired joint acceleration for the next time step
// Required torque for each joint
// ROS Node handle for the publisher
// Proportional controller value
// Differential controller value
// Float64 type for joint controller
MyRobot::MyRobot(int argc, char **argv, std::string& robot_description, ros::NodeHandle node)
// *****need to make it such that it works on everyones cumputer *******
// ******need to make it such that it works on everyones cumputer *******
std::string filepath = “/home/shriyash/catkin_ws/src/niryo_one_ros_control/niryo_one_modified_urdf/urdf/gazebo_niryo_one.urdf.xacro”;
manipulator = loader.parseSkeletonString(robot_description,filepath);
numjoints = manipulator->getNumJoints();
// get the current joint states
qCurrent = manipulator->getPositions();
qdCurrent = manipulator->getVelocities();
qddCurrent = manipulator->getAccelerations();
//set the gravity
gravity = Eigen::Vector3d (0,0,-9.8);
// set the desired to current
qDesired = qCurrent;
qdDesired = qdCurrent;
last_velocity = qdCurrent;
qddDesired = qddCurrent;
// Controller propotional constants
Kp = 1;
KpD = 0;
// create a empty message of the correct size for 6 DOF
node_pub = node;
//Define publishing node
IDPubJoint1 = node_pub.advertise<std_msgs::Float64>("/niryo_one/joint1_effort_controller/command", 1);
IDPubJoint2 = node_pub.advertise<std_msgs::Float64>("/niryo_one/joint2_effort_controller/command", 1);
IDPubJoint3 = node_pub.advertise<std_msgs::Float64>("/niryo_one/joint3_effort_controller/command", 1);
IDPubJoint4 = node_pub.advertise<std_msgs::Float64>("/niryo_one/joint4_effort_controller/command", 1);
IDPubJoint5 = node_pub.advertise<std_msgs::Float64>("/niryo_one/joint5_effort_controller/command", 1);
IDPubJoint6 = node_pub.advertise<std_msgs::Float64>("/niryo_one/joint6_effort_controller/command", 1);
// ROS_ERROR_STREAM((manipulator->getMassMatrix() * ((Kp * (qDesired - qCurrent)) + (KpD * (qdDesired - qdCurrent)) + qddDesired) + manipulator->getCoriolisAndGravityForces()));
void MyRobot::IDCallBack(const sensor_msgs::JointState::ConstPtr& msg)
// ros::Time begin = ros::Time::now(); // Copies data from ros message into a Eigen::Vector and sets it to the DART skeleton
for (int i = 0; i < 6;i++)
qCurrent(i) = (double) msg->position[i];
qdCurrent(i) = (double) msg->velocity[i];
// gets the joint torques for gravity compensation from DART
torques = manipulator->getGravityForces();
// Full controller once gravity is resolved // torques = (manipulator->getMassMatrix() * ((Kp * (qDesired - qCurrent)) + (KpD * (qdDesired - qdCurrent)) + qddDesired) + manipulator->getCoriolisAndGravityForces());
for (int j = 0 ; j < 6;j++)
effort_commands[j].data = torques[j];
// publishes the data to the effort controller
// ROS_ERROR_STREAM(ros::Time::now() - begin);
int main(int argc, char **argv)
// Initailze the ROS node
ros::init(argc, argv, “InverseDynamics”);
// get URDF from the ros param
// Call the constructor to create a DART skeleton objects etc
MyRobot NiryoOne(argc, argv, robot_desc_string, node);
ros::Subscriber sub = node.subscribe("/niryo_one/joint_states", 1,&MyRobot::IDCallBack, &NiryoOne);
I would strongly recommend that you provide a link to source code that’s hosted somewhere like GitHub, GitLab, or Bitbucket instead of copy/pasting it into a forum post.
But also, you haven’t provided any description of what is actually going wrong with your implementation. Without having any hints about what’s wrong, we’d have to carefully study all of your source code, which is a task that falls outside the scope of free help from an online stranger.
I’d suggest trying to narrow down the problems you’re having, and if you can pin down a concise description of the problem that you’re experiencing, then follow up with that additional information.
I will keep in mind for the future.
I am running into the values obtained from getGravity and getCoriolis higher that expected. I compared them with the value I get with a successful controller I implemented, using orocos kdl libraries. I have checked to ensure that the positions , velocities at zero for both but they give different value for joint forces. My only guess can be the way inertia is used or read from the urdf. I haven’t checked the inertia value read by the kdl libraries. Can you explain more how the inertia value/mass value is read and used for calculating gravity forces?