Updated pi_tracker to use newer API for dynamixel_motor packages

pirobot · Jan 21, 2014 · 28b2b8f · 28b2b8f
1 parent bf7c315
commit 28b2b8f
Show file tree

Hide file tree

Showing 39 changed files with 2,604 additions and 863 deletions.
diff --git a/launch/skeleton.launch b/launch/skeleton.launch
@@ -1,11 +1,13 @@
 <launch>
   <arg name="fixed_frame" value="camera_depth_frame" />
+
+  <param name="/use_sim_time" value="False" />
+
 
   <arg name="debug" value="False" />
   <arg name="launch_prefix" value="xterm -e gdb --args" />
 
-  <param name="robot_description" command="$(find xacro)/xacro.py '$(find pi_tracker)/urdf/pi_robot.urdf.xacro'" />
-  <param name="/use_sim_time" value="False" />
+  <param name="robot_description" command="$(find xacro)/xacro.py '$(find pi_tracker)/urdf/pi_robot/pi_robot_with_two_arms.xacro'" />
 
   <node name="robot_state_publisher" pkg="robot_state_publisher" type="state_publisher">
     <param name="publish_frequency" value="20.0"/>
@@ -14,7 +16,7 @@
   <include file="$(find rgbd_launch)/launch/kinect_frames.launch" />
 
   <group if="$(arg debug)">
-    <node launch-prefix="$(arg launch_prefix)" name="skeleton_tracker" pkg="skeleton_markers" type="skeleton_tracker" output="screen"	>
+    <node launch-prefix="$(arg launch_prefix)" pkg="skeleton_markers" name="skeleton_tracker" type="skeleton_tracker" output="screen"	>
       <param name="fixed_frame" value="$(arg fixed_frame)" />
       <param name="load_filepath" value="$(find pi_tracker)/params/SamplesConfigNewOpenNI.xml" />
     </node>

diff --git a/nodes/tracker_joint_controller.py b/nodes/tracker_joint_controller.py
diff --git a/params/SamplesConfig.xml b/params/SamplesConfig.xml
diff --git a/params/dynamixels_params.yaml b/params/dynamixels_params.yaml
@@ -1,6 +1,6 @@
-dynamixels: ['head_pan', 'head_tilt', 'left_shoulder_lift', 'left_shoulder_pan', 'left_arm_roll', 'left_elbow', 'left_forearm', 'left_wrist', 'right_shoulder_lift', 'right_shoulder_pan', 'right_arm_roll', 'right_elbow', 'right_forearm', 'right_wrist']
+joints: ['head_pan', 'head_tilt', 'left_shoulder_lift', 'left_shoulder_pan', 'left_arm_roll', 'left_elbow', 'left_forearm', 'left_wrist', 'right_shoulder_lift', 'right_shoulder_pan', 'right_arm_roll', 'right_elbow', 'right_forearm', 'right_wrist']
 
-head_pan_controller:
+head_pan_joint:
     controller:
         package: ax12_controller_core
         module: joint_position_controller
@@ -13,7 +13,7 @@ head_pan_controller:
         min: 0
         max: 1024
 
-head_tilt_controller:
+head_tilt_joint:
     controller:
         package: ax12_controller_core
         module: joint_position_controller
@@ -26,7 +26,7 @@ head_tilt_controller:
         min: 300
         max: 800
 
-right_shoulder_pan_controller:
+right_shoulder_pan_joint:
     controller:
         package: ax12_controller_core
         module: joint_position_controller
@@ -39,7 +39,7 @@ right_shoulder_pan_controller:
         min: 0
         max: 1023
 
-right_shoulder_lift_controller:
+right_shoulder_lift_joint:
     controller:
         package: ax12_controller_core
         module: joint_position_controller
@@ -52,7 +52,7 @@ right_shoulder_lift_controller:
         min: 0
         max: 1023
 
-right_arm_roll_controller:
+right_arm_roll_joint:
     controller:
         package: ax12_controller_core
         module: joint_position_controller
@@ -65,7 +65,7 @@ right_arm_roll_controller:
         min: 0
         max: 1023
 
-right_elbow_controller:
+right_elbow_joint:
     controller:
         package: ax12_controller_core
         module: joint_position_controller
@@ -78,7 +78,7 @@ right_elbow_controller:
         min: 0
         max: 1023
 
-right_forearm_controller:
+right_forearm_joint:
     controller:
         package: ax12_controller_core
         module: joint_position_controller
@@ -91,7 +91,7 @@ right_forearm_controller:
         min: 0
         max: 1023
 
-right_wrist_controller:
+right_wrist_joint:
     controller:
         package: ax12_controller_core
         module: joint_position_controller
@@ -104,7 +104,7 @@ right_wrist_controller:
         min: 0
         max: 1023
 
-left_shoulder_pan_controller:
+left_shoulder_pan_joint:
     controller:
         package: ax12_controller_core
         module: joint_position_controller
@@ -117,7 +117,7 @@ left_shoulder_pan_controller:
         min: 0
         max: 1023
 
-left_shoulder_lift_controller:
+left_shoulder_lift_joint:
     controller:
         package: ax12_controller_core
         module: joint_position_controller
@@ -130,7 +130,7 @@ left_shoulder_lift_controller:
         min: 0
         max: 1023
 
-left_arm_roll_controller:
+left_arm_roll_joint:
     controller:
         package: ax12_controller_core
         module: joint_position_controller
@@ -143,7 +143,7 @@ left_arm_roll_controller:
         min: 0
         max: 1023
 
-left_elbow_controller:
+left_elbow_joint:
     controller:
         package: ax12_controller_core
         module: joint_position_controller
@@ -156,7 +156,7 @@ left_elbow_controller:
         min: 0
         max: 1023
 
-left_forearm_controller:
+left_forearm_joint:
     controller:
         package: ax12_controller_core
         module: joint_position_controller
@@ -169,7 +169,7 @@ left_forearm_controller:
         min: 0
         max: 1023
 
-left_wrist_controller:
+left_wrist_joint:
     controller:
         package: ax12_controller_core
         module: joint_position_controller
@@ -182,16 +182,4 @@ left_wrist_controller:
         min: 0
         max: 1023
 
-#torso_controller:
-#    controller:
-#        package: ax12_controller_core
-#        module: joint_position_controller
-#        type: JointPositionControllerAX12
-#    joint_name: torso_joint
-#    joint_speed: 1.17
-#    motor:
-#        id: 11
-#        init: 512
-#        min: 0
-#        max: 1023
 
diff --git a/params/tracker_params.yaml b/params/tracker_params.yaml
@@ -7,7 +7,7 @@ max_drive_speed: 0.5
 max_rotation_speed: 0.5
 base_control_side: right
 
-use_real_robot: False
+use_real_robot: True
 
 # Use these values for a typical non-holonomic robot.
 scale_drive_speed: 1.0
@@ -23,16 +23,17 @@ holonomic: False
 
 fixed_frame: openni_depth_frame
 
+# Pedestal Pi
 skel_to_joint_map: {
-   head: head_pan_joint,
-   neck: head_tilt_joint,
-   torso: torso_joint,
-   left_shoulder: left_shoulder_lift_joint,
-   left_elbow: left_elbow_joint,
-   left_hand: left_hand_joint,
-   right_shoulder: right_shoulder_joint,
-   right_elbow: right_elbow_joint,
-   right_hand: right_hand_joint,
+   head: no_joint,
+   neck: no_joint,
+   torso: no_joint,
+   left_shoulder: no_joint,
+   left_elbow: no_joint,
+   left_hand: no_joint,
+   right_shoulder: right_arm_shoulder_lift_joint,
+   right_elbow: right_arm_elbow_flex_joint,
+   right_hand: right_arm_wrist_flex_joint,
    left_hip: no_joint,
    left_knee: no_joint,
    left_foot: no_joint,
@@ -41,3 +42,21 @@ skel_to_joint_map: {
    right_foot: no_joint
 }
 
+#skel_to_joint_map: {
+#   head: head_pan_joint,
+#   neck: head_tilt_joint,
+#   torso: torso_joint,
+#   left_shoulder: left_arm_shoulder_lift_joint,
+#   left_elbow: left_arm_elbow_flex_joint,
+#   left_hand: left_arm_wrist_flex_joint,
+#   right_shoulder: right_arm_shoulder_lift_joint,
+#   right_elbow: right_arm_elbow_flex_joint,
+#   right_hand: right_arm_wrist_flex_joint,
+#   left_hip: no_joint,
+#   left_knee: no_joint,
+#   left_foot: no_joint,
+#   right_hip: no_joint,
+#   right_knee: no_joint,
+#   right_foot: no_joint
+#}
+
diff --git a/src/KinectController.cpp b/src/KinectController.cpp