From c69dd9f4d911d1f1c6d8821bcbf0cd1acfcc9770 Mon Sep 17 00:00:00 2001
From: Carlos Mastalli <carlos.mastalli@gmail.com>
Date: Mon, 28 Oct 2019 23:53:30 +0000
Subject: [PATCH] [anymal] Added the ANYmal with kinova arm
---
.../robots/anymal-kinova.urdf | 1299 +++++++++++++++++
.../srdf/anymal-kinova.srdf | 139 ++
kinova_description/robots/kinova.urdf | 2 +-
python/example_robot_data/__main__.py | 7 +-
python/example_robot_data/robots_loader.py | 10 +-
5 files changed, 1452 insertions(+), 5 deletions(-)
create mode 100644 anymal_b_simple_description/robots/anymal-kinova.urdf
create mode 100644 anymal_b_simple_description/srdf/anymal-kinova.srdf
diff --git a/anymal_b_simple_description/robots/anymal-kinova.urdf b/anymal_b_simple_description/robots/anymal-kinova.urdf
new file mode 100644
index 0000000..b5e2bb5
--- /dev/null
+++ b/anymal_b_simple_description/robots/anymal-kinova.urdf
@@ -0,0 +1,1299 @@
+<?xml version="1.0" ?>
+<!-- =================================================================================== -->
+<!-- | This document was autogenerated by xacro from anymal.urdf.xacro | -->
+<!-- | EDITING THIS FILE BY HAND IS NOT RECOMMENDED | -->
+<!-- =================================================================================== -->
+<!-- This file contains the description of the ANYmal B robot. -->
+<robot name="anymal" xmlns:xacro="http://www.ros.org/wiki/xacro">
+ <material name="black">
+ <color rgba="0.0 0.0 0.0 1.0"/>
+ </material>
+ <material name="blue">
+ <color rgba="0.0 0.0 0.8 1.0"/>
+ </material>
+ <material name="green">
+ <color rgba="0.0 0.8 0.0 1.0"/>
+ </material>
+ <material name="grey">
+ <color rgba="0.2 0.2 0.2 1.0"/>
+ </material>
+ <material name="light_grey">
+ <color rgba="0.4 0.4 0.4 1.0"/>
+ </material>
+ <material name="orange">
+ <color rgba="1.0 0.423529411765 0.0392156862745 1.0"/>
+ </material>
+ <material name="brown">
+ <color rgba="0.870588235294 0.811764705882 0.764705882353 1.0"/>
+ </material>
+ <material name="red">
+ <color rgba="0.8 0.0 0.0 1.0"/>
+ </material>
+ <material name="white">
+ <color rgba="1.0 1.0 1.0 1.0"/>
+ </material>
+ <!-- Material for the visual primitives -->
+ <material name="anymal_material">
+ <color rgba="0.7 0.7 0.7 1."/>
+ </material>
+ <!-- Xacro:Properties -->
+ <!-- [m] -->
+ <!-- Base link -->
+ <link name="base">
+ <visual>
+ <origin rpy="0 0 0" xyz="0 0 0"/>
+ <geometry>
+ <mesh filename="package://anymal_b_simple_description/meshes/base/anymal_base.dae" scale="1 1 1"/>
+ </geometry>
+ <material name="anymal_material"/>
+ </visual>
+ <collision>
+ <!-- Main Body -->
+ <origin rpy="0 0 0" xyz="0 0 0.08"/>
+ <geometry>
+ <box size="0.531 0.27 0.24"/>
+ </geometry>
+ </collision>
+ <collision>
+ <!-- HAA actuators -->
+ <origin rpy="0 1.57079632679 0" xyz="0.227 0.116 0"/>
+ <geometry>
+ <cylinder length="0.1" radius="0.05"/>
+ </geometry>
+ </collision>
+ <collision>
+ <origin rpy="0 1.57079632679 0" xyz="0.227 -0.116 0"/>
+ <geometry>
+ <cylinder length="0.1" radius="0.05"/>
+ </geometry>
+ </collision>
+ <collision>
+ <origin rpy="0 1.57079632679 0" xyz="-0.227 0.116 0"/>
+ <geometry>
+ <cylinder length="0.1" radius="0.05"/>
+ </geometry>
+ </collision>
+ <collision>
+ <origin rpy="0 1.57079632679 0" xyz="-0.227 -0.116 0"/>
+ <geometry>
+ <cylinder length="0.1" radius="0.05"/>
+ </geometry>
+ </collision>
+ <collision>
+ <!-- Belly plate front bump -->
+ <origin rpy="0 0 0" xyz="0.2155 0.0 -0.09"/>
+ <geometry>
+ <box size="0.1 0.1 0.07"/>
+ </geometry>
+ </collision>
+ <collision>
+ <!-- Belly plate hind bump-->
+ <origin rpy="0 0 0" xyz="-0.2155 0.0 -0.09"/>
+ <geometry>
+ <box size="0.1 0.1 0.07"/>
+ </geometry>
+ </collision>
+ <collision>
+ <!-- Belly plate middle bump-->
+ <origin rpy="0 0 0" xyz="0 0 -0.09"/>
+ <geometry>
+ <box size="0.531 0.02 0.07"/>
+ </geometry>
+ </collision>
+ </link>
+ <!-- Fixed joint to add dummy inertia link -->
+ <joint name="base_to_base_inertia" type="fixed">
+ <parent link="base"/>
+ <child link="base_inertia"/>
+ <origin rpy="0 0 0" xyz="0 0 0"/>
+ </joint>
+ <!-- Dummy inertia link, because KDL cannot have inertia on the base link -->
+ <link name="base_inertia">
+ <inertial>
+ <origin rpy="0 0 0" xyz="-0.001960558279 -0.001413217745 0.050207125344"/>
+ <mass value="16.793507758"/>
+ <inertia ixx="0.217391101503" ixy="-0.00132873239126" ixz="-0.00228200226173" iyy="0.639432546734" iyz="-0.00138078263145" izz="0.62414077654"/>
+ </inertial>
+ </link>
+ <!-- Xacro:Properties -->
+ <!-- [kg * m^2] -->
+ <!-- [A] -->
+ <link name="LF_HIP">
+ <visual>
+ <origin rpy="0 0 0.0" xyz="0 0 0"/>
+ <geometry>
+ <mesh filename="package://anymal_b_simple_description/meshes/hip/anymal_hip_l.dae" scale="1 1 1"/>
+ </geometry>
+ <material name="anymal_material"/>
+ </visual>
+ <collision>
+ <!-- Protector -->
+ <origin rpy="1.57079632679 0 0" xyz="0.0635 -0.009 0.0"/>
+ <geometry>
+ <cylinder length="0.1" radius="0.08"/>
+ </geometry>
+ </collision>
+ <collision>
+ <!-- Heatfins -->
+ <origin rpy="1.57079632679 0 0" xyz="0.0635 -0.074 0.0"/>
+ <geometry>
+ <cylinder length="0.03" radius="0.045"/>
+ </geometry>
+ </collision>
+ <inertial>
+ <origin rpy="0 0 0" xyz="0.064516258147 -0.003787101702 -0.000152184388"/>
+ <mass value="1.42462064"/>
+ <inertia ixx="0.00243023349564" ixy="-1.53023971e-05" ixz="-2.1819095354e-05" iyy="0.00230257239103" iyz="2.6473021273e-05" izz="0.0019806759227"/>
+ </inertial>
+ </link>
+ <!-- Hip joint -->
+ <joint name="LF_HAA" type="revolute">
+ <parent link="base"/>
+ <child link="LF_HIP"/>
+ <origin xyz="0.277 0.116 0.0"/>
+ <axis xyz="1 0 0"/>
+ <limit command_effort="40" current="10" effort="80" gear_velocity="10" lower="-9.42" upper="9.42" velocity="15"/>
+ <dynamics damping="0.0" friction="0.0"/>
+ </joint>
+ <!-- Xacro:Properties -->
+ <!-- [m] -->
+ <!-- [A] -->
+ <link name="LF_THIGH">
+ <visual>
+ <origin rpy="0 0 0" xyz="0 0 0"/>
+ <geometry>
+ <mesh filename="package://anymal_b_simple_description/meshes/thigh/anymal_thigh.dae" scale="1 1 1"/>
+ </geometry>
+ <material name="anymal_material"/>
+ </visual>
+ <collision>
+ <!-- thigh with heat fins -->
+ <origin rpy="0.145 0 0" xyz="0 0.035 -0.125"/>
+ <geometry>
+ <box size="0.08 0.04 0.25"/>
+ </geometry>
+ </collision>
+ <collision>
+ <!-- KFE actuator -->
+ <origin rpy="1.57079632679 0 0" xyz="0.0 0.069 -0.25"/>
+ <geometry>
+ <cylinder length="0.12" radius="0.06"/>
+ </geometry>
+ </collision>
+ <collision>
+ <!-- upper protector -->
+ <origin rpy="1.71579632679 0 0" xyz="0 -0.005 0"/>
+ <geometry>
+ <cylinder length="0.12" radius="0.066"/>
+ </geometry>
+ </collision>
+ <inertial>
+ <origin rpy="0 0 0" xyz="-0.003897968082 0.054226618537 -0.214583373795"/>
+ <mass value="1.634976467"/>
+ <inertia ixx="0.0120367944369" ixy="6.762065206e-05" ixz="0.000287806340448" iyy="0.0120643637939" iyz="-0.00140610131218" izz="0.00249422574881"/>
+ </inertial>
+ </link>
+ <!-- Thigh joint -->
+ <joint name="LF_HFE" type="revolute">
+ <parent link="LF_HIP"/>
+ <child link="LF_THIGH"/>
+ <origin xyz="0.0635 0.041 0.0"/>
+ <axis xyz="0 1 0"/>
+ <limit command_effort="40" current="10" effort="80" gear_velocity="10" lower="-9.42" upper="9.42" velocity="15.0"/>
+ <dynamics damping="0.0" friction="0.0"/>
+ </joint>
+ <!-- Xacro:Properties -->
+ <!-- [m] -->
+ <!-- [A] -->
+ <link name="LF_SHANK">
+ <visual>
+ <origin rpy="0 0 0.0" xyz="0 0 0"/>
+ <geometry>
+ <mesh filename="package://anymal_b_simple_description/meshes/shank/anymal_shank_l.dae" scale="1 1 1"/>
+ </geometry>
+ <material name="anymal_material"/>
+ </visual>
+ <collision>
+ <origin rpy="0 1.57079632679 0" xyz="0.065 -0.015 0.01"/>
+ <geometry>
+ <box size="0.08 0.07 0.13"/>
+ </geometry>
+ </collision>
+ <inertial>
+ <origin rpy="0 0 0" xyz="0.030816858139 -0.004617229294 0.000893125713"/>
+ <mass value="0.207204302"/>
+ <inertia ixx="0.0002104880248" ixy="-5.6750980345e-05" ixz="1.0127699391e-05" iyy="0.000676270210023" iyz="-8.22869024e-07" izz="0.000545032674924"/>
+ </inertial>
+ </link>
+ <!-- Shank joint -->
+ <joint name="LF_KFE" type="revolute">
+ <parent link="LF_THIGH"/>
+ <child link="LF_SHANK"/>
+ <origin xyz="0.0 0.109 -0.25"/>
+ <axis xyz="0 1 0"/>
+ <limit command_effort="40" current="10" effort="80" gear_velocity="10" lower="-9.42" upper="9.42" velocity="15.0"/>
+ <dynamics damping="0.0" friction="0.0"/>
+ </joint>
+ <!-- Xacro:Properties -->
+ <!-- [m] -->
+ <!-- Shank to Adapter joint -->
+ <joint name="LF_SHANK_TO_ADAPTER" type="fixed">
+ <parent link="LF_SHANK"/>
+ <child link="LF_ADAPTER"/>
+ <origin rpy="0.0 0.0 0.0" xyz="0.1 -0.02 0.0"/>
+ </joint>
+ <!-- Adapter link -->
+ <link name="LF_ADAPTER">
+ <visual>
+ <origin rpy="0 0 0" xyz="0 0 0.032"/>
+ <geometry>
+ <mesh filename="package://anymal_b_simple_description/meshes/foot/anymal_foot.dae" scale="1 1 1"/>
+ </geometry>
+ <material name="anymal_material"/>
+ </visual>
+ <collision>
+ <origin rpy="0 0 0" xyz="0 0 -0.160625"/>
+ <geometry>
+ <cylinder length="0.32125" radius="0.015"/>
+ </geometry>
+ <material name="anymal_material"/>
+ </collision>
+ <inertial>
+ <origin rpy="0 0 0" xyz="-8.66e-10 -1.472e-09 -0.244345749188"/>
+ <mass value="0.140170767"/>
+ <inertia ixx="0.00159938741862" ixy="-9.32e-13" ixz="1.039e-11" iyy="0.00159938741932" iyz="1.7563e-11" izz="5.4423177329e-05"/>
+ </inertial>
+ </link>
+ <!-- Adapter to Foot joint -->
+ <joint name="LF_ADAPTER_TO_FOOT" type="fixed">
+ <parent link="LF_ADAPTER"/>
+ <child link="LF_FOOT"/>
+ <origin rpy="0.0 0.0 0.0" xyz="0.0 0.0 -0.32125"/>
+ </joint>
+ <!-- Foot link -->
+ <link name="LF_FOOT">
+ <collision>
+ <origin xyz="0 0 0.02325"/>
+ <geometry>
+ <sphere radius="0.031"/>
+ </geometry>
+ </collision>
+ </link>
+ <!-- Gazebo customization -->
+ <gazebo reference="LF_FOOT">
+ <kp>1000000.0</kp>
+ <kd>100.0</kd>
+ <mu1>0.8</mu1>
+ <mu2>0.8</mu2>
+ </gazebo>
+ <!-- Xacro:Properties -->
+ <!-- [kg * m^2] -->
+ <!-- [A] -->
+ <link name="RF_HIP">
+ <visual>
+ <origin rpy="0 0 0" xyz="0 0 0"/>
+ <geometry>
+ <mesh filename="package://anymal_b_simple_description/meshes/hip/anymal_hip_r.dae" scale="1 1 1"/>
+ </geometry>
+ <material name="anymal_material"/>
+ </visual>
+ <collision>
+ <!-- Protector -->
+ <origin rpy="1.57079632679 0 0" xyz="0.0635 0.009 0.0"/>
+ <geometry>
+ <cylinder length="0.1" radius="0.08"/>
+ </geometry>
+ </collision>
+ <collision>
+ <!-- Heatfins -->
+ <origin rpy="1.57079632679 0 0" xyz="0.0635 0.074 0.0"/>
+ <geometry>
+ <cylinder length="0.03" radius="0.045"/>
+ </geometry>
+ </collision>
+ <inertial>
+ <origin rpy="0 0 0" xyz="0.064516258147 0.003787101702 -0.000152184388"/>
+ <mass value="1.42462064"/>
+ <inertia ixx="0.00243023349564" ixy="1.53023971e-05" ixz="-2.1819095354e-05" iyy="0.00230257239103" iyz="-2.6473021273e-05" izz="0.0019806759227"/>
+ </inertial>
+ </link>
+ <!-- Hip joint -->
+ <joint name="RF_HAA" type="revolute">
+ <parent link="base"/>
+ <child link="RF_HIP"/>
+ <origin xyz="0.277 -0.116 0.0"/>
+ <axis xyz="1 0 0"/>
+ <limit command_effort="40" current="10" effort="80" gear_velocity="10" lower="-9.42" upper="9.42" velocity="15"/>
+ <dynamics damping="0.0" friction="0.0"/>
+ </joint>
+ <!-- Xacro:Properties -->
+ <!-- [m] -->
+ <!-- [A] -->
+ <link name="RF_THIGH">
+ <visual>
+ <origin rpy="3.1416 3.1416 0.0" xyz="0 0 0"/>
+ <geometry>
+ <mesh filename="package://anymal_b_simple_description/meshes/thigh/anymal_thigh.dae" scale="1 1 1"/>
+ </geometry>
+ <material name="anymal_material"/>
+ </visual>
+ <collision>
+ <!-- thigh with heat fins -->
+ <origin rpy="-0.145 0 0" xyz="0 -0.035 -0.125"/>
+ <geometry>
+ <box size="0.08 0.04 0.25"/>
+ </geometry>
+ </collision>
+ <collision>
+ <!-- KFE actuator -->
+ <origin rpy="1.57079632679 0 0" xyz="0.0 -0.069 -0.25"/>
+ <geometry>
+ <cylinder length="0.12" radius="0.06"/>
+ </geometry>
+ </collision>
+ <collision>
+ <!-- upper protector -->
+ <origin rpy="-1.71579632679 0 0" xyz="0 0.005 0"/>
+ <geometry>
+ <cylinder length="0.12" radius="0.066"/>
+ </geometry>
+ </collision>
+ <inertial>
+ <origin rpy="0 0 0" xyz="-0.003897968082 -0.054226618537 -0.214583373795"/>
+ <mass value="1.634976467"/>
+ <inertia ixx="0.0120367944369" ixy="-6.762065206e-05" ixz="0.000287806340448" iyy="0.0120643637939" iyz="0.00140610131218" izz="0.00249422574881"/>
+ </inertial>
+ </link>
+ <!-- Thigh joint -->
+ <joint name="RF_HFE" type="revolute">
+ <parent link="RF_HIP"/>
+ <child link="RF_THIGH"/>
+ <origin xyz="0.0635 -0.041 0.0"/>
+ <axis xyz="0 1 0"/>
+ <limit command_effort="40" current="10" effort="80" gear_velocity="10" lower="-9.42" upper="9.42" velocity="15.0"/>
+ <dynamics damping="0.0" friction="0.0"/>
+ </joint>
+ <!-- Xacro:Properties -->
+ <!-- [m] -->
+ <!-- [A] -->
+ <link name="RF_SHANK">
+ <visual>
+ <origin rpy="0 0 0.0" xyz="0 0 0"/>
+ <geometry>
+ <mesh filename="package://anymal_b_simple_description/meshes/shank/anymal_shank_r.dae" scale="1 1 1"/>
+ </geometry>
+ <material name="anymal_material"/>
+ </visual>
+ <collision>
+ <origin rpy="0 1.57079632679 0" xyz="0.065 0.015 0.01"/>
+ <geometry>
+ <box size="0.08 0.07 0.13"/>
+ </geometry>
+ </collision>
+ <inertial>
+ <origin rpy="0 0 0" xyz="0.030816858139 0.004617229294 0.000893125713"/>
+ <mass value="0.207204302"/>
+ <inertia ixx="0.0002104880248" ixy="5.6750980345e-05" ixz="1.0127699391e-05" iyy="0.000676270210023" iyz="8.22869024e-07" izz="0.000545032674924"/>
+ </inertial>
+ </link>
+ <!-- Shank joint -->
+ <joint name="RF_KFE" type="revolute">
+ <parent link="RF_THIGH"/>
+ <child link="RF_SHANK"/>
+ <origin xyz="0.0 -0.109 -0.25"/>
+ <axis xyz="0 1 0"/>
+ <limit command_effort="40" current="10" effort="80" gear_velocity="10" lower="-9.42" upper="9.42" velocity="15.0"/>
+ <dynamics damping="0.0" friction="0.0"/>
+ </joint>
+ <!-- Xacro:Properties -->
+ <!-- [m] -->
+ <!-- Shank to Adapter joint -->
+ <joint name="RF_SHANK_TO_ADAPTER" type="fixed">
+ <parent link="RF_SHANK"/>
+ <child link="RF_ADAPTER"/>
+ <origin rpy="-0.0 0.0 -0.0" xyz="0.1 0.02 0.0"/>
+ </joint>
+ <!-- Adapter link -->
+ <link name="RF_ADAPTER">
+ <visual>
+ <origin rpy="0 0 0" xyz="0 0 0.032"/>
+ <geometry>
+ <mesh filename="package://anymal_b_simple_description/meshes/foot/anymal_foot.dae" scale="1 1 1"/>
+ </geometry>
+ <material name="anymal_material"/>
+ </visual>
+ <collision>
+ <origin rpy="0 0 0" xyz="0 0 -0.160625"/>
+ <geometry>
+ <cylinder length="0.32125" radius="0.015"/>
+ </geometry>
+ <material name="anymal_material"/>
+ </collision>
+ <inertial>
+ <origin rpy="0 0 0" xyz="-8.66e-10 -1.472e-09 -0.244345749188"/>
+ <mass value="0.140170767"/>
+ <inertia ixx="0.00159938741862" ixy="-9.32e-13" ixz="1.039e-11" iyy="0.00159938741932" iyz="1.7563e-11" izz="5.4423177329e-05"/>
+ </inertial>
+ </link>
+ <!-- Adapter to Foot joint -->
+ <joint name="RF_ADAPTER_TO_FOOT" type="fixed">
+ <parent link="RF_ADAPTER"/>
+ <child link="RF_FOOT"/>
+ <origin rpy="-0.0 0.0 -0.0" xyz="0.0 -0.0 -0.32125"/>
+ </joint>
+ <!-- Foot link -->
+ <link name="RF_FOOT">
+ <collision>
+ <origin xyz="0 0 0.02325"/>
+ <geometry>
+ <sphere radius="0.031"/>
+ </geometry>
+ </collision>
+ </link>
+ <!-- Gazebo customization -->
+ <gazebo reference="RF_FOOT">
+ <kp>1000000.0</kp>
+ <kd>100.0</kd>
+ <mu1>0.8</mu1>
+ <mu2>0.8</mu2>
+ </gazebo>
+ <!-- Xacro:Properties -->
+ <!-- [kg * m^2] -->
+ <!-- [A] -->
+ <link name="LH_HIP">
+ <visual>
+ <origin rpy="0 0 -3.14159265359" xyz="0 0 0"/>
+ <geometry>
+ <mesh filename="package://anymal_b_simple_description/meshes/hip/anymal_hip_r.dae" scale="1 1 1"/>
+ </geometry>
+ <material name="anymal_material"/>
+ </visual>
+ <collision>
+ <!-- Protector -->
+ <origin rpy="-1.57079632679 0 0" xyz="-0.0635 -0.009 0.0"/>
+ <geometry>
+ <cylinder length="0.1" radius="0.08"/>
+ </geometry>
+ </collision>
+ <collision>
+ <!-- Heatfins -->
+ <origin rpy="-1.57079632679 0 0" xyz="-0.0635 -0.074 0.0"/>
+ <geometry>
+ <cylinder length="0.03" radius="0.045"/>
+ </geometry>
+ </collision>
+ <inertial>
+ <origin rpy="0 0 0" xyz="-0.064516258147 -0.003787101702 -0.000152184388"/>
+ <mass value="1.42462064"/>
+ <inertia ixx="0.00243023349564" ixy="1.53023971e-05" ixz="2.1819095354e-05" iyy="0.00230257239103" iyz="2.6473021273e-05" izz="0.0019806759227"/>
+ </inertial>
+ </link>
+ <!-- Hip joint -->
+ <joint name="LH_HAA" type="revolute">
+ <parent link="base"/>
+ <child link="LH_HIP"/>
+ <origin xyz="-0.277 0.116 0.0"/>
+ <axis xyz="1 0 0"/>
+ <limit command_effort="40" current="10" effort="80" gear_velocity="10" lower="-9.42" upper="9.42" velocity="15"/>
+ <dynamics damping="0.0" friction="0.0"/>
+ </joint>
+ <!-- Xacro:Properties -->
+ <!-- [m] -->
+ <!-- [A] -->
+ <link name="LH_THIGH">
+ <visual>
+ <origin rpy="0 0 0" xyz="0 0 0"/>
+ <geometry>
+ <mesh filename="package://anymal_b_simple_description/meshes/thigh/anymal_thigh.dae" scale="1 1 1"/>
+ </geometry>
+ <material name="anymal_material"/>
+ </visual>
+ <collision>
+ <!-- thigh with heat fins -->
+ <origin rpy="0.145 0 0" xyz="0 0.035 -0.125"/>
+ <geometry>
+ <box size="0.08 0.04 0.25"/>
+ </geometry>
+ </collision>
+ <collision>
+ <!-- KFE actuator -->
+ <origin rpy="1.57079632679 0 0" xyz="0.0 0.069 -0.25"/>
+ <geometry>
+ <cylinder length="0.12" radius="0.06"/>
+ </geometry>
+ </collision>
+ <collision>
+ <!-- upper protector -->
+ <origin rpy="1.71579632679 0 0" xyz="0 -0.005 0"/>
+ <geometry>
+ <cylinder length="0.12" radius="0.066"/>
+ </geometry>
+ </collision>
+ <inertial>
+ <origin rpy="0 0 0" xyz="0.003897968082 0.054226618537 -0.214583373795"/>
+ <mass value="1.634976467"/>
+ <inertia ixx="0.0120367944369" ixy="-6.762065206e-05" ixz="-0.000287806340448" iyy="0.0120643637939" iyz="-0.00140610131218" izz="0.00249422574881"/>
+ </inertial>
+ </link>
+ <!-- Thigh joint -->
+ <joint name="LH_HFE" type="revolute">
+ <parent link="LH_HIP"/>
+ <child link="LH_THIGH"/>
+ <origin xyz="-0.0635 0.041 0.0"/>
+ <axis xyz="0 1 0"/>
+ <limit command_effort="40" current="10" effort="80" gear_velocity="10" lower="-9.42" upper="9.42" velocity="15.0"/>
+ <dynamics damping="0.0" friction="0.0"/>
+ </joint>
+ <!-- Xacro:Properties -->
+ <!-- [m] -->
+ <!-- [A] -->
+ <link name="LH_SHANK">
+ <visual>
+ <origin rpy="0 0 -3.14159265359" xyz="0 0 0"/>
+ <geometry>
+ <mesh filename="package://anymal_b_simple_description/meshes/shank/anymal_shank_r.dae" scale="1 1 1"/>
+ </geometry>
+ <material name="anymal_material"/>
+ </visual>
+ <collision>
+ <origin rpy="0 1.57079632679 0" xyz="-0.065 -0.015 0.01"/>
+ <geometry>
+ <box size="0.08 0.07 0.13"/>
+ </geometry>
+ </collision>
+ <inertial>
+ <origin rpy="0 0 0" xyz="-0.030816858139 -0.004617229294 0.000893125713"/>
+ <mass value="0.207204302"/>
+ <inertia ixx="0.0002104880248" ixy="5.6750980345e-05" ixz="-1.0127699391e-05" iyy="0.000676270210023" iyz="-8.22869024e-07" izz="0.000545032674924"/>
+ </inertial>
+ </link>
+ <!-- Shank joint -->
+ <joint name="LH_KFE" type="revolute">
+ <parent link="LH_THIGH"/>
+ <child link="LH_SHANK"/>
+ <origin xyz="-0.0 0.109 -0.25"/>
+ <axis xyz="0 1 0"/>
+ <limit command_effort="40" current="10" effort="80" gear_velocity="10" lower="-9.42" upper="9.42" velocity="15.0"/>
+ <dynamics damping="0.0" friction="0.0"/>
+ </joint>
+ <!-- Xacro:Properties -->
+ <!-- [m] -->
+ <!-- Shank to Adapter joint -->
+ <joint name="LH_SHANK_TO_ADAPTER" type="fixed">
+ <parent link="LH_SHANK"/>
+ <child link="LH_ADAPTER"/>
+ <origin rpy="0.0 -0.0 -0.0" xyz="-0.1 -0.02 0.0"/>
+ </joint>
+ <!-- Adapter link -->
+ <link name="LH_ADAPTER">
+ <visual>
+ <origin rpy="0 0 -3.14159265359" xyz="0 0 0.032"/>
+ <geometry>
+ <mesh filename="package://anymal_b_simple_description/meshes/foot/anymal_foot.dae" scale="1 1 1"/>
+ </geometry>
+ <material name="anymal_material"/>
+ </visual>
+ <collision>
+ <origin rpy="0 0 0" xyz="0 0 -0.160625"/>
+ <geometry>
+ <cylinder length="0.32125" radius="0.015"/>
+ </geometry>
+ <material name="anymal_material"/>
+ </collision>
+ <inertial>
+ <origin rpy="0 0 0" xyz="-8.66e-10 -1.472e-09 -0.244345749188"/>
+ <mass value="0.140170767"/>
+ <inertia ixx="0.00159938741862" ixy="-9.32e-13" ixz="1.039e-11" iyy="0.00159938741932" iyz="1.7563e-11" izz="5.4423177329e-05"/>
+ </inertial>
+ </link>
+ <!-- Adapter to Foot joint -->
+ <joint name="LH_ADAPTER_TO_FOOT" type="fixed">
+ <parent link="LH_ADAPTER"/>
+ <child link="LH_FOOT"/>
+ <origin rpy="0.0 -0.0 -0.0" xyz="-0.0 0.0 -0.32125"/>
+ </joint>
+ <!-- Foot link -->
+ <link name="LH_FOOT">
+ <collision>
+ <origin xyz="0 0 0.02325"/>
+ <geometry>
+ <sphere radius="0.031"/>
+ </geometry>
+ </collision>
+ </link>
+ <!-- Gazebo customization -->
+ <gazebo reference="LH_FOOT">
+ <kp>1000000.0</kp>
+ <kd>100.0</kd>
+ <mu1>0.8</mu1>
+ <mu2>0.8</mu2>
+ </gazebo>
+ <!-- Xacro:Properties -->
+ <!-- [kg * m^2] -->
+ <!-- [A] -->
+ <link name="RH_HIP">
+ <visual>
+ <origin rpy="0 0 -3.14159265359" xyz="0 0 0"/>
+ <geometry>
+ <mesh filename="package://anymal_b_simple_description/meshes/hip/anymal_hip_l.dae" scale="1 1 1"/>
+ </geometry>
+ <material name="anymal_material"/>
+ </visual>
+ <collision>
+ <!-- Protector -->
+ <origin rpy="-1.57079632679 0 0" xyz="-0.0635 0.009 0.0"/>
+ <geometry>
+ <cylinder length="0.1" radius="0.08"/>
+ </geometry>
+ </collision>
+ <collision>
+ <!-- Heatfins -->
+ <origin rpy="-1.57079632679 0 0" xyz="-0.0635 0.074 0.0"/>
+ <geometry>
+ <cylinder length="0.03" radius="0.045"/>
+ </geometry>
+ </collision>
+ <inertial>
+ <origin rpy="0 0 0" xyz="-0.064516258147 0.003787101702 -0.000152184388"/>
+ <mass value="1.42462064"/>
+ <inertia ixx="0.00243023349564" ixy="-1.53023971e-05" ixz="2.1819095354e-05" iyy="0.00230257239103" iyz="-2.6473021273e-05" izz="0.0019806759227"/>
+ </inertial>
+ </link>
+ <!-- Hip joint -->
+ <joint name="RH_HAA" type="revolute">
+ <parent link="base"/>
+ <child link="RH_HIP"/>
+ <origin xyz="-0.277 -0.116 0.0"/>
+ <axis xyz="1 0 0"/>
+ <limit command_effort="40" current="10" effort="80" gear_velocity="10" lower="-9.42" upper="9.42" velocity="15"/>
+ <dynamics damping="0.0" friction="0.0"/>
+ </joint>
+ <!-- Xacro:Properties -->
+ <!-- [m] -->
+ <!-- [A] -->
+ <link name="RH_THIGH">
+ <visual>
+ <origin rpy="0 0 -3.14159265359" xyz="0 0 0"/>
+ <geometry>
+ <mesh filename="package://anymal_b_simple_description/meshes/thigh/anymal_thigh.dae" scale="1 1 1"/>
+ </geometry>
+ <material name="anymal_material"/>
+ </visual>
+ <collision>
+ <!-- thigh with heat fins -->
+ <origin rpy="-0.145 0 0" xyz="0 -0.035 -0.125"/>
+ <geometry>
+ <box size="0.08 0.04 0.25"/>
+ </geometry>
+ </collision>
+ <collision>
+ <!-- KFE actuator -->
+ <origin rpy="1.57079632679 0 0" xyz="0.0 -0.069 -0.25"/>
+ <geometry>
+ <cylinder length="0.12" radius="0.06"/>
+ </geometry>
+ </collision>
+ <collision>
+ <!-- upper protector -->
+ <origin rpy="-1.71579632679 0 0" xyz="0 0.005 0"/>
+ <geometry>
+ <cylinder length="0.12" radius="0.066"/>
+ </geometry>
+ </collision>
+ <inertial>
+ <origin rpy="0 0 0" xyz="0.003897968082 -0.054226618537 -0.214583373795"/>
+ <mass value="1.634976467"/>
+ <inertia ixx="0.0120367944369" ixy="6.762065206e-05" ixz="-0.000287806340448" iyy="0.0120643637939" iyz="0.00140610131218" izz="0.00249422574881"/>
+ </inertial>
+ </link>
+ <!-- Thigh joint -->
+ <joint name="RH_HFE" type="revolute">
+ <parent link="RH_HIP"/>
+ <child link="RH_THIGH"/>
+ <origin xyz="-0.0635 -0.041 0.0"/>
+ <axis xyz="0 1 0"/>
+ <limit command_effort="40" current="10" effort="80" gear_velocity="10" lower="-9.42" upper="9.42" velocity="15.0"/>
+ <dynamics damping="0.0" friction="0.0"/>
+ </joint>
+ <!-- Xacro:Properties -->
+ <!-- [m] -->
+ <!-- [A] -->
+ <link name="RH_SHANK">
+ <visual>
+ <origin rpy="0 0 -3.14159265359" xyz="0 0 0"/>
+ <geometry>
+ <mesh filename="package://anymal_b_simple_description/meshes/shank/anymal_shank_l.dae" scale="1 1 1"/>
+ </geometry>
+ <material name="anymal_material"/>
+ </visual>
+ <collision>
+ <origin rpy="0 1.57079632679 0" xyz="-0.065 0.015 0.01"/>
+ <geometry>
+ <box size="0.08 0.07 0.13"/>
+ </geometry>
+ </collision>
+ <inertial>
+ <origin rpy="0 0 0" xyz="-0.030816858139 0.004617229294 0.000893125713"/>
+ <mass value="0.207204302"/>
+ <inertia ixx="0.0002104880248" ixy="-5.6750980345e-05" ixz="-1.0127699391e-05" iyy="0.000676270210023" iyz="8.22869024e-07" izz="0.000545032674924"/>
+ </inertial>
+ </link>
+ <!-- Shank joint -->
+ <joint name="RH_KFE" type="revolute">
+ <parent link="RH_THIGH"/>
+ <child link="RH_SHANK"/>
+ <origin xyz="-0.0 -0.109 -0.25"/>
+ <axis xyz="0 1 0"/>
+ <limit command_effort="40" current="10" effort="80" gear_velocity="10" lower="-9.42" upper="9.42" velocity="15.0"/>
+ <dynamics damping="0.0" friction="0.0"/>
+ </joint>
+ <!-- Xacro:Properties -->
+ <!-- [m] -->
+ <!-- Shank to Adapter joint -->
+ <joint name="RH_SHANK_TO_ADAPTER" type="fixed">
+ <parent link="RH_SHANK"/>
+ <child link="RH_ADAPTER"/>
+ <origin rpy="-0.0 -0.0 0.0" xyz="-0.1 0.02 0.0"/>
+ </joint>
+ <!-- Adapter link -->
+ <link name="RH_ADAPTER">
+ <visual>
+ <origin rpy="0 0 -3.14159265359" xyz="0 0 0.032"/>
+ <geometry>
+ <mesh filename="package://anymal_b_simple_description/meshes/foot/anymal_foot.dae" scale="1 1 1"/>
+ </geometry>
+ <material name="anymal_material"/>
+ </visual>
+ <collision>
+ <origin rpy="0 0 0" xyz="0 0 -0.160625"/>
+ <geometry>
+ <cylinder length="0.32125" radius="0.015"/>
+ </geometry>
+ <material name="anymal_material"/>
+ </collision>
+ <inertial>
+ <origin rpy="0 0 0" xyz="-8.66e-10 -1.472e-09 -0.244345749188"/>
+ <mass value="0.140170767"/>
+ <inertia ixx="0.00159938741862" ixy="-9.32e-13" ixz="1.039e-11" iyy="0.00159938741932" iyz="1.7563e-11" izz="5.4423177329e-05"/>
+ </inertial>
+ </link>
+ <!-- Adapter to Foot joint -->
+ <joint name="RH_ADAPTER_TO_FOOT" type="fixed">
+ <parent link="RH_ADAPTER"/>
+ <child link="RH_FOOT"/>
+ <origin rpy="-0.0 -0.0 0.0" xyz="-0.0 -0.0 -0.32125"/>
+ </joint>
+ <!-- Foot link -->
+ <link name="RH_FOOT">
+ <collision>
+ <origin xyz="0 0 0.02325"/>
+ <geometry>
+ <sphere radius="0.031"/>
+ </geometry>
+ </collision>
+ </link>
+ <!-- Gazebo customization -->
+ <gazebo reference="RH_FOOT">
+ <kp>1000000.0</kp>
+ <kd>100.0</kd>
+ <mu1>0.8</mu1>
+ <mu2>0.8</mu2>
+ </gazebo>
+ <link name="imu_link">
+ <visual>
+ <origin rpy="0 0 0" xyz="0 0 0.011"/>
+ <geometry>
+ <box size="0.058 0.058 0.022"/>
+ </geometry>
+ <material name="orange"/>
+ </visual>
+ <collision>
+ <origin rpy="0 0 0" xyz="0 0 0.011"/>
+ <geometry>
+ <box size="0.058 0.058 0.022"/>
+ </geometry>
+ </collision>
+ <inertial>
+ <mass value="0.05"/>
+ <origin xyz="0 0 0"/>
+ <inertia ixx="1.60333333333e-05" ixy="0.0" ixz="0.0" iyy="2.80333333333e-05" iyz="0.0" izz="1.60333333333e-05"/>
+ </inertial>
+ </link>
+ <joint name="imu_joint" type="fixed">
+ <parent link="base"/>
+ <child link="imu_link"/>
+ <origin rpy="0 3.14159265359 0" xyz="0.038 0.06245 0.1837"/>
+ </joint>
+
+ <link name="jaco_front_hatch_support_v2">
+ <visual>
+ <geometry>
+ <mesh filename="package://kinova_description/meshes/jaco_front_hatch_support_v2.dae"/>
+ </geometry>
+ </visual>
+ </link>
+ <joint name="base_to_jaco_support" type="fixed">
+ <parent link="base"/>
+ <child link="jaco_front_hatch_support_v2"/>
+ <origin rpy="0 0 0" xyz="0.2692 0.00 0.06713"/>
+ </joint>
+
+ <link name="jaco_mounting_block">
+ <visual>
+ <geometry>
+ <mesh filename="package://kinova_description/meshes/jaco_mounting_block.dae"/>
+ </geometry>
+ </visual>
+ </link>
+ <joint name="jaco_support_to_jaco_mounting_block" type="fixed">
+ <parent link="jaco_front_hatch_support_v2"/>
+ <child link="jaco_mounting_block"/>
+ <origin rpy="0 0 0" xyz="0.045 0.00 -0.007626"/>
+ </joint>
+
+ <link name="j2s6s200_link_base">
+ <visual>
+ <geometry>
+ <mesh filename="package://kinova_description/meshes/base.dae"/>
+ </geometry>
+ <material name="carbon_fiber">
+ <color rgba="0.3 0.3 0.3 1"/>
+ </material>
+ </visual>
+ <collision>
+ <geometry>
+ <mesh filename="package://kinova_description/meshes/base.dae"/>
+ </geometry>
+ </collision>
+ <inertial>
+ <mass value="0.46784"/>
+ <origin rpy="0 0 0" xyz="0 0 0.1255"/>
+ <inertia ixx="0.000951270861568" ixy="0" ixz="0" iyy="0.000951270861568" iyz="0" izz="0.000374272"/>
+ </inertial>
+ </link>
+ <joint name="jaco_mounting_block_to_j2s6s200_link_base" type="fixed">
+ <parent link="jaco_mounting_block"/>
+ <child link="j2s6s200_link_base"/>
+ <axis xyz="0 0 0"/>
+ <limit effort="0" lower="0" upper="0" velocity="0"/>
+ <origin rpy="0 0 1.57079632679" xyz="0.000 0.000 0.000"/>
+ <dynamics damping="0.0" friction="0.0"/>
+ </joint>
+ <link name="j2s6s200_link_1">
+ <visual>
+ <geometry>
+ <mesh filename="package://kinova_description/meshes/shoulder.dae"/>
+ </geometry>
+ <material name="carbon_fiber">
+ <color rgba="0.3 0.3 0.3 1"/>
+ </material>
+ </visual>
+ <visual>
+ <geometry>
+ <mesh filename="package://kinova_description/meshes/ring_big.dae"/>
+ </geometry>
+ </visual>
+ <collision>
+ <geometry>
+ <mesh filename="package://kinova_description/meshes/shoulder.dae"/>
+ </geometry>
+ </collision>
+ <inertial>
+ <mass value="0.7477"/>
+ <origin xyz="0 -0.002 -0.0605"/>
+ <inertia ixx="0.00152031725204" ixy="0" ixz="0" iyy="0.00152031725204" iyz="0" izz="0.00059816"/>
+ </inertial>
+ </link>
+ <joint name="j2s6s200_joint_1" type="continuous">
+ <parent link="j2s6s200_link_base"/>
+ <child link="j2s6s200_link_1"/>
+ <axis xyz="0 0 1"/>
+ <limit effort="40" lower="-6.28318530718" upper="6.28318530718" velocity="0.628318530718"/>
+ <origin rpy="0 3.14159265359 0" xyz="0 0 0.15675"/>
+ <dynamics damping="0.0" friction="0.0"/>
+ </joint>
+ <transmission name="j2s6s200_joint_1_trans">
+ <type>transmission_interface/SimpleTransmission</type>
+ <joint name="j2s6s200_joint_1">
+ <hardwareInterface>hardware_interface/EffortJointInterface</hardwareInterface>
+ </joint>
+ <actuator name="j2s6s200_joint_1_actuator">
+ <hardwareInterface>hardware_interface/EffortJointInterface</hardwareInterface>
+ <mechanicalReduction>160</mechanicalReduction>
+ </actuator>
+ </transmission>
+ <link name="j2s6s200_link_2">
+ <visual>
+ <geometry>
+ <mesh filename="package://kinova_description/meshes/arm.dae"/>
+ </geometry>
+ <material name="carbon_fiber">
+ <color rgba="0.3 0.3 0.3 1"/>
+ </material>
+ </visual>
+ <visual>
+ <geometry>
+ <mesh filename="package://kinova_description/meshes/ring_big.dae"/>
+ </geometry>
+ </visual>
+ <collision>
+ <geometry>
+ <mesh filename="package://kinova_description/meshes/arm.dae"/>
+ </geometry>
+ </collision>
+ <inertial>
+ <mass value="0.99"/>
+ <origin xyz="0 -0.2065 -0.01"/>
+ <inertia ixx="0.010502207991" ixy="0" ixz="0" iyy="0.000792" iyz="0" izz="0.010502207991"/>
+ </inertial>
+ </link>
+ <joint name="j2s6s200_joint_2" type="revolute">
+ <parent link="j2s6s200_link_1"/>
+ <child link="j2s6s200_link_2"/>
+ <axis xyz="0 0 1"/>
+ <limit effort="80" lower="0.820304748437" upper="5.46288055874" velocity="0.628318530718"/>
+ <origin rpy="-1.57079632679 0 3.14159265359" xyz="0 0.0016 -0.11875"/>
+ <dynamics damping="0.0" friction="0.0"/>
+ </joint>
+ <transmission name="j2s6s200_joint_2_trans">
+ <type>transmission_interface/SimpleTransmission</type>
+ <joint name="j2s6s200_joint_2">
+ <hardwareInterface>hardware_interface/EffortJointInterface</hardwareInterface>
+ </joint>
+ <actuator name="j2s6s200_joint_2_actuator">
+ <hardwareInterface>hardware_interface/EffortJointInterface</hardwareInterface>
+ <mechanicalReduction>160</mechanicalReduction>
+ </actuator>
+ </transmission>
+ <link name="j2s6s200_link_3">
+ <visual>
+ <geometry>
+ <mesh filename="package://kinova_description/meshes/forearm.dae"/>
+ </geometry>
+ <material name="carbon_fiber">
+ <color rgba="0.3 0.3 0.3 1"/>
+ </material>
+ </visual>
+ <visual>
+ <geometry>
+ <mesh filename="package://kinova_description/meshes/ring_big.dae"/>
+ </geometry>
+ </visual>
+ <collision>
+ <geometry>
+ <mesh filename="package://kinova_description/meshes/forearm.dae"/>
+ </geometry>
+ </collision>
+ <inertial>
+ <mass value="0.6763"/>
+ <origin xyz="0 0.081 -0.0086"/>
+ <inertia ixx="0.00142022431908" ixy="0" ixz="0" iyy="0.000304335" iyz="0" izz="0.00142022431908"/>
+ </inertial>
+ </link>
+ <joint name="j2s6s200_joint_3" type="revolute">
+ <parent link="j2s6s200_link_2"/>
+ <child link="j2s6s200_link_3"/>
+ <axis xyz="0 0 1"/>
+ <limit effort="40" lower="0.331612557879" upper="5.9515727493" velocity="0.628318530718"/>
+ <origin rpy="0 3.14159265359 0" xyz="0 -0.410 0"/>
+ <dynamics damping="0.0" friction="0.0"/>
+ </joint>
+ <transmission name="j2s6s200_joint_3_trans">
+ <type>transmission_interface/SimpleTransmission</type>
+ <joint name="j2s6s200_joint_3">
+ <hardwareInterface>hardware_interface/EffortJointInterface</hardwareInterface>
+ </joint>
+ <actuator name="j2s6s200_joint_3_actuator">
+ <hardwareInterface>hardware_interface/EffortJointInterface</hardwareInterface>
+ <mechanicalReduction>160</mechanicalReduction>
+ </actuator>
+ </transmission>
+ <link name="j2s6s200_link_4">
+ <visual>
+ <geometry>
+ <mesh filename="package://kinova_description/meshes/wrist_spherical_1.dae"/>
+ </geometry>
+ <material name="carbon_fiber">
+ <color rgba="0.3 0.3 0.3 1"/>
+ </material>
+ </visual>
+ <visual>
+ <geometry>
+ <mesh filename="package://kinova_description/meshes/ring_small.dae"/>
+ </geometry>
+ </visual>
+ <collision>
+ <geometry>
+ <mesh filename="package://kinova_description/meshes/wrist_spherical_1.dae"/>
+ </geometry>
+ </collision>
+ <inertial>
+ <mass value="0.463"/>
+ <origin xyz="0 0.0028848942 -0.0541932613"/>
+ <inertia ixx="0.0004321316048" ixy="0" ixz="0" iyy="0.0004321316048" iyz="0" izz="9.26e-05"/>
+ </inertial>
+ </link>
+ <joint name="j2s6s200_joint_4" type="continuous">
+ <parent link="j2s6s200_link_3"/>
+ <child link="j2s6s200_link_4"/>
+ <axis xyz="0 0 1"/>
+ <limit effort="20" lower="-6.28318530718" upper="6.28318530718" velocity="0.837758040957"/>
+ <origin rpy="-1.57079632679 0 3.14159265359" xyz="0 0.2073 -0.0114"/>
+ <dynamics damping="0.0" friction="0.0"/>
+ </joint>
+ <transmission name="j2s6s200_joint_4_trans">
+ <type>transmission_interface/SimpleTransmission</type>
+ <joint name="j2s6s200_joint_4">
+ <hardwareInterface>hardware_interface/EffortJointInterface</hardwareInterface>
+ </joint>
+ <actuator name="j2s6s200_joint_4_actuator">
+ <hardwareInterface>hardware_interface/EffortJointInterface</hardwareInterface>
+ <mechanicalReduction>160</mechanicalReduction>
+ </actuator>
+ </transmission>
+ <link name="j2s6s200_link_5">
+ <visual>
+ <geometry>
+ <mesh filename="package://kinova_description/meshes/wrist_spherical_2.dae"/>
+ </geometry>
+ <material name="carbon_fiber">
+ <color rgba="0.3 0.3 0.3 1"/>
+ </material>
+ </visual>
+ <visual>
+ <geometry>
+ <mesh filename="package://kinova_description/meshes/ring_small.dae"/>
+ </geometry>
+ </visual>
+ <collision>
+ <geometry>
+ <mesh filename="package://kinova_description/meshes/wrist_spherical_2.dae"/>
+ </geometry>
+ </collision>
+ <inertial>
+ <mass value="0.463"/>
+ <origin xyz="0 0.0497208855 -0.0028562765"/>
+ <inertia ixx="0.0004321316048" ixy="0" ixz="0" iyy="9.26e-05" iyz="0" izz="0.0004321316048"/>
+ </inertial>
+ </link>
+ <joint name="j2s6s200_joint_5" type="revolute">
+ <parent link="j2s6s200_link_4"/>
+ <child link="j2s6s200_link_5"/>
+ <axis xyz="0 0 1"/>
+ <limit effort="20" lower="0.523598775598" upper="5.75958653158" velocity="0.837758040957"/>
+ <origin rpy="1.57079632679 0 3.14159265359" xyz="0 0 -0.10375"/>
+ <dynamics damping="0.0" friction="0.0"/>
+ </joint>
+ <transmission name="j2s6s200_joint_5_trans">
+ <type>transmission_interface/SimpleTransmission</type>
+ <joint name="j2s6s200_joint_5">
+ <hardwareInterface>hardware_interface/EffortJointInterface</hardwareInterface>
+ </joint>
+ <actuator name="j2s6s200_joint_5_actuator">
+ <hardwareInterface>hardware_interface/EffortJointInterface</hardwareInterface>
+ <mechanicalReduction>160</mechanicalReduction>
+ </actuator>
+ </transmission>
+ <link name="j2s6s200_link_6">
+ <visual>
+ <geometry>
+ <mesh filename="package://kinova_description/meshes/hand_2finger.dae"/>
+ </geometry>
+ <material name="carbon_fiber">
+ <color rgba="0.3 0.3 0.3 1"/>
+ </material>
+ </visual>
+ <visual>
+ <geometry>
+ <mesh filename="package://kinova_description/meshes/ring_small.dae"/>
+ </geometry>
+ </visual>
+ <collision>
+ <geometry>
+ <mesh filename="package://kinova_description/meshes/hand_2finger.dae"/>
+ </geometry>
+ </collision>
+ <inertial>
+ <mass value="0.99"/>
+ <origin xyz="0 0 -0.06"/>
+ <inertia ixx="0.0003453236187" ixy="0" ixz="0" iyy="0.0003453236187" iyz="0" izz="0.0005816"/>
+ </inertial>
+ </link>
+ <joint name="j2s6s200_joint_6" type="continuous">
+ <parent link="j2s6s200_link_5"/>
+ <child link="j2s6s200_link_6"/>
+ <axis xyz="0 0 1"/>
+ <limit effort="20" lower="-6.28318530718" upper="6.28318530718" velocity="0.837758040957"/>
+ <origin rpy="-1.57079632679 0 3.14159265359" xyz="0 0.10375 0"/>
+ <dynamics damping="0.0" friction="0.0"/>
+ </joint>
+ <transmission name="j2s6s200_joint_6_trans">
+ <type>transmission_interface/SimpleTransmission</type>
+ <joint name="j2s6s200_joint_6">
+ <hardwareInterface>hardware_interface/EffortJointInterface</hardwareInterface>
+ </joint>
+ <actuator name="j2s6s200_joint_6_actuator">
+ <hardwareInterface>hardware_interface/EffortJointInterface</hardwareInterface>
+ <mechanicalReduction>160</mechanicalReduction>
+ </actuator>
+ </transmission>
+ <link name="j2s6s200_end_effector"/>
+ <joint name="j2s6s200_joint_end_effector" type="fixed">
+ <parent link="j2s6s200_link_6"/>
+ <child link="j2s6s200_end_effector"/>
+ <axis xyz="0 0 0"/>
+ <limit effort="2000" lower="0" upper="0" velocity="1"/>
+ <!-- <origin rpy="3.14159265359 0 1.57079632679" xyz="0 0 -0.1600"/> -->
+ <origin rpy="3.14159265359 1.57079632679 0" xyz="0 0 -0.1600"/>
+ </joint>
+ <link name="j2s6s200_link_finger_1">
+ <visual>
+ <geometry>
+ <mesh filename="package://kinova_description/meshes/finger_proximal.dae"/>
+ </geometry>
+ <material name="carbon_fiber">
+ <color rgba="0.792156862745098 0.819607843137255 0.933333333333333 1"/>
+ </material>
+ </visual>
+ <collision>
+ <geometry>
+ <mesh filename="package://kinova_description/meshes/finger_proximal.dae"/>
+ </geometry>
+ </collision>
+ <inertial>
+ <mass value="0.01"/>
+ <origin xyz="0.022 0 0"/>
+ <inertia ixx="7.8999684e-07" ixy="0" ixz="0" iyy="7.8999684e-07" iyz="0" izz="8e-08"/>
+ </inertial>
+ </link>
+ <joint name="j2s6s200_joint_finger_1" type="fixed">
+ <parent link="j2s6s200_link_6"/>
+ <child link="j2s6s200_link_finger_1"/>
+ <axis xyz="0 0 1"/>
+ <origin rpy="-1.570796327 .649262481663582 1.57079632679490" xyz="-0.0025 0.03095 -0.11482"/>
+ <limit effort="2" lower="0" upper="1.51" velocity="1"/>
+ </joint>
+ <transmission name="j2s6s200_joint_finger_1_trans">
+ <type>transmission_interface/SimpleTransmission</type>
+ <joint name="j2s6s200_joint_finger_1">
+ <hardwareInterface>hardware_interface/EffortJointInterface</hardwareInterface>
+ </joint>
+ <actuator name="j2s6s200_joint_finger_1_actuator">
+ <hardwareInterface>hardware_interface/EffortJointInterface</hardwareInterface>
+ <mechanicalReduction>1</mechanicalReduction>
+ </actuator>
+ </transmission>
+ <link name="j2s6s200_link_finger_tip_1">
+ <visual>
+ <geometry>
+ <mesh filename="package://kinova_description/meshes/finger_distal.dae"/>
+ </geometry>
+ <material name="carbon_fiber">
+ <color rgba="0.792156862745098 0.819607843137255 0.933333333333333 1"/>
+ </material>
+ </visual>
+ <collision>
+ <geometry>
+ <mesh filename="package://kinova_description/meshes/finger_distal.dae"/>
+ </geometry>
+ </collision>
+ <inertial>
+ <mass value="0.01"/>
+ <origin xyz="0.022 0 0"/>
+ <inertia ixx="7.8999684e-07" ixy="0" ixz="0" iyy="7.8999684e-07" iyz="0" izz="8e-08"/>
+ </inertial>
+ </link>
+ <joint name="j2s6s200_joint_finger_tip_1" type="fixed">
+ <parent link="j2s6s200_link_finger_1"/>
+ <child link="j2s6s200_link_finger_tip_1"/>
+ <axis xyz="0 0 1"/>
+ <origin rpy="0 0 0" xyz="0.044 -0.003 0"/>
+ <limit effort="2" lower="0" upper="2" velocity="1"/>
+ </joint>
+ <transmission name="j2s6s200_joint_finger_tip_1_trans">
+ <type>transmission_interface/SimpleTransmission</type>
+ <joint name="j2s6s200_joint_finger_tip_1">
+ <hardwareInterface>hardware_interface/EffortJointInterface</hardwareInterface>
+ </joint>
+ <actuator name="j2s6s200_joint_finger_tip_1_actuator">
+ <hardwareInterface>hardware_interface/EffortJointInterface</hardwareInterface>
+ <mechanicalReduction>1</mechanicalReduction>
+ </actuator>
+ </transmission>
+ <link name="j2s6s200_link_finger_2">
+ <visual>
+ <geometry>
+ <mesh filename="package://kinova_description/meshes/finger_proximal.dae"/>
+ </geometry>
+ <material name="carbon_fiber">
+ <color rgba="0.792156862745098 0.819607843137255 0.933333333333333 1"/>
+ </material>
+ </visual>
+ <collision>
+ <geometry>
+ <mesh filename="package://kinova_description/meshes/finger_proximal.dae"/>
+ </geometry>
+ </collision>
+ <inertial>
+ <mass value="0.01"/>
+ <origin xyz="0.022 0 0"/>
+ <inertia ixx="7.8999684e-07" ixy="0" ixz="0" iyy="7.8999684e-07" iyz="0" izz="8e-08"/>
+ </inertial>
+ </link>
+ <joint name="j2s6s200_joint_finger_2" type="fixed">
+ <parent link="j2s6s200_link_6"/>
+ <child link="j2s6s200_link_finger_2"/>
+ <axis xyz="0 0 1"/>
+ <origin rpy="-1.570796327 .649262481663582 -1.57079632679490" xyz="-0.0025 -0.03095 -0.11482"/>
+ <limit effort="2" lower="0" upper="1.51" velocity="1"/>
+ </joint>
+ <transmission name="j2s6s200_joint_finger_2_trans">
+ <type>transmission_interface/SimpleTransmission</type>
+ <joint name="j2s6s200_joint_finger_2">
+ <hardwareInterface>hardware_interface/EffortJointInterface</hardwareInterface>
+ </joint>
+ <actuator name="j2s6s200_joint_finger_2_actuator">
+ <hardwareInterface>hardware_interface/EffortJointInterface</hardwareInterface>
+ <mechanicalReduction>1</mechanicalReduction>
+ </actuator>
+ </transmission>
+ <link name="j2s6s200_link_finger_tip_2">
+ <visual>
+ <geometry>
+ <mesh filename="package://kinova_description/meshes/finger_distal.dae"/>
+ </geometry>
+ <material name="carbon_fiber">
+ <color rgba="0.792156862745098 0.819607843137255 0.933333333333333 1"/>
+ </material>
+ </visual>
+ <collision>
+ <geometry>
+ <mesh filename="package://kinova_description/meshes/finger_distal.dae"/>
+ </geometry>
+ </collision>
+ <inertial>
+ <mass value="0.01"/>
+ <origin xyz="0.022 0 0"/>
+ <inertia ixx="7.8999684e-07" ixy="0" ixz="0" iyy="7.8999684e-07" iyz="0" izz="8e-08"/>
+ </inertial>
+ </link>
+ <joint name="j2s6s200_joint_finger_tip_2" type="fixed">
+ <parent link="j2s6s200_link_finger_2"/>
+ <child link="j2s6s200_link_finger_tip_2"/>
+ <axis xyz="0 0 1"/>
+ <origin rpy="0 0 0" xyz="0.044 -0.003 0"/>
+ <limit effort="2" lower="0" upper="2" velocity="1"/>
+ </joint>
+ <transmission name="j2s6s200_joint_finger_tip_2_trans">
+ <type>transmission_interface/SimpleTransmission</type>
+ <joint name="j2s6s200_joint_finger_tip_2">
+ <hardwareInterface>hardware_interface/EffortJointInterface</hardwareInterface>
+ </joint>
+ <actuator name="j2s6s200_joint_finger_tip_2_actuator">
+ <hardwareInterface>hardware_interface/EffortJointInterface</hardwareInterface>
+ <mechanicalReduction>1</mechanicalReduction>
+ </actuator>
+ </transmission>
+
+</robot>
+
diff --git a/anymal_b_simple_description/srdf/anymal-kinova.srdf b/anymal_b_simple_description/srdf/anymal-kinova.srdf
new file mode 100644
index 0000000..67c2dd0
--- /dev/null
+++ b/anymal_b_simple_description/srdf/anymal-kinova.srdf
@@ -0,0 +1,139 @@
+<?xml version="1.0" ?>
+<robot name="anymal">
+
+ <group name="lf_leg">
+ <joint name="LF_HAA" />
+ <joint name="LF_HFE" />
+ <joint name="LF_KFE" />
+ <chain base_link="base" tip_link="LF_FOOT" />
+ </group>
+ <group name="lh_leg">
+ <joint name="LH_HAA" />
+ <joint name="LH_HFE" />
+ <joint name="LH_KFE" />
+ <chain base_link="base" tip_link="LH_FOOT" />
+ </group>
+ <group name="rf_leg">
+ <joint name="RF_HAA" />
+ <joint name="RF_HFE" />
+ <joint name="RF_KFE" />
+ <chain base_link="base" tip_link="RF_FOOT" />
+ </group>
+ <group name="rh_leg">
+ <joint name="RH_HAA" />
+ <joint name="RH_HFE" />
+ <joint name="RH_KFE" />
+ <chain base_link="base" tip_link="RH_FOOT" />
+ </group>
+ <group name="all_legs">
+ <group name="lf" />
+ <group name="rf" />
+ <group name="lh" />
+ <group name="rh" />
+ </group>
+ <group name="r_legs">
+ <group name="rf" />
+ <group name="rh" />
+ </group>
+ <group name="l_legs">
+ <group name="lf" />
+ <group name="lh" />
+ </group>
+ <group name="f_legs">
+ <group name="lf" />
+ <group name="rf" />
+ </group>
+ <group name="h_legs">
+ <group name="lh" />
+ <group name="rh" />
+ </group>
+ <group name="ld_legs">
+ <group name="lf" />
+ <group name="rh" />
+ </group>
+ <group name="rd_legs">
+ <group name="rf" />
+ <group name="lh" />
+ </group>
+
+ <end_effector name="lf_foot" parent_link="LF_FOOT" group="lf_leg" />
+ <end_effector name="rf_foot" parent_link="RF_FOOT" group="rf_leg" />
+ <end_effector name="lh_foot" parent_link="LH_FOOT" group="lh_leg" />
+ <end_effector name="rh_foot" parent_link="RH_FOOT" group="rh_leg" />
+
+ <group_state name="half_sitting" group="all_legs">
+ <joint name="root_joint" value="0. 0. 0.4792 0. 0. 0. 1." />
+ <joint name="LF_HAA" value="-0.1" />
+ <joint name="LF_HFE" value="0.7" />
+ <joint name="LF_KFE" value="-1." />
+ <joint name="RF_HAA" value="0.1" />
+ <joint name="RF_HFE" value="0.7" />
+ <joint name="RF_KFE" value="-1." />
+ <joint name="LH_HAA" value="-0.1" />
+ <joint name="LH_HFE" value="-0.7" />
+ <joint name="LH_KFE" value="1." />
+ <joint name="RH_HAA" value="0.1" />
+ <joint name="RH_HFE" value="-0.7" />
+ <joint name="RH_KFE" value="1." />
+ <joint name="j2s6s200_joint_1" value="1.5707" />
+ <joint name="j2s6s200_joint_2" value="2.618" />
+ <joint name="j2s6s200_joint_3" value="-1.5707" />
+ <joint name="j2s6s200_joint_4" value="3.1415" />
+ <joint name="j2s6s200_joint_5" value="2.618" />
+ <joint name="j2s6s200_joint_6" value="0." />
+ </group_state>
+
+ <disable_collisions link1="LF_HIP" link2="LF_THIGH" reason="Adjacent" />
+ <disable_collisions link1="LF_HIP" link2="LF_SHANK" reason="Never" />
+ <disable_collisions link1="LF_HIP" link2="LF_FOOT" reason="Never" />
+ <disable_collisions link1="LF_HIP" link2="RF_HIP" reason="Never" />
+ <disable_collisions link1="LF_HIP" link2="RF_THIGH" reason="Never" />
+ <disable_collisions link1="LF_HIP" link2="RF_SHANK" reason="Never" />
+ <disable_collisions link1="LF_HIP" link2="RF_FOOT" reason="Never" />
+ <disable_collisions link1="LF_HIP" link2="LH_HIP" reason="Never" />
+ <disable_collisions link1="LF_HIP" link2="LH_THIGH" reason="Never" />
+ <disable_collisions link1="LF_HIP" link2="LH_SHANK" reason="Never" />
+ <disable_collisions link1="LF_HIP" link2="LH_FOOT" reason="Never" />
+ <disable_collisions link1="LF_HIP" link2="RH_HIP" reason="Never" />
+ <disable_collisions link1="LF_HIP" link2="RH_THIGH" reason="Never" />
+ <disable_collisions link1="LF_HIP" link2="RH_SHANK" reason="Never" />
+ <disable_collisions link1="LF_HIP" link2="RH_FOOT" reason="Never" />
+
+ <disable_collisions link1="base" link2="LF_HIP" reason="Adjacent" />
+ <disable_collisions link1="base" link2="LH_HIP" reason="Adjacent" />
+ <disable_collisions link1="base" link2="RF_HIP" reason="Adjacent" />
+ <disable_collisions link1="base" link2="RH_HIP" reason="Adjacent" />
+ <disable_collisions link1="LF_HIP" link2="LF_THIGH" reason="Adjacent" />
+ <disable_collisions link1="LH_HIP" link2="LH_THIGH" reason="Adjacent" />
+ <disable_collisions link1="RF_HIP" link2="RF_THIGH" reason="Adjacent" />
+ <disable_collisions link1="RH_HIP" link2="RH_THIGH" reason="Adjacent" />
+ <disable_collisions link1="LF_THIGH" link2="LF_SHANK" reason="Adjacent" />
+ <disable_collisions link1="LH_THIGH" link2="LH_SHANK" reason="Adjacent" />
+ <disable_collisions link1="RF_THIGH" link2="RF_SHANK" reason="Adjacent" />
+ <disable_collisions link1="RH_THIGH" link2="RH_SHANK" reason="Adjacent" />
+ <disable_collisions link1="LF_SHANK" link2="LF_FOOT" reason="Adjacent" />
+ <disable_collisions link1="LH_SHANK" link2="LH_FOOT" reason="Adjacent" />
+ <disable_collisions link1="RF_SHANK" link2="RF_FOOT" reason="Adjacent" />
+ <disable_collisions link1="RH_SHANK" link2="RH_FOOT" reason="Adjacent" />
+
+ <disable_collisions link1="LF_ADAPTER" link2="LF_FOOT" reason="Adjacent" />
+ <disable_collisions link1="LH_ADAPTER" link2="LH_FOOT" reason="Adjacent" />
+ <disable_collisions link1="RF_ADAPTER" link2="RF_FOOT" reason="Adjacent" />
+ <disable_collisions link1="RH_ADAPTER" link2="RH_FOOT" reason="Adjacent" />
+
+ <disable_collisions link1="LF_ADAPTER" link2="LF_SHANK" reason="Adjacent" />
+ <disable_collisions link1="LH_ADAPTER" link2="LH_SHANK" reason="Adjacent" />
+ <disable_collisions link1="RF_ADAPTER" link2="RF_SHANK" reason="Adjacent" />
+ <disable_collisions link1="RH_ADAPTER" link2="RH_SHANK" reason="Adjacent" />
+
+ <disable_collisions link1="base" link2="imu_link" reason="Adjacent" />
+
+ <!-- Need to check these 4 ? there may be auto-collisions ... -->
+ <disable_collisions link1="base" link2="LF_THIGH" reason="Adjacent" />
+ <disable_collisions link1="base" link2="LH_THIGH" reason="Adjacent" />
+ <disable_collisions link1="base" link2="RF_THIGH" reason="Adjacent" />
+ <disable_collisions link1="base" link2="RH_THIGH" reason="Adjacent" />
+
+
+
+</robot>
diff --git a/kinova_description/robots/kinova.urdf b/kinova_description/robots/kinova.urdf
index e15c7d3..b148e4b 100644
--- a/kinova_description/robots/kinova.urdf
+++ b/kinova_description/robots/kinova.urdf
@@ -4,7 +4,7 @@
<!-- | EDITING THIS FILE BY HAND IS NOT RECOMMENDED | -->
<!-- =================================================================================== -->
<!-- This file contains the description of the ANYmal Bedi robot. -->
-<robot name="anymal"
+<robot name="kinova"
xmlns:xacro="http://www.ros.org/wiki/xacro">
<material name="black">
<color rgba="0.0 0.0 0.0 1.0"/>
diff --git a/python/example_robot_data/__main__.py b/python/example_robot_data/__main__.py
index a8d0e4e..81ddda1 100644
--- a/python/example_robot_data/__main__.py
+++ b/python/example_robot_data/__main__.py
@@ -3,7 +3,7 @@ from argparse import ArgumentParser
from . import robots_loader
ROBOTS = [
- 'anymal', 'hyq', 'solo', 'solo12', 'talos', 'talos_arm', 'talos_legs', 'kinova','tiago', 'tiago_no_hand', 'icub', 'ur5'
+ 'anymal', 'anymal_kinova', 'hyq', 'solo', 'solo12', 'talos', 'talos_arm', 'talos_legs', 'kinova','tiago', 'tiago_no_hand', 'icub', 'ur5'
]
parser = ArgumentParser()
@@ -16,6 +16,11 @@ if args.robot == 'anymal':
anymal.initViewer(loadModel=True)
anymal.display(anymal.q0)
+elif args.robot == 'anymal_kinova':
+ anymal = robots_loader.loadANYmal(withArm='kinova')
+ anymal.initViewer(loadModel=True)
+ anymal.display(anymal.q0)
+
elif args.robot == 'hyq':
hyq = robots_loader.loadHyQ()
hyq.initViewer(loadModel=True)
diff --git a/python/example_robot_data/robots_loader.py b/python/example_robot_data/robots_loader.py
index b4c9c07..26ea8b3 100644
--- a/python/example_robot_data/robots_loader.py
+++ b/python/example_robot_data/robots_loader.py
@@ -44,10 +44,14 @@ def addFreeFlyerJointLimits(robot):
rmodel.lowerPositionLimit = lb
-def loadANYmal():
- URDF_FILENAME = "anymal.urdf"
+def loadANYmal(withArm=None):
+ if withArm is None:
+ URDF_FILENAME = "anymal.urdf"
+ SRDF_FILENAME = "anymal.srdf"
+ elif withArm is "kinova":
+ URDF_FILENAME = "anymal-kinova.urdf"
+ SRDF_FILENAME = "anymal-kinova.srdf"
URDF_SUBPATH = "/anymal_b_simple_description/robots/" + URDF_FILENAME
- SRDF_FILENAME = "anymal.srdf"
SRDF_SUBPATH = "/anymal_b_simple_description/srdf/" + SRDF_FILENAME
modelPath = getModelPath(URDF_SUBPATH)
# Load URDF file
--
GitLab