Hello Gazebosim community.
I have an issue that i have so far been unable to work out.
I initially had a look at the demo world for ignition fortress joint_trajectory_controller.sdf
I wanted to learn how to simply mimic movements in Gazebo Ignition for a robot i specified. I just want to be able to say. Joint 1 go to this position, Joint 2 this… etc. And it would go to those positions and stop.
This seems to work fine the example code but when i apply it to my own SDF in which the robot is entirely kinematic and gravity is disabled. It does indeed move towards the desired position but then oscilated back and forth. It is kinda wobly.
Here is my code.
<?xml version="1.0" ?>
<sdf version="1.6">
<world name="p2">
<gravity>0 0 0</gravity>
<scene>
<ambient>1.0 1.0 1.0</ambient>
<background>0.8 0.8 0.8</background>
</scene>
<light type="directional" name="sun">
<cast_shadows>true</cast_shadows>
<pose>0 0 10 0 0 0</pose>
<diffuse>0.8 0.8 0.8 1</diffuse>
<specular>0.2 0.2 0.2 1</specular>
<attenuation>
<range>1000</range>
<constant>0.9</constant>
<linear>0.01</linear>
<quadratic>0.001</quadratic>
</attenuation>
<direction>-0.5 0.1 -0.9</direction>
</light>
<model name="ground_plane">
<static>true</static>
<link name="link">
<collision name="collision">
<geometry>
<plane>
<normal>0 0 1</normal>
<size>100 100</size>
</plane>
</geometry>
</collision>
<visual name="visual">
<geometry>
<plane>
<normal>0 0 1</normal>
<size>100 100</size>
</plane>
</geometry>
<material>
<ambient>0.8 0.8 0.8 1</ambient>
<diffuse>0.8 0.8 0.8 1</diffuse>
<specular>0.8 0.8 0.8 1</specular>
</material>
</visual>
</link>
</model>
<model name="schneider_VRKP2">
<pose>0 0 1 0 0 0</pose>
<link name="link_base">
<kinematic>true</kinematic>
<pose relative_to="__model__">0 0 0 0 0 0</pose>
</link>
<!-- ++++ Links (Arm 0) ++++ -->
<link name="link_robot_arm0_axis">
<kinematic>true</kinematic>
<pose relative_to="joint_arm0_base_to_axis">0 0 0 0 0 0</pose>
</link>
<link name="link_robot_arm0_shoulder">
<kinematic>true</kinematic>
<pose relative_to="joint_arm0_axis_to_shoulder">0 0 0 0 0 0</pose>
<visual name="visual">
<geometry>
<mesh>
<uri>p2_upper_arm.dae</uri>
</mesh>
</geometry>
<material>
<ambient>0.5 0.5 0.5 1</ambient>
<diffuse>0.8 0.8 0.8 1</diffuse>
<specular>0.8 0.8 0.8 1</specular>
</material>
</visual>
</link>
<link name="link_robot_arm0_upper">
<kinematic>true</kinematic>
<pose relative_to="joint_arm0_shoulder_to_upper">0 0 0 0 0 0</pose>
</link>
<link name="link_robot_arm0_lower">
<kinematic>true</kinematic>
<pose relative_to="joint_arm0_upper_to_lower">0 0 0 0 0 0</pose>
<visual name="visual">
<geometry>
<mesh>
<uri>p2_lower_arm.dae</uri>
</mesh>
</geometry>
<material>
<ambient>0.5 0.5 0.5 1</ambient>
<diffuse>0.8 0.8 0.8 1</diffuse>
<specular>0.8 0.8 0.8 1</specular>
</material>
</visual>
</link>
<link name="link_robot_arm0_plate">
<kinematic>true</kinematic>
<pose relative_to="joint_arm0_lower_to_plate">0 0 0 0 0 0</pose>
</link>
<link name="link_robot_arm0_TCP">
<kinematic>true</kinematic>
<pose relative_to="joint_arm0_plate_to_TCP">0 0 0 0 0 0</pose>
<visual name="visual">
<geometry>
<mesh>
<uri>p2_plate.dae</uri>
</mesh>
</geometry>
<material>
<ambient>0.5 0.5 0.5 1</ambient>
<diffuse>0.8 0.8 0.8 1</diffuse>
<specular>0.8 0.8 0.8 1</specular>
</material>
</visual>
</link>
<!-- ++++ Joints (Arm 0) ++++ -->
<joint name="joint_arm0_base_to_axis" type="fixed">
<pose relative_to="link_base">0 0 0 0 0 0</pose>
<parent>link_base</parent>
<child>link_robot_arm0_axis</child>
</joint>
<joint name="joint_arm0_axis_to_shoulder" type="revolute">
<pose relative_to="link_robot_arm0_axis">0.12 0 0 1.570796 0 0</pose>
<parent>link_robot_arm0_axis</parent>
<child>link_robot_arm0_shoulder</child>
<axis>
<xyz>0 0 1</xyz>
<dynamics>
<damping>0.9</damping>
</dynamics>
</axis>
</joint>
<joint name="joint_arm0_shoulder_to_upper" type="revolute">
<pose relative_to="link_robot_arm0_shoulder">0.28 0 0 0 0 0</pose>
<parent>link_robot_arm0_shoulder</parent>
<child>link_robot_arm0_upper</child>
<axis>
<xyz>0 0 1</xyz>
<dynamics>
<damping>0.7</damping>
</dynamics>
</axis>
</joint>
<joint name="joint_arm0_upper_to_lower" type="revolute">
<pose relative_to="link_robot_arm0_upper">0 0 0 0 0 -2.194238</pose>
<parent>link_robot_arm0_upper</parent>
<child>link_robot_arm0_lower</child>
<axis>
<xyz>0 0 1</xyz>
<dynamics>
<damping>0.6</damping>
</dynamics>
</axis>
</joint>
<joint name="joint_arm0_lower_to_plate" type="revolute">
<pose relative_to="link_robot_arm0_lower">0.6 0 0 0 0 -0.947354</pose>
<parent>link_robot_arm0_lower</parent>
<child>link_robot_arm0_plate</child>
<axis>
<xyz>0 0 1</xyz>
<dynamics>
<damping>0.25</damping>
</dynamics>
</axis>
</joint>
<joint name="joint_arm0_plate_to_TCP" type="fixed">
<pose relative_to="link_robot_arm0_plate">0.0497 0 0 -1.570796 0 0</pose>
<parent>link_robot_arm0_plate</parent>
<child>link_robot_arm0_TCP</child>
</joint>
<!-- +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ -->
<!-- +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ -->
<!-- +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ -->
<!-- ++++ Links (Arm 1) ++++ -->
<link name="link_robot_arm1_axis">
<kinematic>true</kinematic>
<pose relative_to="joint_arm1_base_to_axis">0 0 0 0 0 0</pose>
</link>
<link name="link_robot_arm1_shoulder">
<kinematic>true</kinematic>
<pose relative_to="joint_arm1_axis_to_shoulder">0 0 0 0 0 0</pose>
</link>
<link name="link_robot_arm1_upper">
<kinematic>true</kinematic>
<pose relative_to="joint_arm1_shoulder_to_upper">0 0 0 0 0 0</pose>
</link>
<link name="link_robot_arm1_lower">
<kinematic>true</kinematic>
<pose relative_to="joint_arm1_upper_to_lower">0 0 0 0 0 0</pose>
</link>
<link name="link_robot_arm1_plate">
<kinematic>true</kinematic>
<pose relative_to="joint_arm1_lower_to_plate">0 0 0 0 0 0</pose>
</link>
<link name="link_robot_arm1_TCP">
<kinematic>true</kinematic>
<pose relative_to="joint_arm1_plate_to_TCP">0 0 0 0 0 0</pose>
</link>
<!-- ++++ Joints (Arm 1) ++++ -->
<joint name="joint_arm1_base_to_axis" type="fixed">
<pose relative_to="link_base">0 0 0 0 0 2.0943951</pose>
<parent>link_base</parent>
<child>link_robot_arm1_axis</child>
</joint>
<joint name="joint_arm1_axis_to_shoulder" type="revolute">
<pose relative_to="link_robot_arm1_axis">0.12 0 0 1.570796 0 0</pose>
<parent>link_robot_arm1_axis</parent>
<child>link_robot_arm1_shoulder</child>
</joint>
<joint name="joint_arm1_shoulder_to_upper" type="revolute">
<pose relative_to="link_robot_arm1_shoulder">0.28 0 0 0 0 0</pose>
<parent>link_robot_arm1_shoulder</parent>
<child>link_robot_arm1_upper</child>
</joint>
<joint name="joint_arm1_upper_to_lower" type="revolute">
<pose relative_to="link_robot_arm1_upper">0 0 0 0 0 -2.194238</pose>
<parent>link_robot_arm1_upper</parent>
<child>link_robot_arm1_lower</child>
</joint>
<joint name="joint_arm1_lower_to_plate" type="revolute">
<pose relative_to="link_robot_arm1_lower">0.6 0 0 0 0 -0.947354</pose>
<parent>link_robot_arm1_lower</parent>
<child>link_robot_arm1_plate</child>
</joint>
<joint name="joint_arm1_plate_to_TCP" type="fixed">
<pose relative_to="link_robot_arm1_plate">0.0497 0 0 -1.570796 0 0</pose>
<parent>link_robot_arm1_plate</parent>
<child>link_robot_arm1_TCP</child>
</joint>
<!-- +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ -->
<!-- +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ -->
<!-- +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ -->
<!-- +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ -->
<!-- +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ -->
<!-- +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ -->
<!-- ++++ Links (Arm 2) ++++ -->
<link name="link_robot_arm2_axis">
<kinematic>true</kinematic>
<pose relative_to="joint_arm2_base_to_axis">0 0 0 0 0 0</pose>
</link>
<link name="link_robot_arm2_shoulder">
<kinematic>true</kinematic>
<pose relative_to="joint_arm2_axis_to_shoulder">0 0 0 0 0 0</pose>
</link>
<link name="link_robot_arm2_upper">
<kinematic>true</kinematic>
<pose relative_to="joint_arm2_shoulder_to_upper">0 0 0 0 0 0</pose>
</link>
<link name="link_robot_arm2_lower">
<kinematic>true</kinematic>
<pose relative_to="joint_arm2_upper_to_lower">0 0 0 0 0 0</pose>
</link>
<link name="link_robot_arm2_plate">
<kinematic>true</kinematic>
<pose relative_to="joint_arm2_lower_to_plate">0 0 0 0 0 0</pose>
</link>
<link name="link_robot_arm2_TCP">
<kinematic>true</kinematic>
<pose relative_to="joint_arm2_plate_to_TCP">0 0 0 0 0 0</pose>
</link>
<!-- ++++ Joints (Arm 1) ++++ -->
<joint name="joint_arm2_base_to_axis" type="fixed">
<pose relative_to="link_base">0 0 0 0 0 -2.0943951</pose>
<parent>link_base</parent>
<child>link_robot_arm2_axis</child>
</joint>
<joint name="joint_arm2_axis_to_shoulder" type="revolute">
<pose relative_to="link_robot_arm2_axis">0.12 0 0 1.570796 0 0</pose>
<parent>link_robot_arm2_axis</parent>
<child>link_robot_arm2_shoulder</child>
</joint>
<joint name="joint_arm2_shoulder_to_upper" type="revolute">
<pose relative_to="link_robot_arm2_shoulder">0.28 0 0 0 0 0</pose>
<parent>link_robot_arm2_shoulder</parent>
<child>link_robot_arm2_upper</child>
</joint>
<joint name="joint_arm2_upper_to_lower" type="revolute">
<pose relative_to="link_robot_arm2_upper">0 0 0 0 0 -2.194238</pose>
<parent>link_robot_arm2_upper</parent>
<child>link_robot_arm2_lower</child>
</joint>
<joint name="joint_arm2_lower_to_plate" type="revolute">
<pose relative_to="link_robot_arm2_lower">0.6 0 0 0 0 -0.947354</pose>
<parent>link_robot_arm2_lower</parent>
<child>link_robot_arm2_plate</child>
</joint>
<joint name="joint_arm2_plate_to_TCP" type="fixed">
<pose relative_to="link_robot_arm2_plate">0.0497 0 0 -1.570796 0 0</pose>
<parent>link_robot_arm2_plate</parent>
<child>link_robot_arm2_TCP</child>
</joint>
<!-- +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ -->
<!-- +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ -->
<!-- +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ -->
<joint name="World_Anchor" type="fixed">
<parent>world</parent>
<child>link_base</child>
</joint>
<!-- +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ -->
<!-- +++++++++++++++++++++++CONTROLLER++++++++++++++++++++++++++ -->
<!-- +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ -->
<plugin
filename="libignition-gazebo-joint-trajectory-controller-system.so"
name="ignition::gazebo::systems::JointTrajectoryController">
<!-- ARM 0 JOINTS -->
<joint_name>joint_arm0_axis_to_shoulder</joint_name>
<initial_position>0</initial_position>
<position_p_gain>30</position_p_gain>
<position_i_gain>0.4</position_i_gain>
<position_d_gain>1.0</position_d_gain>
<position_i_min>-1</position_i_min>
<position_i_max>1</position_i_max>
<position_cmd_min>-20</position_cmd_min>
<position_cmd_max>20</position_cmd_max>
<joint_name>joint_arm0_shoulder_to_upper</joint_name>
<initial_position>0</initial_position>
<position_p_gain>30</position_p_gain>
<position_i_gain>0.4</position_i_gain>
<position_d_gain>1.0</position_d_gain>
<position_i_min>-1</position_i_min>
<position_i_max>1</position_i_max>
<position_cmd_min>-20</position_cmd_min>
<position_cmd_max>20</position_cmd_max>
<joint_name>joint_arm0_upper_to_lower</joint_name>
<initial_position>0</initial_position>
<position_p_gain>30</position_p_gain>
<position_i_gain>0.4</position_i_gain>
<position_d_gain>1.0</position_d_gain>
<position_i_min>-1</position_i_min>
<position_i_max>1</position_i_max>
<position_cmd_min>-20</position_cmd_min>
<position_cmd_max>20</position_cmd_max>
<joint_name>joint_arm0_lower_to_plate</joint_name>
<initial_position>0</initial_position>
<position_p_gain>30</position_p_gain>
<position_i_gain>0.4</position_i_gain>
<position_d_gain>1.0</position_d_gain>
<position_i_min>-1</position_i_min>
<position_i_max>1</position_i_max>
<position_cmd_min>-20</position_cmd_min>
<position_cmd_max>20</position_cmd_max>
<!-- ++++++++++++++++++++++++++++++++++++++++++++ -->
<!-- ARM 0 JOINTS -->
<joint_name>joint_arm1_axis_to_shoulder</joint_name>
<initial_position>0</initial_position>
<position_p_gain>30</position_p_gain>
<position_i_gain>0.4</position_i_gain>
<position_d_gain>1.0</position_d_gain>
<position_i_min>-1</position_i_min>
<position_i_max>1</position_i_max>
<position_cmd_min>-20</position_cmd_min>
<position_cmd_max>20</position_cmd_max>
<joint_name>joint_arm1_shoulder_to_upper</joint_name>
<initial_position>0</initial_position>
<position_p_gain>30</position_p_gain>
<position_i_gain>0.4</position_i_gain>
<position_d_gain>1.0</position_d_gain>
<position_i_min>-1</position_i_min>
<position_i_max>1</position_i_max>
<position_cmd_min>-20</position_cmd_min>
<position_cmd_max>20</position_cmd_max>
<joint_name>joint_arm1_upper_to_lower</joint_name>
<initial_position>0</initial_position>
<position_p_gain>30</position_p_gain>
<position_i_gain>0.4</position_i_gain>
<position_d_gain>1.0</position_d_gain>
<position_i_min>-1</position_i_min>
<position_i_max>1</position_i_max>
<position_cmd_min>-20</position_cmd_min>
<position_cmd_max>20</position_cmd_max>
<joint_name>joint_arm1_lower_to_plate</joint_name>
<initial_position>0</initial_position>
<position_p_gain>30</position_p_gain>
<position_i_gain>0.4</position_i_gain>
<position_d_gain>10.0</position_d_gain>
<position_i_min>-1</position_i_min>
<position_i_max>1</position_i_max>
<position_cmd_min>-20</position_cmd_min>
<position_cmd_max>20</position_cmd_max>
</plugin>
</model>
</world>
</sdf>
<!--
Try sending joint trajectory commands:
- Position control (Red RR robot) -
ign topic -t "/model/schneider_VRKP2/joint_trajectory" -m ignition.msgs.JointTrajectory -p '
joint_names: "joint_arm0_axis_to_shoulder"
points {
positions: 1.570796
time_from_start {
sec: 0
nsec: 250000000
}
}'
ign topic -t "/model/schneider_VRKP2/joint_trajectory" -m ignition.msgs.JointTrajectory -p '
joint_names: "joint_arm0_lower_to_plate"
points {
positions: -1.570796
time_from_start {
sec: 0
nsec: 250000000
}
}'
ign topic -t "/model/schneider_VRKP2/joint_trajectory" -m ignition.msgs.JointTrajectory -p '
joint_names: "joint_arm0_axis_to_shoulder"
joint_names: "joint_arm0_shoulder_to_upper"
joint_names: "joint_arm0_upper_to_lower"
points {
positions: -0.2
positions: -0.2
positions: -0.2
time_from_start {
sec: 0
nsec: 250000000
}
}
points {
positions: 0.2
positions: 0.2
positions: 0.2
time_from_start {
sec: 10
nsec: 250000000
}
}
points {
positions: 0
positions: 0
positions: 0
time_from_start {
sec: 30
nsec: 250000000
}
}'
ign topic -t "/model/schneider_VRKP2/joint_trajectory" -m ignition.msgs.JointTrajectory -p '
joint_names: "joint_arm0_axis_to_shoulder"
points {
positions: -1.570796
time_from_start {
sec: 0
nsec: 250000000
}
}'
-->
I’ve included an image of the frames here.
I’m trying to build a Schnieder Delta P2 robot. However i have been stopped to one arm at the moment due to lack of closed kinematic chains support.
