A joint control plugin for a parallel robot, the plugin does not work!
Hi, I hope you are doing well. I have provided the SDF model of a parallel robot to simulate it in the Gazebo and control its end-effector. Based on my knowledge, the closed kinematic chain must be provided in SDF format so I could not use the URDF and URDF based plugin to control my robot. However, I have inserted my robot in the Gazebo and I can set the joint angle and velocity by means of the provided plan in the right side of the Gazebo. It seems to work well! I have used the gazeboros plugin such as "jointtrajectoryplugin", "jointstatepublisher" in order to control the robot joints and get feedback them. I am able to get the feedback by "jointstatepublisher" but I could never manage to set the position or velocity by means of the "jointtrajectory_plugin". Predicated on this failure, I have written a plugin in order to set the joint position. But the plugin does not work and the position of the intended link does not change. I will appreciate if you help me.
The SDF of the considering robot is presented as below:
<?xml version="1.0" encoding="utf-8"?>
<sdf version="1.5">
<model name="Agile-eye">
<pose>0 0 0.0523364278812051 1.5707963267949 0 0</pose>
<link name="Base">
<gravity>1</gravity>
<self_collide>0</self_collide>
<pose>0 0 0 0 0 0</pose>
<must_be_base_link>1</must_be_base_link>
<velocity_decay>
<linear>0</linear>
<angular>0</angular>
</velocity_decay>
<inertial>
<mass>2.45302262810541</mass>
<pose>0 0 0 0 0 0</pose>
<inertia>
<ixx>0.0174089005567679</ixx>
<ixy>0.00386321027734225</ixy>
<ixz>0.00224453213339039</ixz>
<iyy>0.0163888871488208</iyy>
<iyz>0.00191258456874747</iyz>
<izz>0.021215632267225</izz>
</inertia>
</inertial>
<collision name="Base_collision">
<pose>0 0 0 0 0 0</pose>
<geometry>
<mesh>
<scale>1 1 1</scale>
<uri>model://Agile-eye/meshes/Base_col.STL</uri>
</mesh>
</geometry>
<surface>
<contact>
<ode>
<kp>100000</kp>
<kd>1</kd>
</ode>
</contact>
<friction>
<ode>
<mu>0.2</mu>
<mu2>0.1</mu2>
</ode>
</friction>
</surface>
</collision>
<visual name="Base_visual">
<pose>0 0 0 0 0 0</pose>
<material>
<ambient>0.752941176470588 0.752941176470588 0.752941176470588 1</ambient>
<diffuse>0.752941176470588 0.752941176470588 0.752941176470588 1</diffuse>
</material>
<geometry>
<mesh>
<scale>1 1 1</scale>
<uri>model://Agile-eye/meshes/Base.STL</uri>
</mesh>
</geometry>
</visual>
</link>
<joint name="fixed to ground" type="revolute">
<parent>world</parent>
<child>Base</child>
<axis>
<limit>
<lower>0</lower>
<upper>0</upper>
</limit>
<xyz>0 0 1</xyz>
</axis>
</joint>
<link name="arm1">
<gravity>1</gravity>
<self_collide>0</self_collide>
<pose>0.09692 0.05666 0.0654 -1.5707963267949 0 0</pose>
<must_be_base_link>0</must_be_base_link>
<velocity_decay>
<linear>0</linear>
<angular>0</angular>
</velocity_decay>
<inertial>
<mass>0.0402007076630336</mass>
<pose>0 0 0 0 0 0</pose>
<inertia>
<ixx>0.00012884674184318</ixx>
<ixy>6.13154456211418E-05</ixy>
<ixz>4.16049597669714E-17</ixz>
<iyy>4.84154728485539E-05</iyy>
<iyz>4.6668041639536E-15</iyz>
<izz>0.000176009253214268</izz>
</inertia>
</inertial>
<collision name="arm1_collision">
<pose>0 0 0 0 0 0</pose>
<geometry>
<mesh>
<scale>1 1 1</scale>
<uri>model://Agile-eye/meshes/arm1_col.dae</uri>
</mesh>
</geometry>
<surface>
<contact>
<ode>
<kp>10000</kp>
<kd>1</kd>
</ode>
</contact>
<friction>
<ode>
<mu>0.2</mu>
<mu2>0.1</mu2>
</ode>
</friction>
</surface>
</collision>
<visual name="arm1_visual">
<pose>0 0 0 0 0 0</pose>
<material>
<ambient>0.752941176470588 0.752941176470588 0.752941176470588 1</ambient>
<diffuse>0.752941176470588 0.752941176470588 0.752941176470588 1</diffuse>
</material>
<geometry>
<mesh>
<scale>1 1 1</scale>
<uri>model://Agile-eye/meshes/arm1.dae</uri>
</mesh>
</geometry>
</visual>
</link>
<joint name="Base-arm1" type="revolute">
<parent>Base</parent>
<child>arm1</child>
<pose>0 0 0 0 0 0</pose>
<axis>
<xyz>0 0 1</xyz>
<dynamics>
<damping>0.0005</damping>
<friction>0.01</friction>
</dynamics>
<limit>
<effort>1</effort>
</limit>
</axis>
</joint>
<link name="arm2">
<gravity>1</gravity>
<self_collide>0</self_collide>
<pose>-0.01058 0.19916 0.0654 0 1.5707963267949 0</pose>
<must_be_base_link>0</must_be_base_link>
<velocity_decay>
<linear>0</linear>
<angular>0</angular>
</velocity_decay>
<inertial>
<mass>0.0394518844656552</mass>
<pose>0 0 0 0 0 0</pose>
<inertia>
<ixx>8.80507112156287E-05</ixx>
<ixy>8.16537708214733E-12</ixy>
<ixz>6.7697456986363E-05</ixz>
<iyy>0.000163597433078288</iyy>
<iyz>2.88873087792653E-17</iyz>
<izz>7.67871947536992E-05</izz>
</inertia>
</inertial>
<collision name="arm2_collision">
<pose>0 0 0 0 0 0</pose>
<geometry>
<mesh>
<scale>1 1 1</scale>
<uri>model://Agile-eye/meshes/arm2_col.dae</uri>
</mesh>
</geometry>
<surface>
<contact>
<ode>
<kp>100000</kp>
<kd>1</kd>
</ode>
</contact>
<friction>
<ode>
<mu>0.2</mu>
<mu2>0.1</mu2>
</ode>
</friction>
</surface>
</collision>
<visual name="arm2_visual">
<pose>0 0 0 0 0 0</pose>
<material>
<ambient>0.752941176470588 0.752941176470588 0.752941176470588 1</ambient>
<diffuse>0.752941176470588 0.752941176470588 0.752941176470588 1</diffuse>
</material>
<geometry>
<mesh>
<scale>1 1 1</scale>
<uri>model://Agile-eye/meshes/arm2.dae</uri>
</mesh>
</geometry>
</visual>
</link>
<joint name="Base-arm2" type="revolute">
<parent>Base</parent>
<child>arm2</child>
<pose>0 0 0 0 0 0</pose>
<axis>
<xyz>0 0 1</xyz>
<dynamics>
<damping>0.0005</damping>
<friction>0.01</friction>
</dynamics>
<limit>
<effort>1</effort>
</limit>
</axis>
</joint>
<link name="arm3">
<gravity>1</gravity>
<self_collide>0</self_collide>
<pose>0.09692 0.19916 -0.0471 0 0 0</pose>
<must_be_base_link>0</must_be_base_link>
<velocity_decay>
<linear>0</linear>
<angular>0</angular>
</velocity_decay>
<inertial>
<mass>0.0423186704337124</mass>
<pose>0 0 0 0 0 0</pose>
<inertia>
<ixx>0.000171875114453318</ixx>
<ixy>6.38011539929127E-18</ixy>
<ixz>1.06032786454784E-12</ixz>
<iyy>9.53930746301887E-05</iyy>
<iyz>6.94374385724101E-05</iyz>
<izz>7.79025102333746E-05</izz>
</inertia>
</inertial>
<collision name="arm3_collision">
<pose>0 0 0 0 0 0</pose>
<geometry>
<mesh>
<scale>1 1 1</scale>
<uri>model://Agile-eye/meshes/arm3_col.dae</uri>
</mesh>
</geometry>
<surface>
<contact>
<ode>
<kp>100000</kp>
<kd>1</kd>
</ode>
</contact>
<friction>
<ode>
<mu>0.2</mu>
<mu2>0.1</mu2>
</ode>
</friction>
</surface>
</collision>
<visual name="arm3_visual">
<pose>0 0 0 0 0 0</pose>
<material>
<ambient>0.752941176470588 0.752941176470588 0.752941176470588 1</ambient>
<diffuse>0.752941176470588 0.752941176470588 0.752941176470588 1</diffuse>
</material>
<geometry>
<mesh>
<scale>1 1 1</scale>
<uri>model://Agile-eye/meshes/arm3.dae</uri>
</mesh>
</geometry>
</visual>
</link>
<joint name="arm2_arm3" type="revolute">
<parent>arm2</parent>
<child>arm3</child>
<pose>0 0 0 0 0 0</pose>
<axis>
<xyz>0 0 -1</xyz>
<dynamics>
<damping>0.0005</damping>
<friction>0.01</friction>
</dynamics>
<limit>
<effort>1</effort>
</limit>
</axis>
</joint>
<link name="EE">
<gravity>1</gravity>
<self_collide>0</self_collide>
<pose>0.09692 0.19916 0.0664 0 0 0</pose>
<must_be_base_link>0</must_be_base_link>
<velocity_decay>
<linear>0</linear>
<angular>0</angular>
</velocity_decay>
<inertial>
<mass>0.120954906173173</mass>
<pose>0 0 0 0 0 0</pose>
<inertia>
<ixx>0.000180443560480093</ixx>
<ixy>2.59283560938739E-05</ixy>
<ixz>5.1788503641712E-08</ixz>
<iyy>0.000159007046378964</iyy>
<iyz>5.7355413287255E-08</iyz>
<izz>0.000337451752236695</izz>
</inertia>
</inertial>
<collision name="EE_collision">
<pose>0 0 0 0 0 0</pose>
<geometry>
<mesh>
<scale>1 1 1</scale>
<uri>model://Agile-eye/meshes/EE_col.dae</uri>
</mesh>
</geometry>
<surface>
<contact>
<ode>
<kp>10000</kp>
<kd>1</kd>
</ode>
</contact>
<friction>
<ode>
<mu>0.2</mu>
<mu2>0.1</mu2>
</ode>
</friction>
</surface>
</collision>
<visual name="EE_visual">
<pose>0 0 0 0 0 0</pose>
<material>
<ambient>0.501960784313725 0.501960784313725 0.501960784313725 1</ambient>
<diffuse>0.501960784313725 0.501960784313725 0.501960784313725 1</diffuse>
</material>
<geometry>
<mesh>
<scale>1 1 1</scale>
<uri>model://Agile-eye/meshes/EE.dae</uri>
</mesh>
</geometry>
</visual>
</link>
<joint name="arm3_EE" type="revolute">
<parent>arm3</parent>
<child>EE</child>
<pose>0 0.107500 0.001 0 0 0</pose>
<axis>
<xyz>0 1 0</xyz>
<dynamics>
<damping>0.0005</damping>
<friction>0.01</friction>
</dynamics>
<limit>
<effort>1</effort>
</limit>
</axis>
</joint>
<joint name="arm1_EE" type="revolute">
<parent>arm1</parent>
<child>EE</child>
<pose>0.098500 0 0.001 0 0 0</pose>
<axis>
<xyz>1 0 0</xyz>
<dynamics>
<damping>0.0005</damping>
<friction>0.01</friction>
</dynamics>
<limit>
<effort>1</effort>
</limit>
</axis>
</joint>
<link name="Camera">
<gravity>1</gravity>
<self_collide>0</self_collide>
<pose>0.09692 0.19916 0.05 0 0 -1.5707963267949</pose>
<must_be_base_link>0</must_be_base_link>
<velocity_decay>
<linear>0</linear>
<angular>0</angular>
</velocity_decay>
<inertial>
<mass>0.000231288188899073</mass>
<pose>0 0 0 0 0 0</pose>
<inertia>
<ixx>1.21274710171044E-07</ixx>
<ixy>1.54460737605478E-26</ixy>
<ixz>9.13885054509812E-24</ixz>
<iyy>1.21330112893285E-07</iyy>
<iyz>1.18966874340782E-24</iyz>
<izz>4.73348422886319E-09</izz>
</inertia>
</inertial>
<visual name="Camera_visual">
<pose>0 0 0 0 0 0</pose>
<material>
<ambient>0 0 0 1</ambient>
<diffuse>0 0 0 1</diffuse>
</material>
<geometry>
<mesh>
<scale>1 1 1</scale>
<uri>model://Agile-eye/meshes/SJ.dae</uri>
</mesh>
</geometry>
</visual>
<joint name="EE_Camera" type="revolute">
<parent>EE</parent>
<child>Camera</child>
<pose>0 8.32667268468867E-17 0.0103050372196336 0 0 0</pose>
<axis>
<xyz>0 0 1</xyz>
<dynamics>
<damping>0.0005</damping>
<friction>0.01</friction>
</dynamics>
<limit>
<upper>0</upper>
<lower>0</lower>
<effort>1</effort>
</limit>
</axis>
</joint>
<link name="Rot">
<gravity>1</gravity>
<self_collide>0</self_collide>
<pose>0.03141924 -0.05233643 0.03465241 -1.5707963267949 0 0</pose>
<must_be_base_link>0</must_be_base_link>
<velocity_decay>
<linear>0</linear>
<angular>0</angular>
</velocity_decay>
<inertial>
<mass>0.00231288188899073</mass>
<pose>0 0 0 0 0 0</pose>
<inertia>
<ixx>1.21330112893285E-07</ixx>
<ixy>8.59712467698188E-24</ixy>
<ixz>2.54174663867409E-25</ixz>
<iyy>4.7334842288632E-09</iyy>
<iyz>1.33401971451468E-24</iyz>
<izz>1.21274710171044E-07</izz>
</inertia>
</inertial>
<visual name="Rot_visual">
<pose>0 0 0 0 0 0</pose>
<material>
<ambient>1 0 0 1</ambient>
<diffuse>1 0 0 1</diffuse>
</material>
<geometry>
<mesh>
<scale>1 1 1</scale>
<uri>model://Agile-eye/meshes/SJ.dae</uri>
</mesh>
</geometry>
</visual>
</link>
<joint name="Base_Rot" type="revolute">
<parent>Base</parent>
<child>Rot</child>
<pose>0 -0.000305037219633625 0 0 0 0</pose>
<axis>
<xyz>0 1 0</xyz>
<dynamics>
<damping>0.0005</damping>
<friction>0.01</friction>
</dynamics>
<limit>
<upper>0</upper>
<lower>0</lower>
<effort>1</effort>
</limit>
</axis>
</joint>
<plugin filename="libgazebo_ros_joint_trajectory.so" name="joint_trajectory_plugin">
<topicName>/set_joint</topicName>
</plugin>
<plugin name="joint_state_publisher" filename="libgazebo_ros_joint_state_publisher.so">
<robotNamespace>/</robotNamespace>
<jointName>Base-arm1, Base-arm2</jointName>
<updateRate>100.0</updateRate>
<alwaysOn>true</alwaysOn>
</plugin>
<plugin name="c_joints" filename="libcjoints.so"/>
</model>
</sdf>
Moreover, the provided plugin is written as below:
#include <boost/bind.hpp>
#include <gazebo/gazebo.hh>
#include <gazebo/physics/physics.hh>
#include <gazebo/common/common.hh>
#include <gazebo/common/Plugin.hh>
#include <ros/ros.h>
#include <stdio.h>
namespace gazebo
{
class Cjoints : public ModelPlugin
{ public: Cjoints() : ModelPlugin() {}
public: void Load(physics::ModelPtr _parent, sdf::ElementPtr /*_sdf*/)
{
if (!ros::isInitialized())
{
ROS_FATAL_STREAM("A ROS node for Gazebo has not been initialized, unable to load plugin. "
<< "Load the Gazebo system plugin 'libgazebo_ros_api_plugin.so' in the gazebo_ros package)");
return;
}
this->model = _parent;
this->j_controller = new physics::JointController(model);
this->joint1_ = this->model->GetJoint("Up_base-Arm2");
this->updateConnection = event::Events::ConnectWorldUpdateBegin(
boost::bind(&Cjoints::OnUpdate, this, _1));
}
public: void OnUpdate(const common::UpdateInfo & /*_info*/)
{
double angle(0.0);
j_controller->SetJointPosition(this->joint1_, angle);
}
private: physics::ModelPtr model;
private: event::ConnectionPtr updateConnection;
private: physics::JointController * j_controller;
private: physics::JointPtr joint1_;
};
GZ_REGISTER_MODEL_PLUGIN(Cjoints)
}
What is the wrong part? what should I do ? I am using ubunto 14.04, ROS indigo and Gazebo 7.3.
Asked by Mojzar on 2016-10-13 13:53:44 UTC
Comments