Modified bringup

talos_bringup/config/talos_motions.yaml

+play_motion:
+  controllers: [right_arm_controller, left_arm_controller, left_leg_controller, right_leg_controller,
+    head_controller, torso_controller]
+  motions:
+    hands_front:
+      joints: [arm_right_3_joint, arm_right_4_joint, arm_right_1_joint, arm_left_4_joint,
+        arm_left_1_joint, arm_left_2_joint, arm_right_2_joint, arm_left_3_joint]
+      points:
+      - positions: [-1.0, 1.65, 1.2, 1.65, 1.2, 0.3, 0.3, -1.0]
+        time_from_start: 0.0
+    hands_up:
+      joints: [torso_2_joint, arm_right_6_joint, arm_right_5_joint, arm_left_5_joint,
+        torso_1_joint, arm_left_4_joint, head_2_joint, arm_right_2_joint, arm_left_3_joint,
+        head_1_joint, arm_right_7_joint, arm_right_3_joint, arm_right_4_joint, arm_right_1_joint,
+        arm_left_1_joint, arm_left_2_joint]
+      points:
+      - positions: [0.0, 0.0, -0.5, -0.5, 0.0, 1.3, 0.34, 0.0, -1.6, 0.0, 0.0, -1.6,
+          1.4, 1.2, 0.9, 0.2]
+        time_from_start: 0.0
+    home: # NOTE: arm_*_2_joint is nonzero!
+      joints: [torso_1_joint, torso_2_joint, head_1_joint, head_2_joint, arm_left_1_joint,
+        arm_left_2_joint, arm_left_3_joint, arm_left_4_joint, arm_left_5_joint, arm_left_6_joint,
+        arm_left_7_joint, arm_right_1_joint, arm_right_2_joint, arm_right_3_joint,
+        arm_right_4_joint, arm_right_5_joint, arm_right_6_joint, arm_right_7_joint,
+        hand_left_thumb_joint, hand_left_index_joint, hand_left_middle_joint, hand_right_thumb_joint,
+        hand_right_index_joint, hand_right_middle_joint]
+      points:
+      - positions: [0.0, 0.0, 0.0, 0.0, 0.0, 0.1, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.1,
+          0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0]
+        time_from_start: 0.0
+      meta:
+        name: Home
+        usage: posture
+        description: Both arms down and completely stretched.
+    home_legs:
+      joints: [leg_left_1_joint, leg_left_2_joint, leg_left_3_joint, leg_left_4_joint,
+               leg_left_5_joint, leg_left_6_joint,
+               leg_right_1_joint, leg_right_2_joint, leg_right_3_joint,leg_right_4_joint,
+               leg_right_5_joint, leg_right_6_joint]
+      points:
+      - positions: [0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0]
+        time_from_start: 0.0
+    interact:
+      joints: [torso_1_joint, torso_2_joint, head_1_joint, head_2_joint, arm_left_1_joint,
+        arm_left_2_joint, arm_left_3_joint, arm_left_4_joint, arm_left_5_joint, arm_left_6_joint,
+        arm_left_7_joint, arm_right_1_joint, arm_right_2_joint, arm_right_3_joint,
+        arm_right_4_joint, arm_right_5_joint, arm_right_6_joint, arm_right_7_joint,
+        hand_left_thumb_joint, hand_left_index_joint, hand_left_middle_joint, hand_right_thumb_joint,
+        hand_right_index_joint, hand_right_middle_joint]
+      points:
+      - positions: [0.0, 0.0, 0.0, 0.0, -0.4, 0.25, -0.1, 0.6109, 0.0, 0.0, 0.0, -0.4,
+          0.25, -0.1, 0.6109, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0]
+        time_from_start: 0.0
+      meta:
+        name: Interact
+        usage: posture
+        description: Same posture as in Hands Down.
+    small_squat:
+      joints: [leg_left_4_joint, leg_right_3_joint, leg_right_5_joint, leg_left_5_joint,
+        leg_right_4_joint, leg_left_3_joint]
+      points:
+      - positions: [1.0, -0.5, -0.5, -0.5, 1.0, -0.5]
+        time_from_start: 0.0
+    test_motion:
+      joints: [torso_2_joint, arm_right_6_joint, arm_right_5_joint, arm_left_5_joint,
+        torso_1_joint, arm_left_4_joint, head_2_joint, arm_right_2_joint, arm_left_3_joint,
+        head_1_joint, arm_right_7_joint, arm_right_3_joint, arm_right_4_joint, arm_right_1_joint,
+        arm_left_1_joint, arm_left_2_joint]
+      points:
+      - positions: [0.0, 0.0, -0.5, -0.5, 0.0, 1.3, 0.34, 0.0, -1.6, 0.0, 0.0, -1.6,
+          1.4, 1.2, 0.9, 0.2]
+        time_from_start: 0.0
+      - positions: [0.0, 0.0, 0.0, 0.0, 0.0, 1.65, 0.0, 0.2, -1.0, 0.0, 0.0, -1.0,
+          1.65, 1.2, 1.2, 0.3]
+        velocities: [0.0, 0.0, 0.0, 0.0, 0.0, 0.5, 0.0, 0.5, 0.5, 0.0, 0.0, 0.5,
+          0.5, 0.5, 0.5, 0.5]
+        time_from_start: 3.0
+      - positions: [0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0,
+          0.0, 0.0, 0.0, 0.0]
+        time_from_start: 6.0
+    squat:
+      joints: [arm_left_7_joint, leg_left_1_joint, arm_right_6_joint, torso_2_joint,
+        arm_right_5_joint, arm_left_5_joint, arm_left_6_joint, arm_left_1_joint, torso_1_joint,
+        arm_left_4_joint, head_2_joint, leg_right_3_joint, leg_left_2_joint, arm_right_2_joint,
+        leg_left_6_joint, head_1_joint, leg_right_6_joint, arm_right_7_joint, arm_right_3_joint,
+        leg_left_5_joint, leg_right_4_joint, arm_right_4_joint, arm_left_3_joint,
+        leg_right_1_joint, leg_left_4_joint, leg_right_5_joint, arm_left_2_joint,
+        arm_right_1_joint, leg_right_2_joint, leg_left_3_joint]
+      points:
+      - positions: [0.0, 0.0, 0.0, -0.15, 0.0, 0.0, 0.0, 0.0, 0.0, 0.9, 0.25, -1.35,
+          0.0, 1.3, 0.0, 0.0, 0.0, 0.0, 0.0, -1.3, 2.6, 0.9, 0.0, 0.0, 2.6, -1.3,
+          1.3, 0.0, 0.0, -1.35]
+        time_from_start: 0.0

talos_bringup/launch/play_motion.launch

+<?xml version="1.0" encoding="UTF-8"?>
+
+<launch>
+  <arg name="robot" default="full_ft_hey5"/>
+
+  <!-- execute pre-recorded motions -->
+  <node pkg="talos_bringup" type="motions_loader.py" name="motions_loader" args="robot=$(arg robot)"/>
+  <node pkg="play_motion" type="play_motion" name="play_motion">
+    <rosparam file="$(find talos_bringup)/config/approach_planner_$(arg robot).yaml" command="load" />
+  </node>
+  <node pkg="play_motion" type="is_already_there.py" name="is_already_there" />
+</launch>

talos_bringup/launch/talos_bringup.launch

@@ -5,7 +5,7 @@
   <rosparam command="load" file="$(find talos_bringup)/config/talos_$(arg robot)_hardware.yaml" />
   
   <!-- Load default controllers -->
-  <include file="$(find talos_controller_configuration)/launch/talos_default_controllers.launch" />
+  <include file="$(find talos_controller_configuration)/launch/bringup_controllers.launch" />
 
   <!-- Robot state publisher -->
   <node pkg="robot_state_publisher" type="state_publisher" name="robot_state_publisher">
@@ -23,5 +23,7 @@
 
   <!-- Joystick controller -->
   <include file="$(find talos_bringup)/launch/joystick_teleop.launch" />
-
+  
+  <include file="$(find talos_moveit_config)/launch/move_group.launch" />
+  
 </launch>

talos_controller_configuration/config/joint_trajectory_controllers.yaml

@@ -93,12 +93,12 @@ right_arm_controller:
 right_gripper_controller:
   type: "position_controllers/JointTrajectoryController"
   joints:
-    - right_gripper_joint
+    - gripper_right_joint
 
   constraints:
     goal_time: *goal_time_constraint
     stopped_velocity_tolerance: *stopped_velocity_constraint
-    right_gripper_joint:
+    gripper_right_joint:
       goal: *goal_pos_constraint
 
   stop_trajectory_duration: 0.0
@@ -106,12 +106,12 @@ right_gripper_controller:
 left_gripper_controller:
   type: "position_controllers/JointTrajectoryController"
   joints:
-    - left_gripper_joint
+    - gripper_left_joint
 
   constraints:
     goal_time: *goal_time_constraint
     stopped_velocity_tolerance: *stopped_velocity_constraint
-    left_gripper_joint:
+    gripper_left_joint:
       goal: *goal_pos_constraint
 
   stop_trajectory_duration: 0.0

talos_controller_configuration/launch/default_controllers.launch

+<launch>
+  
+  <include file="$(find talos_walking)/launch/walking_controller.launch" />
+  
+  <include file="$(find talos_controller_configuration)/launch/upper_body_position_controllers.launch" />
+  
+</launch>

talos_controller_configuration/launch/legs_position_controllers.launch

@@ -3,7 +3,7 @@
   <rosparam command="load" file="$(find talos_controller_configuration)/config/joint_trajectory_controllers.yaml" />
 
   <!-- Spawn legs joint trajectory controllers -->
-  <node name="upper_body_controllers_spawner"
+  <node name="lower_body_controllers_spawner"
         pkg="controller_manager" type="spawner" output="screen"
         args="left_leg_controller
               right_leg_controller" />