openarm/openarm_ros2
1
0
Fork 0
Code Issues Pull Requests Actions 1 Packages Projects Releases Wiki Activity

Add moveit2 package with dual arm/gripper (#5)

Browse Source 
* 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 2025-04-22 18:46:27 +09:00 committed by GitHub
parent ce06ea14c1
commit 1a1084c627
No known key found for this signature in database
GPG Key ID: B5690EEEBB952194
16 changed files with 471 additions and 99 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

4
openarm_bimanual_description/urdf/openarm_bimanual.urdf.xacro
View File

@ -2,8 +2,8 @@
<robot xmlns:xacro="http://www.ros.org/wiki/xacro" name="openarm_bimanual"> <robot xmlns:xacro="http://www.ros.org/wiki/xacro" name="openarm_bimanual">
<xacro:include filename="$(find openarm_description)/urdf/openarm.xacro"/> <xacro:include filename="$(find openarm_description)/urdf/openarm.xacro"/>
<xacro:include filename="openarm_pedestal.urdf"/> <xacro:include filename="$(find openarm_bimanual_description)/urdf/openarm_pedestal.urdf"/>
<!-- <xacro:include filename="openarm_bimanual_sensors.xacro"/> --> <xacro:include filename="openarm_bimanual_sensors.xacro"/>
<link name="world"/> <link name="world"/>
<joint name="dummy_joint" type="fixed"> <joint name="dummy_joint" type="fixed">

2
openarm_bimanual_description/urdf/openarm_bimanual_sensors.xacro
View File

@ -2,7 +2,7 @@
<robot xmlns:xacro="http://www.ros.org/wiki/xacro"> <robot xmlns:xacro="http://www.ros.org/wiki/xacro">
<xacro:include filename="$(find realsense2_description)/urdf/_d435i.urdf.xacro"/> <xacro:include filename="$(find realsense2_description)/urdf/_d435i.urdf.xacro"/>
<xacro:sensor_d435i parent="pedestal_link" use_nominal_extrinsics="true"> <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> </xacro:sensor_d435i>
</robot> </robot>

2
openarm_bimanual_moveit_config/.setup_assistant
View File

@ -7,4 +7,4 @@ moveit_setup_assistant_config:
package_settings: package_settings:
author_name: Thomason Zhou author_name: Thomason Zhou
author_email: t95zhou@uwaterloo.ca author_email: t95zhou@uwaterloo.ca
generated_timestamp: 1744266716 generated_timestamp: 1745222265

2
openarm_bimanual_moveit_config/config/initial_positions.yaml
View File

@ -1,6 +1,7 @@
# Default initial positions for openarm_bimanual's ros2_control fake system # Default initial positions for openarm_bimanual's ros2_control fake system
initial_positions: initial_positions:
left_left_pris1: 0
left_rev1: 0 left_rev1: 0
left_rev2: 0 left_rev2: 0
left_rev3: 0 left_rev3: 0
@ -8,6 +9,7 @@ initial_positions:
left_rev5: 0 left_rev5: 0
left_rev6: 0 left_rev6: 0
left_rev7: 0 left_rev7: 0
right_left_pris1: 0
right_rev1: 0 right_rev1: 0
right_rev2: 0 right_rev2: 0
right_rev3: 0 right_rev3: 0

10
openarm_bimanual_moveit_config/config/joint_limits.yaml
View File

@ -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 can be turned off with [has_velocity_limits, has_acceleration_limits]
joint_limits: joint_limits:
left_left_pris1: left_left_pris1:
has_velocity_limits: false has_velocity_limits: true
max_velocity: 0 max_velocity: 0.10000000000000001
has_acceleration_limits: false has_acceleration_limits: false
max_acceleration: 0 max_acceleration: 0
left_rev1: left_rev1:
@ -48,9 +48,9 @@ joint_limits:
max_velocity: 0 max_velocity: 0
has_acceleration_limits: false has_acceleration_limits: false
max_acceleration: 0 max_acceleration: 0
left_right_pris2: right_left_pris1:
has_velocity_limits: false has_velocity_limits: true
max_velocity: 0 max_velocity: 0.10000000000000001
has_acceleration_limits: false has_acceleration_limits: false
max_acceleration: 0 max_acceleration: 0
right_rev1: right_rev1:

16
openarm_bimanual_moveit_config/config/kinematics.yaml
View File

@ -2,3 +2,19 @@ left_arm:
kinematics_solver: kdl_kinematics_plugin/KDLKinematicsPlugin kinematics_solver: kdl_kinematics_plugin/KDLKinematicsPlugin
kinematics_solver_search_resolution: 0.0050000000000000001 kinematics_solver_search_resolution: 0.0050000000000000001
kinematics_solver_timeout: 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

14
openarm_bimanual_moveit_config/config/moveit_controllers.yaml
View File

@ -6,6 +6,8 @@ moveit_simple_controller_manager:
controller_names: controller_names:
- left_arm_controller - left_arm_controller
- right_arm_controller - right_arm_controller
- left_gripper_controller
- right_gripper_controller
left_arm_controller: left_arm_controller:
type: FollowJointTrajectory type: FollowJointTrajectory
@ -31,3 +33,15 @@ moveit_simple_controller_manager:
- right_rev7 - right_rev7
action_ns: follow_joint_trajectory action_ns: follow_joint_trajectory
default: true 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

142
openarm_bimanual_moveit_config/config/openarm_bimanual.ros2_control.xacro Normal file
View File

@ -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>

100
openarm_bimanual_moveit_config/config/openarm_bimanual.srdf
View File

@ -18,6 +18,7 @@
<link name="left_link6"/> <link name="left_link6"/>
<link name="left_link7"/> <link name="left_link7"/>
<link name="left_link8"/> <link name="left_link8"/>
<link name="left_gripper_center"/>
<joint name="left_rev1"/> <joint name="left_rev1"/>
<joint name="left_rev2"/> <joint name="left_rev2"/>
<joint name="left_rev3"/> <joint name="left_rev3"/>
@ -25,6 +26,7 @@
<joint name="left_rev5"/> <joint name="left_rev5"/>
<joint name="left_rev6"/> <joint name="left_rev6"/>
<joint name="left_rev7"/> <joint name="left_rev7"/>
<joint name="left_virtual_gripper_center"/>
<chain base_link="left_link1" tip_link="left_link8"/> <chain base_link="left_link1" tip_link="left_link8"/>
</group> </group>
<group name="right_arm"> <group name="right_arm">
@ -36,6 +38,7 @@
<link name="right_link6"/> <link name="right_link6"/>
<link name="right_link7"/> <link name="right_link7"/>
<link name="right_link8"/> <link name="right_link8"/>
<link name="right_gripper_center"/>
<joint name="right_rev1"/> <joint name="right_rev1"/>
<joint name="right_rev2"/> <joint name="right_rev2"/>
<joint name="right_rev3"/> <joint name="right_rev3"/>
@ -43,20 +46,109 @@
<joint name="right_rev5"/> <joint name="right_rev5"/>
<joint name="right_rev6"/> <joint name="right_rev6"/>
<joint name="right_rev7"/> <joint name="right_rev7"/>
<joint name="right_virtual_gripper_center"/>
<chain base_link="right_link1" tip_link="right_link8"/> <chain base_link="right_link1" tip_link="right_link8"/>
</group> </group>
<group name="left_gripper"> <group name="left_gripper">
<link name="left_link_left_jaw"/> <link name="left_link_left_jaw"/>
<link name="left_link_right_jaw"/>
<joint name="left_left_pris1"/> <joint name="left_left_pris1"/>
<joint name="left_right_pris2"/>
</group> </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: Purpose: Represent information about an end effector.-->
<end_effector name="left_gripper" parent_link="left_link8" group="left_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_arm"/> <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: 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"/> <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: 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_link2" reason="Adjacent"/>
<disable_collisions link1="left_link1" link2="left_link3" reason="User"/> <disable_collisions link1="left_link1" link2="left_link3" reason="User"/>
<disable_collisions link1="left_link1" link2="left_link4" reason="User"/> <disable_collisions link1="left_link1" link2="left_link4" reason="User"/>

12
openarm_bimanual_moveit_config/config/openarm_bimanual.urdf.xacro
View File

@ -5,4 +5,16 @@
<!-- Import openarm_bimanual urdf file --> <!-- Import openarm_bimanual urdf file -->
<xacro:include filename="$(find openarm_bimanual_description)/urdf/openarm_bimanual.urdf.xacro" /> <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> </robot>

85
openarm_bimanual_moveit_config/config/ros2_controllers.yaml
View File

@ -11,6 +11,26 @@ controller_manager:
type: joint_trajectory_controller/JointTrajectoryController 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: joint_state_broadcaster:
type: joint_state_broadcaster/JointStateBroadcaster type: joint_state_broadcaster/JointStateBroadcaster
@ -46,3 +66,68 @@ right_arm_controller:
state_interfaces: state_interfaces:
- position - position
- velocity - 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

14
openarm_bimanual_moveit_config/config/sensors_3d.yaml
View File

@ -2,16 +2,14 @@ sensors:
- default_sensor - default_sensor
- kinect_depthimage - kinect_depthimage
default_sensor: default_sensor:
far_clipping_plane_distance: 5.0
filtered_cloud_topic: filtered_cloud filtered_cloud_topic: filtered_cloud
image_topic: /head_mount_kinect/depth_registered/image_raw max_range: 5.0
max_update_rate: 1.0 max_update_rate: 1.0
near_clipping_plane_distance: 0.3 padding_offset: 0.1
padding_offset: 0.03 padding_scale: 1.0
padding_scale: 4.0 point_cloud_topic: /camera/camera/depth/color/points
queue_size: 5 point_subsample: 1
sensor_plugin: occupancy_map_monitor/DepthImageOctomapUpdater sensor_plugin: occupancy_map_monitor/PointCloudOctomapUpdater
shadow_threshold: 0.2
kinect_depthimage: kinect_depthimage:
far_clipping_plane_distance: 5.0 far_clipping_plane_distance: 5.0
filtered_cloud_topic: filtered_cloud filtered_cloud_topic: filtered_cloud

84
openarm_bimanual_teleop/config/controllers.yaml
View File

@ -2,49 +2,47 @@ controller_manager:
ros__parameters: ros__parameters:
update_rate: 100 # Hz update_rate: 100 # Hz
left_arm_controller:
type: joint_trajectory_controller/JointTrajectoryController
right_arm_controller:
type: joint_trajectory_controller/JointTrajectoryController
joint_state_broad: joint_state_broad:
type: joint_state_broadcaster/JointStateBroadcaster type: joint_state_broadcaster/JointStateBroadcaster
left_arm_controller: # left_arm_controller:
ros__parameters: # type: joint_trajectory_controller/JointTrajectoryController
joints:
- left_rev1 # right_arm_controller:
- left_rev2 # type: joint_trajectory_controller/JointTrajectoryController
- left_rev3
- left_rev4
- left_rev5 # left_arm_controller:
- left_rev6 # ros__parameters:
- left_rev7 # joints:
command_interfaces: # - left_rev1
- position # - left_rev2
- velocity # - left_rev3
- effort # - left_rev4
state_interfaces: # - left_rev5
- position # - left_rev6
- velocity # - left_rev7
- effort # command_interfaces:
right_arm_controller: # - position
ros__parameters: # - velocity
joints: # state_interfaces:
- right_rev1 # - position
- right_rev2 # - velocity
- right_rev3 # - effort
- right_rev4 # right_arm_controller:
- right_rev5 # ros__parameters:
- right_rev6 # joints:
- right_rev7 # - right_rev1
command_interfaces: # - right_rev2
- position # - right_rev3
- velocity # - right_rev4
- effort # - right_rev5
state_interfaces: # - right_rev6
- position # - right_rev7
- velocity # command_interfaces:
- effort # - position
# - velocity
# state_interfaces:
# - position
# - velocity
# - effort

12
openarm_description/urdf/openarm.xacro
View File

@ -240,15 +240,15 @@
<parent link="${prefix}link2"/> <parent link="${prefix}link2"/>
<child link="${prefix}link3"/> <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"/> <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}"/> <axis xyz="0 0 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"/> <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>
<joint name="${prefix}rev3" type="revolute"> <joint name="${prefix}rev3" type="revolute">
<parent link="${prefix}link3"/> <parent link="${prefix}link3"/>
<child link="${prefix}link4"/> <child link="${prefix}link4"/>
<origin rpy="-1.5707963267949054 ${rotate} -1.562038143900564" xyz="-0.0612477 -0.000536432 0.02975"/> <origin rpy="-1.5707963267949054 ${rotate} -1.562038143900564" xyz="-0.0612477 -0.000536432 0.02975"/>
<axis xyz="0 0 -1"/> <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>
<joint name="${prefix}rev4" type="revolute"> <joint name="${prefix}rev4" type="revolute">
<parent link="${prefix}link4"/> <parent link="${prefix}link4"/>
@ -282,14 +282,14 @@
<parent link="${prefix}link8"/> <parent link="${prefix}link8"/>
<child link="${prefix}link_left_jaw"/> <child link="${prefix}link_left_jaw"/>
<origin rpy="1.570796326795101 -0.014066001454929162 0.00875904933542146" xyz="-0.1071 0.0768568 0.0132053"/> <origin rpy="1.570796326795101 -0.014066001454929162 0.00875904933542146" xyz="-0.1071 0.0768568 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"/> <limit effort="0.0" lower="-0.0451" upper="0.0" velocity="0.1"/>
</joint> </joint>
<joint name="${prefix}right_pris2" type="prismatic"> <joint name="${prefix}right_pris2" type="prismatic">
<parent link="${prefix}link8"/> <parent link="${prefix}link8"/>
<child link="${prefix}link_right_jaw"/> <child link="${prefix}link_right_jaw"/>
<origin rpy="1.570796326794883 -0.014066001454927403 0.008759049336290027" xyz="-0.10571 -0.0781373 0.0132053"/> <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"/> <limit effort="0.0" lower="-0.0451" upper="0.0" velocity="0.0"/>
<mimic joint="${prefix}left_pris1" multiplier="-1.0" offset="0.0"/> <mimic joint="${prefix}left_pris1" multiplier="-1.0" offset="0.0"/>
</joint> </joint>

6
openarm_hardware/include/openarm_hardware/openarm_hardware.hpp
View File

@ -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 GRIPPER_DOF = 1;
static const std::size_t TOTAL_DOF = ARM_DOF + GRIPPER_DOF; 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> 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> KD = {2.75, 2.5, 0.7, 0.4, 0.7, 0.6, 0.5, 0.1};
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 double START_POS_TOLERANCE_RAD = 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 bool USING_GRIPPER = true;
static const double GRIPPER_REFERENCE_GEAR_RADIUS_M = 0.00853; 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; static const int GRIPPER_INDEX = TOTAL_DOF - 1;
class OpenArmHW : public hardware_interface::SystemInterface class OpenArmHW : public hardware_interface::SystemInterface

65
openarm_hardware/src/openarm_hardware.cpp
View File

@ -71,9 +71,7 @@ hardware_interface::CallbackReturn OpenArmHW::on_init(
motors_.resize(curr_dof); motors_.resize(curr_dof);
for(size_t i = 0; i < curr_dof; ++i){ 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]); motors_[i] = std::make_unique<Motor>(motor_types[i], can_device_ids[i], can_master_ids[i]);
} motor_control_->addMotor(*motors_[i]);
for(const auto& motor: motors_){
motor_control_->addMotor(*motor);
} }
pos_states_.resize(curr_dof, 0.0); pos_states_.resize(curr_dof, 0.0);
@ -141,27 +139,39 @@ hardware_interface::CallbackReturn OpenArmHW::on_activate(
const rclcpp_lifecycle::State & /*previous_state*/) const rclcpp_lifecycle::State & /*previous_state*/)
{ {
read(rclcpp::Time(0), rclcpp::Duration(0, 0)); read(rclcpp::Time(0), rclcpp::Duration(0, 0));
bool zeroed = false;
// 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();
for(const auto& motor: motors_){ for(const auto& motor: motors_){
motor_control_->enable(*motor); 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)); read(rclcpp::Time(0), rclcpp::Duration(0, 0));
return CallbackReturn::SUCCESS; return CallbackReturn::SUCCESS;
@ -187,9 +197,9 @@ hardware_interface::return_type OpenArmHW::read(
tau_states_[i] = motors_[i]->getTorque(); tau_states_[i] = motors_[i]->getTorque();
} }
if(USING_GRIPPER){ if(USING_GRIPPER){
pos_states_[GRIPPER_INDEX] = -motors_[GRIPPER_INDEX]->getPosition() * 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; 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; 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_){ if (disable_torque_){
// refresh motor state on write // 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); motor_control_->controlMIT(*motors_[i], 0.0, 0.0, 0.0, 0.0, 0.0);
return hardware_interface::return_type::OK; 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]); motor_control_->controlMIT(*motors_[i], KP.at(i), KD.at(i), pos_commands_[i], vel_commands_[i], tau_ff_commands_[i]);
} }
if(USING_GRIPPER){ 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; return hardware_interface::return_type::OK;
} }