Unverified Commit f316c84b authored by Justin Carpentier's avatar Justin Carpentier Committed by GitHub
Browse files

Merge pull request #91 from jviereck/jviereck/finger_edu

Add finger_edu robot
parents 8e882859 c3e1dffe
Pipeline #14937 passed with stage
in 1 minute and 26 seconds
Subproject commit 3a52692a40839b10f38352c1b06ccfebc0b53f36
Subproject commit 7e797cbcae0947acce802c0bc63765a2b7c75828
......@@ -266,6 +266,14 @@ def loadSolo(solo=True):
return loader().robot
class FingerEduLoader(RobotLoader):
path = 'finger_edu_description'
urdf_filename = "finger_edu.urdf"
srdf_filename = "finger_edu.srdf"
ref_posture = "hanging"
free_flyer = False
class KinovaLoader(RobotLoader):
path = "kinova_description"
urdf_filename = "kinova.urdf"
......@@ -482,6 +490,7 @@ ROBOTS = {
'simple_humanoid_classical': SimpleHumanoidClassicalLoader,
'solo': SoloLoader,
'solo12': Solo12Loader,
'finger_edu': FingerEduLoader,
'talos': TalosLoader,
'talos_box': TalosBoxLoader,
'talos_arm': TalosArmLoader,
......
<?xml version="1.0" ?>
<!-- =================================================================================== -->
<!-- | This document was autogenerated by xacro from /home/jviereck_local/dev/nyu_finger/workspace/src/catkin/robots/robot_properties/robot_properties_fingers/xacro/edu/fingeredu.urdf.xacro | -->
<!-- | EDITING THIS FILE BY HAND IS NOT RECOMMENDED | -->
<!-- =================================================================================== -->
<robot name="fingeredu" xmlns:xacro="https://ei.is.tuebingen.mpg.de/xacro">
<!--
Frames are defined such that they all align with the base frame when all
joints are at position zero.
In zero-configuration the origins of all joints are in one line.
In zero-configuration the origin of the base link is exactly above the
finger tip which should make it easy to place the finger in the world.
-->
<!--
Frames are defined such that they all align with the base frame when all
joints are at position zero.
In zero-configuration the origins of all joints are in one line.
-->
<material name="fingeredu_material">
<color rgba="0.6 0.6 0.6 1.0"/>
</material>
<link name="base_link">
<inertial>
<origin rpy="0 0 0" xyz="0 0 0"/>
<mass value="1"/>
<inertia ixx="4.16666666667e-06" ixy="0" ixz="0" iyy="4.16666666667e-06" iyz="0" izz="4.16666666667e-06"/>
</inertial>
</link>
<!--
Fixed links for the finger base (parts where the upper link is mounted).
-->
<link name="finger_base_link">
<visual>
<origin rpy="0 0 0" xyz="-0.17995 0 0"/>
<geometry>
<mesh filename="package://example-robot-data/robots/finger_edu_description/meshes/base_back.stl" scale="1 1 1"/>
</geometry>
<material name="fingeredu_material"/>
</visual>
<collision>
<origin rpy="0 0 0" xyz="-0.17995 0 0"/>
<geometry>
<mesh filename="package://example-robot-data/robots/finger_edu_description/meshes/base_back.stl" scale="1 1 1"/>
</geometry>
</collision>
<visual>
<origin rpy="0 0 0" xyz="0.0255 0 0"/>
<geometry>
<mesh filename="package://example-robot-data/robots/finger_edu_description/meshes/base_front.stl" scale="1 1 1"/>
</geometry>
<material name="fingeredu_material"/>
</visual>
<collision>
<origin rpy="0 0 0" xyz="0.0255 0 0"/>
<geometry>
<mesh filename="package://example-robot-data/robots/finger_edu_description/meshes/base_front.stl" scale="1 1 1"/>
</geometry>
</collision>
<visual>
<origin rpy="0 0 0" xyz="0.0255 0.02 0.08"/>
<geometry>
<mesh filename="package://example-robot-data/robots/finger_edu_description/meshes/base_side_left.stl" scale="1 1 1"/>
</geometry>
<material name="fingeredu_material"/>
</visual>
<collision>
<origin rpy="0 0 0" xyz="0.0255 0.02 0.08"/>
<geometry>
<mesh filename="package://example-robot-data/robots/finger_edu_description/meshes/base_side_left.stl" scale="1 1 1"/>
</geometry>
</collision>
<!-- Disable this part. It is not very relevant for collisions but
would cause trouble in pybullet due to its concavity.
<xacro:add_geometry
rpy="0 0 0"
xyz="${offset_x_base_to_front} -0.02 ${offset_z_base_to_top}"
mesh_file="${mesh_dir}/base_side_right.stl"
material="${material}" />
-->
<visual>
<origin rpy="0 0 0" xyz="0.0255 0 0.08"/>
<geometry>
<mesh filename="package://example-robot-data/robots/finger_edu_description/meshes/base_top.stl" scale="1 1 1"/>
</geometry>
<material name="fingeredu_material"/>
</visual>
<collision>
<origin rpy="0 0 0" xyz="0.0255 0 0.08"/>
<geometry>
<mesh filename="package://example-robot-data/robots/finger_edu_description/meshes/base_top.stl" scale="1 1 1"/>
</geometry>
</collision>
<inertial>
<origin rpy="0 0 0" xyz="-0.1 0 0.4"/>
<mass value="1"/>
<inertia ixx="0.0608333333333" ixy="0" ixz="0" iyy="0.0566666666667" iyz="0" izz="0.0108333333333"/>
</inertial>
</link>
<!-- The movable links (upper, middle and lower) of the finger. -->
<!-- FIXME inertias are not correct! -->
<link name="finger_upper_link">
<visual>
<origin rpy="0 0 0" xyz="0.0195 0 0"/>
<geometry>
<mesh filename="package://example-robot-data/robots/finger_edu_description/meshes/upper_link.stl" scale="1 1 1"/>
</geometry>
<material name="fingeredu_material"/>
</visual>
<collision>
<origin rpy="0 0 0" xyz="0.0195 0 0"/>
<geometry>
<mesh filename="package://example-robot-data/robots/finger_edu_description/meshes/upper_link.stl" scale="1 1 1"/>
</geometry>
</collision>
<inertial>
<!-- Note: This uses the values from the middle link, assuming
that it is similar enough. -->
<!-- CoM is only estimated based -->
<origin rpy="0 0 0" xyz="-0.079 0 0"/>
<mass value="0.14854"/>
<inertia ixx="0.00003" ixy="0.00005" ixz="0.00000" iyy="0.00041" iyz="0.00000" izz="0.00041"/>
</inertial>
</link>
<link name="finger_middle_link">
<visual>
<origin rpy="0 0 0" xyz="0 0 0"/>
<geometry>
<mesh filename="package://example-robot-data/robots/finger_edu_description/meshes/middle_link.stl" scale="1 1 1"/>
</geometry>
<material name="fingeredu_material"/>
</visual>
<collision>
<origin rpy="0 0 0" xyz="0 0 0"/>
<geometry>
<mesh filename="package://example-robot-data/robots/finger_edu_description/meshes/middle_link.stl" scale="1 1 1"/>
</geometry>
</collision>
<inertial>
<origin rpy="0 0 0" xyz="0 -0.019 -0.079"/>
<mass value="0.14854"/>
<inertia ixx="0.00041" ixy="0.00000" ixz="0.00000" iyy="0.00041" iyz="0.00005" izz="0.00003"/>
</inertial>
</link>
<link name="finger_lower_link">
<visual>
<origin rpy="0 0 0" xyz="0 0 0"/>
<geometry>
<mesh filename="package://example-robot-data/robots/finger_edu_description/meshes/lower_link.stl" scale="1 1 1"/>
</geometry>
<material name="fingeredu_material"/>
</visual>
<collision>
<origin rpy="0 0 0" xyz="0 0 0"/>
<geometry>
<mesh filename="package://example-robot-data/robots/finger_edu_description/meshes/lower_link.stl" scale="1 1 1"/>
</geometry>
</collision>
<!-- TODO: these are the interial values from the Solo lower leg
link which is similar but not exactly the same to the FingerEdu
lower link. -->
<inertial>
<origin rpy="0 0 0" xyz="0 -0.009 -0.089"/>
<mass value="0.03070"/>
<inertia ixx="0.00012" ixy="0.00000" ixz="0.00000" iyy="0.00012" iyz="0.00000" izz="0.00000"/>
</inertial>
</link>
<!-- fixed link for finger tip -->
<link name="finger_tip_link">
<inertial>
<origin rpy="0 0 0" xyz="0 0 0"/>
<mass value="0.01"/>
<inertia ixx="1.66666666667e-07" ixy="0" ixz="0" iyy="1.66666666667e-07" iyz="0" izz="1.66666666667e-07"/>
</inertial>
</link>
<joint name="finger_lower_to_tip_joint" type="fixed">
<parent link="finger_lower_link"/>
<child link="finger_tip_link"/>
<origin xyz="0 -0.008 -0.16"/>
</joint>
<!-- kinematics -->
<joint name="finger_base_to_upper_joint" type="revolute">
<parent link="finger_base_link"/>
<child link="finger_upper_link"/>
<limit effort="1000" lower="-1.57079632679" upper="1.57079632679" velocity="1000"/>
<axis xyz="-1 0 0"/>
<origin rpy="0 0 0" xyz="0 0 0"/>
</joint>
<joint name="finger_upper_to_middle_joint" type="revolute">
<parent link="finger_upper_link"/>
<child link="finger_middle_link"/>
<limit effort="1000" lower="-1.57079632679" upper="1.57079632679" velocity="1000"/>
<axis xyz="0 1 0"/>
<origin rpy="0 0 0" xyz="0 -0.014 0"/>
</joint>
<joint name="finger_middle_to_lower_joint" type="revolute">
<parent link="finger_middle_link"/>
<child link="finger_lower_link"/>
<limit effort="1000" lower="-3.14159265359" upper="3.14159265359" velocity="1000"/>
<axis xyz="0 1 0"/>
<origin rpy="0 0 0" xyz="0 -0.03745 -0.16"/>
</joint>
<!--
Define the global base link and place the finger relative to it.
-->
<joint name="base_to_finger" type="fixed">
<parent link="base_link"/>
<child link="finger_base_link"/>
<origin xyz="0 0 0.283"/>
</joint>
</robot>
<?xml version="1.0" ?>
<robot name="finger_edu">
<!-- leg -->
<group name="leg">
<joint name="finger_base_to_upper_joint" />
<joint name="finger_upper_to_middle_joint" />
<joint name="finger_middle_to_lower_joint" />
<chain base_link="finger_base_link" tip_link="finger_tip_link" />
</group>
<end_effector name="tip" parent_link="finger_tip_link" group="leg" />
<group_state name="hanging" group="leg">
<joint name="base_to_finger" value="0. 0. 0. 0. 0. 0. 1." />
<joint name="finger_base_to_upper_joint" value="0.0" />
<joint name="finger_upper_to_middle_joint" value="0.0" />
<joint name="finger_middle_to_lower_joint" value="0.0" />
</group_state>
<disable_collisions link1="finger_upper_link" link2="finger_base_link" reason="Adjacent" />
<disable_collisions link1="finger_middle_link" link2="finger_upper_link" reason="Adjacent" />
<disable_collisions link1="finger_lower_link" link2="finger_middle_link" reason="Adjacent" />
</robot>
......@@ -79,6 +79,9 @@ class RobotTestCase(unittest.TestCase):
def test_solo12(self):
self.check('solo12', 19, 18)
def test_finger_edu(self):
self.check('finger_edu', 3, 3)
def test_talos(self):
self.check('talos', 39, 38)
......
Markdown is supported
0% or .
You are about to add 0 people to the discussion. Proceed with caution.
Finish editing this message first!
Please register or to comment