diff --git a/src/factory_sim/config/config.yaml b/src/factory_sim/config/config.yaml index fc7d3c107..30f59ae70 100644 --- a/src/factory_sim/config/config.yaml +++ b/src/factory_sim/config/config.yaml @@ -96,6 +96,7 @@ ros2_control: controllers_active_at_startup: - "joint_trajectory_admittance_controller" - "joint_state_broadcaster" + - "force_torque_sensor_broadcaster" - "tool_attach_controller" - "suction_cup_controller" # Load but do not start these controllers so they can be activated later if needed. diff --git a/src/factory_sim/config/control/picknik_fanuc.ros2_control.yaml b/src/factory_sim/config/control/picknik_fanuc.ros2_control.yaml index ae9a63c13..5c6d1600b 100644 --- a/src/factory_sim/config/control/picknik_fanuc.ros2_control.yaml +++ b/src/factory_sim/config/control/picknik_fanuc.ros2_control.yaml @@ -5,6 +5,8 @@ controller_manager: type: joint_state_broadcaster/JointStateBroadcaster joint_trajectory_admittance_controller: type: joint_trajectory_admittance_controller/JointTrajectoryAdmittanceController + force_torque_sensor_broadcaster: + type: force_torque_sensor_broadcaster/ForceTorqueSensorBroadcaster # A controller to enable/disable the tool attachment interface at the robot flange. # In sim, this is modeled as a weld constraint in Mujoco. # In a real robot, this controller would activate a mechanism to attach/detach the tool at the robot flange. @@ -48,17 +50,32 @@ joint_state_broadcaster: - position - velocity +force_torque_sensor_broadcaster: + ros__parameters: + # Hardware interface name. Matches the MuJoCo name; MujocoSystem + # exposes "/force.{x,y,z}" and "/torque.{x,y,z}" state interfaces. + sensor_name: wrist_ft_sensor + state_interface_names: + - force.x + - force.y + - force.z + - torque.x + - torque.y + - torque.z + # The frame the sensor reading is published in. + frame_id: tool0 + joint_trajectory_admittance_controller: ros__parameters: # Joint group to control. planning_group_name: manipulator - # Specifies the frame/link name of the force torque sensor. Must exist in the robot description. - sensor_frame: link_6 - # Specifies the frame/link name of the end-effector frame. Must exist in the robot description. - ee_frame: grasp_link + # Specifies the frame/link names of the force torque sensor(s). Must exist in the robot description. + sensor_frames: [tool0] + # Specifies the frame/link names of the end-effector frame(s). Must exist in the robot description. + ee_frames: [grasp_link] # The name of the force/torque ros2_control hardware interface which will be used in the admittance calculation. # If empty, the controller will run without admittance. - ft_sensor_name: "" + ft_sensor_name: wrist_ft_sensor # Joint accelerations to use for immediate stops (e.g. when cancelling a running trajectory). # Chosen to match MoveIt configs. stop_accelerations: [30.0, 30.0, 30.0, 30.0, 30.0, 30.0] diff --git a/src/factory_sim/description/scene.xml b/src/factory_sim/description/scene.xml index e3778f581..a8f1a3661 100644 --- a/src/factory_sim/description/scene.xml +++ b/src/factory_sim/description/scene.xml @@ -419,6 +419,14 @@ + + + + + + diff --git a/src/factory_sim/description/suction_tool.xml b/src/factory_sim/description/suction_tool.xml index 04b27afe3..1146b7d49 100644 --- a/src/factory_sim/description/suction_tool.xml +++ b/src/factory_sim/description/suction_tool.xml @@ -14,6 +14,11 @@ armature="0" /> + + diff --git a/src/factory_sim/objectives/vacuum_pick_bracket_part_subtree.xml b/src/factory_sim/objectives/vacuum_pick_bracket_part_subtree.xml index 044a4d878..236adf775 100644 --- a/src/factory_sim/objectives/vacuum_pick_bracket_part_subtree.xml +++ b/src/factory_sim/objectives/vacuum_pick_bracket_part_subtree.xml @@ -1,9 +1,8 @@ - @@ -20,14 +19,17 @@ output_pose="{approach_pose}" translation_xyz="0;0;0.15" quaternion_xyzw="-0.0; -1.0; -0.0; -0.0" - name="Create approach pose before straight pick" + name="Create approach pose above bracket" /> + @@ -53,28 +55,89 @@ trajectory_sampling_rate="100" velocity_scale_factor="1.000000" /> - + + + + + + + + + + + + + + + +