Add moveit2 package with dual arm/gripper (#5)
* Create mujoco_ros2_control package * Enable Mujoco hardware interface for OpenArm * Verify connection with ros2_control * Update actuators to pos/vel * Add motor after creation * Tune KD for smoother motion * Restore realsense camera * Change camera to be 10 tilt from vertical to match physical model * Repair urdf to match physical robot * Checkpoint moveit2_configuration * Remove collision between camera and pedestal and remove additional ros2_control interfaces for mock * Enable octomap perception * Fix base_link to world transform * Update openarm to support moveit2 bimanual planning with end effectors * Improve safety of openarm_hardware zero position autocalibration * Remove openarm_mujoco_ros2_control from moveit2 branch * Restore sensor functionality
This commit is contained in:
thomason
GitHub
parent
ce06ea14c1
commit
1a1084c627
@ -2,8 +2,8 @@
|
||||
<robot xmlns:xacro="http://www.ros.org/wiki/xacro" name="openarm_bimanual">
|
||||
|
||||
<xacro:include filename="$(find openarm_description)/urdf/openarm.xacro"/>
|
||||
<xacro:include filename="openarm_pedestal.urdf"/>
|
||||
<!-- <xacro:include filename="openarm_bimanual_sensors.xacro"/> -->
|
||||
<xacro:include filename="$(find openarm_bimanual_description)/urdf/openarm_pedestal.urdf"/>
|
||||
<xacro:include filename="openarm_bimanual_sensors.xacro"/>
|
||||
|
||||
<link name="world"/>
|
||||
<joint name="dummy_joint" type="fixed">
|
||||
|
||||
@ -2,7 +2,7 @@
|
||||
<robot xmlns:xacro="http://www.ros.org/wiki/xacro">
|
||||
<xacro:include filename="$(find realsense2_description)/urdf/_d435i.urdf.xacro"/>
|
||||
<xacro:sensor_d435i parent="pedestal_link" use_nominal_extrinsics="true">
|
||||
<origin xyz="0.0 -0.025 0.995216" rpy="0.0 0.0 ${-pi/2.0}"/>
|
||||
<origin xyz="0.0 -0.025 0.995216" rpy="0.0 ${80 * 2.0 * pi / 360.0} ${-pi/2.0}"/>
|
||||
</xacro:sensor_d435i>
|
||||
|
||||
</robot>
|
||||
|
||||
@ -7,4 +7,4 @@ moveit_setup_assistant_config:
|
||||
package_settings:
|
||||
author_name: Thomason Zhou
|
||||
author_email: t95zhou@uwaterloo.ca
|
||||
generated_timestamp: 1744266716
|
||||
generated_timestamp: 1745222265
|
||||
@ -1,6 +1,7 @@
|
||||
# Default initial positions for openarm_bimanual's ros2_control fake system
|
||||
|
||||
initial_positions:
|
||||
left_left_pris1: 0
|
||||
left_rev1: 0
|
||||
left_rev2: 0
|
||||
left_rev3: 0
|
||||
@ -8,6 +9,7 @@ initial_positions:
|
||||
left_rev5: 0
|
||||
left_rev6: 0
|
||||
left_rev7: 0
|
||||
right_left_pris1: 0
|
||||
right_rev1: 0
|
||||
right_rev2: 0
|
||||
right_rev3: 0
|
||||
|
||||
@ -9,8 +9,8 @@ default_acceleration_scaling_factor: 0.1
|
||||
# Joint limits can be turned off with [has_velocity_limits, has_acceleration_limits]
|
||||
joint_limits:
|
||||
left_left_pris1:
|
||||
has_velocity_limits: false
|
||||
max_velocity: 0
|
||||
has_velocity_limits: true
|
||||
max_velocity: 0.10000000000000001
|
||||
has_acceleration_limits: false
|
||||
max_acceleration: 0
|
||||
left_rev1:
|
||||
@ -48,9 +48,9 @@ joint_limits:
|
||||
max_velocity: 0
|
||||
has_acceleration_limits: false
|
||||
max_acceleration: 0
|
||||
left_right_pris2:
|
||||
has_velocity_limits: false
|
||||
max_velocity: 0
|
||||
right_left_pris1:
|
||||
has_velocity_limits: true
|
||||
max_velocity: 0.10000000000000001
|
||||
has_acceleration_limits: false
|
||||
max_acceleration: 0
|
||||
right_rev1:
|
||||
|
||||
@ -2,3 +2,19 @@ left_arm:
|
||||
kinematics_solver: kdl_kinematics_plugin/KDLKinematicsPlugin
|
||||
kinematics_solver_search_resolution: 0.0050000000000000001
|
||||
kinematics_solver_timeout: 0.0050000000000000001
|
||||
left_side:
|
||||
kinematics_solver: cached_ik_kinematics_plugin/CachedKDLKinematicsPlugin
|
||||
kinematics_solver_search_resolution: 0.0050000000000000001
|
||||
kinematics_solver_timeout: 0.0050000000000000001
|
||||
right_arm:
|
||||
kinematics_solver: kdl_kinematics_plugin/KDLKinematicsPlugin
|
||||
kinematics_solver_search_resolution: 0.0050000000000000001
|
||||
kinematics_solver_timeout: 0.0050000000000000001
|
||||
right_side:
|
||||
kinematics_solver: kdl_kinematics_plugin/KDLKinematicsPlugin
|
||||
kinematics_solver_search_resolution: 0.0050000000000000001
|
||||
kinematics_solver_timeout: 0.0050000000000000001
|
||||
upper_body:
|
||||
kinematics_solver: kdl_kinematics_plugin/KDLKinematicsPlugin
|
||||
kinematics_solver_search_resolution: 0.0050000000000000001
|
||||
kinematics_solver_timeout: 0.0050000000000000001
|
||||
@ -6,6 +6,8 @@ moveit_simple_controller_manager:
|
||||
controller_names:
|
||||
- left_arm_controller
|
||||
- right_arm_controller
|
||||
- left_gripper_controller
|
||||
- right_gripper_controller
|
||||
|
||||
left_arm_controller:
|
||||
type: FollowJointTrajectory
|
||||
@ -31,3 +33,15 @@ moveit_simple_controller_manager:
|
||||
- right_rev7
|
||||
action_ns: follow_joint_trajectory
|
||||
default: true
|
||||
left_gripper_controller:
|
||||
type: GripperCommand
|
||||
joints:
|
||||
- left_left_pris1
|
||||
action_ns: gripper_cmd
|
||||
default: true
|
||||
right_gripper_controller:
|
||||
type: GripperCommand
|
||||
joints:
|
||||
- right_left_pris1
|
||||
action_ns: gripper_cmd
|
||||
default: true
|
||||
@ -0,0 +1,142 @@
|
||||
<?xml version="1.0"?>
|
||||
<robot xmlns:xacro="http://www.ros.org/wiki/xacro">
|
||||
<xacro:macro name="openarm_bimanual_ros2_control" params="name initial_positions_file">
|
||||
<xacro:property name="initial_positions" value="${load_yaml(initial_positions_file)['initial_positions']}"/>
|
||||
|
||||
<ros2_control name="${name}" type="system">
|
||||
<hardware>
|
||||
<!-- By default, set up controllers for simulation. This won't work on real hardware -->
|
||||
<plugin>mock_components/GenericSystem</plugin>
|
||||
</hardware>
|
||||
<joint name="left_rev1">
|
||||
<command_interface name="position"/>
|
||||
<command_interface name="velocity"/>
|
||||
<state_interface name="position">
|
||||
<param name="initial_value">${initial_positions['left_rev1']}</param>
|
||||
</state_interface>
|
||||
<state_interface name="velocity"/>
|
||||
</joint>
|
||||
<joint name="left_rev2">
|
||||
<command_interface name="position"/>
|
||||
<command_interface name="velocity"/>
|
||||
<state_interface name="position">
|
||||
<param name="initial_value">${initial_positions['left_rev2']}</param>
|
||||
</state_interface>
|
||||
<state_interface name="velocity"/>
|
||||
</joint>
|
||||
<joint name="left_rev3">
|
||||
<command_interface name="position"/>
|
||||
<command_interface name="velocity"/>
|
||||
<state_interface name="position">
|
||||
<param name="initial_value">${initial_positions['left_rev3']}</param>
|
||||
</state_interface>
|
||||
<state_interface name="velocity"/>
|
||||
</joint>
|
||||
<joint name="left_rev4">
|
||||
<command_interface name="position"/>
|
||||
<command_interface name="velocity"/>
|
||||
<state_interface name="position">
|
||||
<param name="initial_value">${initial_positions['left_rev4']}</param>
|
||||
</state_interface>
|
||||
<state_interface name="velocity"/>
|
||||
</joint>
|
||||
<joint name="left_rev5">
|
||||
<command_interface name="position"/>
|
||||
<command_interface name="velocity"/>
|
||||
<state_interface name="position">
|
||||
<param name="initial_value">${initial_positions['left_rev5']}</param>
|
||||
</state_interface>
|
||||
<state_interface name="velocity"/>
|
||||
</joint>
|
||||
<joint name="left_rev6">
|
||||
<command_interface name="position"/>
|
||||
<command_interface name="velocity"/>
|
||||
<state_interface name="position">
|
||||
<param name="initial_value">${initial_positions['left_rev6']}</param>
|
||||
</state_interface>
|
||||
<state_interface name="velocity"/>
|
||||
</joint>
|
||||
<joint name="left_rev7">
|
||||
<command_interface name="position"/>
|
||||
<command_interface name="velocity"/>
|
||||
<state_interface name="position">
|
||||
<param name="initial_value">${initial_positions['left_rev7']}</param>
|
||||
</state_interface>
|
||||
<state_interface name="velocity"/>
|
||||
</joint>
|
||||
<joint name="right_rev1">
|
||||
<command_interface name="position"/>
|
||||
<command_interface name="velocity"/>
|
||||
<state_interface name="position">
|
||||
<param name="initial_value">${initial_positions['right_rev1']}</param>
|
||||
</state_interface>
|
||||
<state_interface name="velocity"/>
|
||||
</joint>
|
||||
<joint name="right_rev2">
|
||||
<command_interface name="position"/>
|
||||
<command_interface name="velocity"/>
|
||||
<state_interface name="position">
|
||||
<param name="initial_value">${initial_positions['right_rev2']}</param>
|
||||
</state_interface>
|
||||
<state_interface name="velocity"/>
|
||||
</joint>
|
||||
<joint name="right_rev3">
|
||||
<command_interface name="position"/>
|
||||
<command_interface name="velocity"/>
|
||||
<state_interface name="position">
|
||||
<param name="initial_value">${initial_positions['right_rev3']}</param>
|
||||
</state_interface>
|
||||
<state_interface name="velocity"/>
|
||||
</joint>
|
||||
<joint name="right_rev4">
|
||||
<command_interface name="position"/>
|
||||
<command_interface name="velocity"/>
|
||||
<state_interface name="position">
|
||||
<param name="initial_value">${initial_positions['right_rev4']}</param>
|
||||
</state_interface>
|
||||
<state_interface name="velocity"/>
|
||||
</joint>
|
||||
<joint name="right_rev5">
|
||||
<command_interface name="position"/>
|
||||
<command_interface name="velocity"/>
|
||||
<state_interface name="position">
|
||||
<param name="initial_value">${initial_positions['right_rev5']}</param>
|
||||
</state_interface>
|
||||
<state_interface name="velocity"/>
|
||||
</joint>
|
||||
<joint name="right_rev6">
|
||||
<command_interface name="position"/>
|
||||
<command_interface name="velocity"/>
|
||||
<state_interface name="position">
|
||||
<param name="initial_value">${initial_positions['right_rev6']}</param>
|
||||
</state_interface>
|
||||
<state_interface name="velocity"/>
|
||||
</joint>
|
||||
<joint name="right_rev7">
|
||||
<command_interface name="position"/>
|
||||
<command_interface name="velocity"/>
|
||||
<state_interface name="position">
|
||||
<param name="initial_value">${initial_positions['right_rev7']}</param>
|
||||
</state_interface>
|
||||
<state_interface name="velocity"/>
|
||||
</joint>
|
||||
<joint name="left_left_pris1">
|
||||
<command_interface name="position"/>
|
||||
<command_interface name="velocity"/>
|
||||
<state_interface name="position">
|
||||
<param name="initial_value">${initial_positions['left_left_pris1']}</param>
|
||||
</state_interface>
|
||||
<state_interface name="velocity"/>
|
||||
</joint>
|
||||
<joint name="right_left_pris1">
|
||||
<command_interface name="position"/>
|
||||
<command_interface name="velocity"/>
|
||||
<state_interface name="position">
|
||||
<param name="initial_value">${initial_positions['right_left_pris1']}</param>
|
||||
</state_interface>
|
||||
<state_interface name="velocity"/>
|
||||
</joint>
|
||||
|
||||
</ros2_control>
|
||||
</xacro:macro>
|
||||
</robot>
|
||||
@ -18,6 +18,7 @@
|
||||
<link name="left_link6"/>
|
||||
<link name="left_link7"/>
|
||||
<link name="left_link8"/>
|
||||
<link name="left_gripper_center"/>
|
||||
<joint name="left_rev1"/>
|
||||
<joint name="left_rev2"/>
|
||||
<joint name="left_rev3"/>
|
||||
@ -25,6 +26,7 @@
|
||||
<joint name="left_rev5"/>
|
||||
<joint name="left_rev6"/>
|
||||
<joint name="left_rev7"/>
|
||||
<joint name="left_virtual_gripper_center"/>
|
||||
<chain base_link="left_link1" tip_link="left_link8"/>
|
||||
</group>
|
||||
<group name="right_arm">
|
||||
@ -36,6 +38,7 @@
|
||||
<link name="right_link6"/>
|
||||
<link name="right_link7"/>
|
||||
<link name="right_link8"/>
|
||||
<link name="right_gripper_center"/>
|
||||
<joint name="right_rev1"/>
|
||||
<joint name="right_rev2"/>
|
||||
<joint name="right_rev3"/>
|
||||
@ -43,20 +46,109 @@
|
||||
<joint name="right_rev5"/>
|
||||
<joint name="right_rev6"/>
|
||||
<joint name="right_rev7"/>
|
||||
<joint name="right_virtual_gripper_center"/>
|
||||
<chain base_link="right_link1" tip_link="right_link8"/>
|
||||
</group>
|
||||
<group name="left_gripper">
|
||||
<link name="left_link_left_jaw"/>
|
||||
<link name="left_link_right_jaw"/>
|
||||
<joint name="left_left_pris1"/>
|
||||
<joint name="left_right_pris2"/>
|
||||
</group>
|
||||
<group name="right_gripper">
|
||||
<link name="right_link_left_jaw"/>
|
||||
<joint name="right_left_pris1"/>
|
||||
</group>
|
||||
<group name="left_side">
|
||||
<group name="left_arm"/>
|
||||
<group name="left_gripper"/>
|
||||
</group>
|
||||
<group name="right_side">
|
||||
<group name="right_gripper"/>
|
||||
<group name="right_arm"/>
|
||||
</group>
|
||||
<group name="upper_body">
|
||||
<group name="left_side"/>
|
||||
<group name="right_side"/>
|
||||
</group>
|
||||
<!--GROUP STATES: Purpose: Define a named state for a particular group, in terms of joint values. This is useful to define states like 'folded arms'-->
|
||||
<group_state name="home" group="upper_body">
|
||||
<joint name="left_left_pris1" value="0"/>
|
||||
<joint name="left_rev1" value="0"/>
|
||||
<joint name="left_rev2" value="0"/>
|
||||
<joint name="left_rev3" value="0"/>
|
||||
<joint name="left_rev4" value="0"/>
|
||||
<joint name="left_rev5" value="0"/>
|
||||
<joint name="left_rev6" value="0"/>
|
||||
<joint name="left_rev7" value="0"/>
|
||||
<joint name="right_left_pris1" value="0"/>
|
||||
<joint name="right_rev1" value="0"/>
|
||||
<joint name="right_rev2" value="0"/>
|
||||
<joint name="right_rev3" value="0"/>
|
||||
<joint name="right_rev4" value="0"/>
|
||||
<joint name="right_rev5" value="0"/>
|
||||
<joint name="right_rev6" value="0"/>
|
||||
<joint name="right_rev7" value="0"/>
|
||||
</group_state>
|
||||
<group_state name="ready" group="upper_body">
|
||||
<joint name="left_left_pris1" value="0"/>
|
||||
<joint name="left_rev1" value="0.2"/>
|
||||
<joint name="left_rev2" value="0"/>
|
||||
<joint name="left_rev3" value="0"/>
|
||||
<joint name="left_rev4" value="1"/>
|
||||
<joint name="left_rev5" value="0"/>
|
||||
<joint name="left_rev6" value="0"/>
|
||||
<joint name="left_rev7" value="0"/>
|
||||
<joint name="right_left_pris1" value="0"/>
|
||||
<joint name="right_rev1" value="-0.2"/>
|
||||
<joint name="right_rev2" value="0"/>
|
||||
<joint name="right_rev3" value="0"/>
|
||||
<joint name="right_rev4" value="1"/>
|
||||
<joint name="right_rev5" value="0"/>
|
||||
<joint name="right_rev6" value="0"/>
|
||||
<joint name="right_rev7" value="0"/>
|
||||
</group_state>
|
||||
<group_state name="open" group="left_gripper">
|
||||
<joint name="left_left_pris1" value="0"/>
|
||||
</group_state>
|
||||
<group_state name="half_closed" group="left_gripper">
|
||||
<joint name="left_left_pris1" value="-0.0225"/>
|
||||
</group_state>
|
||||
<group_state name="closed" group="left_gripper">
|
||||
<joint name="left_left_pris1" value="-0.0451"/>
|
||||
</group_state>
|
||||
<group_state name="open" group="right_gripper">
|
||||
<joint name="right_left_pris1" value="0"/>
|
||||
</group_state>
|
||||
<group_state name="half_closed" group="right_gripper">
|
||||
<joint name="right_left_pris1" value="-0.0225"/>
|
||||
</group_state>
|
||||
<group_state name="center grip" group="upper_body">
|
||||
<joint name="left_left_pris1" value="0"/>
|
||||
<joint name="left_rev1" value="0.2"/>
|
||||
<joint name="left_rev2" value="0"/>
|
||||
<joint name="left_rev3" value="0.5"/>
|
||||
<joint name="left_rev4" value="1.6"/>
|
||||
<joint name="left_rev5" value="0"/>
|
||||
<joint name="left_rev6" value="0"/>
|
||||
<joint name="left_rev7" value="0"/>
|
||||
<joint name="right_left_pris1" value="0"/>
|
||||
<joint name="right_rev1" value="-0.2"/>
|
||||
<joint name="right_rev2" value="0"/>
|
||||
<joint name="right_rev3" value="-0.5"/>
|
||||
<joint name="right_rev4" value="1.8379"/>
|
||||
<joint name="right_rev5" value="0.0116"/>
|
||||
<joint name="right_rev6" value="-0.2517"/>
|
||||
<joint name="right_rev7" value="0.0053"/>
|
||||
</group_state>
|
||||
<!--END EFFECTOR: Purpose: Represent information about an end effector.-->
|
||||
<end_effector name="left_gripper" parent_link="left_link8" group="left_arm"/>
|
||||
<end_effector name="right_gripper" parent_link="right_link8" group="right_arm"/>
|
||||
<end_effector name="left_gripper" parent_link="left_link8" group="left_gripper" parent_group="left_side"/>
|
||||
<end_effector name="right_gripper" parent_link="right_link8" group="right_gripper" parent_group="right_side"/>
|
||||
<!--VIRTUAL JOINT: Purpose: this element defines a virtual joint between a robot link and an external frame of reference (considered fixed with respect to the robot)-->
|
||||
<virtual_joint name="virtual_joint" type="fixed" parent_frame="base_link" child_link="pedestal_link"/>
|
||||
<!--PASSIVE JOINT: Purpose: this element is used to mark joints that are not actuated-->
|
||||
<passive_joint name="left_right_pris2"/>
|
||||
<passive_joint name="right_right_pris2"/>
|
||||
<!--DISABLE COLLISIONS: By default it is assumed that any link of the robot could potentially come into collision with any other link in the robot. This tag disables collision checking between a specified pair of links. -->
|
||||
<disable_collisions link1="camera_link" link2="pedestal_link" reason="Adjacent"/>
|
||||
<disable_collisions link1="left_link1" link2="left_link2" reason="Adjacent"/>
|
||||
<disable_collisions link1="left_link1" link2="left_link3" reason="User"/>
|
||||
<disable_collisions link1="left_link1" link2="left_link4" reason="User"/>
|
||||
|
||||
@ -5,4 +5,16 @@
|
||||
<!-- Import openarm_bimanual urdf file -->
|
||||
<xacro:include filename="$(find openarm_bimanual_description)/urdf/openarm_bimanual.urdf.xacro" />
|
||||
|
||||
<link name="base_link"/>
|
||||
<joint name="world_to_pedestal" type="fixed">
|
||||
<parent link="world"/>
|
||||
<child link="base_link"/>
|
||||
<origin xyz="0.0 0.0 0.0" rpy="0.0 0.0 0.0"/>
|
||||
</joint>
|
||||
<!-- Import control_xacro -->
|
||||
<!-- <xacro:include filename="openarm_bimanual.ros2_control.xacro" />
|
||||
|
||||
|
||||
<xacro:openarm_bimanual_ros2_control name="FakeSystem" initial_positions_file="$(arg initial_positions_file)"/> -->
|
||||
|
||||
</robot>
|
||||
|
||||
@ -11,6 +11,26 @@ controller_manager:
|
||||
type: joint_trajectory_controller/JointTrajectoryController
|
||||
|
||||
|
||||
left_gripper_controller:
|
||||
type: position_controllers/GripperActionController
|
||||
|
||||
|
||||
right_gripper_controller:
|
||||
type: position_controllers/GripperActionController
|
||||
|
||||
|
||||
left_side_controller:
|
||||
type: joint_trajectory_controller/JointTrajectoryController
|
||||
|
||||
|
||||
right_side_controller:
|
||||
type: joint_trajectory_controller/JointTrajectoryController
|
||||
|
||||
|
||||
upper_body_controller:
|
||||
type: joint_trajectory_controller/JointTrajectoryController
|
||||
|
||||
|
||||
joint_state_broadcaster:
|
||||
type: joint_state_broadcaster/JointStateBroadcaster
|
||||
|
||||
@ -46,3 +66,68 @@ right_arm_controller:
|
||||
state_interfaces:
|
||||
- position
|
||||
- velocity
|
||||
left_gripper_controller:
|
||||
ros__parameters:
|
||||
joint: left_left_pris1
|
||||
right_gripper_controller:
|
||||
ros__parameters:
|
||||
joint: right_left_pris1
|
||||
left_side_controller:
|
||||
ros__parameters:
|
||||
joints:
|
||||
- left_rev1
|
||||
- left_rev2
|
||||
- left_rev3
|
||||
- left_rev4
|
||||
- left_rev5
|
||||
- left_rev6
|
||||
- left_rev7
|
||||
- left_left_pris1
|
||||
command_interfaces:
|
||||
- position
|
||||
- velocity
|
||||
state_interfaces:
|
||||
- position
|
||||
- velocity
|
||||
right_side_controller:
|
||||
ros__parameters:
|
||||
joints:
|
||||
- right_rev1
|
||||
- right_rev2
|
||||
- right_rev3
|
||||
- right_rev4
|
||||
- right_rev5
|
||||
- right_rev6
|
||||
- right_rev7
|
||||
- right_left_pris1
|
||||
command_interfaces:
|
||||
- position
|
||||
- velocity
|
||||
state_interfaces:
|
||||
- position
|
||||
- velocity
|
||||
upper_body_controller:
|
||||
ros__parameters:
|
||||
joints:
|
||||
- left_rev1
|
||||
- left_rev2
|
||||
- left_rev3
|
||||
- left_rev4
|
||||
- left_rev5
|
||||
- left_rev6
|
||||
- left_rev7
|
||||
- left_left_pris1
|
||||
- right_rev1
|
||||
- right_rev2
|
||||
- right_rev3
|
||||
- right_rev4
|
||||
- right_rev5
|
||||
- right_rev6
|
||||
- right_rev7
|
||||
- right_left_pris1
|
||||
command_interfaces:
|
||||
- position
|
||||
- velocity
|
||||
state_interfaces:
|
||||
- position
|
||||
- velocity
|
||||
@ -2,16 +2,14 @@ sensors:
|
||||
- default_sensor
|
||||
- kinect_depthimage
|
||||
default_sensor:
|
||||
far_clipping_plane_distance: 5.0
|
||||
filtered_cloud_topic: filtered_cloud
|
||||
image_topic: /head_mount_kinect/depth_registered/image_raw
|
||||
max_range: 5.0
|
||||
max_update_rate: 1.0
|
||||
near_clipping_plane_distance: 0.3
|
||||
padding_offset: 0.03
|
||||
padding_scale: 4.0
|
||||
queue_size: 5
|
||||
sensor_plugin: occupancy_map_monitor/DepthImageOctomapUpdater
|
||||
shadow_threshold: 0.2
|
||||
padding_offset: 0.1
|
||||
padding_scale: 1.0
|
||||
point_cloud_topic: /camera/camera/depth/color/points
|
||||
point_subsample: 1
|
||||
sensor_plugin: occupancy_map_monitor/PointCloudOctomapUpdater
|
||||
kinect_depthimage:
|
||||
far_clipping_plane_distance: 5.0
|
||||
filtered_cloud_topic: filtered_cloud
|
||||
|
||||
@ -2,49 +2,47 @@ controller_manager:
|
||||
ros__parameters:
|
||||
update_rate: 100 # Hz
|
||||
|
||||
left_arm_controller:
|
||||
type: joint_trajectory_controller/JointTrajectoryController
|
||||
|
||||
|
||||
right_arm_controller:
|
||||
type: joint_trajectory_controller/JointTrajectoryController
|
||||
|
||||
joint_state_broad:
|
||||
type: joint_state_broadcaster/JointStateBroadcaster
|
||||
|
||||
left_arm_controller:
|
||||
ros__parameters:
|
||||
joints:
|
||||
- left_rev1
|
||||
- left_rev2
|
||||
- left_rev3
|
||||
- left_rev4
|
||||
- left_rev5
|
||||
- left_rev6
|
||||
- left_rev7
|
||||
command_interfaces:
|
||||
- position
|
||||
- velocity
|
||||
- effort
|
||||
state_interfaces:
|
||||
- position
|
||||
- velocity
|
||||
- effort
|
||||
right_arm_controller:
|
||||
ros__parameters:
|
||||
joints:
|
||||
- right_rev1
|
||||
- right_rev2
|
||||
- right_rev3
|
||||
- right_rev4
|
||||
- right_rev5
|
||||
- right_rev6
|
||||
- right_rev7
|
||||
command_interfaces:
|
||||
- position
|
||||
- velocity
|
||||
- effort
|
||||
state_interfaces:
|
||||
- position
|
||||
- velocity
|
||||
- effort
|
||||
# left_arm_controller:
|
||||
# type: joint_trajectory_controller/JointTrajectoryController
|
||||
|
||||
# right_arm_controller:
|
||||
# type: joint_trajectory_controller/JointTrajectoryController
|
||||
|
||||
|
||||
# left_arm_controller:
|
||||
# ros__parameters:
|
||||
# joints:
|
||||
# - left_rev1
|
||||
# - left_rev2
|
||||
# - left_rev3
|
||||
# - left_rev4
|
||||
# - left_rev5
|
||||
# - left_rev6
|
||||
# - left_rev7
|
||||
# command_interfaces:
|
||||
# - position
|
||||
# - velocity
|
||||
# state_interfaces:
|
||||
# - position
|
||||
# - velocity
|
||||
# - effort
|
||||
# right_arm_controller:
|
||||
# ros__parameters:
|
||||
# joints:
|
||||
# - right_rev1
|
||||
# - right_rev2
|
||||
# - right_rev3
|
||||
# - right_rev4
|
||||
# - right_rev5
|
||||
# - right_rev6
|
||||
# - right_rev7
|
||||
# command_interfaces:
|
||||
# - position
|
||||
# - velocity
|
||||
# state_interfaces:
|
||||
# - position
|
||||
# - velocity
|
||||
# - effort
|
||||
@ -240,15 +240,15 @@
|
||||
<parent link="${prefix}link2"/>
|
||||
<child link="${prefix}link3"/>
|
||||
<origin rpy="-1.5707963267948968 ${pi/2 if zero_pos=='up' else 0.0} 0.0" xyz="0.0 -0.02975 0.04475"/>
|
||||
<axis xyz="0 0 ${reflect * -1}"/>
|
||||
<limit effort="0.0" lower="${-pi/2.0 if zero_pos=='up' else -pi + rotate}" upper="${pi/2 if zero_pos=='up' else 0 + rotate}" velocity="0.0"/>
|
||||
<axis xyz="0 0 1"/>
|
||||
<limit effort="0.0" lower="${-pi/2.0 if zero_pos=='up' else 0}" upper="${pi/2 if zero_pos=='up' else pi}" velocity="0.0"/>
|
||||
</joint>
|
||||
<joint name="${prefix}rev3" type="revolute">
|
||||
<parent link="${prefix}link3"/>
|
||||
<child link="${prefix}link4"/>
|
||||
<origin rpy="-1.5707963267949054 ${rotate} -1.562038143900564" xyz="-0.0612477 -0.000536432 0.02975"/>
|
||||
<axis xyz="0 0 -1"/>
|
||||
<limit effort="0.0" lower="-3.6651914291880923" upper="0.5235987755982988" velocity="0.0"/>
|
||||
<limit effort="0.0" lower="${-0.52359 if side=='right' else -3.66519}" upper="${3.66519 if side=='right' else 0.52359}" velocity="0.0"/>
|
||||
</joint>
|
||||
<joint name="${prefix}rev4" type="revolute">
|
||||
<parent link="${prefix}link4"/>
|
||||
@ -282,14 +282,14 @@
|
||||
<parent link="${prefix}link8"/>
|
||||
<child link="${prefix}link_left_jaw"/>
|
||||
<origin rpy="1.570796326795101 -0.014066001454929162 0.00875904933542146" xyz="-0.1071 0.0768568 0.0132053"/>
|
||||
<axis xyz="0 0 1"/>
|
||||
<limit effort="0.0" lower="-0.0451" upper="0.0" velocity="0.0"/>
|
||||
<axis xyz="0 0 -1"/>
|
||||
<limit effort="0.0" lower="-0.0451" upper="0.0" velocity="0.1"/>
|
||||
</joint>
|
||||
<joint name="${prefix}right_pris2" type="prismatic">
|
||||
<parent link="${prefix}link8"/>
|
||||
<child link="${prefix}link_right_jaw"/>
|
||||
<origin rpy="1.570796326794883 -0.014066001454927403 0.008759049336290027" xyz="-0.10571 -0.0781373 0.0132053"/>
|
||||
<axis xyz="0 0 1"/>
|
||||
<axis xyz="0 0 -1"/>
|
||||
<limit effort="0.0" lower="-0.0451" upper="0.0" velocity="0.0"/>
|
||||
<mimic joint="${prefix}left_pris1" multiplier="-1.0" offset="0.0"/>
|
||||
</joint>
|
||||
|
||||
@ -43,13 +43,13 @@ static const std::size_t ARM_DOF = 7;
|
||||
static const std::size_t GRIPPER_DOF = 1;
|
||||
static const std::size_t TOTAL_DOF = ARM_DOF + GRIPPER_DOF;
|
||||
static const std::array<double, TOTAL_DOF> KP = {80.0, 80.0, 20.0, 55.0, 5.0, 5.0, 5.0, 0.5};
|
||||
static const std::array<double, TOTAL_DOF> KD = {1.25, 0.17, 0.015, 0.07, 0.07, 0.05, 0.05, 0.01};
|
||||
static const std::array<double, TOTAL_DOF> SLOW_KP = {10.0, 10.0, 10.0, 7.5, 5.0, 5.0, 5.0, 0.5};
|
||||
static const std::array<double, TOTAL_DOF> KD = {2.75, 2.5, 0.7, 0.4, 0.7, 0.6, 0.5, 0.1};
|
||||
static const double START_POS_TOLERANCE_RAD = 0.1;
|
||||
static const double POS_JUMP_TOLERANCE_RAD = 3.1415 / 2.0;
|
||||
static const double POS_JUMP_TOLERANCE_RAD = 3.1415 / 16.0;
|
||||
|
||||
static const bool USING_GRIPPER = true;
|
||||
static const double GRIPPER_REFERENCE_GEAR_RADIUS_M = 0.00853;
|
||||
static const double GRIPPER_GEAR_DIRECTION_MULTIPLIER = -1.0;
|
||||
static const int GRIPPER_INDEX = TOTAL_DOF - 1;
|
||||
|
||||
class OpenArmHW : public hardware_interface::SystemInterface
|
||||
|
||||
@ -71,9 +71,7 @@ hardware_interface::CallbackReturn OpenArmHW::on_init(
|
||||
motors_.resize(curr_dof);
|
||||
for(size_t i = 0; i < curr_dof; ++i){
|
||||
motors_[i] = std::make_unique<Motor>(motor_types[i], can_device_ids[i], can_master_ids[i]);
|
||||
}
|
||||
for(const auto& motor: motors_){
|
||||
motor_control_->addMotor(*motor);
|
||||
motor_control_->addMotor(*motors_[i]);
|
||||
}
|
||||
|
||||
pos_states_.resize(curr_dof, 0.0);
|
||||
@ -141,27 +139,39 @@ hardware_interface::CallbackReturn OpenArmHW::on_activate(
|
||||
const rclcpp_lifecycle::State & /*previous_state*/)
|
||||
{
|
||||
read(rclcpp::Time(0), rclcpp::Duration(0, 0));
|
||||
|
||||
// for (std::size_t m = 0; m < curr_dof; ++m){
|
||||
// double diff = pos_states_[m] - pos_commands_[m];
|
||||
// while(abs(diff) > START_POS_TOLERANCE_RAD){
|
||||
// // linear interpolation
|
||||
// // take the min of max_step and the difference
|
||||
|
||||
// double max_step = std::min(POS_JUMP_TOLERANCE_RAD, std::abs(diff));
|
||||
// if (diff > 0){
|
||||
// pos_commands_[m] = pos_states_[m] - max_step;
|
||||
// }
|
||||
// else{
|
||||
// pos_commands_[m] = pos_states_[m] + max_step;
|
||||
// }
|
||||
// motor_control_->controlMIT(*motors_[m], SLOW_KP[m], KD[m], pos_commands_[m], 0.0, 0.0);
|
||||
// }
|
||||
// }
|
||||
refresh_motors();
|
||||
bool zeroed = false;
|
||||
for(const auto& motor: motors_){
|
||||
motor_control_->enable(*motor);
|
||||
}
|
||||
|
||||
while(!zeroed){
|
||||
bool all_zero = true;
|
||||
for (std::size_t m = 0; m < curr_dof; ++m){
|
||||
const double diff = pos_commands_[m] - pos_states_[m];
|
||||
if (std::abs(diff) > START_POS_TOLERANCE_RAD){
|
||||
all_zero = false;
|
||||
}
|
||||
|
||||
const double max_step = std::min(POS_JUMP_TOLERANCE_RAD, std::abs(diff));
|
||||
double command = pos_states_[m];
|
||||
if (pos_states_[m] < pos_commands_[m]){
|
||||
command += max_step;
|
||||
}
|
||||
else{
|
||||
command -= max_step;
|
||||
}
|
||||
motor_control_->controlMIT(*motors_[m], KP[m], KD[m], command, 0.0, 0.0);
|
||||
|
||||
}
|
||||
if (all_zero){
|
||||
zeroed = true;
|
||||
}
|
||||
else{
|
||||
sleep(0.01);
|
||||
read(rclcpp::Time(0), rclcpp::Duration(0, 0));
|
||||
}
|
||||
}
|
||||
|
||||
read(rclcpp::Time(0), rclcpp::Duration(0, 0));
|
||||
|
||||
return CallbackReturn::SUCCESS;
|
||||
@ -187,9 +197,9 @@ hardware_interface::return_type OpenArmHW::read(
|
||||
tau_states_[i] = motors_[i]->getTorque();
|
||||
}
|
||||
if(USING_GRIPPER){
|
||||
pos_states_[GRIPPER_INDEX] = -motors_[GRIPPER_INDEX]->getPosition() * GRIPPER_REFERENCE_GEAR_RADIUS_M;
|
||||
vel_states_[GRIPPER_INDEX] = motors_[GRIPPER_INDEX]->getVelocity() * GRIPPER_REFERENCE_GEAR_RADIUS_M;
|
||||
tau_states_[GRIPPER_INDEX] = motors_[GRIPPER_INDEX]->getTorque() * GRIPPER_REFERENCE_GEAR_RADIUS_M;
|
||||
pos_states_[GRIPPER_INDEX] = -motors_[GRIPPER_INDEX]->getPosition() * GRIPPER_REFERENCE_GEAR_RADIUS_M * GRIPPER_GEAR_DIRECTION_MULTIPLIER;
|
||||
vel_states_[GRIPPER_INDEX] = motors_[GRIPPER_INDEX]->getVelocity() * GRIPPER_REFERENCE_GEAR_RADIUS_M * GRIPPER_GEAR_DIRECTION_MULTIPLIER;
|
||||
tau_states_[GRIPPER_INDEX] = motors_[GRIPPER_INDEX]->getTorque() * GRIPPER_REFERENCE_GEAR_RADIUS_M * GRIPPER_GEAR_DIRECTION_MULTIPLIER;
|
||||
}
|
||||
|
||||
|
||||
@ -201,7 +211,7 @@ hardware_interface::return_type OpenArmHW::write(
|
||||
{
|
||||
if (disable_torque_){
|
||||
// refresh motor state on write
|
||||
for(size_t i = 0; i < TOTAL_DOF; ++i){
|
||||
for(size_t i = 0; i < curr_dof; ++i){
|
||||
motor_control_->controlMIT(*motors_[i], 0.0, 0.0, 0.0, 0.0, 0.0);
|
||||
return hardware_interface::return_type::OK;
|
||||
}
|
||||
@ -217,7 +227,10 @@ hardware_interface::return_type OpenArmHW::write(
|
||||
motor_control_->controlMIT(*motors_[i], KP.at(i), KD.at(i), pos_commands_[i], vel_commands_[i], tau_ff_commands_[i]);
|
||||
}
|
||||
if(USING_GRIPPER){
|
||||
motor_control_->controlMIT(*motors_[GRIPPER_INDEX], KP.at(GRIPPER_INDEX), KD.at(GRIPPER_INDEX), -pos_commands_[GRIPPER_INDEX] / GRIPPER_REFERENCE_GEAR_RADIUS_M, vel_commands_[GRIPPER_INDEX] / GRIPPER_REFERENCE_GEAR_RADIUS_M, tau_ff_commands_[GRIPPER_INDEX] / GRIPPER_REFERENCE_GEAR_RADIUS_M);
|
||||
motor_control_->controlMIT(*motors_[GRIPPER_INDEX], KP.at(GRIPPER_INDEX), KD.at(GRIPPER_INDEX),
|
||||
-pos_commands_[GRIPPER_INDEX] / GRIPPER_REFERENCE_GEAR_RADIUS_M * GRIPPER_GEAR_DIRECTION_MULTIPLIER,
|
||||
vel_commands_[GRIPPER_INDEX] / GRIPPER_REFERENCE_GEAR_RADIUS_M * GRIPPER_GEAR_DIRECTION_MULTIPLIER,
|
||||
tau_ff_commands_[GRIPPER_INDEX] / GRIPPER_REFERENCE_GEAR_RADIUS_M * GRIPPER_GEAR_DIRECTION_MULTIPLIER);
|
||||
}
|
||||
return hardware_interface::return_type::OK;
|
||||
}
|
||||
|
||||
Loading…
Reference in New Issue
Block a user