[Ninja] install a joint between baselink and docking arm

sugihara-16 · Jan 1, 2024 · b1ed6c0 · b1ed6c0
1 parent 7c85f30
commit b1ed6c0
Show file tree

Hide file tree

Showing 5 changed files with 90 additions and 45 deletions.
diff --git a/robots/ninja/config/Servo.yaml b/robots/ninja/config/Servo.yaml
@@ -1,7 +1,24 @@
 servo_controller:
+  state_sub_topic: servo/states
+  ctrl_pub_topic: servo/target_states
+  torque_pub_topic: servo/torque_enable
+
+  joints:
+    angle_sgn: 1
+    angle_scale: 0.00076699
+    zero_point_offset: 2047
+    torque_scale: 0.03483 # convert to N*m
+
+    simulation:  # you can specify for each controller
+      type: effort_controllers/JointPositionController
+      pid: {p: 5.0, i: 0.1, d: 0.1, i_clamp_max: 2.0, i_clamp_min: -2.0}
+      init_value: 0
+
+    controller1:
+      id: 0
+      name: docking_arm_joint
+
   gimbals:
-    ctrl_pub_topic: servo/target_states
-    torque_pub_topic: servo/torque_enable
     angle_sgn: -1
     angle_scale: 0.001534
     zero_point_offset: 2047

diff --git a/robots/ninja/robots/ninja.gazebo.xacro b/robots/ninja/robots/ninja.gazebo.xacro
@@ -1,9 +1,9 @@
 <?xml version="1.0"?>
 <robot
-    xmlns:xacro="http://www.ros.org/wiki/xacro" name="beetle" >
-  <xacro:arg name="robot_name" default="beetle" />
+    xmlns:xacro="http://www.ros.org/wiki/xacro" name="ninja" >
+  <xacro:arg name="robot_name" default="ninja" />
 
-  <xacro:include filename="$(find beetle)/robots/beetle.urdf.xacro" />
+  <xacro:include filename="$(find ninja)/robots/ninja.urdf.xacro" />
 
   <!-- gazebo plugin for default controller and sensors -->
   <xacro:include filename="$(find aerial_robot_simulation)/xacro/spinal.gazebo.xacro" />

diff --git a/robots/ninja/robots/ninja.urdf.xacro b/robots/ninja/robots/ninja.urdf.xacro
@@ -1,38 +1,23 @@
 <?xml version="1.0"?>
 <robot
-    xmlns:xacro="http://www.ros.org/wiki/xacro" name="beetle" >
+    xmlns:xacro="http://www.ros.org/wiki/xacro" name="ninja" >
 
-  <xacro:include filename="$(find beetle)/urdf/beetle.urdf.xacro" />
-  <xacro:include filename="$(find beetle)/urdf/common.xacro" />
+  <xacro:include filename="$(find ninja)/urdf/ninja.urdf.xacro" />
+  <xacro:include filename="$(find ninja)/urdf/common.xacro" />
 
-  <xacro:beetle_center_link child1="1" child2="2" child3="3" child4="4"/>
-  <xacro:beetle_thrust_link self="1" rotor_direction="-1"/>
-  <xacro:beetle_thrust_link self="2" rotor_direction="1"/>
-  <xacro:beetle_thrust_link self="3" rotor_direction="-1"/>
-  <xacro:beetle_thrust_link self="4" rotor_direction="1"/>
+  <xacro:ninja_center_link child1="1" child2="2" child3="3" child4="4"/>
+  <xacro:ninja_thrust_link self="1" rotor_direction="-1"/>
+  <xacro:ninja_thrust_link self="2" rotor_direction="1"/>
+  <xacro:ninja_thrust_link self="3" rotor_direction="-1"/>
+  <xacro:ninja_thrust_link self="4" rotor_direction="1"/>
 
 
   ##### dummy grasp links ####
-  <joint name="base_dummy_arm_joint" type="fixed">
-    <parent link="base_link"/>
-    <child link="dummy_arm"/>
-    <origin xyz="0.22 0 0.1455" rpy="0 0 0"/>
-  </joint>
-
-  <link name="dummy_arm">
-    <inertial>
-      <mass value = "0.01" />
-      <origin xyz="0.05 0 0" rpy="0 0 0"/>
-      <inertia
-          ixx="0.0001" ixy="0.0000" ixz="0.0000"
-          iyy="0.0001" iyz="0.0000" izz="0.0001"/>
-    </inertial>
-  </link>
 
   <joint name="dummy_arm_palm_joint" type="revolute">
-    <parent link="dummy_arm"/>
+    <parent link="docking_arm"/>
     <child link="dummy_palm"/>
-    <origin xyz="0.02 0 0" rpy="0 0 0"/>
+    <origin xyz="0.025 0 0" rpy="0 0 0"/>
     <axis xyz="1 0 0"/>
     <limit lower="0" upper="0" effort="0" velocity="0"/>
     <dynamics damping="0.2" friction="0.02"/>
@@ -57,7 +42,7 @@
   <joint name="dummy_palm_gripper_joint" type="revolute">
     <parent link="dummy_palm"/>
     <child link="dummy_gripper"/>
-    <origin xyz="0 0 0" rpy="0 0 0"/>
+    <origin xyz="0.01 0 0" rpy="0 0 0"/>
     <axis xyz="1 0 0"/>
     <limit lower="0" upper="0" effort="0" velocity="0"/>
     <dynamics damping="0.2" friction="0.02"/>
@@ -75,13 +60,13 @@
     <collision>
       <origin xyz="0 0 0" rpy="0 0 0"/>
       <geometry>
-        <box size="0.04 0.04 0.04"/>
+        <box size="0.02 0.1 0.1"/>
       </geometry>
     </collision>
     <visual>
       <origin xyz="0 0 0" rpy="0.0 0 0"/>
       <geometry>
-        <box size="0.04 0.04 0.04"/>
+        <box size="0.02 0.1 0.1"/>
       </geometry>
     </visual>
   </link>

diff --git a/robots/ninja/urdf/common.xacro b/robots/ninja/urdf/common.xacro
@@ -1,6 +1,6 @@
 <?xml version="1.0"?>
 <robot
-    xmlns:xacro="http://www.ros.org/wiki/xacro" name="beetle_common" >
+    xmlns:xacro="http://www.ros.org/wiki/xacro" name="ninja_common" >
 
   <!-- general attribute -->
   <baselink name="fc" />

diff --git a/robots/ninja/urdf/ninja.urdf.xacro b/robots/ninja/urdf/ninja.urdf.xacro
@@ -1,8 +1,8 @@
 <?xml version="1.0"?>
 <robot
-    xmlns:xacro="http://www.ros.org/wiki/xacro" name="beetle_link" >
+    xmlns:xacro="http://www.ros.org/wiki/xacro" name="ninja_link" >
 
-  <xacro:macro name="beetle_center_link" params="child1 child2 child3 child4">
+  <xacro:macro name="ninja_center_link" params="child1 child2 child3 child4">
     ## dummy
     <link name="root">
       <inertial>
@@ -33,13 +33,13 @@
       <visual>
         <origin xyz="0 0 0" rpy="0 0 3.141592"/>
         <geometry>
-          <mesh filename="package://beetle/urdf/mesh/base_link.dae"/>
+          <box size="0.52 0.05 0.05"/>
         </geometry>
       </visual>
       <collision>
         <origin xyz="0 0 0.1205" rpy="0 0 0"/>
         <geometry>
-          <box size="0.52 0.52 0.241"/>
+          <box size="0.52 0.05 0.05"/>
         </geometry>
       </collision>
     </link>
@@ -86,10 +86,10 @@
     </xacro:macro>
 
     ## dummy rotor arms
-    <xacro:dummy_rotor_arm id = "${child1}" x = "0.12129" y="0.12150" z = "0.1185" yaw = "${pi/4.0}"/>
-    <xacro:dummy_rotor_arm id = "${child2}" x = "-0.12129" y="0.12150" z = "0.1185" yaw = "${pi*3.0/4.0}"/>
-    <xacro:dummy_rotor_arm id = "${child3}" x = "-0.12129" y="-0.12150" z = "0.1185" yaw = "${pi*5.0/4.0}"/>
-    <xacro:dummy_rotor_arm id = "${child4}" x = "0.12129" y="-0.12150" z = "0.1185" yaw = "${-pi/4.0}"/>
+    <xacro:dummy_rotor_arm id = "${child1}" x = "0.12129" y="0.12150" z = "0" yaw = "${pi/4.0}"/>
+    <xacro:dummy_rotor_arm id = "${child2}" x = "-0.12129" y="0.12150" z = "0" yaw = "${pi*3.0/4.0}"/>
+    <xacro:dummy_rotor_arm id = "${child3}" x = "-0.12129" y="-0.12150" z = "0" yaw = "${pi*5.0/4.0}"/>
+    <xacro:dummy_rotor_arm id = "${child4}" x = "0.12129" y="-0.12150" z = "0" yaw = "${-pi/4.0}"/>
 
     ## dummy link for the contact point coordinate
       <link name="contact_point">
@@ -112,7 +112,7 @@
     <xacro:friction self="base_link"/>
   </xacro:macro>
 
-  <xacro:macro name="beetle_thrust_link" params="self rotor_direction">
+  <xacro:macro name="ninja_thrust_link" params="self rotor_direction">
     ## link
     #### rotor parent link (virtual) ####
     <link name="gimbal_link${self}">
@@ -126,13 +126,13 @@
       <collision>
         <origin xyz="0.0 0 0" rpy="0 0 0"/>
         <geometry>
-          <cylinder radius="0.02" length="0.033"/>
+          <cylinder radius="0.05" length="0.033"/>
         </geometry>
       </collision>
       <visual>
         <origin xyz="0 0 0" rpy="0 0 0"/>
         <geometry>
-          <mesh filename="package://beetle/urdf/mesh/rotor_module.dae"/>
+          <cylinder radius="0.05" length="0.033"/>
         </geometry>
       </visual>
     </link>
@@ -214,4 +214,47 @@
     </transmission>
 
   </xacro:macro>
+
+  <joint name="docking_arm_joint" type="revolute">
+    <parent link="base_link"/>
+    <child link="docking_arm"/>
+    <origin xyz="${0.26 + 0.025} 0 0" rpy="0 0 0"/>
+    <axis xyz="0 0 1"/>
+    <limit lower="${-pi/2}" upper="${pi/2}" effort="10.0" velocity="0.5"/>
+    <dynamics damping="0.01" friction="0.0"/>
+  </joint>
+
+  <link name="docking_arm">
+    <inertial>
+      <mass value = "0.1" />
+      <origin xyz="0.0 0 0" rpy="0 0 0"/>
+      <inertia
+          ixx="0.00017" ixy="0.0000" ixz="0.0000"
+          iyy="0.00017" iyz="0.0000"
+          izz="0.00017"/>
+    </inertial>
+    <collision>
+      <origin xyz="0.0 0 0" rpy="0 0 0"/>
+      <geometry>
+        <cylinder radius="0.025" length="0.05"/>
+      </geometry>
+    </collision>
+    <visual>
+      <origin xyz="0.0 0 0" rpy="0 0 0"/>
+      <geometry>
+        <cylinder radius="0.025" length="0.05"/>
+      </geometry>
+    </visual>
+  </link>
+
+  <transmission name="docking_joint">
+    <type>transmission_interface/SimpleTransmission</type>
+    <joint name="docking_arm_joint">
+      <hardwareInterface>hardware_interface/EffortJointInterface</hardwareInterface>
+    </joint>
+    <actuator name="docking_servo">
+      <hardwareInterface>hardware_interface/EffortJointInterface</hardwareInterface>
+      <mechanicalReduction>1</mechanicalReduction>
+    </actuator>
+  </transmission>
 </robot>