SetJointPosition is not working properly (angles don't stay steady)
Hi all,
I am using the SetJointPosition
function in a simple Gazebo plugin to get my robot into a solid start position. The model is manually loaded in the default gazebo world.
At a first glance everything looked good, but then one hinge (hinge joint 2 (hinge_joint_1_hinge_joint_2_joint
)) slips downwards and the robot started to slowly move forward. At some time the attended visuals of hinge_joint_2_link
(connector visuals) fall apart while the following joints and links stay in position. Shortly after that the falling visual seem to hit the robot base and the complete robot flies through the air.
I have no idea what I am doing wrong. I think I use the function as explained in Basic Joint Control Question.
Here are my sdf models and the plugin code.
Robot Base:
<?xml version="1.0" ?>
<sdf version="1.3">
<!-- Model Name = Namespace -->
<model name="scitos">
<static>false</static>
<!-- Base Link-->
<link name="base_link">
<pose>0 0 0.34 0 0 0</pose>
<visual name="base_visual">
<geometry>
<cylinder>
<radius>0.30</radius>
<length>0.58</length>
</cylinder>
</geometry>
<material>
<ambient>0 0 0 1</ambient>
</material>
</visual>
<collision name="base_collision">
<geometry>
<cylinder>
<radius>0.30</radius>
<length>0.58</length>
</cylinder>
</geometry>
</collision>
<!-- Tail Link und Laufrolle als Visuals-->
<visual name="tail_visual">
<pose>-0.30 0 -0.19 0 0 0</pose>
<geometry>
<cylinder>
<radius>0.15</radius>
<length>0.20</length>
</cylinder>
</geometry>
<material>
<ambient>0 0 0 1</ambient>
</material>
</visual>
<collision name="tail_collision">
<pose>-0.30 0 -0.19 0 0 0</pose>
<geometry>
<cylinder>
<radius>0.15</radius>
<length>0.20</length>
</cylinder>
</geometry>
</collision>
<!-- Caster Wheel -->
<visual name="caster_visual">
<pose>-0.275 0 -0.29 0 0 0</pose>
<geometry>
<sphere>
<radius>0.05</radius>
</sphere>
</geometry>
</visual>
<collision name="caster_collision">
<pose>-0.275 0 -0.29 0 0 0</pose>
<geometry>
<sphere>
<radius>0.05</radius>
</sphere>
</geometry>
<surface>
<friction>
<ode>
<mu>0</mu>
<mu2>0</mu2>
<slip1>1.0</slip1>
<slip2>1.0</slip2>
</ode>
</friction>
</surface>
</collision>
</link>
<!-- Left Wheel Link -->
<link name="left_wheel_link">
<pose>0.075 0.155 0.05 -1.57 0 0</pose>
<collision name="left_wheel_collision">
<pose>0 0 0 0 0 0</pose>
<geometry>
<cylinder>
<radius>0.05</radius>
<length>0.025</length>
</cylinder>
</geometry>
</collision>
<visual name="left_wheel_visual">
<pose>0 0 0 0 0 0</pose>
<geometry>
<cylinder>
<radius>0.05</radius>
<length>0.025</length>
</cylinder>
</geometry>
</visual>
</link>
<!-- Right Wheel Link -->
<link name="right_wheel_link">
<pose>0.075 -0.155 0.05 -1.57 0 0</pose>
<collision name="right_wheel_collision">
<pose>0 0 0 0 0 0</pose>
<geometry>
<cylinder>
<radius>0.05</radius>
<length>0.025</length>
</cylinder>
</geometry>
</collision>
<visual name="right_wheel_visual">
<pose>0 0 0 0 0 0</pose>
<geometry>
<cylinder>
<radius>0.05</radius>
<length>0.025</length>
</cylinder>
</geometry>
</visual>
</link>
<!-- Base Left Wheel Joint-->
<joint name="base_left_wheel_joint" type="revolute">
<pose>0 0 0 0 0 0</pose>
<parent>base_link</parent>
<child>left_wheel_link</child>
<axis>
<xyz>0 1 0</xyz>
</axis>
</joint>
<!-- Base Right Wheel Joint-->
<joint name="base_right_wheel_joint" type="revolute">
<pose>0 0 0 0 0 0</pose>
<parent>base_link</parent>
<child>right_wheel_link</child>
<axis>
<xyz>0 1 0</xyz>
</axis>
</joint>
<!-- Arm Model include-->
<include>
<!-- Folder name of the model -->
<uri>model://scitos_arm</uri>
<pose>0 0 0.63 0 0 0</pose>
</include>
<joint name="base_link_arm_base_joint" type="revolute">
<parent>base_link</parent>
<!-- Namespace::name of the link where the arm will be connected -->
<child>scitos_arm::arm_base_link</child>
<axis>
<xyz>0 0 1</xyz>
<limit>
<upper>0</upper>
<lower>0</lower>
</limit>
</axis>
</joint>
<!-- Plugins includes-->
<!-- <plugin name="gazebo_plugin_test" filename="build/libgazebo_plugin_test.so"/> -->
<!-- first gazebo plugin / test for arm control plugin - Pluginpfad zu Eclipse -->
<plugin name="gazebo_plugin_test" filename="/home/christoph/eclipse_workspace/test/build/libgazebo_plugin_test.so"/>
<!-- platform_control plugin - Pluginpfad zu Eclipse -->
<!--<plugin name="gazebo_plugin_test" filename="/home/christoph/eclipse_workspace/plattform_control/build/libplattform_control.so" />-->
</model>
</sdf>
Robot Arm:
<?xml version='1.0'?>
<sdf version='1.3'>
<model name='scitos_arm'>
<static>false</static>
<!-- !!! Drehachsen beziehen sich immer auf das Basiskoordinatensystem (Megaabfuck), d.h. Drechachsen auswählen nach dem Basismodell, auch wenn die visuelle Darstellung nicht mehr stimmt-->
<!-- Basis der Koordinatensysteme: Drehgelenk in der Basis (link pose = 0), Knickgelenk in der Mitte (link pose + halbe höhe des zylinders)-->
<!-- Anpassung der Koordinatensysteme: Wenn im Ursprung, dann link pose = 0 und anpassung über visiual (siehe base link), wenn nicht im ursprung Link-Pose anpassen (siehe...)
<!-- LINKS LINKS LINKS -->
<!-- Arm Base Link -->
<link name='arm_base_link'>
<pose>0 0 0 0 0 0</pose>
<visual name='arm_base_visual'>
<pose>0 0 0.03 0 0 0</pose>
<geometry>
<cylinder>
<radius>0.08</radius>
<length>0.06</length>
</cylinder>
</geometry>
<material>
<ambient>0 0 0 1</ambient>
</material>
</visual>
<collision name='arm_base_collision'>
<pose>0 0 0.03 0 0 0</pose>
<geometry>
<cylinder>
<radius>0.08</radius>
<length>0.06</length>
</cylinder>
</geometry>
</collision>
</link>
<!-- Hinge joint 1 link (Drehgelenk 1); incl. connectors to the next joint -->
<!-- Referenziert zu Arm Base-->
<!-- l(t)=, b=, h= -->
<link name='hinge_joint_1_link'>
<pose>0 0 0.127 0 0 0</pose>
<visual name='hinge_joint_1_visual'>
<pose>0 0 0 0 0 0</pose>
<geometry>
<cylinder>
<radius>0.056 </radius>
<length>0.134</length>
</cylinder>
</geometry>
<material>
<ambient>0.83 0.55 0.09 1</ambient>
</material>
</visual>
<collision name='hinge_joint_1_collision'>
<pose>0 0 0 0 0 0</pose>
<geometry>
<cylinder>
<radius>0.056 </radius>
<length>0.134</length>
</cylinder>
</geometry>
</collision>
<!-- connector base / Unterteil Verbinder -->
<visual name='hinge_joint_1_visual_2'>
<pose>0 0 0.076 0 0 0</pose>
<geometry>
<box>
<size>0.112 0.134 0.018</size>
</box>
</geometry>
<material>
<ambient>0 0 0 1</ambient>
</material>
</visual>
<collision name='hinge_joint_1_collision_2'>
<pose>0 0 0.076 0 0 0</pose>
<geometry>
<box>
<size>0.112 0.134 0.018</size>
</box>
</geometry>
</collision>
<!-- connector side part / Seitenteil Verbinder -->
<visual name='hinge_joint_1_visual_3'>
<pose>0 -0.0745 0.132 0 0 0</pose>
<geometry>
<box>
<size>0.112 0.015 0.130</size>
</box>
</geometry>
<material>
<ambient>0 0 0 1</ambient>
</material>
</visual>
<collision name='hinge_joint_1_collision_3'>
<pose>0 0 0 0 0 0</pose>
<geometry>
<box>
<size>0.112 0.015 0.130</size>
</box>
</geometry>
</collision>
</link>
<!-- Hinge joint 1 -->
<joint name='arm_base_hinge_joint_1_joint' type='revolute'>
<pose>0 0 0 0 0 0</pose>
<parent>arm_base_link</parent>
<child>hinge_joint_1_link</child>
<axis>
<xyz>0 0 1</xyz>
</axis>
</joint>
<!-- Hinge joint 2 link (Knickgelenk 1); incl. connectors to the next joint -->
<!-- Referenziert zu Drehgelenk 1 Link-->
<link name='hinge_joint_2_link'>
<self_collide>true</self_collide>
<pose>0 0 0.268 -1.57 0 0</pose>
<visual name='hinge_joint_2_visual'>
<pose>0 0 0 0 0 0</pose>
<geometry>
<cylinder>
<radius>0.056 </radius>
<length>0.134</length>
</cylinder>
</geometry>
<material>
<ambient>0.83 0.55 0.09 1</ambient>
</material>
</visual>
<collision name='hinge_joint_2_collision'>
<pose>0 0 0 0 0 0</pose>
<geometry>
<cylinder>
<radius>0.056 </radius>
<length>0.134</length>
</cylinder>
</geometry>
</collision>
<!-- long side connector 1-->
<visual name='hinge_joint_2_visual_2'>
<pose>0 -0.198 0.0855 0 0 0</pose>
<geometry>
<box>
<size>0.105 0.500 0.037</size>
</box>
</geometry>
<material>
<ambient>0 0 0 1</ambient>
</material>
</visual>
<collision name='hinge_joint_2_collision_2'>
<pose>0 -0.198 0.0855 0 0 0</pose>
<geometry>
<box>
<size>0.105 0.500 0.037</size>
</box>
</geometry>
</collision>
</link>
<!-- Hinge joint 2 -->
<joint name='hinge_joint_1_hinge_joint_2_joint' type='revolute'>
<pose>0 0 0 0 0 0</pose>
<parent>hinge_joint_1_link</parent>
<child>hinge_joint_2_link</child>
<axis>
<xyz>0 1 0</xyz>
</axis>
</joint>
<!-- Hinge joint 3 link (Knickgelenk 2); incl. connector to the next joint -->
<link name='hinge_joint_3_link'>
<self_collide>true</self_collide>
<pose>0 0 0.668 1.57 0 0</pose>
<visual name='hinge_joint_3_visual'>
<pose>0 0 0 0 0 0</pose>
<geometry>
<cylinder>
<radius>0.056 </radius>
<length>0.134</length>
</cylinder>
</geometry>
<material>
<ambient>0.83 0.55 0.09 1</ambient>
</material>
</visual>
<collision name='hinge_joint_3_collision'>
<pose>0 0 0 0 0 0</pose>
<geometry>
<cylinder>
<radius>0.056 </radius>
<length>0.134</length>
</cylinder>
</geometry>
</collision>
<!-- Langer Seitenverbinder 2-->
<visual name='hinge_joint_3_visual_2'>
<pose>0 0.144 0.0855 0 0 0</pose>
<geometry>
<box>
<size>0.095 0.400 0.037</size>
</box>
</geometry>
<material>
<ambient>0 0 0 1</ambient>
</material>
</visual>
<collision name='hinge_joint_3_collision_2'>
<pose>0 0.180 0.0855 0 0 0</pose>
<geometry>
<box>
<size>0.095 0.400 0.037</size>
</box>
</geometry>
</collision>
</link>
<!-- Hinge joint 3 -->
<joint name='hinge_joint_2_hinge_joint_3_joint' type='revolute'>
<pose>0 0 0 0 0 0</pose>
<parent>hinge_joint_2_link</parent>
<child>hinge_joint_3_link</child>
<axis>
<xyz>0 -1 0</xyz>
</axis>
</joint>
<!-- Hinge joint 4 link (Knickgelenk 3); incl. connector to the next joint -->
<!-- !!!!!!!!!!!! Visual 2 und 3 ggf noch mal überarbeiten !!!!!!!!!!!!!!-->
<link name='hinge_joint_4_link'>
<self_collide>true</self_collide>
<pose>0 0 0.968 -1.57 0 0</pose>
<visual name='hinge_joint_4_visual'>
<pose>0 0 0 0 0 0</pose>
<geometry>
<cylinder>
<radius>0.0445 </radius>
<length>0.1125</length>
</cylinder>
</geometry>
<material>
<ambient>0.83 0.55 0.09 1</ambient>
</material>
</visual>
<collision name='hinge_joint_4_collision'>
<pose>0 0 0 0 0 0</pose>
<geometry>
<cylinder>
<radius>0.0445 </radius>
<length>0.1125</length>
</cylinder>
</geometry>
</collision>
<!-- connector base / Unterteil Verbinder -->
<visual name='hinge_joint_4_visual_2'>
<pose>0 0 0.067 0 0 0</pose>
<geometry>
<box>
<size>0.089 0.089 0.015</size>
</box>
</geometry>
<material>
<ambient>0 0 0 1</ambient>
</material>
</visual>
<collision name='hinge_joint_4_collision_2'>
<pose>0 0 0.067 0 0 0</pose>
<geometry>
<box>
<size>0.089 0.089 0.015</size>
</box>
</geometry>
</collision>
<!-- connector side / Seitenteil Verbinder -->
<visual name='hinge_joint_4_visual_3'>
<pose>0 -0.052 0 0 0 0</pose>
<geometry>
<box>
<size>0.1125 0.015 0.1425</size>
</box>
</geometry>
<material>
<ambient>0 0 0 1</ambient>
</material>
</visual>
<collision name='hinge_joint_4_collision_3'>
<pose>0 -0.052 0 0 0 0</pose>
<geometry>
<box>
<size>0.1125 0.015 0.1425</size>
</box>
</geometry>
</collision>
</link>
<!-- Hinge joint 4 -->
<joint name='hinge_joint_3_hinge_joint_4_joint' type='revolute'>
<pose>0 0 0 0 0 0</pose>
<parent>hinge_joint_3_link</parent>
<child>hinge_joint_4_link</child>
<axis>
<xyz>0 1 0</xyz>
</axis>
</joint>
<!-- Hinge joint 5 link (Drehgelenk 2); incl. connector and gripper base -->
<link name='hinge_joint_5_link'>
<self_collide>true</self_collide>
<pose>0 0 1.0785 0 0 0</pose>
<visual name='hinge_joint_5_visual'>
<pose>0 0 0 0 0 0</pose>
<geometry>
<cylinder>
<radius>0.0375 </radius>
<length>0.102</length>
</cylinder>
</geometry>
<material>
<ambient>0.83 0.55 0.09 1</ambient>
</material>
</visual>
<collision name='hinge_joint_5_collision'>
<pose>0 0 0 0 0 0</pose>
<geometry>
<cylinder>
<radius>0.0375 </radius>
<length>0.102</length>
</cylinder>
</geometry>
</collision>
<!-- connector to gripper base -->
<visual name='hinge_joint_5_visual_2'>
<pose>0 0 0.07375 0 0 0</pose>
<geometry>
<box>
<size>0.0435 0.112 0.0435</size>
</box>
</geometry>
<material>
<ambient>0 0 0 1</ambient>
</material>
</visual>
<collision name='hinge_joint_5_collision_2'>
<pose>0 0 0.07775 0 0 0</pose>
<geometry>
<box>
<size>0.0435 0.112 0.0435</size>
</box>
</geometry>
</collision>
<!-- gripper base -->
<visual name='hinge_joint_5_visual_3'>
<pose>0 0.0 0.1400 0 0 0</pose>
<geometry>
<box>
<size>0.091 0.112 0.091</size>
</box>
</geometry>
<material>
<ambient>0 0 0 1</ambient>
</material>
</visual>
<collision name='hinge_joint_5_collision_3'>
<pose>0 0 0 0 0 0</pose>
<geometry>
<box>
<size>0.091 0.112 0.091</size>
</box>
</geometry>
</collision>
</link>
<!-- Hinge joint 5 -->
<joint name='hinge_joint_4_hinge_joint_5_joint' type='revolute'>
<pose>0 0 0 0 0 0</pose>
<parent>hinge_joint_4_link</parent>
<child>hinge_joint_5_link</child>
<axis>
<xyz>0 0 1</xyz>
</axis>
</joint>
<!-- Left finger link -->
<link name='left_finger_link'>
<self_collide>true</self_collide>
<pose>0 0.010 1.299 0 0 0</pose>
<visual name='left_finger_visual'>
<pose>0 0 0 0 0 0</pose>
<geometry>
<box>
<size>0.030 0.020 0.070</size>
</box>
</geometry>
<material>
<ambient>0.83 0.55 0.09 1</ambient>
</material>
</visual>
<collision name='left_finger_collision'>
<pose>0 0 0 0 0 0</pose>
<geometry>
<box>
<size>0.030 0.020 0.070</size>
</box>
</geometry>
</collision>
</link>
<!-- Left finger joint -->
<joint name='hinge_joint_5_left_finger_joint' type='prismatic'>
<pose>0 0 0 0 0 0</pose>
<parent>hinge_joint_5_link</parent>
<child>left_finger_link</child>
<axis>
<xyz>0 1 0</xyz>
<limit>
<upper>0.034</upper>
<lower>0</lower>
</limit>
</axis>
</joint>
<!-- Right finger link -->
<link name='right_finger_link'>
<self_collide>true</self_collide>
<pose>0 -0.010 1.299 0 0 0</pose>
<visual name='right_finger_visual'>
<pose>0 0 0 0 0 0</pose>
<geometry>
<box>
<size>0.030 0.020 0.070</size>
</box>
</geometry>
<material>
<ambient>0.83 0.55 0.09 1</ambient>
</material>
</visual>
<collision name='right_finger_collision'>
<pose>0 0 0 0 0 0</pose>
<geometry>
<box>
<size>0.030 0.020 0.070</size>
</box>
</geometry>
</collision>
</link>
<!-- Right finger joint -->
<joint name='hinge_joint_5_right_finger_joint' type='prismatic'>
<pose>0 0 0 0 0 0</pose>
<parent>hinge_joint_5_link</parent>
<child>right_finger_link</child>
<axis>
<xyz>0 -1 0</xyz>
<limit>
<upper>0.034</upper>
<lower>0</lower>
</limit>
</axis>
</joint>
</model>
</sdf>
Plugin Code:
#include <boost/bind.hpp>
#include <gazebo.hh>
#include <physics/physics.hh>
#include <common/common.hh>
#include <stdio.h>
#include <string>
namespace gazebo
{
class GazeboPluginTest : public ModelPlugin
{
public: void Load(physics::ModelPtr _parent, sdf::ElementPtr _sdf)
{
// Store the pointer to the model
this->model = _parent;
// Joint Controller
this->myJointController = new physics::JointController(model);
// Set joint names
hinge_joint1 = "scitos_arm::arm_base_hinge_joint_1_joint";
hinge_joint2 ="scitos_arm::hinge_joint_1_hinge_joint_2_joint";
hinge_joint3 = "scitos_arm::hinge_joint_2_hinge_joint_3_joint";
hinge_joint4 = "scitos_arm::hinge_joint_3_hinge_joint_4_joint";
hinge_joint5 = "scitos_arm::hinge_joint_4_hinge_joint_5_joint";
left_finger_joint = "scitos_arm::hinge_joint_5_left_finger_joint";
right_finger_joint = "scitos_arm::hinge_joint_5_right_finger_joint";
// Get Pointer to the joints
hinge_joint1_ptr = this->model->GetJoint(hinge_joint1);
hinge_joint2_ptr = this->model->GetJoint(hinge_joint2);
hinge_joint3_ptr = this->model->GetJoint(hinge_joint3);
hinge_joint4_ptr = this->model->GetJoint(hinge_joint4);
hinge_joint5_ptr = this->model->GetJoint(hinge_joint5);
left_finger_ptr = this->model->GetJoint(left_finger_joint);
right_finger_ptr = this->model->GetJoint(right_finger_joint);
// Add joints to the joint controller
//myJointController->AddJoint(drehgelenk1_pointer);
//myJointController->AddJoint(knickgelenk1_pointer);
//myJointController->AddJoint(knickgelenk2_pointer);
//myJointController->AddJoint(knickgelenk3_pointer);
//myJointController->AddJoint(drehgelenk2_pointer);
//myJointController->AddJoint(greifer_links_pointer);
//myJointController->AddJoint(greifer_rechts_pointer);
// Listen to the update event. This event is broadcast every
// simulation iteration.
this->updateConnection = event::Events::ConnectWorldUpdateBegin(
boost::bind(&GazeboPluginTest::OnUpdate, this));
}
// Called by the world update start event
public: void OnUpdate()
{
double PI = 3.141592654;
// Set joint positions
myJointController->SetJointPosition(hinge_joint1_ptr, 0.0);
myJointController->SetJointPosition(hinge_joint2_ptr, PI/2);
myJointController->SetJointPosition(hinge_joint3_ptr, PI/2);
myJointController->SetJointPosition(hinge_joint4_ptr, PI/2);
myJointController->SetJointPosition(hinge_joint5_ptr, 0.0);
myJointController->SetJointPosition(left_finger_ptr, 0.0);
myJointController->SetJointPosition(right_finger_ptr, 0.0);
}
// Pointer to the model
private: physics::ModelPtr model;
// Pointer to the update event connection
private: event::ConnectionPtr updateConnection;
// Pointer to the JointContoller
private: physics::JointController * myJointController;
//private: physics::JointControllerPtr myJointController2;
// Joint name variables
private:
std::string hinge_joint1;
std::string hinge_joint2;
std::string hinge_joint3;
std::string hinge_joint4;
std::string hinge_joint5;
std::string left_finger_joint;
std::string right_finger_joint;
// Pointer to the joints
private:
physics::JointPtr hinge_joint1_ptr;
physics::JointPtr hinge_joint2_ptr;
physics::JointPtr hinge_joint3_ptr;
physics::JointPtr hinge_joint4_ptr;
physics::JointPtr hinge_joint5_ptr;
physics::JointPtr left_finger_ptr;
physics::JointPtr right_finger_ptr;
};
// Register this plugin with the simulator
GZ_REGISTER_MODEL_PLUGIN(GazeboPluginTest)
}
Thank you
Christoph
Asked by Christoph on 2013-04-22 10:48:37 UTC
Answers
You could try creating a controller yourself for the joints, and controlling them via forces not by position. Or most probably there are already some packages that do this. Here is a link of something similar, how to create a PID controller for your joints. http://answers.gazebosim.org/question/2341/set-and-get-position-of-gazebo-model-using-ros/
Asked by AndreiHaidu on 2013-04-24 05:44:28 UTC
Comments
What are you trying to achieve with the code? Because now, you are setting the robots joint on every update ( 1000/sec) to that position when you could be doing it only once in the Load() function, most probably that is the reason of the problem.
Asked by AndreiHaidu on 2013-04-23 11:21:04 UTC
Hey Andrei! I want to get the robot arm in one fixed position and see how that function works in the simulation. I tried setting the position in the Load() function as you recommended. The robot set the right angle and collapsed after that. I want the joints to stay fixed until I change them again.
Asked by Christoph on 2013-04-24 05:13:07 UTC