forked from duISIR/chonk_pushing
-
Notifications
You must be signed in to change notification settings - Fork 0
Commit
This commit does not belong to any branch on this repository, and may belong to a fork outside of the repository.
integrate 6-DOF force/torque control into motion planning
- Loading branch information
Showing
12 changed files
with
3,986 additions
and
6 deletions.
There are no files selected for viewing
96 changes: 96 additions & 0 deletions
96
...PC_BC_operational_pick_localsensorAD_6DOF_obstacle_wholetrajectory_withOrientation.launch
This file contains bidirectional Unicode text that may be interpreted or compiled differently than what appears below. To review, open the file in an editor that reveals hidden Unicode characters.
Learn more about bidirectional Unicode characters
| Original file line number | Diff line number | Diff line change |
|---|---|---|
| @@ -0,0 +1,96 @@ | ||
| <?xml version="1.0"?> | ||
|
|
||
| <launch> | ||
| <!-- set global arguments --> | ||
| <arg name="robot_name" default="chonk"/> | ||
|
|
||
| <!-- Spawn position controller --> | ||
| <node | ||
| name="trajectory_controller" | ||
| pkg="controller_manager" | ||
| type="spawner" | ||
| ns="/$(arg robot_name)" | ||
| args="/$(arg robot_name)/trajectory_controller" | ||
| respawn="false" | ||
| output="screen" | ||
| /> | ||
|
|
||
| <!-- Launch a mux to send commands to the robot only from one source/controller --> | ||
| <node | ||
| name="mux_joint_position" | ||
| pkg="topic_tools" | ||
| type="mux" | ||
| ns="/$(arg robot_name)" | ||
| args="trajectory_controller/command | ||
| streaming_controller/command | ||
| DefaultPositionController/command | ||
| ActionCmdPosePositionController/command | ||
| ActionCmdConfigPositionController/command | ||
| ActionTeleop2DPositionController/command | ||
| mux:=mux_joint_position" | ||
| /> | ||
| <!-- Action Service Node for commanding end-effector poses --> | ||
| <node | ||
| name="action_server_cmd_config" | ||
| pkg="chonk_pushing" | ||
| ns="/$(arg robot_name)" | ||
| type="action_server_cmd_config_wholetrajectory.py" | ||
| output="screen" | ||
| > | ||
| <param name="cmd_topic_name" type="string" value="/$(arg robot_name)/ActionCmdConfigPositionController/command"/> | ||
| <remap from="/joint_states" to="/$(arg robot_name)/joint_states"/> | ||
| <remap from="/mux_selected" to="/$(arg robot_name)/mux_joint_position/selected"/> | ||
| </node> | ||
| <!-- Action Service Node for planning joint positions and build MPC to track the planning results --> | ||
| <node | ||
| name="action_server_cmd_pose_MPC_BC_operational_AD" | ||
| pkg="chonk_pushing" | ||
| ns="/$(arg robot_name)" | ||
| type="action_server_cmd_pose_MPC_BC_operational_pick_localsensorAD_6DOF_obstacle_wholetrajectory_withOrientation.py" | ||
| output="screen" | ||
| > | ||
| <param name="link_donkey" type="string" value="link_donkey"/> | ||
| <!-- <param name="link_ee_right" type="string" value="RARM_END_EFFECTOR_finger"/> | ||
| <param name="link_ee_left" type="string" value="LARM_END_EFFECTOR_finger"/>--> | ||
| <param name="link_ee_right" type="string" value="RARM_END_EFFECTOR_grasp"/> | ||
| <param name="link_ee_left" type="string" value="LARM_END_EFFECTOR_grasp"/> | ||
| <!-- <param name="link_sensor_right" type="string" value="RARM_changer_link_base"/> | ||
| <param name="link_sensor_left" type="string" value="LARM_changer_link_base"/>--> | ||
| <param name="link_head" type="string" value="CAMERA_HEAD_HEADING_Link"/> | ||
| <param name="link_gaze" type="string" value="gaze_ref"/> | ||
| <param name="cmd_topic_name" type="string" value="/$(arg robot_name)/ActionCmdPosePositionController/command"/> | ||
| <remap from="/joint_states" to="/$(arg robot_name)/joint_states"/> | ||
| <remap from="/mux_selected" to="/$(arg robot_name)/mux_joint_position/selected"/> | ||
| </node> | ||
| <!-- Action Service Node for teleoperating robot in 2D --> | ||
| <node | ||
| name="action_server_teleop_2d" | ||
| pkg="chonk_pushing" | ||
| ns="/$(arg robot_name)" | ||
| type="action_server_teleop_2d.py" | ||
| output="screen" | ||
| > | ||
| <param name="link_ee_right" type="string" value="RARM_END_EFFECTOR_finger"/> | ||
| <param name="link_ee_left" type="string" value="LARM_END_EFFECTOR_finger"/> | ||
| <param name="link_head" type="string" value="CAMERA_HEAD_HEADING_Link"/> | ||
| <param name="link_ref" type="string" value="teleop_ref"/> | ||
| <param name="link_gaze" type="string" value="gaze_ref"/> | ||
| <param name="cmd_topic_name" type="string" value="/$(arg robot_name)/ActionTeleop2DPositionController/command"/> | ||
| <param name="x_min" type="double" value="0.2"/> | ||
| <param name="x_max" type="double" value="0.8"/> | ||
| <param name="y_min" type="double" value="-0.5"/> | ||
| <param name="y_max" type="double" value="0.5"/> | ||
| <!-- <param name="roll_min" type="double" value="-25"/> | ||
| <param name="roll_max" type="double" value="25"/> | ||
| <param name="pitch_min" type="double" value="-20"/> | ||
| <param name="pitch_max" type="double" value="70"/> --> | ||
| <param name="roll_min" type="double" value="0"/> | ||
| <param name="roll_max" type="double" value="0"/> | ||
| <param name="pitch_min" type="double" value="0"/> | ||
| <param name="pitch_max" type="double" value="0"/> | ||
| <param name="yaw_min" type="double" value="-120"/> | ||
| <param name="yaw_max" type="double" value="120"/> | ||
| <remap from="/joint_states" to="/$(arg robot_name)/joint_states"/> | ||
| <remap from="/mux_selected" to="/$(arg robot_name)/mux_joint_position/selected"/> | ||
| </node> | ||
| </launch> |
96 changes: 96 additions & 0 deletions
96
..._operational_pick_localsensorAD_6DOF_obstacle_wholetrajectory_withOrientation_turn.launch
This file contains bidirectional Unicode text that may be interpreted or compiled differently than what appears below. To review, open the file in an editor that reveals hidden Unicode characters.
Learn more about bidirectional Unicode characters
| Original file line number | Diff line number | Diff line change |
|---|---|---|
| @@ -0,0 +1,96 @@ | ||
| <?xml version="1.0"?> | ||
|
|
||
| <launch> | ||
| <!-- set global arguments --> | ||
| <arg name="robot_name" default="chonk"/> | ||
|
|
||
| <!-- Spawn position controller --> | ||
| <node | ||
| name="trajectory_controller" | ||
| pkg="controller_manager" | ||
| type="spawner" | ||
| ns="/$(arg robot_name)" | ||
| args="/$(arg robot_name)/trajectory_controller" | ||
| respawn="false" | ||
| output="screen" | ||
| /> | ||
|
|
||
| <!-- Launch a mux to send commands to the robot only from one source/controller --> | ||
| <node | ||
| name="mux_joint_position" | ||
| pkg="topic_tools" | ||
| type="mux" | ||
| ns="/$(arg robot_name)" | ||
| args="trajectory_controller/command | ||
| streaming_controller/command | ||
| DefaultPositionController/command | ||
| ActionCmdPosePositionController/command | ||
| ActionCmdConfigPositionController/command | ||
| ActionTeleop2DPositionController/command | ||
| mux:=mux_joint_position" | ||
| /> | ||
| <!-- Action Service Node for commanding end-effector poses --> | ||
| <node | ||
| name="action_server_cmd_config" | ||
| pkg="chonk_pushing" | ||
| ns="/$(arg robot_name)" | ||
| type="action_server_cmd_config_wholetrajectory.py" | ||
| output="screen" | ||
| > | ||
| <param name="cmd_topic_name" type="string" value="/$(arg robot_name)/ActionCmdConfigPositionController/command"/> | ||
| <remap from="/joint_states" to="/$(arg robot_name)/joint_states"/> | ||
| <remap from="/mux_selected" to="/$(arg robot_name)/mux_joint_position/selected"/> | ||
| </node> | ||
| <!-- Action Service Node for planning joint positions and build MPC to track the planning results --> | ||
| <node | ||
| name="action_server_cmd_pose_MPC_BC_operational_AD" | ||
| pkg="chonk_pushing" | ||
| ns="/$(arg robot_name)" | ||
| type="action_server_cmd_pose_MPC_BC_operational_pick_localsensorAD_6DOF_obstacle_wholetrajectory_withOrientation_turn.py" | ||
| output="screen" | ||
| > | ||
| <param name="link_donkey" type="string" value="link_donkey"/> | ||
| <!-- <param name="link_ee_right" type="string" value="RARM_END_EFFECTOR_finger"/> | ||
| <param name="link_ee_left" type="string" value="LARM_END_EFFECTOR_finger"/>--> | ||
| <param name="link_ee_right" type="string" value="RARM_END_EFFECTOR_grasp"/> | ||
| <param name="link_ee_left" type="string" value="LARM_END_EFFECTOR_grasp"/> | ||
| <!-- <param name="link_sensor_right" type="string" value="RARM_changer_link_base"/> | ||
| <param name="link_sensor_left" type="string" value="LARM_changer_link_base"/>--> | ||
| <param name="link_head" type="string" value="CAMERA_HEAD_HEADING_Link"/> | ||
| <param name="link_gaze" type="string" value="gaze_ref"/> | ||
| <param name="cmd_topic_name" type="string" value="/$(arg robot_name)/ActionCmdPosePositionController/command"/> | ||
| <remap from="/joint_states" to="/$(arg robot_name)/joint_states"/> | ||
| <remap from="/mux_selected" to="/$(arg robot_name)/mux_joint_position/selected"/> | ||
| </node> | ||
| <!-- Action Service Node for teleoperating robot in 2D --> | ||
| <node | ||
| name="action_server_teleop_2d" | ||
| pkg="chonk_pushing" | ||
| ns="/$(arg robot_name)" | ||
| type="action_server_teleop_2d.py" | ||
| output="screen" | ||
| > | ||
| <param name="link_ee_right" type="string" value="RARM_END_EFFECTOR_finger"/> | ||
| <param name="link_ee_left" type="string" value="LARM_END_EFFECTOR_finger"/> | ||
| <param name="link_head" type="string" value="CAMERA_HEAD_HEADING_Link"/> | ||
| <param name="link_ref" type="string" value="teleop_ref"/> | ||
| <param name="link_gaze" type="string" value="gaze_ref"/> | ||
| <param name="cmd_topic_name" type="string" value="/$(arg robot_name)/ActionTeleop2DPositionController/command"/> | ||
| <param name="x_min" type="double" value="0.2"/> | ||
| <param name="x_max" type="double" value="0.8"/> | ||
| <param name="y_min" type="double" value="-0.5"/> | ||
| <param name="y_max" type="double" value="0.5"/> | ||
| <!-- <param name="roll_min" type="double" value="-25"/> | ||
| <param name="roll_max" type="double" value="25"/> | ||
| <param name="pitch_min" type="double" value="-20"/> | ||
| <param name="pitch_max" type="double" value="70"/> --> | ||
| <param name="roll_min" type="double" value="0"/> | ||
| <param name="roll_max" type="double" value="0"/> | ||
| <param name="pitch_min" type="double" value="0"/> | ||
| <param name="pitch_max" type="double" value="0"/> | ||
| <param name="yaw_min" type="double" value="-120"/> | ||
| <param name="yaw_max" type="double" value="120"/> | ||
| <remap from="/joint_states" to="/$(arg robot_name)/joint_states"/> | ||
| <remap from="/mux_selected" to="/$(arg robot_name)/mux_joint_position/selected"/> | ||
| </node> | ||
| </launch> |
82 changes: 82 additions & 0 deletions
82
...PC_BC_operational_pick_localsensorAD_6DOF_obstacle_wholetrajectory_withOrientation.launch
This file contains bidirectional Unicode text that may be interpreted or compiled differently than what appears below. To review, open the file in an editor that reveals hidden Unicode characters.
Learn more about bidirectional Unicode characters
| Original file line number | Diff line number | Diff line change |
|---|---|---|
| @@ -0,0 +1,82 @@ | ||
| <?xml version="1.0"?> | ||
|
|
||
| <launch> | ||
| <!-- set global arguments --> | ||
| <arg name="robot_name" default="chonk"/> | ||
| <param name="robot_description" command="$(find xacro)/xacro --inorder '$(find chonk_pushing)/urdf/chonk_pushing.urdf.xacro'"/> | ||
| <param name="robot_description_wholebody" command="$(find xacro)/xacro --inorder '$(find chonk_pushing)/urdf/chonk_pushing_wholebody.urdf'"/> | ||
|
|
||
|
|
||
| <!-- Loads the Gazebo world --> | ||
| <include file="$(find gazebo_ros)/launch/empty_world.launch"> | ||
| <arg name="world_name" value="$(find chonk_pushing)/world/chonk_pick_sensorAD_obstacle.world"/> | ||
| <arg name="debug" value="false" /> | ||
| <arg name="gui" value="true" /> | ||
| <arg name="gui_required" value="true"/> | ||
| <arg name="paused" value="false"/> | ||
| <arg name="use_sim_time" value="true"/> | ||
| <arg name="recording" value="false"/> | ||
| </include> | ||
|
|
||
| <!-- Run a python script to send a service call to gazebo_ros to spawn a URDF robot --> | ||
| <node | ||
| name="urdf_spawner" | ||
| pkg="gazebo_ros" | ||
| type="spawn_model" | ||
| respawn="false" | ||
| output="screen" | ||
| args="-urdf -model $(arg robot_name) -param robot_description -x 0 -y 0 -z 0" | ||
| /> | ||
| <!-- Load joint controller configurations from YAML file to parameter server --> | ||
| <rosparam | ||
| file="$(find chonk_gazebo)/config/chonk_pushing_control_pick_wholetrajectory.yaml" | ||
| command="load" | ||
| ns="$(arg robot_name)" | ||
| /> | ||
|
|
||
| <!-- load the controllers --> | ||
| <node | ||
| name="controller_spawner" | ||
| pkg="controller_manager" | ||
| type="spawner" | ||
| respawn="false" | ||
| output="screen" | ||
| ns="/$(arg robot_name)" | ||
| args="/$(arg robot_name)/joint_state_controller" | ||
| /> | ||
|
|
||
| <!-- convert joint states to TF transforms --> | ||
| <node | ||
| name="robot_state_publisher" | ||
| pkg="robot_state_publisher" | ||
| type="robot_state_publisher" | ||
| respawn="false" | ||
| output="screen" | ||
| > | ||
| <remap from="/joint_states" to="/$(arg robot_name)/joint_states" /> | ||
| </node> | ||
|
|
||
| <!-- Spawn position controller --> | ||
| <!-- <node | ||
| name="position_controller" | ||
| pkg="controller_manager" | ||
| type="spawner" | ||
| ns="/$(arg robot_name)" | ||
| args="/$(arg robot_name)/streaming_controller" | ||
| respawn="false" | ||
| output="screen" | ||
| />--> | ||
|
|
||
| <!-- Spawn velocity controller --> | ||
| <node | ||
| name="velocity_controller" | ||
| pkg="controller_manager" | ||
| type="spawner" | ||
| ns="/$(arg robot_name)" | ||
| args="/$(arg robot_name)/donkey_velocity_controller" | ||
| respawn="false" | ||
| output="screen" | ||
| /> | ||
|
|
||
|
|
||
| </launch> |
83 changes: 83 additions & 0 deletions
83
..._operational_pick_localsensorAD_6DOF_obstacle_wholetrajectory_withOrientation_turn.launch
This file contains bidirectional Unicode text that may be interpreted or compiled differently than what appears below. To review, open the file in an editor that reveals hidden Unicode characters.
Learn more about bidirectional Unicode characters
| Original file line number | Diff line number | Diff line change |
|---|---|---|
| @@ -0,0 +1,83 @@ | ||
| <?xml version="1.0"?> | ||
|
|
||
| <launch> | ||
| <!-- set global arguments --> | ||
| <arg name="robot_name" default="chonk"/> | ||
| <param name="robot_description" command="$(find xacro)/xacro --inorder '$(find chonk_pushing)/urdf/chonk_pushing.urdf.xacro'"/> | ||
| <param name="robot_description_wholebody" command="$(find xacro)/xacro --inorder '$(find chonk_pushing)/urdf/chonk_pushing_wholebody.urdf'"/> | ||
|
|
||
|
|
||
|
|
||
| <!-- Loads the Gazebo world --> | ||
| <include file="$(find gazebo_ros)/launch/empty_world.launch"> | ||
| <arg name="world_name" value="$(find chonk_pushing)/world/chonk_pick_turn_obstacle_experiment.world"/> | ||
| <arg name="debug" value="false" /> | ||
| <arg name="gui" value="true" /> | ||
| <arg name="gui_required" value="true"/> | ||
| <arg name="paused" value="false"/> | ||
| <arg name="use_sim_time" value="true"/> | ||
| <arg name="recording" value="false"/> | ||
| </include> | ||
|
|
||
| <!-- Run a python script to send a service call to gazebo_ros to spawn a URDF robot --> | ||
| <node | ||
| name="urdf_spawner" | ||
| pkg="gazebo_ros" | ||
| type="spawn_model" | ||
| respawn="false" | ||
| output="screen" | ||
| args="-urdf -model $(arg robot_name) -param robot_description -x 0 -y 0 -z 0" | ||
| /> | ||
| <!-- Load joint controller configurations from YAML file to parameter server --> | ||
| <rosparam | ||
| file="$(find chonk_gazebo)/config/chonk_pushing_control_pick_wholetrajectory.yaml" | ||
| command="load" | ||
| ns="$(arg robot_name)" | ||
| /> | ||
|
|
||
| <!-- load the controllers --> | ||
| <node | ||
| name="controller_spawner" | ||
| pkg="controller_manager" | ||
| type="spawner" | ||
| respawn="false" | ||
| output="screen" | ||
| ns="/$(arg robot_name)" | ||
| args="/$(arg robot_name)/joint_state_controller" | ||
| /> | ||
|
|
||
| <!-- convert joint states to TF transforms --> | ||
| <node | ||
| name="robot_state_publisher" | ||
| pkg="robot_state_publisher" | ||
| type="robot_state_publisher" | ||
| respawn="false" | ||
| output="screen" | ||
| > | ||
| <remap from="/joint_states" to="/$(arg robot_name)/joint_states" /> | ||
| </node> | ||
|
|
||
| <!-- Spawn position controller --> | ||
| <!-- <node | ||
| name="position_controller" | ||
| pkg="controller_manager" | ||
| type="spawner" | ||
| ns="/$(arg robot_name)" | ||
| args="/$(arg robot_name)/streaming_controller" | ||
| respawn="false" | ||
| output="screen" | ||
| />--> | ||
|
|
||
| <!-- Spawn velocity controller --> | ||
| <node | ||
| name="velocity_controller" | ||
| pkg="controller_manager" | ||
| type="spawner" | ||
| ns="/$(arg robot_name)" | ||
| args="/$(arg robot_name)/donkey_velocity_controller" | ||
| respawn="false" | ||
| output="screen" | ||
| /> | ||
|
|
||
|
|
||
| </launch> |
Oops, something went wrong.