Robotic Manipulator with spherical joints
Hello,
I am trying to make a robotic manipulator which has 3 links with a spherical joint on the first and last link and a revolute joint in the middle, mimicking a human arm. The issue I am facing is when I want to use inverse kinematics to control the arm, using the jointgrouppositioncontroller, the model breaks in gazebo immediately. Where as if I use inverse dynamics and jointgroupeffortcontroller, it does appear and swings around, but does not reach the target position I give.
I made one with just 2 links with spherical joints on each and it worked fine, but when I add one link and joint, it either doesn't work or fails completely.
My urdf file for the robot arm is
<?xml version="1.0"?>
<robot name="pendulum_robot"
xmlns:xacro="http://www.ros.org/wiki/xacro">
<xacro:include filename="$(find robot)/urdf/my_robot.gazebo" />
<xacro:include filename="$(find robot)/urdf/materials.xacro" />
<!-- <robot name="pendulum_robot"> -->
<link name="world">
</link>
<material name="blue">
<color rgba="1.0 0.8 0.8 1"/>
</material>
<material name="red">
<color rgba="0.8 0 0 1"/>
</material>
<material name="yellow">
<color rgba="0 0.5 0.5 1"/>
</material>
<joint name="base_joint" type="fixed">
<parent link="world"/>
<child link="body_link"/>
<axis xyz="0.0 0.0 0.0"/>
<origin xyz="0.0 0.0 0.0" rpy="0.0 0.0 0.0"/>
</joint>
<!-- First Link -->
<link name="body_link">
<visual>
<origin xyz="0 0 0.75" rpy="0 0 0"/>
<geometry>
<box size="0.1 0.1 1.5"/>
</geometry>
</visual>
<!-- <collision>
<origin xyz="0 0 0.75" rpy="0 0 0"/>
<geometry>
<box size="0.1 0.1 1.0"/>
</geometry>
</collision> -->
<inertial>
<origin xyz="0 0 0.75" rpy="0 0 0"/>
<mass value="1"/>
<inertia ixx="0.18833" ixy="0.0" ixz="0.0" iyy="0.18833" iyz="0.0" izz="0.001666667"/>
</inertial>
</link>
<!-- Shoulder Joint Joint -->
<joint name="shoulder_y" type="continuous">
<parent link="body_link"/>
<child link="dummy_link_1"/>
<axis xyz="0 1 0" />
<origin xyz="0.0 0 1.55" rpy="0 0 0"/>
<!-- <limit effort="50" velocity="2.0"/> -->
<dynamics damping="0.2"/>
</joint>
<link name="dummy_link_1">
<inertial>
<mass value="0.025" />
<inertia ixx="5.8083e-4" ixy="0" ixz="0" iyy="3.0833e-5" iyz="0" izz="5.9083e-4" />
</inertial>
</link>
<joint name="shoulder_z" type="continuous">
<parent link="dummy_link_1"/>
<child link="dummy_link_2"/>
<axis xyz="0 0 1" />
<dynamics damping="0.2"/>
<!-- <limit effort="50" velocity="2.0"/> -->
</joint>
<link name="dummy_link_2">
<inertial>
<mass value="0.025" />
<inertia ixx="5.8083e-4" ixy="0" ixz="0" iyy="3.0833e-5" iyz="0" izz="5.9083e-4" />
</inertial>
</link>
<joint name="shoulder_x" type="continuous">
<parent link="dummy_link_2"/>
<child link="dummy_link_3"/>
<axis xyz="1 0 0" />
<dynamics damping="0.2"/>
<!-- <limit effort="50" velocity="2.0"/> -->
</joint>
<link name="dummy_link_3">
<inertial>
<mass value="0.025" />
<inertia ixx="5.8083e-4" ixy="0" ixz="0" iyy="3.0833e-5" iyz="0" izz="5.9083e-4" />
</inertial>
</link>
<joint name="body_link_jointfixed" type="fixed">
<parent link="dummy_link_3"/>
<child link="bicep_link"/>
</joint>
<transmission name="trans_shoulder_y">
<type>transmission_interface/SimpleTransmission</type>
<joint name="shoulder_y">
<hardwareInterface>hardware_interface/EffortJointInterface</hardwareInterface>
</joint>
<actuator name="actuator_shoulder_y">
<mechanicalReduction>1.0</mechanicalReduction>
<hardwareInterface>hardware_interface/EffortJointInterface</hardwareInterface>
</actuator>
</transmission>
<transmission name="trans_shoulder_z">
<type>transmission_interface/SimpleTransmission</type>
<joint name="shoulder_z">
<hardwareInterface>hardware_interface/EffortJointInterface</hardwareInterface>
</joint>
<actuator name="actuator_shoulder_z">
<mechanicalReduction>1.0</mechanicalReduction>
<hardwareInterface>hardware_interface/EffortJointInterface</hardwareInterface>
</actuator>
</transmission>
<transmission name="trans_shoulder_x">
<type>transmission_interface/SimpleTransmission</type>
<joint name="shoulder_x">
<hardwareInterface>hardware_interface/EffortJointInterface</hardwareInterface>
</joint>
<actuator name="actuator_shoulder_x">
<mechanicalReduction>1.0</mechanicalReduction>
<hardwareInterface>hardware_interface/EffortJointInterface</hardwareInterface>
</actuator>
</transmission>
<!-- Bicep Link-->
<link name="bicep_link">
<!-- <pose>0 0 0 0 0 0</pose> -->
<visual>
<origin xyz="0 0 0.5125" rpy="0 0 0"/>
<geometry>
<box size="0.1 0.1 1"/>
</geometry>
<material name="red"/>
</visual>
<!-- <collision>
<origin xyz="0 0 0.5125" rpy="0 0 0"/>
<geometry>
<box size="0.1 0.1 0.8"/>
</geometry>
</collision> -->
<inertial>
<origin xyz="0 0 0.5125" rpy="0 0 0"/>
<mass value="0.5"/>
<inertia ixx="0.084166667" ixy="0.0" ixz="0.0" iyy="0.084166667" iyz="0.0" izz="0.001666667"/>
</inertial>
</link>
<joint name="elbow" type="revolute">
<parent link="bicep_link"/>
<child link="forearm_link"/>
<axis xyz="0 1 0" />
<origin xyz="0 0 1.05" rpy="0 0 0"/>
<dynamics damping="0.2"/>
<limit effort="500" velocity="2.0"/>
</joint>
<transmission name="trans_elbow">
<type>transmission_interface/SimpleTransmission</type>
<joint name="elbow">
<hardwareInterface>hardware_interface/EffortJointInterface</hardwareInterface>
</joint>
<actuator name="actuator_elbow">
<mechanicalReduction>1.0</mechanicalReduction>
<hardwareInterface>hardware_interface/EffortJointInterface</hardwareInterface>
</actuator>
</transmission>
<!-- Forearm Link-->
<link name="forearm_link">
<!-- <pose>0 0 0 0 0 0</pose> -->
<visual>
<origin xyz="0 0 0.5125" rpy="0 0 0"/>
<geometry>
<box size="0.1 0.1 1"/>
</geometry>
<material name="red"/>
</visual>
<!-- <collision>
<origin xyz="0 0 0.5125" rpy="0 0 0"/>
<geometry>
<box size="0.1 0.1 0.8"/>
</geometry>
</collision> -->
<inertial>
<origin xyz="0 0 0.5125" rpy="0 0 0"/>
<mass value="0.5"/>
<inertia ixx="0.084166667" ixy="0.0" ixz="0.0" iyy="0.084166667" iyz="0.0" izz="0.001666667"/>
</inertial>
</link>
<!-- Second Universal Joint -->
<joint name="wrist_y" type="continuous">
<parent link="forearm_link"/>
<child link="dummy_link_4"/>
<axis xyz="0 1 0" />
<origin xyz="0 0 1.05" rpy="0 0 0"/>
<dynamics damping="0.2"/>
<!-- <limit effort="50" velocity="2.0"/> -->
</joint>
<link name="dummy_link_4">
<inertial>
<mass value="0.025" />
<inertia ixx="5.8083e-4" ixy="0" ixz="0" iyy="3.0833e-5" iyz="0" izz="5.9083e-4" />
</inertial>
</link>
<joint name="wrist_z" type="continuous">
<parent link="dummy_link_4"/>
<child link="dummy_link_5"/>
<axis xyz="0 0 1" />
<dynamics damping="0.2"/>
<!-- <limit effort="50" velocity="2.0"/> -->
</joint>
<link name="dummy_link_5">
<inertial>
<mass value="0.025" />
<inertia ixx="5.8083e-4" ixy="0" ixz="0" iyy="3.0833e-5" iyz="0" izz="5.9083e-4" />
</inertial>
</link>
<joint name="wrist_x" type="continuous">
<parent link="dummy_link_5"/>
<child link="dummy_link_6"/>
<axis xyz="1 0 0" />
<dynamics damping="0.2"/>
<!-- <limit effort="50" velocity="2.0"/> -->
</joint>
<link name="dummy_link_6">
<!-- <visual>
<origin xyz="0 0 -0.0" rpy="0 0 0"/>
<geometry>
<sphere radius="0.075"/>
</geometry>
<material name="yellow"/>
</visual> -->
<inertial>
<mass value="0.025" />
<inertia ixx="5.8083e-4" ixy="0" ixz="0" iyy="3.0833e-5" iyz="0" izz="5.9083e-4" />
</inertial>
</link>
<joint name="link2_jointfixed" type="fixed">
<parent link="dummy_link_6"/>
<child link="hand"/>
</joint>
<transmission name="trans_wrist_y">
<type>transmission_interface/SimpleTransmission</type>
<joint name="wrist_y">
<hardwareInterface>hardware_interface/EffortJointInterface</hardwareInterface>
</joint>
<actuator name="actuator_wrist_y">
<mechanicalReduction>1.0</mechanicalReduction>
<hardwareInterface>hardware_interface/EffortJointInterface</hardwareInterface>
</actuator>
</transmission>
<transmission name="trans_wrist_z">
<type>transmission_interface/SimpleTransmission</type>
<joint name="wrist_z">
<hardwareInterface>hardware_interface/EffortJointInterface</hardwareInterface>
</joint>
<actuator name="actuator_wrist_z">
<mechanicalReduction>1.0</mechanicalReduction>
<hardwareInterface>hardware_interface/EffortJointInterface</hardwareInterface>
</actuator>
</transmission>
<transmission name="trans_wrist_x">
<type>transmission_interface/SimpleTransmission</type>
<joint name="wrist_x">
<hardwareInterface>hardware_interface/EffortJointInterface</hardwareInterface>
</joint>
<actuator name="actuator_wrist_x">
<mechanicalReduction>1.0</mechanicalReduction>
<hardwareInterface>hardware_interface/EffortJointInterface</hardwareInterface>
</actuator>
</transmission>
<!-- Third Link-->
<link name="hand">
<!-- <pose>0 0 0 0 0 0</pose> -->
<visual>
<origin xyz="0 0 0.05125" rpy="0 0 0"/>
<geometry>
<box size="0.1 0.1 0.1"/>
</geometry>
<material name="red"/>
</visual>
<!-- <collision>
<origin xyz="0 0 0.05125" rpy="0 0 0"/>
<geometry>
<box size="0.1 0.1 0.08"/>
</geometry>
</collision> -->
<inertial>
<!-- <origin xyz="0 0 0.5125" rpy="0 0 0"/>
<mass value="0.5"/>
<inertia ixx="0.084166667" ixy="0.0" ixz="0.0" iyy="0.084166667" iyz="0.0" izz="0.001666667"/> -->
<origin xyz="0 0 0.05125" rpy="0 0 0"/>
<mass value="0.1"/>
<inertia ixx="0.0020833" ixy="0.0" ixz="0.0" iyy="0.0020833" iyz="0.0" izz="0.0008333"/>
</inertial>
</link>
<joint name="hand_jointfixed" type="fixed">
<parent link="hand"/>
<child link="sphere3"/>
<origin xyz="0 0 0.1" rpy="0 0 0"/>
</joint>
<link name="sphere3">
<!-- <pose>0 0 0 0 0 0</pose> -->
<visual>
<origin xyz="0 0 -0.0" rpy="0 0 0"/>
<geometry>
<sphere radius="0.01"/>
</geometry>
<material name="yellow"/>
</visual>
<inertial>
<origin xyz="0 0 0.0" rpy="0 0 0"/>
<mass value="0.01"/>
<inertia ixx="0.00001" ixy="0.0" ixz="0.0" iyy="0.00001" iyz="0.0" izz="0.00001"/>
</inertial>
</link>
<gazebo reference='hand_jointfixed'>
<preserveFixedJoint>true</preserveFixedJoint>
</gazebo>
<gazebo>
<plugin name="gazebo_ros_control" filename="libgazebo_ros_control.so">
</plugin>
</gazebo>
<gazebo reference="body_link"> <!-- reference to a existing "link" -->
<!-- NOTE: use Gazebo/DepthMap to take color from mesh (.dae only) -->
<material>Gazebo/GreenGlow</material>
<mu1>0.2</mu1>
<mu2>0.2</mu2>
<selfCollide>True</selfCollide>
</gazebo>
<gazebo reference="bicep_link"> <!-- reference to a existing "link" -->
<!-- NOTE: use Gazebo/DepthMap to take color from mesh (.dae only) -->
<material>Gazebo/TurquoiseGlow</material>
<mu1>0.2</mu1>
<mu2>0.2</mu2>
<selfCollide>True</selfCollide>
</gazebo>
<gazebo reference="hand"> <!-- reference to a existing "link" -->
<!-- NOTE: use Gazebo/DepthMap to take color from mesh (.dae only) -->
<material>Gazebo/RedGlow</material>
<mu1>0.2</mu1>
<mu2>0.2</mu2>
<selfCollide>True</selfCollide>
</gazebo>
<gazebo reference="sphere1"> <!-- reference to a existing "link" -->
<!-- NOTE: use Gazebo/DepthMap to take color from mesh (.dae only) -->
<material>Gazebo/YellowGlow</material>
<mu1>0.2</mu1>
<mu2>0.2</mu2>
<selfCollide>True</selfCollide>
</gazebo>
<gazebo reference="sphere2"> <!-- reference to a existing "link" -->
<!-- NOTE: use Gazebo/DepthMap to take color from mesh (.dae only) -->
<material>Gazebo/YellowGlow</material>
<mu1>0.2</mu1>
<mu2>0.2</mu2>
<selfCollide>True</selfCollide>
</gazebo>
<gazebo reference="sphere3"> <!-- reference to a existing "link" -->
<!-- NOTE: use Gazebo/DepthMap to take color from mesh (.dae only) -->
<material>Gazebo/YellowGlow</material>
<mu1>0.2</mu1>
<mu2>0.2</mu2>
<selfCollide>True</selfCollide>
</gazebo>
</robot>
And the launch file I am using for inverse dynamics is
<?xml version="1.0" encoding="UTF-8"?>
<launch>
<group ns="/pend_bot">
<!-- Arguments for Robot Spawn -->
<arg name="paused" default="true"/>
<arg name="use_sim_time" default="true"/>
<arg name="gui" default="true"/>
<arg name="headless" default="false"/>
<arg name="debug" default="false"/>
<!-- We resume the logic in empty_world.launch -->
<include file="$(find gazebo_ros)/launch/empty_world.launch">
<arg name="world_name" value="$(find pendulum_robot)/worlds/empty_world.world"/>
<arg name="debug" value="$(arg debug)" />
<arg name="gui" value="$(arg gui)" />
<arg name="paused" value="$(arg paused)"/>
<arg name="use_sim_time" value="$(arg use_sim_time)"/>
<arg name="headless" value="$(arg headless)"/>
</include>
<!-- Spawn Robot -->
<param name="robot_description" command="cat '$(find pendulum_robot)/urdf/robot_arm.urdf' " />
<arg name="x" default="0" />
<arg name="y" default="0" />
<arg name="z" default="0" />
<!-- Load Controller -->
<rosparam command="load" file="$(find pendulum_robot)/config/robot_joints.yaml"/>
<node name="robot_state_publisher" pkg="robot_state_publisher" type="robot_state_publisher" respawn="true" output="screen">
</node>
<node name="spawn_pend_bot" pkg="gazebo_ros" type="spawn_model" output="screen"
args = "-urdf -param robot_description
-model pendulum_robot
-x $(arg x)
-y $(arg y)
-z $(arg z)
-gazebo_namespace /pend_bot/gazebo
-robot_namespace /pend_bot
-J shoulder_y -0.5" />
<!-- -J joint_link1_to_sphere1 0.0
-J joint_sphere1_to_link2 2.0
-J joint_link2_to_sphere2 0.0
-J joint_sphere2_to_link3 2.0 -->
<node name="controller_spawner" pkg="controller_manager" type="spawner" respawn="false" output="screen" ns="/pend_bot" args="--namespace=/pend_bot
joint_group_effort_controller
joint_state_controller
--timeout 60">
<!-- joint1_position_controller
joint2_position_controller
joint3_position_controller
joint4_position_controller -->
</node>
</group>
</launch>
Any help on the matter will be greatly appreciated. Thanks in advance.
Asked by A_J on 2023-04-28 12:30:05 UTC
Answers
As far as I know, what you describe is an unsupported configuration.
ros moveit & urdf & the IK plugins all assume that a joint has either zero or one degree of freedom: this means that a spherical arm joint is not supported. I'm not sure if the gazebo joint models (sdf) also have this restriction. To make this work, you will need to write your own custom c++ code to extend ros (and maybe gazebo.)
Asked by Mike Scheutzow on 2023-04-29 13:21:05 UTC
Comments
Hi, Thank you for the reply. I have been successful in making the joint by joining 3 continuous joints and it works if I have a robot with a structure like link->spherical joint->link->spherical joint->link, but when I add another joint inbetween like link->spherical joint->link-> revolute joint -> link->spherical joint->link, the robot breaks in gazebo
Asked by A_J on 2023-05-03 14:58:40 UTC
One trick is to use three revolute joints with mutually perpendicular axes that meet at the same point to represent your spherical joint. The three mutually perpendicular revolute joints will give you the same number of degrees-of-freedom (i.e. 3) as a spherical joint has, and you can control them in Gazebo with ROS, because the two do support revolute joints.
Asked by Farbod on 2023-04-29 16:28:20 UTC
Comments
I have done the exact same thing with continuous joints and it works when I have a robot in this configuration, link->spherical joint->link->spherical joint->link. But as soon as I add a single axis joint in the middle, the model breaks
Asked by A_J on 2023-05-03 15:00:09 UTC
Comments