How to use Gazebo mimic joints plugin?
[TL;DR] I dont know how to implement and use mimic_joint_plugin in my project. I would appreciate some help or an example so I can look at it.
Im developing a quadruped robot, with the particularity that it only has two motors per leg. It has a parallel linkage in the elbow, so the 3rd link is always orthogonal to the ground.
So I made my urdf file using mimic joints, and the legs work as I want to. The problem came when I tried to do the same thing on Gazebo. Gazebo 7 (Im working with ROS KK and Ubuntu 16.04) doesnt support mimic joints, so I need to use mimic joint plugin (because I dont want to make a differente sdf file, so my Gazebo model can integrate with everything else in ROS). I kind of follow this tutorial but I still cant make it work.
In my opinion, I havent fully understand how Im supposed to define the mimic joints with the pluging. This is what I have done on cuadrubot_control.yaml:
cuadrubot:
joint_state_controller:
type: joint_state_controller/JointStateController
publish_rate: 50
legX_jointX_position_controller:
type: effort_controllers/JointPositionController
joint: leg1_joint1
pid: {p: 70.0, i: 60.0, d: 15.0}
Where I repeat legX_jointX_position_controller for all my not mimic joints.
Then on my cuadrubot_control.launch file:
<!-- Load joint controller configurations from YAML file to parameter server -->
<rosparam file="$(find cuadrubot_control)/config/gazebo/cuadrubot_control.yaml"
command="load"/>
<!-- load the controllers -->
<node name="controller_spawner" pkg="controller_manager" type="spawner" respawn="false"
output="screen" ns="/cuadrubot" args="legX_jointX_position_controller
joint_state_controller "/>
As far as I know, this is right.
In my macro.xacro file I have done this for each mimic joint (joint3 of each leg):
<xacro:macro name="joints" params="name origin_rpy front side">
<joint name="${name}_joint1" type="revolute">
<parent link="base_link"/>
<child link="${name}_link1"/>
<limit lower="-${pi/4}" upper ="${pi/4}" effort = "10" velocity = "10"/>
<origin xyz="${ancho_chasis/2*front} ${largo_chasis/2*side} 0" rpy="${origin_rpy}" />
<axis xyz="0 0 1" />
</joint>
<joint name="${name}_joint2" type="revolute">
<parent link="${name}_link1"/>
<child link="${name}_link2"/>
<limit lower="-${pi/2}" upper ="${pi/2}" effort = "10" velocity = "10"/>
<origin xyz= "${largo_link1} 0 0" rpy= "${pi/2} 0 0" />
<axis xyz="0 0 1" />
</joint>
<joint name="${name}_joint3" type="revolute">
<parent link="${name}_link2"/>
<child link="${name}_link3"/>
<limit lower="-${pi/2}" upper="${pi/2}" effort="10" velocity="10"/>
<mimic joint="${name}_joint2" multiplier="-1" offset="-${pi/2}" />
<origin xyz="${largo_link2} 0 0" rpy="0 0 ${pi/2}" />
<axis xyz="0 0 1" />
</joint>
<transmission name="${name}_joint1_trans">
<type>transmission_interface/SimpleTransmission</type>
<joint name="${name}_joint1">
<hardwareInterface>hardware_interface/EffortJointInterface</hardwareInterface>
</joint>
<actuator name="${name}_joint1_motor">
<mechanicalReduction>1</mechanicalReduction> <!-- 26 -->
<hardwareInterface>hardware_interface/EffortJointInterface</hardwareInterface>
</actuator>
</transmission>
<transmission name="${name}_joint2_trans">
<type>transmission_interface/SimpleTransmission</type>
<joint name="${name}_joint2">
<hardwareInterface>hardware_interface/EffortJointInterface</hardwareInterface>
</joint>
<actuator name="${name}_joint2_motor">
<mechanicalReduction>1</mechanicalReduction> <!-- 26 -->
<hardwareInterface>hardware_interface/EffortJointInterface</hardwareInterface>
</actuator>
</transmission>
<mimic_joint_plugin_gazebo
name_prefix="${name}_joint3"
parent_joint="${name}_joint2" mimic_joint="${name}_joint3"
has_pid="true" multiplier="-1.0" offset="-${pi/2 ...