Writing Control for 6DOF using Dart getGravity and getCoriolis

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.

1 Like

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.

#include “ros/ros.h”
#include “std_msgs/String.h”
#include “std_msgs/Float64.h”
#include “sensor_msgs/JointState.h”
#include <Eigen/Core>
#include <boost/scoped_ptr.hpp>
#include <tinyxml2.h>
#include <dart/dart.hpp>
#include <dart/utils/utils.hpp>
#include <dart/utils/urdf/urdf.hpp>
#include <stdio.h>

ros::Publisher IDPubJoint1;
ros::Publisher IDPubJoint2;
ros::Publisher IDPubJoint3;
ros::Publisher IDPubJoint4;
ros::Publisher IDPubJoint5;
ros::Publisher IDPubJoint6;

class MyRobot
{ public:
// Constructor
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);
//Destructor
~MyRobot();
private : // The following object is used to load the URDF into dart kinematic tree and parse it
dart::utils::DartLoader loader;
// A pointer to the Skeleton Object/ the robot object
dart::dynamics::SkeletonPtr manipulator;
// unsigned int to store the number of joints
std::size_t numjoints;
// Vector to save the gravity acceleration from gazebo
Eigen::Vector3d gravity;
// Current joint position acquired from Gazebo
Eigen::VectorXd qCurrent;
// Desired joint position for the next time step
Eigen::VectorXd qDesired;
// Current joint velocity acquired from Gazebo
Eigen::VectorXd qdCurrent;
Eigen::VectorXd last_velocity;
// Desired joint velocity for the next time step
Eigen::VectorXd qdDesired;
// Current joint acceleration acquired from Gazebo
Eigen::VectorXd qddCurrent;
// Desired joint acceleration for the next time step
Eigen::VectorXd qddDesired;
// Required torque for each joint
Eigen::VectorXd torques;
// ROS Node handle for the publisher
ros::NodeHandle node_pub;
// Proportional controller value
double Kp;
// Differential controller value
double KpD;
// Float64 type for joint controller
std_msgs::Float64 torque_msg;
std::vector<std_msgs::Float64> effort_commands;

};

MyRobot::MyRobot(int argc, char **argv, std::string& robot_description, ros::NodeHandle node)
{

 // *****need to make it such that it works on everyones cumputer *******

loader.addPackageDirectory(“niryo_one_description”,"/home/shriyash/catkin_ws/src/niryo_one_ros_simulation-master/niryo_one_description");

// ******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);
manipulator->setName(“NiryoOne”);
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);
manipulator->setGravity(gravity);

// 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

effort_commands.assign(6,torque_msg);
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];

manipulator->getDof(i)->setPosition(qCurrent(i));
manipulator->getDof(i)->setVelocity(qdCurrent(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

IDPubJoint1.publish(effort_commands[0]);
IDPubJoint2.publish(effort_commands[1]);
IDPubJoint3.publish(effort_commands[2]);
IDPubJoint4.publish(effort_commands[3]);
IDPubJoint5.publish(effort_commands[4]);
IDPubJoint6.publish(effort_commands[5]);

// ROS_ERROR_STREAM(ros::Time::now() - begin);

}

MyRobot::~MyRobot()
{
}

int main(int argc, char **argv)
{

// Initailze the ROS node

ros::init(argc, argv, “InverseDynamics”);
ros::NodeHandle node;
std::string robot_desc_string;

// get URDF from the ros param

node.getParam(“robot_description”, robot_desc_string);

// Call the constructor to create a DART skeleton objects etc

MyRobot NiryoOne(argc, argv, robot_desc_string, node);

// Subscriber

ros::Subscriber sub = node.subscribe("/niryo_one/joint_states", 1,&MyRobot::IDCallBack, &NiryoOne);

ros::spin();

return 0;

}

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?