I am currently working on a "two legs" modular robot (cuboctaedric units with some revolute joints) and I have to make it move in Gazebo. Problem is, when I move the joints of my robot and it doesn't touch the ground anymore, it doesn't fall in Gazebo, the cylindrical link is pinned and so is the entire robot : The robot doesn't touch the ground but doesn't fall
Here is what the robot looks like : enter image description here enter image description here
I built this model starting with the cylindrical link at the top (the one between the two "legs"), and there is a fixed joint between this link and a virtual link named world. Here is the URDF file I created for this robot :
<link name="world"/>
<!-- virtual joint -->
<joint name="virtual_joint" type="fixed">
<parent link="world"/>
<child link="link1"/>
<origin rpy="0 0 0" xyz="0 0 0.99"/>
</joint>
<link name="link1">
<visual>
<origin rpy="${pi/2.0} 0 ${pi/2.0}" xyz="0 0 0" />
<geometry>
<cylinder radius="0.1" length="0.04" />
</geometry>
</visual>
<collision>
<origin rpy="${pi/2.0} 0 ${pi/2.0}" xyz="0 0 0" />
<geometry>
<cylinder radius="0.1" length="0.04" />
</geometry>
</collision>
<xacro:cylinder_inertia m="0.5" r="0.1" h="0.04" xyz="0 0 0" rpy="${pi/2.0} 0 ${pi/2.0}" />
</link>
<!-- left arm -->
<joint name="top_left" type="revolute">
<parent link="link1" />
<child link="link2" />
<origin rpy="0 0 0" xyz="-0.02 0 0" />
<axis xyz="1 0 0" />
<limit effort="411" lower="-${2*pi}" upper="${2*pi}" velocity="${pi}"/>
<dynamics damping="60.0" friction="4.0" />
</joint>
<link name="link2">
<visual>
<origin rpy="0 0 ${pi/4.0}" xyz="-0.14 0 0" />
<geometry>
<mesh filename="package://modrobot_description/meshes/cuboctahedron.stl" scale="0.2 0.2 0.2" />
</geometry>
</visual>
<collision>
<origin rpy="0 0 ${pi/4.0}" xyz="-0.14 0 0" />
<geometry>
<mesh filename="package://modrobot_description/meshes/cuboctahedron.stl" scale="0.2 0.2 0.2" />
</geometry>
</collision>
<xacro:box_inertia m="1.0" l="0.28" w="0.28" h="0.28" xyz="-0.14 0 0" rpy="0 0 0" />
</link>
<!-- test représentation cuboctaèdre par cube pour inertia -->
<!-- <link name="link2">
<visual>
<origin rpy="0 0 0" xyz="-0.14 0 0" />
<geometry>
<box size="0.28 0.28 0.28" />
</geometry>
</visual>
</link> -->
<joint name="joint2_3" type="fixed">
<parent link="link2" />
<child link="link3" />
<origin rpy="0 0 0" xyz="-0.14 0 -0.28" />
</joint>
<link name="link3">
<visual>
<origin rpy="0 0 ${pi/4.0}" xyz="0 0 0" />
<geometry>
<mesh filename="package://modrobot_description/meshes/cuboctahedron.stl" scale="0.2 0.2 0.2" />
</geometry>
</visual>
<collision>
<origin rpy="0 0 ${pi/4.0}" xyz="0 0 0" />
<geometry>
<mesh filename="package://modrobot_description/meshes/cuboctahedron.stl" scale="0.2 0.2 0.2" />
</geometry>
</collision>
<xacro:box_inertia m="1.0" l="0.28" w="0.28" h="0.28" xyz="0 0 0" rpy="0 0 0" />
</link>
<joint name="joint3_4" type="fixed">
<parent link="link3" />
<child link="link4" />
<origin rpy="0 0 0" xyz="0 0 -0.15"/>
</joint>
<link name="link4">
<visual>
<origin rpy="-${pi} 0 0" xyz="0 0 -0.01" />
<geometry>
<mesh scale="0.02 0.02 0.02" filename="package://modrobot_description/meshes/joint1_z.stl" />
</geometry>
</visual>
<collision>
<origin rpy="-${pi} 0 0" xyz="0 0 -0.01" />
<geometry>
<mesh scale="0.02 0.02 0.02" filename="package://modrobot_description/meshes/joint1_z.stl" />
</geometry>
</collision>
<xacro:cylinder_inertia m="1.0" r="0.1" h="0.18" xyz="0 0 -0.08" rpy="0 0 0" />
</link>
<!-- test représentation joint1_z par cylindre pour inertia -->
<!-- <link name="link4">
<visual>
<origin rpy="0 0 0" xyz="0 0 -0.08" />
<geometry>
<cylinder radius="0.1" length="0.18" />
</geometry>
</visual>
</link> -->
<joint name="middle_left" type="revolute">
<parent link="link4" />
<child link="link5" />
<origin rpy="0 0 0" xyz="0 0 -0.13" />
<axis xyz="1 0 0" />
<limit effort="411" lower="-${2*pi}" upper="${2*pi}" velocity="${pi}"/>
<dynamics damping="60.0" friction="4.0" />
</joint>
<link name="link5">
<visual>
<origin rpy="0 0 0" xyz="0 0 0" />
<geometry>
<sphere radius="0.065" />
</geometry>
</visual>
<collision>
<origin rpy="0 0 0" xyz="0 0 0" />
<geometry>
<sphere radius="0.065" />
</geometry>
</collision>
<xacro:sphere_inertia m="0.5" r="0.065" xyz="0 0 0" rpy="0 0 0" />
</link>
<joint name="joint5_6" type="fixed">
<parent link="link5" />
<child link="link6" />
<origin rpy="0 0 0" xyz="0 0 -0.13"/>
</joint>
<link name="link6">
<visual>
<origin rpy="0 0 -${pi/2}" xyz="0 0 0" />
<geometry>
<mesh scale="0.02 0.02 0.02" filename="package://modrobot_description/meshes/joint1_z.stl" />
</geometry>
</visual>
<collision>
<origin rpy="0 0 -${pi/2}" xyz="0 0 0" />
<geometry>
<mesh scale="0.02 0.02 0.02" filename="package://modrobot_description/meshes/joint1_z.stl" />
</geometry>
</collision>
<xacro:cylinder_inertia m="1.0" r="0.1" h="0.18" xyz="0 0 0.07" rpy="0 0 0" />
</link>
<!-- test représentation joint1_z par cylindre pour inertia -->
<!-- <link name="link6">
<visual>
<origin rpy="0 0 0" xyz="0 0 0.07" />
<geometry>
<cylinder radius="0.1" length="0.18" />
</geometry>
</visual>
</link> -->
<joint name="bottom_left" type="revolute">
<parent link="link6" />
<child link="link7" />
<origin rpy="0 0 0" xyz="0 0 -0.15" />
<axis xyz="0 0 1" />
<limit effort="411" lower="-${2*pi}" upper="${2*pi}" velocity="${pi}"/>
<dynamics damping="60.0" friction="4.0" />
</joint>
<link name="link7">
<visual>
<origin rpy="0 0 ${pi/4.0}" xyz="0 0 -0.01" />
<geometry>
<mesh filename="package://modrobot_description/meshes/cuboctahedron.stl" scale="0.2 0.2 0.2" />
</geometry>
</visual>
<collision>
<origin rpy="0 0 ${pi/4.0}" xyz="0 0 -0.01" />
<geometry>
<mesh filename="package://modrobot_description/meshes/cuboctahedron.stl" scale="0.2 0.2 0.2" />
</geometry>
</collision>
<xacro:box_inertia m="1.0" l="0.28" w="0.28" h="0.28" xyz="0 0 -0.01" rpy="0 0 0" />
</link>
<!-- right arm -->
<joint name="top_right" type="revolute">
<parent link="link1" />
<child link="link8" />
<origin rpy="0 0 0" xyz="0.02 0 0" />
<axis xyz="1 0 0" />
<limit effort="411" lower="-${2*pi}" upper="${2*pi}" velocity="${pi}"/>
<dynamics damping="60.0" friction="4.0" />
</joint>
<link name="link8">
<visual>
<origin rpy="0 0 ${pi/4.0}" xyz="0.14 0 0" />
<geometry>
<mesh filename="package://modrobot_description/meshes/cuboctahedron.stl" scale="0.2 0.2 0.2" />
</geometry>
</visual>
<collision>
<origin rpy="0 0 ${pi/4.0}" xyz="0.14 0 0" />
<geometry>
<mesh filename="package://modrobot_description/meshes/cuboctahedron.stl" scale="0.2 0.2 0.2" />
</geometry>
</collision>
<xacro:box_inertia m="1.0" l="0.28" w="0.28" h="0.28" xyz="0.14 0 0" rpy="0 0 0" />
</link>
<joint name="joint8_9" type="fixed">
<parent link="link8" />
<child link="link9" />
<origin rpy="0 0 0" xyz="0.14 0 -0.28" />
</joint>
<link name="link9">
<visual>
<origin rpy="0 0 ${pi/4.0}" xyz="0 0 0" />
<geometry>
<mesh filename="package://modrobot_description/meshes/cuboctahedron.stl" scale="0.2 0.2 0.2" />
</geometry>
</visual>
<collision>
<origin rpy="0 0 ${pi/4.0}" xyz="0 0 0" />
<geometry>
<mesh filename="package://modrobot_description/meshes/cuboctahedron.stl" scale="0.2 0.2 0.2" />
</geometry>
</collision>
<xacro:box_inertia m="1.0" l="0.28" w="0.28" h="0.28" xyz="0 0 0" rpy="0 0 0" />
</link>
<joint name="joint9_10" type="fixed">
<parent link="link9" />
<child link="link10" />
<origin rpy="0 0 0" xyz="0 0 -0.15"/>
</joint>
<link name="link10">
<visual>
<origin rpy="-${pi} 0 0" xyz="0 0 -0.01" />
<geometry>
<mesh scale="0.02 0.02 0.02" filename="package://modrobot_description/meshes/joint1_z.stl" />
</geometry>
</visual>
<collision>
<origin rpy="-${pi} 0 0" xyz="0 0 -0.01" />
<geometry>
<mesh scale="0.02 0.02 0.02" filename="package://modrobot_description/meshes/joint1_z.stl" />
</geometry>
</collision>
<xacro:cylinder_inertia m="1.0" r="0.1" h="0.18" xyz="0 0 -0.08" rpy="0 0 0" />
</link>
<joint name="middle_right" type="revolute">
<parent link="link10" />
<child link="link11" />
<origin rpy="0 0 0" xyz="0 0 -0.13" />
<axis xyz="1 0 0" />
<limit effort="411" lower="-${2*pi}" upper="${2*pi}" velocity="${pi}"/>
<dynamics damping="60.0" friction="4.0" />
</joint>
<link name="link11">
<visual>
<origin rpy="0 0 0" xyz="0 0 0" />
<geometry>
<sphere radius="0.065" />
</geometry>
</visual>
<collision>
<origin rpy="0 0 0" xyz="0 0 0" />
<geometry>
<sphere radius="0.065" />
</geometry>
</collision>
<xacro:sphere_inertia m="0.5" r="0.065" xyz="0 0 0" rpy="0 0 0" />
</link>
<joint name="joint11_12" type="fixed">
<parent link="link11" />
<child link="link12" />
<origin rpy="0 0 0" xyz="0 0 -0.13"/>
</joint>
<link name="link12">
<visual>
<origin rpy="0 0 -${pi/2}" xyz="0 0 0" />
<geometry>
<mesh scale="0.02 0.02 0.02" filename="package://modrobot_description/meshes/joint1_z.stl" />
</geometry>
</visual>
<collision>
<origin rpy="0 0 -${pi/2}" xyz="0 0 0" />
<geometry>
<mesh scale="0.02 0.02 0.02" filename="package://modrobot_description/meshes/joint1_z.stl" />
</geometry>
</collision>
<xacro:cylinder_inertia m="1.0" r="0.1" h="0.18" xyz="0 0 0.07" rpy="0 0 0" />
</link>
<joint name="bottom_right" type="revolute">
<parent link="link12" />
<child link="link13" />
<origin rpy="0 0 0" xyz="0 0 -0.15" />
<axis xyz="0 0 1" />
<limit effort="411" lower="-${2*pi}" upper="${2*pi}" velocity="${pi}"/>
<dynamics damping="60.0" friction="4.0" />
</joint>
<link name="link13">
<visual>
<origin rpy="0 0 ${pi/4.0}" xyz="0 0 -0.01" />
<geometry>
<mesh filename="package://modrobot_description/meshes/cuboctahedron.stl" scale="0.2 0.2 0.2" />
</geometry>
</visual>
<collision>
<origin rpy="0 0 ${pi/4.0}" xyz="0 0 -0.01" />
<geometry>
<mesh filename="package://modrobot_description/meshes/cuboctahedron.stl" scale="0.2 0.2 0.2" />
</geometry>
</collision>
<xacro:box_inertia m="1.0" l="0.28" w="0.28" h="0.28" xyz="0 0 -0.01" rpy="0 0 0" />
</link>
<!-- Transmissions-->
<xacro:default_transmission name="top_left"/>
<xacro:default_transmission name="middle_left"/>
<xacro:default_transmission name="bottom_left"/>
<xacro:default_transmission name="top_right"/>
<xacro:default_transmission name="middle_right"/>
<xacro:default_transmission name="bottom_right"/>
I also defined controllers for the joints with the JointTrajectoryController for Gazebo and MoveIt. Here is my controllers.yaml :
controller_manager: ros__parameters: update_rate: 10
left_arm_controller:
type : joint_trajectory_controller/JointTrajectoryController
right_arm_controller:
type : joint_trajectory_controller/JointTrajectoryController
joint_state_broadcaster:
type: joint_state_broadcaster/JointStateBroadcaster
left_arm_controller: ros__parameters: joints: - top_left - middle_left - bottom_left
command_interfaces:
- position
state_interfaces:
- position
open_loop_control: true
allow_integration_in_goal_trajectories: true
right_arm_controller: ros__parameters: joints: - top_right - middle_right - bottom_right
command_interfaces:
- position
state_interfaces:
- position
open_loop_control: true
allow_integration_in_goal_trajectories: true
And here is the moveit_controllers.yaml :
moveit_controller_manager: moveit_simple_controller_manager/MoveItSimpleControllerManager
moveit_simple_controller_manager:
controller_names:
- left_arm_controller
- right_arm_controller
left_arm_controller:
action_ns: follow_joint_trajectory
type: FollowJointTrajectory
default: true
joints:
- top_left
- middle_left
- bottom_left
right_arm_controller:
action_ns: follow_joint_trajectory
type: FollowJointTrajectory
default: true
joints:
- top_right
- middle_right
- bottom_right
Ideally I would like to make the robot move like this : Automatde movement Manual movement
Is ther a way to achieve this in ROS2 ?