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.
add orientation planning and 6DOF force torque control
- Loading branch information
Showing
27 changed files
with
3,480 additions
and
209 deletions.
There are no files selected for viewing
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
95 changes: 95 additions & 0 deletions
95
launch/action_servers_MPC_BC_operational_pick_localsensorAD_6DOF.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,95 @@ | ||
| <?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 | ||
| 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.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> |
65 changes: 65 additions & 0 deletions
65
launch/action_servers_MPC_BC_operational_pick_localsensorAD_6DOF_experiment.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,65 @@ | ||
| <?xml version="1.0"?> | ||
|
|
||
| <launch> | ||
| <!-- set global arguments --> | ||
| <arg name="robot_name" default="chonk"/> | ||
| <param name="robot_description_wholebody" command="$(find xacro)/xacro --inorder '$(find chonk_pushing)/urdf/chonk_pushing_wholebody.urdf'"/> | ||
|
|
||
| <!-- 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 | ||
| 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_experiment.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> | ||
| </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
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
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
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
82 changes: 82 additions & 0 deletions
82
launch/gazebo_planner_MPC_BC_operational_pick_localsensorAD_6DOF_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,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_turn.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.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> |
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
Oops, something went wrong.