diff --git a/franka_control/config/panda_gazebo_control.yaml b/franka_control/config/panda_gazebo_control.yaml
new file mode 100644
index 0000000000000000000000000000000000000000..2039c945c3d6b6967dd0686658c4a8be68476ca8
--- /dev/null
+++ b/franka_control/config/panda_gazebo_control.yaml
@@ -0,0 +1,40 @@
+joint_state_controller:
+ type: joint_state_controller/JointStateController
+ publish_rate: 50
+
+position_joint_trajectory_controller:
+ type: effort_controllers/JointTrajectoryController
+ joints:
+ - panda_joint1
+ - panda_joint2
+ - panda_joint3
+ - panda_joint4
+ - panda_joint5
+ - panda_joint6
+ - panda_joint7
+
+ gains:
+ panda_joint1: { p: 12000, d: 50, i: 0.0, i_clamp: 10000 }
+ panda_joint2: { p: 30000, d: 100, i: 0.02, i_clamp: 10000 }
+ panda_joint3: { p: 18000, d: 50, i: 0.01, i_clamp: 1 }
+ panda_joint4: { p: 18000, d: 70, i: 0.01, i_clamp: 10000 }
+ panda_joint5: { p: 12000, d: 70, i: 0.01, i_clamp: 1 }
+ panda_joint6: { p: 7000, d: 50, i: 0.01, i_clamp: 1 }
+ panda_joint7: { p: 2000, d: 20, i: 0.0, i_clamp: 1 }
+
+ constraints:
+ goal_time: 2.0
+
+ state_publish_rate: 25
+
+franka_gripper:
+ type: effort_controllers/JointTrajectoryController
+ joints:
+ - panda_finger_joint1
+ - panda_finger_joint2
+
+ gains:
+ panda_finger_joint1: { p: 5, d: 3.0, i: 0, i_clamp: 1 }
+ panda_finger_joint2: { p: 5, d: 1.0, i: 0, i_clamp: 1 }
+
+ state_publish_rate: 25
diff --git a/franka_description/robots/hand.xacro b/franka_description/robots/hand.xacro
index 1cd3777e10cb268a4a9d0c6998b9cefccabb30b8..7f83ae4f066370572153410dcfad4a94b7f7b7bd 100644
--- a/franka_description/robots/hand.xacro
+++ b/franka_description/robots/hand.xacro
@@ -19,6 +19,14 @@
<mesh filename="package://franka_description/meshes/collision/hand.stl"/>
</geometry>
</collision>
+
+ <!-- for simulation -->
+ <inertial>
+ <origin xyz="0 0 0" rpy="0 0 0" />
+ <mass value="0.68" />
+ <inertia ixx="0.1" ixy="0.0" ixz="0.0" iyy="0.1" iyz="0.0" izz="0.1" />
+ </inertial>
+ <!-- end for simulation -->
</link>
<link name="${ns}_leftfinger">
<visual>
@@ -31,6 +39,14 @@
<mesh filename="package://franka_description/meshes/collision/finger.stl"/>
</geometry>
</collision>
+
+ <!-- for simulation -->
+ <inertial>
+ <origin xyz="0 0 0" rpy="0 0 0" />
+ <mass value="0.01" />
+ <inertia ixx="0.1" ixy="0.0" ixz="0.0" iyy="0.1" iyz="0.0" izz="0.1" />
+ </inertial>
+ <!-- end for simulation -->
</link>
<link name="${ns}_rightfinger">
<visual>
@@ -45,6 +61,14 @@
<mesh filename="package://franka_description/meshes/collision/finger.stl"/>
</geometry>
</collision>
+
+ <!-- for simulation -->
+ <inertial>
+ <origin xyz="0 0 0" rpy="0 0 0" />
+ <mass value="0.01" />
+ <inertia ixx="0.1" ixy="0.0" ixz="0.0" iyy="0.1" iyz="0.0" izz="0.1" />
+ </inertial>
+ <!-- end for simulation -->
</link>
<joint name="${ns}_finger_joint1" type="prismatic">
<parent link="${ns}_hand"/>
diff --git a/franka_description/robots/panda.gazebo.xacro b/franka_description/robots/panda.gazebo.xacro
new file mode 100644
index 0000000000000000000000000000000000000000..02822caa5688d544fb76b743dcfd9b7e6685facd
--- /dev/null
+++ b/franka_description/robots/panda.gazebo.xacro
@@ -0,0 +1,94 @@
+<?xml version="1.0"?>
+
+<robot xmlns:xacro="http://www.ros.org/wiki/xacro">
+
+ <xacro:macro name="panda_gazebo" params="robot_name">
+
+ <!-- Link0 -->
+ <gazebo reference="${robot_name}_link0">
+ <material>Gazebo/Grey</material>
+ <mu1>0.2</mu1>
+ <mu2>0.2</mu2>
+ </gazebo>
+
+ <!-- Link1 -->
+ <gazebo reference="${robot_name}_link1">
+ <material>Gazebo/White</material>
+ <mu1>0.2</mu1>
+ <mu2>0.2</mu2>
+ </gazebo>
+
+ <!-- Link2 -->
+ <gazebo reference="${robot_name}_link2">
+ <material>Gazebo/White</material>
+ <mu1>0.2</mu1>
+ <mu2>0.2</mu2>
+ </gazebo>
+
+ <!-- Link3 -->
+ <gazebo reference="${robot_name}_link3">
+ <material>Gazebo/White</material>
+ <mu1>0.2</mu1>
+ <mu2>0.2</mu2>
+ </gazebo>
+
+ <!-- Link4 -->
+ <gazebo reference="${robot_name}_link4">
+ <material>Gazebo/White</material>
+ <mu1>0.2</mu1>
+ <mu2>0.2</mu2>
+ </gazebo>
+
+ <!-- Link5 -->
+ <gazebo reference="${robot_name}_link5">
+ <material>Gazebo/White</material>
+ <mu1>0.2</mu1>
+ <mu2>0.2</mu2>
+ </gazebo>
+
+ <!-- Link6 -->
+ <gazebo reference="${robot_name}_link6">
+ <material>Gazebo/White</material>
+ <mu1>0.2</mu1>
+ <mu2>0.2</mu2>
+ </gazebo>
+
+ <!-- Link7 -->
+ <gazebo reference="${robot_name}_link7">
+ <material>Gazebo/Grey</material>
+ <mu1>0.2</mu1>
+ <mu2>0.2</mu2>
+ </gazebo>
+
+ <!-- Link8 -->
+ <gazebo reference="${robot_name}_link8">
+ <material>Gazebo/Grey</material>
+ <mu1>0.2</mu1>
+ <mu2>0.2</mu2>
+ </gazebo>
+
+ <!-- LinkHand -->
+ <gazebo reference="${robot_name}_hand">
+ <material>Gazebo/Grey</material>
+ <mu1>0.2</mu1>
+ <mu2>0.2</mu2>
+ </gazebo>
+
+ <!-- LinkRightFinger -->
+ <gazebo reference="${robot_name}_rightfinger">
+ <material>Gazebo/Grey</material>
+ <mu1>0.2</mu1>
+ <mu2>0.2</mu2>
+ </gazebo>
+
+ <!-- LinkLeftFinger -->
+ <gazebo reference="${robot_name}_leftfinger">
+ <material>Gazebo/Grey</material>
+ <mu1>0.2</mu1>
+ <mu2>0.2</mu2>
+ </gazebo>
+
+ </xacro:macro>
+
+</robot>
+
diff --git a/franka_description/robots/panda.transmission.xacro b/franka_description/robots/panda.transmission.xacro
new file mode 100644
index 0000000000000000000000000000000000000000..343bd7ad7415c1151f4a39f29436677ae978543a
--- /dev/null
+++ b/franka_description/robots/panda.transmission.xacro
@@ -0,0 +1,112 @@
+<?xml version="1.0"?>
+<robot xmlns:xacro="http://www.ros.org/wiki/xacro">
+ <xacro:macro name="panda_transmission" params="robot_name">
+
+
+ <!-- Load Gazebo lib and set the robot namespace -->
+ <gazebo>
+ <plugin name="gazebo_ros_control" filename="libgazebo_ros_control.so">
+ <!-- <robotNamespace>/${robot_name}</robotNamespace> -->
+ </plugin>
+ </gazebo>
+
+ <transmission name="${robot_name}_tran_1">
+ <type>transmission_interface/SimpleTransmission</type>
+ <joint name="${robot_name}_joint1">
+ <hardwareInterface>hardware_interface/EffortJointInterface</hardwareInterface>
+ </joint>
+ <actuator name="${robot_name}_motor_1">
+ <hardwareInterface>hardware_interface/EffortJointInterface</hardwareInterface>
+ <mechanicalReduction>1</mechanicalReduction>
+ </actuator>
+ </transmission>
+
+ <transmission name="${robot_name}_tran_2">
+ <type>transmission_interface/SimpleTransmission</type>
+ <joint name="${robot_name}_joint2">
+ <hardwareInterface>hardware_interface/EffortJointInterface</hardwareInterface>
+ </joint>
+ <actuator name="${robot_name}_motor_2">
+ <hardwareInterface>hardware_interface/EffortJointInterface</hardwareInterface>
+ <mechanicalReduction>1</mechanicalReduction>
+ </actuator>
+ </transmission>
+
+ <transmission name="${robot_name}_tran_3">
+ <type>transmission_interface/SimpleTransmission</type>
+ <joint name="${robot_name}_joint3">
+ <hardwareInterface>hardware_interface/EffortJointInterface</hardwareInterface>
+ </joint>
+ <actuator name="${robot_name}_motor_3">
+ <hardwareInterface>hardware_interface/EffortJointInterface</hardwareInterface>
+ <mechanicalReduction>1</mechanicalReduction>
+ </actuator>
+ </transmission>
+
+ <transmission name="${robot_name}_tran_4">
+ <type>transmission_interface/SimpleTransmission</type>
+ <joint name="${robot_name}_joint4">
+ <hardwareInterface>hardware_interface/EffortJointInterface</hardwareInterface>
+ </joint>
+ <actuator name="${robot_name}_motor_4">
+ <hardwareInterface>hardware_interface/EffortJointInterface</hardwareInterface>
+ <mechanicalReduction>1</mechanicalReduction>
+ </actuator>
+ </transmission>
+
+ <transmission name="${robot_name}_tran_5">
+ <type>transmission_interface/SimpleTransmission</type>
+ <joint name="${robot_name}_joint5">
+ <hardwareInterface>hardware_interface/EffortJointInterface</hardwareInterface>
+ </joint>
+ <actuator name="${robot_name}_motor_5">
+ <hardwareInterface>hardware_interface/EffortJointInterface</hardwareInterface>
+ <mechanicalReduction>1</mechanicalReduction>
+ </actuator>
+ </transmission>
+
+ <transmission name="${robot_name}_tran_6">
+ <type>transmission_interface/SimpleTransmission</type>
+ <joint name="${robot_name}_joint6">
+ <hardwareInterface>hardware_interface/EffortJointInterface</hardwareInterface>
+ </joint>
+ <actuator name="${robot_name}_motor_6">
+ <hardwareInterface>hardware_interface/EffortJointInterface</hardwareInterface>
+ <mechanicalReduction>1</mechanicalReduction>
+ </actuator>
+ </transmission>
+
+ <transmission name="${robot_name}_tran_7">
+ <type>transmission_interface/SimpleTransmission</type>
+ <joint name="${robot_name}_joint7">
+ <hardwareInterface>hardware_interface/EffortJointInterface</hardwareInterface>
+ </joint>
+ <actuator name="${robot_name}_motor_7">
+ <hardwareInterface>hardware_interface/EffortJointInterface</hardwareInterface>
+ <mechanicalReduction>1</mechanicalReduction>
+ </actuator>
+ </transmission>
+
+ <transmission name="${robot_name}_leftfinger">
+ <type>transmission_interface/SimpleTransmission</type>
+ <joint name="${robot_name}_finger_joint1">
+ <hardwareInterface>hardware_interface/EffortJointInterface</hardwareInterface>
+ </joint>
+ <actuator name="${robot_name}_finger_joint1">
+ <hardwareInterface>hardware_interface/EffortJointInterface</hardwareInterface>
+ <mechanicalReduction>1</mechanicalReduction>
+ </actuator>
+ </transmission>
+
+ <transmission name="${robot_name}_rightfinger">
+ <type>transmission_interface/SimpleTransmission</type>
+ <joint name="${robot_name}_finger_joint2">
+ <hardwareInterface>hardware_interface/EffortJointInterface</hardwareInterface>
+ </joint>
+ <actuator name="${robot_name}_finger_joint2">
+ <hardwareInterface>hardware_interface/EffortJointInterface</hardwareInterface>
+ <mechanicalReduction>1</mechanicalReduction>
+ </actuator>
+ </transmission>
+ </xacro:macro>
+</robot>
diff --git a/franka_description/robots/panda_arm.xacro b/franka_description/robots/panda_arm.xacro
index 672c881caa3ce08797ee330a8563dc6fcef4a89d..011b72e77452a50d9358b2d157ef2de827ff75ce 100644
--- a/franka_description/robots/panda_arm.xacro
+++ b/franka_description/robots/panda_arm.xacro
@@ -8,6 +8,19 @@
<origin rpy="${rpy}" xyz="${xyz}"/>
</joint>
</xacro:unless>
+
+ <!-- for simulation -->
+ <link name="world" />
+
+ <joint name="robot_to_world" type="fixed">
+ <parent link="world" />
+ <child link="${arm_id}_link0" />
+ <origin xyz="0.0 0.0 0.0" rpy="0.0 0.0 0.0" />
+ </joint>
+
+ <xacro:property name="joint_damping" value="1.0"/>
+ <!-- end for simulation -->
+
<link name="${arm_id}_link0">
<visual>
<geometry>
@@ -19,6 +32,14 @@
<mesh filename="package://${description_pkg}/meshes/collision/link0.stl"/>
</geometry>
</collision>
+
+ <!-- for simulation -->
+ <inertial>
+ <origin xyz="0 0 0" rpy="0 0 0" />
+ <mass value="3.06" />
+ <inertia ixx="0.3" ixy="0.0" ixz="0.0" iyy="0.3" iyz="0.0" izz="0.3" />
+ </inertial>
+ <!-- end for simulation -->
</link>
<link name="${arm_id}_link1">
<visual>
@@ -31,6 +52,14 @@
<mesh filename="package://${description_pkg}/meshes/collision/link1.stl"/>
</geometry>
</collision>
+
+ <!-- for simulation -->
+ <inertial>
+ <origin xyz="0 0 0" rpy="0 0 0" />
+ <mass value="2.34" />
+ <inertia ixx="0.3" ixy="0.0" ixz="0.0" iyy="0.3" iyz="0.0" izz="0.3" />
+ </inertial>
+ <!-- end for simulation -->
</link>
<joint name="${arm_id}_joint1" type="revolute">
<safety_controller k_position="100.0" k_velocity="40.0" soft_lower_limit="-2.8973" soft_upper_limit="2.8973"/>
@@ -39,6 +68,10 @@
<child link="${arm_id}_link1"/>
<axis xyz="0 0 1"/>
<limit effort="87" lower="-2.8973" upper="2.8973" velocity="2.1750"/>
+
+ <!-- for simulation -->
+ <dynamics damping="${joint_damping}"/>
+ <!-- end for simulation -->
</joint>
<link name="${arm_id}_link2">
<visual>
@@ -51,6 +84,14 @@
<mesh filename="package://${description_pkg}/meshes/collision/link2.stl"/>
</geometry>
</collision>
+
+ <!-- for simulation -->
+ <inertial>
+ <origin xyz="0 0 0" rpy="0 0 0" />
+ <mass value="2.36" />
+ <inertia ixx="0.3" ixy="0.0" ixz="0.0" iyy="0.3" iyz="0.0" izz="0.3" />
+ </inertial>
+ <!-- end for simulation -->
</link>
<joint name="${arm_id}_joint2" type="revolute">
<safety_controller k_position="100.0" k_velocity="40.0" soft_lower_limit="-1.7628" soft_upper_limit="1.7628"/>
@@ -59,6 +100,10 @@
<child link="${arm_id}_link2"/>
<axis xyz="0 0 1"/>
<limit effort="87" lower="-1.7628" upper="1.7628" velocity="2.1750"/>
+
+ <!-- for simulation -->
+ <dynamics damping="${joint_damping}"/>
+ <!-- end for simulation -->
</joint>
<link name="${arm_id}_link3">
<visual>
@@ -71,6 +116,14 @@
<mesh filename="package://${description_pkg}/meshes/collision/link3.stl"/>
</geometry>
</collision>
+
+ <!-- for simulation -->
+ <inertial>
+ <origin xyz="0 0 0" rpy="0 0 0" />
+ <mass value="2.38" />
+ <inertia ixx="0.3" ixy="0.0" ixz="0.0" iyy="0.3" iyz="0.0" izz="0.3" />
+ </inertial>
+ <!-- end for simulation -->
</link>
<joint name="${arm_id}_joint3" type="revolute">
<safety_controller k_position="100.0" k_velocity="40.0" soft_lower_limit="-2.8973" soft_upper_limit="2.8973"/>
@@ -79,6 +132,10 @@
<child link="${arm_id}_link3"/>
<axis xyz="0 0 1"/>
<limit effort="87" lower="-2.8973" upper="2.8973" velocity="2.1750"/>
+
+ <!-- for simulation -->
+ <dynamics damping="${joint_damping}"/>
+ <!-- end for simulation -->
</joint>
<link name="${arm_id}_link4">
<visual>
@@ -91,6 +148,14 @@
<mesh filename="package://${description_pkg}/meshes/collision/link4.stl"/>
</geometry>
</collision>
+
+ <!-- for simulation -->
+ <inertial>
+ <origin xyz="0 0 0" rpy="0 0 0" />
+ <mass value="2.43" />
+ <inertia ixx="0.3" ixy="0.0" ixz="0.0" iyy="0.3" iyz="0.0" izz="0.3" />
+ </inertial>
+ <!-- end for simulation -->
</link>
<joint name="${arm_id}_joint4" type="revolute">
<safety_controller k_position="100.0" k_velocity="40.0" soft_lower_limit="-3.0718" soft_upper_limit="-0.0698"/>
@@ -99,6 +164,10 @@
<child link="${arm_id}_link4"/>
<axis xyz="0 0 1"/>
<limit effort="87" lower="-3.0718" upper="-0.0698" velocity="2.1750"/>
+
+ <!-- for simulation -->
+ <dynamics damping="${joint_damping}"/>
+ <!-- end for simulation -->
</joint>
<link name="${arm_id}_link5">
<visual>
@@ -111,6 +180,14 @@
<mesh filename="package://${description_pkg}/meshes/collision/link5.stl"/>
</geometry>
</collision>
+
+ <!-- for simulation -->
+ <inertial>
+ <origin xyz="0 0 0" rpy="0 0 0" />
+ <mass value="3.5" />
+ <inertia ixx="0.3" ixy="0.0" ixz="0.0" iyy="0.3" iyz="0.0" izz="0.3" />
+ </inertial>
+ <!-- end for simulation -->
</link>
<joint name="${arm_id}_joint5" type="revolute">
<safety_controller k_position="100.0" k_velocity="40.0" soft_lower_limit="-2.8973" soft_upper_limit="2.8973"/>
@@ -119,6 +196,10 @@
<child link="${arm_id}_link5"/>
<axis xyz="0 0 1"/>
<limit effort="12" lower="-2.8973" upper="2.8973" velocity="2.6100"/>
+
+ <!-- for simulation -->
+ <dynamics damping="${joint_damping}"/>
+ <!-- end for simulation -->
</joint>
<link name="${arm_id}_link6">
<visual>
@@ -131,6 +212,14 @@
<mesh filename="package://${description_pkg}/meshes/collision/link6.stl"/>
</geometry>
</collision>
+
+ <!-- for simulation -->
+ <inertial>
+ <origin xyz="0 0 0" rpy="0 0 0" />
+ <mass value="1.47" />
+ <inertia ixx="0.3" ixy="0.0" ixz="0.0" iyy="0.3" iyz="0.0" izz="0.3" />
+ </inertial>
+ <!-- end for simulation -->
</link>
<joint name="${arm_id}_joint6" type="revolute">
<safety_controller k_position="100.0" k_velocity="40.0" soft_lower_limit="-0.0175" soft_upper_limit="3.7525"/>
@@ -139,6 +228,10 @@
<child link="${arm_id}_link6"/>
<axis xyz="0 0 1"/>
<limit effort="12" lower="-0.0175" upper="3.7525" velocity="2.6100"/>
+
+ <!-- for simulation -->
+ <dynamics damping="${joint_damping}"/>
+ <!-- end for simulation -->
</joint>
<link name="${arm_id}_link7">
<visual>
@@ -151,6 +244,14 @@
<mesh filename="package://${description_pkg}/meshes/collision/link7.stl"/>
</geometry>
</collision>
+
+ <!-- for simulation -->
+ <inertial>
+ <origin xyz="0 0 0" rpy="0 0 0" />
+ <mass value="0.45" />
+ <inertia ixx="0.3" ixy="0.0" ixz="0.0" iyy="0.3" iyz="0.0" izz="0.3" />
+ </inertial>
+ <!-- end for simulation -->
</link>
<joint name="${arm_id}_joint7" type="revolute">
<safety_controller k_position="100.0" k_velocity="40.0" soft_lower_limit="-2.8973" soft_upper_limit="2.8973"/>
@@ -159,8 +260,20 @@
<child link="${arm_id}_link7"/>
<axis xyz="0 0 1"/>
<limit effort="12" lower="-2.8973" upper="2.8973" velocity="2.6100"/>
+
+ <!-- for simulation -->
+ <dynamics damping="${joint_damping}"/>
+ <!-- end for simulation -->
</joint>
- <link name="${arm_id}_link8"/>
+ <link name="${arm_id}_link8">
+ <!-- for simulation -->
+ <inertial>
+ <origin xyz="0 0 0" rpy="0 0 0" />
+ <mass value="0.0" />
+ <inertia ixx="0.3" ixy="0.0" ixz="0.0" iyy="0.3" iyz="0.0" izz="0.3" />
+ </inertial>
+ <!-- end for simulation -->
+ </link>
<joint name="${arm_id}_joint8" type="fixed">
<origin rpy="0 0 0" xyz="0 0 0.107"/>
<parent link="${arm_id}_link7"/>
diff --git a/franka_description/robots/panda_arm_hand.urdf.xacro b/franka_description/robots/panda_arm_hand.urdf.xacro
index 4c0f2fcea82ddc1e99dcd85a760a9ede37313b3d..e8e72fdce85aa40efa1cf6e9cae28b2fd81d17ca 100644
--- a/franka_description/robots/panda_arm_hand.urdf.xacro
+++ b/franka_description/robots/panda_arm_hand.urdf.xacro
@@ -2,6 +2,18 @@
<robot xmlns:xacro="http://www.ros.org/wiki/xacro" name="panda">
<xacro:include filename="$(find franka_description)/robots/panda_arm.xacro"/>
<xacro:include filename="$(find franka_description)/robots/hand.xacro"/>
+
+ <!-- for simulation -->
+ <xacro:include filename="$(find franka_description)/robots/panda.gazebo.xacro"/>
+ <xacro:include filename="$(find franka_description)/robots/panda.transmission.xacro"/>
+ <!-- end for simulation -->
+
<xacro:panda_arm />
<xacro:hand ns="panda" rpy="0 0 ${-pi/4}" connected_to="panda_link8"/>
+
+ <!-- for simulation -->
+ <xacro:arg name="robot_name" default="panda"/>
+ <xacro:panda_gazebo robot_name="$(arg robot_name)" />
+ <xacro:panda_transmission robot_name="$(arg robot_name)" />
+ <!-- end for simulation -->
</robot>