Hi,
I am currently writing an urdf file for a human model. The human model ran
fine in rviz when I used geometric shapes for links. Recently I have
uploaded STL files and changed the link geometry to that of a mesh. This has
generated a "No root link found" error even though nothing has been changed
in parent-child relationships in joints and names of links and joints have
stayed constant. I have also opened up my STL files in blender and they all
have no problem opening and displaying the models (they have been exported
from Maya). I'm not quite sure what I am missing in my urdf or my
state_publisher file. It may be something simple that I am not aware of. I
have attached the two files (model.xml and state_publisher.cpp). The error
generated is below:
yglee@yglee-laptop:~/ros/ros/HUMAN$ roslaunch HUMAN launch_file.launch
... logging to
/home/yglee/.ros/log/7c565b12-8dce-11df-b146-00236c960005/roslaunch-yglee-laptop-2123.log
started roslaunch server
http://yglee-laptop:42319/
SUMMARY
========
PARAMETERS
* /robot_description
NODES
/
robot_state_publisher (robot_state_publisher/state_publisher)
state_publisher (HUMAN/state_publisher)
WARNING: IP address 127.0.1.1 for local hostname 'yglee-laptop' does not
appear to match
any local IP address (127.0.0.1,192.168.2.153). Your ROS nodes may fail to
communicate.
Please use ROS_IP to set the correct IP address to use.
starting new master (master configured for auto start)
process[master]: started with pid [2137]
ROS_MASTER_URI=
http://yglee-laptop:11311/
setting /run_id to 7c565b12-8dce-11df-b146-00236c960005
process[rosout-1]: started with pid [2148]
started core service [/rosout]
process[robot_state_publisher-2]: started with pid [2161]
process[state_publisher-3]: started with pid [2163]
[ERROR] 1278950383.696006000: No root link found. The robot xml is not a
valid tree.
[ERROR] 1278950383.696138000: failed to find root link
[ERROR] 1278950383.696279000: Could not generate robot model
[ERROR] 1278950383.696612000: Failed to extract kdl tree from xml robot
description
[robot_state_publisher-2] process has died [pid 2161, exit code 255].
log files:
/home/yglee/.ros/log/7c565b12-8dce-11df-b146-00236c960005/robot_state_publisher-2*.log
^C[state_publisher-3] killing on exit
[rosout-1] killing on exit
[master] killing on exit
exiting...
shutting down processing monitor...
... shutting down processing monitor complete
done
Thank you in advance for your help.
Grace L.
<?xml version="1.0"?>
<robot name="human">
<resource type ="stl_meshes" location = "home/yglee/ros/ros/HUMAN/stl_files">
<!--anthropometric data is from paper by Gergana Stefanova Nikolova and Yuli Emilov Toshev on Bulgarian population" -->
<!--head length approx from vitruvian man proportions (1/8 of body)-->
<!--inertia calc. using formula from Zatsiorsky p. 613-->
<!--------------------------LINKS------------------------------------->
<link name="upper_torso">
<inertial>
<mass value="28.71"/> <!--input in kg-->
<inertia ixx="0.679888688" ixy="0" ixz="0" iyy="0.679888688" iyz="0" izz="0.390814875" /> <!--inputs in m^2*kg-->
<origin/>
</inertial>
<visual>
<origin xyz="0 0 0" rpy="0 0 0"/>
<!--geometry>
<cylinder radius=".165" length=".45"/>
</geometry-->
<geometry name="upper_torso">
<mesh filename="package://HUMAN/stl_files/upper_torso.stl"/>
</geometry>
<material name="red">
<color rgba="1 0 0 1"/>
</material>
</visual>
</link>
<!--gazebo reference="upper_torso">
<material>Gazebo/LightWood</material>
<mu1>50.0</mu1>
<mu2>50.0</mu2>
<kp>1000000.0</kp>
<kd>1.0</kd>
</gazebo-->
<link name="virtual_torso_1"/>
<link name="virtual_torso_2"/>
<link name="lower_torso">
<inertial>
<mass value="5.69"/>
<inertia ixx="0.03753693" ixy="0" ixz="0" iyy="0.03753693" iyz="0" izz="0.0640125" />
<origin/>
</inertial>
<visual>
<origin xyz="0 0 -.054" rpy = "0 0 0"/>
<!--geometry>
<cylinder radius=".15" length=".108"/>
</geometry-->
<geometry name="lower_torso">
<mesh filename="package://HUMAN/stl_files/lower_torso.stl"/>
</geometry>
<material name="gray">
<color rgba=".2 .2 .2 .2" />
</material>
</visual>
</link>
<!--gazebo reference="lower_torso">
<material>Gazebo/LightWood</material>
<mu1>50.0</mu1>
<mu2>50.0</mu2>
<kp>1000000.0</kp>
<kd>1.0</kd>
</gazebo-->
<!--legs -->
<link name="right_thigh">
<inertial>
<mass value="11"/>
<inertia ixx="0.26324375" ixy="0" ixz="0" iyy="0.26324375" iyz="0" izz="0.0496375" />
<origin/>
</inertial>
<visual>
<origin xyz="0 -.095 -.255" rpy = "0 0 0"/>
<!--geometry>
<cylinder radius=".095" length=".51"/>
</geometry-->
<geometry name="right_thigh">
<mesh filename="package://HUMAN/stl_files/right_thigh.stl"/>
</geometry>
<material name="green">
<color rgba=".1 .2 .1 .1" />
</material>
</visual>
</link>
<!--gazebo reference="right_thigh">
<material>Gazebo/LightWood</material>
<mu1>50.0</mu1>
<mu2>50.0</mu2>
<kp>1000000.0</kp>
<kd>1.0</kd>
</gazebo-->
<link name="virtual_right_thigh_1"/>
<link name="virtual_right_thigh_2"/>
<link name="left_thigh">
<inertial>
<mass value="11"/>
<inertia ixx="0.26324375" ixy="0" ixz="0" iyy="0.26324375" iyz="0" izz="0.0496375" />
<origin/>
</inertial>
<visual>
<origin xyz="0 .095 -.255" rpy = "0 0 0"/>
<!--geometry>
<cylinder radius=".095" length=".51"/>
</geometry-->
<geometry name="left_thigh">
<mesh filename="package://HUMAN/stl_files/left_thigh.stl"/>
</geometry>
<material name="green">
<color rgba=".1 .2 .1 .1" />
</material>
</visual>
</link>
<!--gazebo reference="left_thigh">
<material>Gazebo/LightWood</material>
<mu1>50.0</mu1>
<mu2>50.0</mu2>
<kp>1000000.0</kp>
<kd>1.0</kd>
</gazebo-->
<link name="virtual_left_thigh_1"/>
<link name="virtual_left_thigh_2"/>
<link name="right_calf">
<inertial>
<mass value="3.3"/>
<inertia ixx="0.0380556" ixy="0" ixz="0" iyy="0.0380556" iyz="0" izz="0.00499125" />
<origin/>
</inertial>
<visual>
<origin xyz="0 0 -.186" rpy = "0 0 0"/>
<!--geometry>
<cylinder radius=".055" length=".372"/>
</geometry-->
<geometry name="right_thigh">
<mesh filename="package://HUMAN/stl_files/right_thigh.stl"/>
</geometry>
<material name="red">
<color rgba=".3 .1 .1 .1" />
</material>
</visual>
</link>
<!--gazebo reference="right_calf">
<material>Gazebo/LightWood</material>
<mu1>50.0</mu1>
<mu2>50.0</mu2>
<kp>1000000.0</kp>
<kd>1.0</kd>
</gazebo-->
<link name="left_calf">
<inertial>
<mass value="3.3"/>
<inertia ixx="0.0380556" ixy="0" ixz="0" iyy="0.0380556" iyz="0" izz="0.00499125" />
<origin/>
</inertial>
<visual>
<origin xyz="0 0 -.186" rpy = "0 0 0"/>
<!--geometry>
<cylinder radius=".055" length=".372"/>
</geometry-->
<geometry name="left_calf">
<mesh filename="package://HUMAN/stl_files/left_calf.stl"/>
</geometry>
<material name="red">
<color rgba=".3 .1 .1 .1" />
</material>
</visual>
</link>
<!--gazebo reference="left_calf">
<material>Gazebo/LightWood</material>
<mu1>50.0</mu1>
<mu2>50.0</mu2>
<kp>1000000.0</kp>
<kd>1.0</kd>
</gazebo-->
<link name="right_foot">
<inertial>
<mass value="1"/>
<inertia ixx="0.00629308333" ixy="0" ixz="0" iyy="0.00629308333" iyz="0" izz="0.001058" />
<origin/>
</inertial>
<visual>
<origin xyz=".1315 0 0" rpy = "0 1.57 0"/>
<!--geometry>
<cylinder radius=".046" length=".263"/>
</geometry-->
<geometry name="right_foot">
<mesh filename="package://HUMAN/stl_files/right_foot.stl"/>
</geometry>
<material name="blue">
<color rgba=".1 .1 .3 .1" />
</material>
</visual>
</link>
<!--gazebo reference="right_foot">
<material>Gazebo/LightWood</material>
<mu1>50.0</mu1>
<mu2>50.0</mu2>
<kp>1000000.0</kp>
<kd>1.0</kd>
</gazebo-->
<link name="virtual_right_foot_1"/>
<link name="virtual_right_foot_2"/>
<link name="left_foot">
<inertial>
<mass value="1"/>
<inertia ixx="0.00629308333" ixy="0" ixz="0" iyy="0.00629308333" iyz="0" izz="0.001058" />
<origin/>
</inertial>
<visual>
<origin xyz=".1315 0 0" rpy = "0 1.57 0"/>
<!--geometry>
<cylinder radius=".046" length=".263"/>
</geometry-->
<geometry name="left_foot">
<mesh filename="package://HUMAN/stl_files/left_foot.stl"/>
</geometry>
<material name="blue">
<color rgba=".1 .1 .3 .1" />
</material>
</visual>
</link>
<!--gazebo reference="left_foot">
<material>Gazebo/LightWood</material>
<mu1>50.0</mu1>
<mu2>50.0</mu2>
<kp>1000000.0</kp>
<kd>1.0</kd>
</gazebo-->
<link name="virtual_left_foot_1"/>
<link name="virtual_left_foot_2"/>
<!--inertia start here-->
<!--arms-->
<link name="right_upper_arm">
<inertial>
<mass value="2.1"/>
<inertia ixx="0.018021675" ixy="0" ixz="0" iyy="0.018021675" iyz="0" izz="0.002625" />
<origin/>
</inertial>
<visual>
<origin xyz="0 0 -.1545" rpy = "0 0 0"/>
<!--geometry>
<cylinder radius=".05" length=".309"/>
</geometry-->
<geometry name="right_upper_arm">
<mesh filename="package://HUMAN/stl_files/right_upper_arm.stl"/>
</geometry>
<material name="blue">
<color rgba=".1 .1 .3 .1" />
</material>
</visual>
</link>
<!--gazebo reference="right_upper_arm">
<material>Gazebo/LightWood</material>
<mu1>50.0</mu1>
<mu2>50.0</mu2>
<kp>1000000.0</kp>
<kd>1.0</kd>
</gazebo-->
<link name="virtual_right_upper_arm"/>
<link name="left_upper_arm">
<inertial>
<mass value="2.1"/>
<inertia ixx="0.018021675" ixy="0" ixz="0" iyy="0.018021675" iyz="0" izz="0.002625" />
<origin/>
</inertial>
<visual>
<origin xyz="0 0 -.1545" rpy = "0 0 0"/>
<!--geometry>
<cylinder radius=".05" length=".309"/>
</geometry-->
<geometry name="left_upper_arm">
<mesh filename="package://HUMAN/stl_files/left_upper_arm.stl"/>
</geometry>
<material name="blue">
<color rgba=".1 .1 .3 .1" />
</material>
</visual>
</link>
<!--gazebo reference="left_upper_arm">
<material>Gazebo/LightWood</material>
<mu1>50.0</mu1>
<mu2>50.0</mu2>
<kp>1000000.0</kp>
<kd>1.0</kd>
</gazebo-->
<link name="virtual_left_upper_arm"/>
<link name="right_lower_arm">
<inertial>
<mass value="1.2"/>
<inertia ixx="0.0066817" ixy="0" ixz="0" iyy="0.0066817" iyz="0" izz="0.0011616" />
<origin/>
</inertial>
<visual>
<origin xyz="0 0 -.1235" rpy = "0 0 0"/>
<!--geometry>
<cylinder radius=".044" length=".247"/>
</geometry-->
<geometry name="right_lower_arm">
<mesh filename="package://HUMAN/stl_files/right_lower_arm.stl"/>
</geometry>
<material name="red">
<color rgba=".3 .1 .1 .1" />
</material>
</visual>
</link>
<!--gazebo reference="right_lower_arm">
<material>Gazebo/LightWood</material>
<mu1>50.0</mu1>
<mu2>50.0</mu2>
<kp>1000000.0</kp>
<kd>1.0</kd>
</gazebo-->
<link name="left_lower_arm">
<inertial>
<mass value="1.2"/>
<inertia ixx="0.0066817" ixy="0" ixz="0" iyy="0.0066817" iyz="0" izz="0.0011616" />
<origin/>
</inertial>
<visual>
<origin xyz="0 0 -.1235" rpy = "0 0 0"/>
<!--geometry>
<cylinder radius=".044" length=".247"/>
</geometry-->
<geometry name="left_lower_arm">
<mesh filename="package://HUMAN/stl_files/left_lower_arm.stl"/>
</geometry>
<material name="red">
<color rgba=".3 .1 .1 .1" />
</material>
</visual>
</link>
<!--gazebo reference="left_lower_arm">
<material>Gazebo/LightWood</material>
<mu1>50.0</mu1>
<mu2>50.0</mu2>
<kp>1000000.0</kp>
<kd>1.0</kd>
</gazebo-->
<link name="right_hand">
<inertial>
<mass value=".5"/>
<inertia ixx="0.0004418" ixy="0" ixz="0" iyy="0.0004418" iyz="0" izz="0.0004418" />
<origin/>
</inertial>
<visual>
<origin xyz="0 0 -.047" rpy = "0 0 0"/>
<!--geometry>
<sphere radius=".094"/>
</geometry-->
<geometry name="right_hand">
<mesh filename="package://HUMAN/stl_files/right_hand.stl"/>
</geometry>
<material name="blue">
<color rgba=".1 .1 .3 .1" />
</material>
</visual>
</link>
<!--gazebo reference="right_hand">
<material>Gazebo/LightWood</material>
<mu1>50.0</mu1>
<mu2>50.0</mu2>
<kp>1000000.0</kp>
<kd>1.0</kd>
</gazebo-->
<link name="left_hand">
<inertial>
<mass value=".5"/>
<inertia ixx="0.0004418" ixy="0" ixz="0" iyy="0.0004418" iyz="0" izz="0.0004418" />
<origin/>
</inertial>
<visual>
<origin xyz="0 0 -.047" rpy = "0 0 0"/>
<!--geometry>
<sphere radius=".094"/>
</geometry-->
<geometry name="left_hand">
<mesh filename="package://HUMAN/stl_files/left_hand.stl"/>
</geometry>
<material name="blue">
<color rgba=".1 .1 .3 .1" />
</material>
</visual>
</link>
<!--gazebo reference="left_hand">
<material>Gazebo/LightWood</material>
<mu1>50.0</mu1>
<mu2>50.0</mu2>
<kp>1000000.0</kp>
<kd>1.0</kd>
</gazebo-->
<!--column of the neck is slightly curved forward even when head is straight
according to Bridgman's Human Machine-->
<link name="neck_1">
<inertial>
<mass value=".05"/>
<inertia ixx=".00009142" ixy="0" ixz="0" iyy=".00009142" iyz="0" izz="0.000180625" />
<origin/>
</inertial>
<visual>
<origin xyz=".01 0 .0163" rpy = "0 0 0"/>
<!--geometry>
<cylinder radius=".085" length=".0163"/>
</geometry-->
<geometry name="neck_1">
<mesh filename="package://HUMAN/stl_files/neck_1.stl"/>
</geometry>
<material name="blue">
<color rgba=".1 .1 .3 .1" />
</material>
</visual>
</link>
<!--gazebo reference="neck_1">
<material>Gazebo/LightWood</material>
<mu1>50.0</mu1>
<mu2>50.0</mu2>
<kp>1000000.0</kp>
<kd>1.0</kd>
</gazebo-->
<link name="virtual_tilt_neck_1"/>
<link name="virtual_ext_neck_1"/>
<link name="virtual_shake_neck_1"/>
<link name="neck_2">
<inertial>
<mass value=".05"/>
<inertia ixx=".00009142" ixy="0" ixz="0" iyy=".00009142" iyz="0" izz=".000180625" />
<origin/>
</inertial>
<visual>
<origin xyz=".03 0 .0163" rpy = "0 0 0"/>
<!--geometry>
<cylinder radius=".085" length=".0163"/>
</geometry-->
<geometry name="neck_2">
<mesh filename="package://HUMAN/stl_files/neck_2.stl"/>
</geometry>
<material name="green">
<color rgba=".1 .3 .1 .1" />
</material>
</visual>
</link>
<!--gazebo reference="neck_2">
<material>Gazebo/LightWood</material>
<mu1>50.0</mu1>
<mu2>50.0</mu2>
<kp>1000000.0</kp>
<kd>1.0</kd>
</gazebo-->
<link name="virtual_neck_2"/>
<link name="neck_3">
<inertial>
<mass value=".05"/>
<inertia ixx=".00009142" ixy="0" ixz="0" iyy=".00009142" iyz="0" izz=".000180625" />
<origin/>
</inertial>
<visual>
<origin xyz="0 0 .0163" rpy = "0 0 0"/>
<!--geometry>
<cylinder radius=".085" length=".0163"/>
</geometry-->
<geometry name="neck_3">
<mesh filename="package://HUMAN/stl_files/neck_3.stl"/>
</geometry>
<material name="blue">
<color rgba=".1 .1 .3 .1" />
</material>
</visual>
</link>
<!--gazebo reference="neck_3">
<material>Gazebo/LightWood</material>
<mu1>50.0</mu1>
<mu2>50.0</mu2>
<kp>1000000.0</kp>
<kd>1.0</kd>
</gazebo-->
<link name="virtual_neck_3"/>
<link name="head">
<inertial>
<mass value="4.96"/>
<inertia ixx="0.0218736" ixy="0" ixz="0" iyy="0.0218736" iyz="0" izz="0.0218736" />
<origin/>
</inertial>
<visual>
<origin xyz="0 0 .11" rpy = "0 0 0"/>
<!--geometry>
<sphere radius=".21"/>
</geometry-->
<geometry name="head">
<mesh filename="package://HUMAN/stl_files/head.stl"/>
</geometry>
<material name="red">
<color rgba=".3 .1 .1 .1" />
</material>
</visual>
</link>
<!--gazebo reference="head">
<material>Gazebo/LightWood</material>
<mu1>50.0</mu1>
<mu2>50.0</mu2>
<kp>1000000.0</kp>
<kd>1.0</kd>
</gazebo-->
<!--------------------------JOINTS------------------------------------->
<!--anatomical terms from
http://www.amputee-online.com/amputee/movements.html-->
<!--and
http://www.jrank.org/food/article_images/www.jrank.org_food/body-movements.1.jpg-->
<!--torso tilt-->
<joint name="waist_tilt_joint" type="revolute">
<origin xyz="0 0 -.225" rpy="0 0 0"/>
<axis xyz= "1 0 0"/>
<limit lower= "-0.785398163" upper= "0.785398163" effort = "2.645" velocity="6"/>
<parent link="upper_torso"/>
<child link="virtual_torso_1"/>
</joint>
<!--torso lateral_rotation-->
<joint name="waist_rotate_joint" type="revolute">
<origin xyz="0 0 0" rpy="0 0 0"/>
<axis xyz= "0 0 1"/>
<limit lower= "-0.785398163" upper= "0.785398163" effort = "2.645" velocity="6"/>
<parent link="virtual_torso_1"/>
<child link="virtual_torso_2"/>
</joint>
<!--torso bow-->
<joint name="waist_bow_joint" type="revolute">
<origin xyz="0 0 0" rpy="0 0 0"/>
<axis xyz= "0 1 0"/>
<limit lower= "-1.57079633" upper= "0" effort = "2.645" velocity="6"/>
<parent link="virtual_torso_2"/>
<child link="lower_torso"/>
</joint>
<!--thigh bend-->
<joint name="right_thigh_bend_joint" type="revolute">
<origin xyz="0 .15 -.108" rpy="0 0 0"/>
<axis xyz= "0 1 0"/>
<limit lower= "-1.57079633" upper= "1.57079633" effort = "2.645" velocity="6"/>
<parent link="lower_torso"/>
<child link="virtual_right_thigh_1"/>
</joint>
<joint name="left_thigh_bend_joint" type="revolute">
<origin xyz="0 -.15 -.108" rpy="0 0 0"/>
<axis xyz= "0 1 0"/>
<limit lower= "-1.57079633" upper= "1.57079633" effort = "2.645" velocity="6"/>
<parent link="lower_torso"/>
<child link="virtual_left_thigh_1"/>
</joint>
<!--thigh lateral_rotation-->
<joint name="right_thigh_lat_rotate_joint" type="continuous">
<origin xyz="0 0 0" rpy="0 0 0"/>
<axis xyz= "0 0 1"/>
<!--limit lower= "-1.57079633" upper= "1.57079633" effort = "2.645" velocity="6"/-->
<parent link="virtual_right_thigh_1"/>
<child link="virtual_right_thigh_2"/>
</joint>
<joint name="left_thigh_lat_rotate_joint" type="continuous">
<origin xyz="0 0 0" rpy="0 0 0"/>
<axis xyz= "0 0 1"/>
<parent link="virtual_left_thigh_1"/>
<child link="virtual_left_thigh_2"/>
</joint>
<!--thigh abduction-->
<joint name="right_thigh_abduction_joint" type="revolute">
<origin xyz="0 0 0" rpy="0 0 0"/>
<axis xyz= "1 0 0"/>
<limit lower= "-0.261799388" upper= "1.57079633" effort = "2.645" velocity="6"/>
<parent link="virtual_right_thigh_2"/>
<child link="right_thigh"/>
</joint>
<joint name="left_thigh_abduction_joint" type="revolute">
<origin xyz="0 0 0" rpy="0 0 0"/>
<axis xyz= "1 0 0"/>
<limit lower= "-0.261799388" upper= "1.57079633" effort = "2.645" velocity="6"/>
<parent link="virtual_left_thigh_2"/>
<child link="left_thigh"/>
</joint>
<!--knee flexion and extension-->
<joint name="right_knee_flex_joint" type="revolute">
<origin xyz="0 -.095 -.51" rpy="0 0 0"/>
<axis xyz= "0 1 0"/>
<limit lower= "0" upper= "1.57079633" effort = "2.645" velocity="6"/>
<parent link="right_thigh"/>
<child link="right_calf"/>
</joint>
<joint name="left_knee_flex_joint" type="revolute">
<origin xyz="0 .095 -.51" rpy="0 0 0"/>
<axis xyz= "0 1 0"/>
<limit lower= "0" upper= "1.57079633" effort = "2.645" velocity="6"/>
<parent link="left_thigh"/>
<child link="left_calf"/>
</joint>
<!--feet inversion (sole of feet towards & away from body)-->
<joint name="right_foot_inversion_joint" type="revolute">
<origin xyz="0 0 -.372" rpy="0 0 0"/>
<axis xyz= "1 0 0"/>
<limit lower= "-1.57079633" upper= "1.57079633" effort = "2.645" velocity="6"/>
<parent link="right_calf"/>
<child link="virtual_right_foot_1"/>
</joint>
<joint name="left_foot_inversion_joint" type="revolute">
<origin xyz="0 0 -.372" rpy="0 0 0"/>
<axis xyz= "1 0 0"/>
<limit lower= "-1.57079633" upper= "1.57079633" effort = "2.645" velocity="6"/>
<parent link="left_calf"/>
<child link="virtual_left_foot_1"/>
</joint>
<!--feet lateral rotation-->
<joint name="right_foot_lat_rotation_joint" type="revolute">
<origin xyz="0 0 0" rpy="0 0 0"/>
<axis xyz= "0 0 1"/>
<limit lower= "-0.785398163" upper= "0.785398163" effort = "2.645" velocity="6"/>
<parent link="virtual_right_foot_1"/>
<child link="virtual_right_foot_2"/>
</joint>
<joint name="left_foot_lat_rotation_joint" type="revolute">
<origin xyz="0 0 0" rpy="0 0 0"/>
<axis xyz= "0 0 1"/>
<limit lower= "-0.785398163" upper= "0.785398163" effort = "2.645" velocity="6"/>
<parent link="virtual_left_foot_1"/>
<child link="virtual_left_foot_2"/>
</joint>
<!--feet flexion-->
<joint name="right_foot_flex_joint" type="revolute">
<origin xyz="0 0 0" rpy="0 0 0"/>
<axis xyz= "0 1 0"/>
<limit lower= "-0.785398163" upper= "1.57079633" effort = "2.645" velocity="6"/>
<parent link="virtual_right_foot_2"/>
<child link="right_foot"/>
</joint>
<joint name="left_foot_flex_joint" type="revolute">
<origin xyz="0 0 0" rpy="0 0 0"/>
<axis xyz= "0 1 0"/>
<limit lower= "-0.785398163" upper= "1.57079633" effort = "2.645" velocity="6"/>
<parent link="virtual_left_foot_2"/>
<child link="left_foot"/>
</joint>
<!--arms circumduction-->
<joint name="right_shoulder_circ_joint" type="continuous">
<origin xyz="0 .17 .225" rpy="0 0 0"/>
<axis xyz= "0 1 0"/>
<!--limit lower= "-0.261799388" upper= "0.261799388" effort = "2.645" velocity="6"/-->
<parent link="upper_torso"/>
<child link="virtual_right_upper_arm"/>
</joint>
<joint name="left_shoulder_circ_joint" type="continuous">
<origin xyz="0 -.17 .225" rpy=" 0 0 0"/>
<axis xyz= "0 1 0"/>
<!--limit lower= "-0.261799388" upper= "0.261799388" effort = "2.645" velocity="6"/-->
<parent link="upper_torso"/>
<child link="virtual_left_upper_arm"/>
</joint>
<!--arms abduction (vit. man position)-->
<joint name="right_shoulder_abduction_joint" type="revolute">
<origin xyz="0 0 0" rpy="0 0 0"/>
<axis xyz= "1 0 0"/>
<limit lower= "-1.57" upper= "1.57" effort = "2.645" velocity="6"/>
<parent link="virtual_right_upper_arm"/>
<child link="right_upper_arm"/>
</joint>
<joint name="left_shoulder_abduction_joint" type="revolute">
<origin xyz="0 0 0" rpy=" 0 0 0"/>
<axis xyz= "1 0 0"/>
<limit lower= "-1.57" upper= "1.57" effort = "2.645" velocity="6"/>
<parent link="virtual_left_upper_arm"/>
<child link="left_upper_arm"/>
</joint>
<!--elbow extension and flexion-->
<joint name="right_elbow_flex_joint" type="revolute">
<origin xyz="0 0 -.309" rpy="0 0 0"/>
<axis xyz= "0 1 0"/>
<limit lower= "-1.57079633" upper= "1.57" effort = "2.645" velocity="6"/>
<parent link="right_upper_arm"/>
<child link="right_lower_arm"/>
</joint>
<joint name="left_elbow_flex_joint" type="revolute">
<origin xyz="0 0 -.309" rpy="0 0 0"/>
<axis xyz= "0 1 0"/>
<limit lower= "-1.57079633" upper= "1.57" effort = "2.645" velocity="6"/>
<parent link="left_upper_arm"/>
<child link="left_lower_arm"/>
</joint>
<!--wrists flexion-->
<joint name="right_wrist_flex_joint" type="revolute">
<origin xyz="0 0 -.247" rpy="0 0 0"/>
<axis xyz= "0 1 0"/>
<limit lower= "-1.57079633" upper= "1.57" effort = "2.645" velocity="6"/>
<parent link="right_lower_arm"/>
<child link="right_hand"/>
</joint>
<joint name="left_wrist_flex_joint" type="revolute">
<origin xyz="0 0 -.247" rpy="0 0 0"/>
<axis xyz= "0 1 0"/>
<limit lower= "-1.57079633" upper= "1.57" effort = "2.645" velocity="6"/>
<parent link="left_lower_arm"/>
<child link="left_hand"/>
</joint>
<!--referenced head and neck anatomy from -->
<!--
http://en.wikipedia.org/wiki/Head_and_neck_anatomy-->
<!--
www.imeko.org/publications/wc-2003/PWC-2003-TC18-013.pdfve-->
<!--
http://www.cks.nhs.uk/Media/50_-15896_100.png-->
<joint name="neck_joint_3" type="fixed">
<origin xyz="0 0 .225" rpy="0 0 0"/>
<!--axis xyz= "1 0 0"/-->
<!--limit lower= "-0.261799388" upper= "0.261799388" effort = "2.645" velocity="6"/-->
<parent link="upper_torso"/>
<child link="neck_3"/>
</joint>
<!--head tilt-->
<joint name="neck_tilt_joint_2" type="revolute">
<origin xyz="0 0 0" rpy="0 0 0"/>
<axis xyz= "1 0 0"/>
<limit lower= "-1.04719755" upper= "1.04719755" effort = "2.645" velocity="6"/>
<parent link="neck_3"/>
<child link="virtual_neck_3"/>
</joint>
<!--head nod-->
<joint name="neck_nod_joint_2" type="revolute">
<origin xyz="0 0 .0163" rpy="0 0 0"/>
<axis xyz= "0 1 0"/>
<limit lower= "-1.57079633" upper= "1.57079633" effort = "2.645" velocity="6"/>
<parent link="virtual_neck_3"/>
<child link="neck_2"/>
</joint>
<!--head tilt-->
<joint name="neck_tilt_joint_1" type="revolute">
<origin xyz="0 0 0" rpy="0 0 0"/>
<axis xyz= "1 0 0"/>
<limit lower= "-1.57079633" upper= "1.57079633" effort = "2.645" velocity="6"/>
<parent link="neck_2"/>
<child link="virtual_neck_2"/>
</joint>
<!--head nod-->
<joint name="neck_nod_joint_1" type="revolute">
<origin xyz="0 0 .0163" rpy="0 0 0"/>
<axis xyz= "0 1 0"/>
<limit lower= "-1.57079633" upper= "1.57079633" effort = "2.645" velocity="6"/>
<parent link="virtual_neck_2"/>
<child link="neck_1"/>
</joint>
<!--head tilt-->
<joint name="head_tilt_joint" type="revolute">
<origin xyz="0 0 0" rpy="0 0 0"/>
<axis xyz= "1 0 0"/>
<limit lower= "-1.57079633" upper= "1.57079633" effort = "2.645" velocity="6"/>
<parent link="neck_1"/>
<child link="virtual_tilt_neck_1"/>
</joint>
<!--lateral extension (turtle neck)-->
<joint name="head_lateral_ext_joint" type="prismatic">
<origin xyz="0 0 0" rpy="0 0 0"/>
<axis xyz= "1 0 0"/>
<limit lower= "-0.02" upper= "0.05" effort = "2.645" velocity="6"/>
<parent link="virtual_tilt_neck_1"/>
<child link="virtual_ext_neck_1"/>
</joint>
<!--head shake-->
<joint name="head_shake_joint" type="revolute">
<origin xyz="0 0 0" rpy="0 0 0"/>
<axis xyz= "0 0 1"/>
<limit lower= "-1.57079633" upper= "1.57079633" effort = "2.645" velocity="6"/>
<parent link="virtual_ext_neck_1"/>
<child link="virtual_shake_neck_1"/>
</joint>
<!--head nod-->
<joint name="head_nod_joint" type="revolute">
<origin xyz="0 0 .0163" rpy="0 0 0"/>
<axis xyz= "0 1 0"/>
<limit lower= "-1.57079633" upper= "1.57079633" effort = "2.645" velocity="6"/>
<parent link="virtual_shake_neck_1"/>
<child link="head"/>
</joint>
</robot>
#include <string>
#include <ros/ros.h>
#include <sensor_msgs/JointState.h>
#include <tf/transform_broadcaster.h>
int main(int argc, char** argv) {
ros::init(argc, argv, "intial_state_publisher");
ros::NodeHandle n;
ros::Publisher joint_pub = n.advertise<sensor_msgs::JointState>("joint_states", 1);
tf::TransformBroadcaster broadcaster;
ros::Rate loop_rate(30);
// robot state
double tinc=.03, waist_tilt_joint=0, waist_rotate_joint=0,
waist_bow_joint=0,right_thigh_bend_joint=0,left_thigh_bend_joint=0,
right_thigh_lat_rotate_joint=0,left_thigh_lat_rotate_joint=0,
right_thigh_abduction_joint=0,left_thigh_abduction_joint=0,
right_knee_flex_joint=0,left_knee_flex_joint=0,
right_foot_inversion_joint=0,left_foot_inversion_joint=0,
right_foot_lat_rotation_joint=0,left_foot_lat_rotation_joint=0,
right_foot_flex_joint=0,left_foot_flex_joint=0,right_shoulder_circ_joint=0,
left_shoulder_circ_joint=0,right_shoulder_abduction_joint=0,
left_shoulder_abduction_joint=0,right_elbow_flex_joint=0,
left_elbow_flex_joint=0,right_wrist_pronation_joint=0,left_wrist_pronation_joint=0,
right_wrist_flex_joint=0,left_wrist_flex_joint=0,neck_joint_3=0,
neck_tilt_joint_2=0,neck_nod_joint_2=0,neck_tilt_joint_1=0,
head_tilt_joint=0,neck_nod_joint_1=0,head_lateral_ext_joint=0,
head_shake_joint=0,head_nod_joint=0;
//*** initialize here
// message declarations
geometry_msgs::TransformStamped odom_trans;
sensor_msgs::JointState joint_state;
odom_trans.header.frame_id = "odom";
odom_trans.child_frame_id = "upper_torso";
while (ros::ok()) {
//update joint_state
joint_state.header.stamp = ros::Time::now();
joint_state.header.frame_id="upper_torso";
joint_state.name.resize(36);
joint_state.position.resize(36);
joint_state.name[0] = "waist_tilt_joint";
joint_state.position[0] = waist_tilt_joint;
joint_state.name[1] = "waist_rotate_joint";
joint_state.position[1] =waist_rotate_joint;
joint_state.name[2] ="waist_bow_joint";
joint_state.position[2] =waist_bow_joint;
joint_state.name[3] ="right_thigh_bend_joint";
joint_state.position[3] =right_thigh_bend_joint;
joint_state.name[4] ="left_thigh_bend_joint";
joint_state.position[4] =left_thigh_bend_joint;
joint_state.name[5] ="right_thigh_lat_rotate_joint";
joint_state.position[5] =right_thigh_lat_rotate_joint;
joint_state.name[6] ="left_thigh_lat_rotate_joint";
joint_state.position[6] =left_thigh_lat_rotate_joint;
joint_state.name[7] ="right_thigh_abduction_joint";
joint_state.position[7] =right_thigh_abduction_joint;
joint_state.name[8] ="left_thigh_abduction_joint";
joint_state.position[8] =left_thigh_abduction_joint;
joint_state.name[9] ="right_knee_flex_joint";
joint_state.position[9] =right_knee_flex_joint;
joint_state.name[10] ="left_knee_flex_joint";
joint_state.position[10] =left_knee_flex_joint;
joint_state.name[11] ="right_foot_inversion_joint";
joint_state.position[11] =right_foot_inversion_joint;
joint_state.name[12] ="left_foot_inversion_joint";
joint_state.position[12] =left_foot_inversion_joint;
joint_state.name[13] ="right_foot_lat_rotation_joint";
joint_state.position[13] =right_foot_lat_rotation_joint;
joint_state.name[14] ="left_foot_lat_rotation_joint";
joint_state.position[14] =left_foot_lat_rotation_joint;
joint_state.name[15] ="right_foot_flex_joint";
joint_state.position[15] =right_foot_flex_joint;
joint_state.name[16] ="left_foot_flex_joint";
joint_state.position[16] =left_foot_flex_joint;
joint_state.name[17] ="right_shoulder_circ_joint";
joint_state.position[17] =right_shoulder_circ_joint;
joint_state.name[18] ="left_shoulder_circ_joint";
joint_state.position[18] =left_shoulder_circ_joint;
joint_state.name[19] ="right_shoulder_abduction_joint";
joint_state.position[19] =right_shoulder_abduction_joint;
joint_state.name[20] ="left_shoulder_abduction_joint";
joint_state.position[20] =left_shoulder_abduction_joint;
joint_state.name[21] ="right_elbow_flex_joint";
joint_state.position[21] =right_elbow_flex_joint;
joint_state.name[22] ="left_elbow_flex_joint";
joint_state.position[22] =left_elbow_flex_joint;
joint_state.name[23] ="right_wrist_pronation_joint";
joint_state.position[23] =right_wrist_pronation_joint;
joint_state.name[24] ="left_wrist_pronation_joint";
joint_state.position[24] =left_wrist_pronation_joint;
joint_state.name[25] ="right_wrist_flex_joint";
joint_state.position[25] =right_wrist_flex_joint;
joint_state.name[26] ="left_wrist_flex_joint";
joint_state.position[26] =left_wrist_flex_joint;
joint_state.name[27] ="neck_joint_3";
joint_state.position[27] =neck_joint_3;
joint_state.name[28] ="neck_tilt_joint_2";
joint_state.position[28] =neck_tilt_joint_2;
joint_state.name[29] ="neck_nod_joint_2";
joint_state.position[29] =neck_nod_joint_2;
joint_state.name[30] ="neck_tilt_joint_1";
joint_state.position[30] =neck_tilt_joint_1;
joint_state.name[31] ="neck_nod_joint_1";
joint_state.position[31] =neck_nod_joint_1;
joint_state.name[32] ="head_tilt_joint";
joint_state.position[32] =head_tilt_joint;
joint_state.name[33] ="head_lateral_ext_joint";
joint_state.position[33] =head_lateral_ext_joint;
joint_state.name[34] ="head_shake_joint";
joint_state.position[34] =head_shake_joint;
joint_state.name[35] ="head_nod_joint";
joint_state.position[35] =head_nod_joint;
//send the joint state and transform
joint_pub.publish(joint_state);
broadcaster.sendTransform(odom_trans);
//create robot state
waist_tilt_joint += tinc;
if (waist_tilt_joint<-0.785398163 || waist_tilt_joint>0.785398163) tinc *= -1;
/*
waist_rotate_joint += tinc;
if (waist_rotate_joint<-0.785398163 || waist_rotate_joint>0.785398163) tinc *= -1;
waist_bow_joint += tinc;
if (waist_bow_joint<-1.57079633 || waist_bow_joint>0) tinc *= -1;
right_thigh_bend_joint += tinc;
if (right_thigh_bend_joint<-1.57079633 || right_thigh_bend_joint>1.57079633) tinc *= -1;
left_thigh_bend_joint += tinc;
if (left_thigh_bend_joint<-1.57079633 || left_thigh_bend_joint>1.57079633) tinc *= -1;
right_thigh_abduction_joint += tinc;
if (right_thigh_abduction_joint<-0.261799388 || right_thigh_abduction_joint>0.261799388) tinc *= -1;
left_thigh_abduction_joint += tinc;
if (left_thigh_abduction_joint<-0.261799388 || left_thigh_abduction_joint>0.261799388) tinc *= -1;
right_knee_flex_joint += tinc;
if (right_knee_flex_joint<0 || right_knee_flex_joint>1.57079633) tinc *= -1;
left_knee_flex_joint += tinc;
if (left_knee_flex_joint<0 || left_knee_flex_joint>1.57079633) tinc *= -1;
right_foot_inversion_joint += tinc;
if (right_foot_inversion_joint<-1.57079633 || right_foot_inversion_joint>1.57079633) tinc *= -1;
left_foot_inversion_joint += tinc;
if (left_foot_inversion_joint<-1.57079633 || left_foot_inversion_joint>1.57079633) tinc *= -1;
right_foot_lat_rotation_joint += tinc;
if (right_foot_lat_rotation_joint<-0.785398163 || right_foot_lat_rotation_joint>0.785398163) tinc *= -1;
left_foot_lat_rotation_joint += tinc;
if (left_foot_lat_rotation_joint<-0.785398163 || left_foot_lat_rotation_joint>0.785398163) tinc *= -1;
right_foot_flex_joint += tinc;
if (right_foot_flex_joint<-0.785398163 || right_foot_flex_joint>0.785398163) tinc *= -1;
left_foot_flex_joint += tinc;
if (left_foot_flex_joint<-0.785398163 || left_foot_flex_joint>0.785398163) tinc *= -1;
right_shoulder_abduction_joint += tinc;
if (right_shoulder_abduction_joint<-1.57 || right_shoulder_abduction_joint>1.57) tinc *= -1;
left_shoulder_abduction_joint += tinc;
if (left_shoulder_abduction_joint<-1.57 || left_shoulder_abduction_joint>1.57) tinc *= -1;
right_elbow_flex_joint += tinc;
if (right_elbow_flex_joint<-1.57 || right_elbow_flex_joint>1.57) tinc *= -1;
left_elbow_flex_joint += tinc;
if (left_elbow_flex_joint<-1.57 || left_elbow_flex_joint>1.57) tinc *= -1;
right_wrist_pronation_joint += tinc;
if (right_wrist_pronation_joint<-1.57079633 || right_wrist_pronation_joint>1.57079633) tinc *= -1;
left_wrist_pronation_joint += tinc;
if (left_wrist_pronation_joint<-1.57079633 || left_wrist_pronation_joint>1.57079633) tinc *= -1;
//start here for limits
right_wrist_flex_joint += tinc;
if (right_wrist_flex_joint<-1.57 || right_wrist_flex_joint>1.57) tinc *= -1;
left_wrist_flex_joint += tinc;
if (left_wrist_flex_joint<-1.57 || left_wrist_flex_joint>1.57) tinc *= -1;
neck_tilt_joint_2 += tinc;
if (neck_tilt_joint_2<-1.57 || neck_tilt_joint_2>1.57) tinc *= -1;
neck_nod_joint_2 += tinc;
if (neck_nod_joint_2<-1.57 || neck_nod_joint_2>1.57) tinc *= -1;
neck_tilt_joint_1 += tinc;
if (neck_tilt_joint_1<-1.57 || neck_tilt_joint_1>1.57) tinc *= -1;
neck_nod_joint_1 += tinc;
if (neck_nod_joint_1<-1.57 || neck_nod_joint_1>1.57) tinc *= -1;
head_tilt_joint += tinc;
if (head_tilt_joint<-1.57 || head_tilt_joint>1.57) tinc *= -1;
head_lateral_ext_joint += tinc;
if (head_lateral_ext_joint<-1.57 || head_lateral_ext_joint>1.57) tinc *= -1;
head_shake_joint += tinc;
if (head_shake_joint<-1.57 || head_shake_joint>1.57) tinc *= -1;
head_nod_joint += tinc;
if (head_nod_joint<-1.57 || head_nod_joint>1.57) tinc *= -1;
*/
ros::spinOnce();
loop_rate.sleep();
}
return 0;
}
(text/plain)
