From d86690e04e7b1a5ee3830c5903f1415fa45d8dd2 Mon Sep 17 00:00:00 2001
From: Hilario Tome <hilario.tome@pal-robotics.com>
Date: Mon, 31 Oct 2016 19:04:16 +0100
Subject: [PATCH] Modified bringup
---
talos_bringup/config/talos_motions.yaml | 93 +++++++++++++++++++
talos_bringup/launch/play_motion.launch | 12 +++
talos_bringup/launch/talos_bringup.launch | 6 +-
.../config/joint_trajectory_controllers.yaml | 8 +-
...lers.launch => bringup_controllers.launch} | 0
.../launch/default_controllers.launch | 7 ++
.../launch/legs_position_controllers.launch | 2 +-
7 files changed, 121 insertions(+), 7 deletions(-)
create mode 100644 talos_bringup/config/talos_motions.yaml
create mode 100644 talos_bringup/launch/play_motion.launch
rename talos_controller_configuration/launch/{talos_default_controllers.launch => bringup_controllers.launch} (100%)
create mode 100644 talos_controller_configuration/launch/default_controllers.launch
diff --git a/talos_bringup/config/talos_motions.yaml b/talos_bringup/config/talos_motions.yaml
new file mode 100644
index 0000000..b55677e
--- /dev/null
+++ b/talos_bringup/config/talos_motions.yaml
@@ -0,0 +1,93 @@
+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
diff --git a/talos_bringup/launch/play_motion.launch b/talos_bringup/launch/play_motion.launch
new file mode 100644
index 0000000..50c594a
--- /dev/null
+++ b/talos_bringup/launch/play_motion.launch
@@ -0,0 +1,12 @@
+<?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>
diff --git a/talos_bringup/launch/talos_bringup.launch b/talos_bringup/launch/talos_bringup.launch
index b4654e0..f050b0b 100644
--- a/talos_bringup/launch/talos_bringup.launch
+++ b/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>
diff --git a/talos_controller_configuration/config/joint_trajectory_controllers.yaml b/talos_controller_configuration/config/joint_trajectory_controllers.yaml
index ea3f7da..903b987 100644
--- a/talos_controller_configuration/config/joint_trajectory_controllers.yaml
+++ b/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
diff --git a/talos_controller_configuration/launch/talos_default_controllers.launch b/talos_controller_configuration/launch/bringup_controllers.launch
similarity index 100%
rename from talos_controller_configuration/launch/talos_default_controllers.launch
rename to talos_controller_configuration/launch/bringup_controllers.launch
diff --git a/talos_controller_configuration/launch/default_controllers.launch b/talos_controller_configuration/launch/default_controllers.launch
new file mode 100644
index 0000000..76cc67e
--- /dev/null
+++ b/talos_controller_configuration/launch/default_controllers.launch
@@ -0,0 +1,7 @@
+<launch>
+
+ <include file="$(find talos_walking)/launch/walking_controller.launch" />
+
+ <include file="$(find talos_controller_configuration)/launch/upper_body_position_controllers.launch" />
+
+</launch>
diff --git a/talos_controller_configuration/launch/legs_position_controllers.launch b/talos_controller_configuration/launch/legs_position_controllers.launch
index 630ba49..beb9482 100644
--- a/talos_controller_configuration/launch/legs_position_controllers.launch
+++ b/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" />
--
GitLab