Merge pull request #5 from ljq-lv/master

Update 22 season engineer's code
rm-controls · Nov 3, 2022 · 987a402 · 987a402
2 parents 89bf8dd + 9c12e3c
commit 987a402
Show file tree

Hide file tree

Showing 24 changed files with 16,082 additions and 2,350 deletions.
diff --git a/engineer_arm_config/config/engineer.srdf b/engineer_arm_config/config/engineer.srdf
@@ -15,37 +15,63 @@
         <joint name="joint3"/>
         <joint name="joint4"/>
         <joint name="joint5"/>
+        <joint name="joint6"/>
+        <joint name="joint7"/>
     </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="engineer_arm">
-        <joint name="joint1" value="0"/>
-        <joint name="joint2" value="-2.1802"/>
-        <joint name="joint3" value="2.1595"/>
+        <joint name="joint1" value="0.005"/>
+        <joint name="joint2" value="0"/>
+        <joint name="joint3" value="0.218"/>
         <joint name="joint4" value="0"/>
-        <joint name="joint5" value="0"/>
+        <joint name="joint5" value="0.1"/>
+        <joint name="joint6" value="0"/>
+        <joint name="joint7" value="0"/>
     </group_state>
-    <!--END EFFECTOR: Purpose: Represent information about an end effector.-->
     <!--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="base_link" link2="link1" reason="Adjacent"/>
-    <disable_collisions link1="left_finger" link2="link3" reason="Never"/>
-    <disable_collisions link1="left_finger" link2="link4" reason="Never"/>
-    <disable_collisions link1="left_finger" link2="link5" reason="Never"/>
-    <disable_collisions link1="left_finger" link2="palm" reason="Adjacent"/>
-    <disable_collisions link1="left_finger" link2="right_finger" reason="Never"/>
-    <disable_collisions link1="link1" link2="link2" reason="Adjacent"/>
+    <disable_collisions link1="base_link" link2="link2_1" reason="Default"/>
+    <disable_collisions link1="base_link" link2="link2_2" reason="Never"/>
+    <disable_collisions link1="base_link" link2="mimic_link1" reason="Adjacent"/>
+    <disable_collisions link1="base_link" link2="mimic_link2_1" reason="Never"/>
+    <disable_collisions link1="base_link" link2="mimic_link2_2" reason="Never"/>
+    <disable_collisions link1="link1" link2="link2_1" reason="Adjacent"/>
+    <disable_collisions link1="link1" link2="link2_2" reason="Never"/>
     <disable_collisions link1="link1" link2="link3" reason="Never"/>
-    <disable_collisions link1="link2" link2="link3" reason="Adjacent"/>
-    <disable_collisions link1="link2" link2="link4" reason="Never"/>
-    <disable_collisions link1="link2" link2="link5" reason="Never"/>
+    <disable_collisions link1="link1" link2="link4" reason="Never"/>
+    <disable_collisions link1="link1" link2="mimic_link1" reason="Never"/>
+    <disable_collisions link1="link1" link2="mimic_link2_1" reason="Adjacent"/>
+    <disable_collisions link1="link1" link2="mimic_link2_2" reason="Never"/>
+    <disable_collisions link1="link2_1" link2="link2_2" reason="Adjacent"/>
+    <disable_collisions link1="link2_1" link2="link3" reason="Never"/>
+    <disable_collisions link1="link2_1" link2="link4" reason="Never"/>
+    <disable_collisions link1="link2_1" link2="mimic_link1" reason="Never"/>
+    <disable_collisions link1="link2_1" link2="mimic_link2_1" reason="Never"/>
+    <disable_collisions link1="link2_1" link2="mimic_link2_2" reason="Adjacent"/>
+    <disable_collisions link1="link2_2" link2="link3" reason="Adjacent"/>
+    <disable_collisions link1="link2_2" link2="link4" reason="Never"/>
+    <disable_collisions link1="link2_2" link2="mimic_link1" reason="User"/>
+    <disable_collisions link1="link2_2" link2="mimic_link2_1" reason="Never"/>
+    <disable_collisions link1="link2_2" link2="mimic_link2_2" reason="Default"/>
     <disable_collisions link1="link3" link2="link4" reason="Adjacent"/>
     <disable_collisions link1="link3" link2="link5" reason="Default"/>
-    <disable_collisions link1="link3" link2="right_finger" reason="Never"/>
+    <disable_collisions link1="link3" link2="mimic_link1" reason="User"/>
+    <disable_collisions link1="link3" link2="mimic_link2_1" reason="Never"/>
+    <disable_collisions link1="link3" link2="mimic_link2_2" reason="Never"/>
     <disable_collisions link1="link4" link2="link5" reason="Adjacent"/>
-    <disable_collisions link1="link4" link2="palm" reason="Never"/>
-    <disable_collisions link1="link4" link2="right_finger" reason="Never"/>
-    <disable_collisions link1="link5" link2="palm" reason="Adjacent"/>
-    <disable_collisions link1="link5" link2="right_finger" reason="Never"/>
-    <disable_collisions link1="palm" link2="right_finger" reason="Adjacent"/>
-    <disable_collisions link1="base_link" link2="mast" reason="Adjacent"/>
+    <disable_collisions link1="link4" link2="link6" reason="Never"/>
+    <disable_collisions link1="link4" link2="mimic_link1" reason="User"/>
+    <disable_collisions link1="link4" link2="mimic_link2_1" reason="Never"/>
+    <disable_collisions link1="link4" link2="mimic_link2_2" reason="Never"/>
+    <disable_collisions link1="link5" link2="link6" reason="Adjacent"/>
+    <disable_collisions link1="link5" link2="mimic_link1" reason="User"/>
+    <disable_collisions link1="link5" link2="mimic_link2_1" reason="User"/>
+    <disable_collisions link1="link5" link2="mimic_link2_2" reason="User"/>
+    <disable_collisions link1="link6" link2="mimic_link1" reason="User"/>
+    <disable_collisions link1="link6" link2="mimic_link2_1" reason="User"/>
+    <disable_collisions link1="link6" link2="mimic_link2_2" reason="User"/>
+    <disable_collisions link1="mimic_link1" link2="mimic_link2_1" reason="Never"/>
+    <disable_collisions link1="mimic_link1" link2="mimic_link2_2" reason="Never"/>
+    <disable_collisions link1="mimic_link2_1" link2="mimic_link2_2" reason="Never"/>
+    <disable_collisions link1="base_link" link2="link2_1" reason="Never"/>
 </robot>
diff --git a/engineer_arm_config/config/fake_controllers.yaml b/engineer_arm_config/config/fake_controllers.yaml
@@ -6,9 +6,8 @@ controller_list:
       - joint3
       - joint4
       - joint5
-  - name: fake_hand_controller
-    joints:
-      - right_finger_joint
+      - joint6
+      - joint7
 initial: # Define initial robot poses.
   - group: engineer_arm
     pose: home
diff --git a/engineer_arm_config/config/joint_limits.yaml b/engineer_arm_config/config/joint_limits.yaml
@@ -10,36 +10,36 @@ default_acceleration_scaling_factor: 0.1
 joint_limits:
   joint1:
     has_velocity_limits: true
-    max_velocity: 10
+    max_velocity: 0.5
     has_acceleration_limits: true
-    max_acceleration: 20
+    max_acceleration: 3
   joint2:
     has_velocity_limits: true
-    max_velocity: 10
+    max_velocity: 1.4
     has_acceleration_limits: true
-    max_acceleration: 30
+    max_acceleration: 3.8
   joint3:
     has_velocity_limits: true
-    max_velocity: 10
+    max_velocity: 0.9
     has_acceleration_limits: true
-    max_acceleration: 50
+    max_acceleration: 2.1
   joint4:
     has_velocity_limits: true
-    max_velocity: 13.5
+    max_velocity: 3
     has_acceleration_limits: true
-    max_acceleration: 120
+    max_acceleration: 6.
   joint5:
     has_velocity_limits: true
-    max_velocity: 18
+    max_velocity: 18.53
     has_acceleration_limits: true
-    max_acceleration: 120
-  left_finger_joint:
+    max_acceleration: 40.
+  joint6:
     has_velocity_limits: true
-    max_velocity: 3.772
-    has_acceleration_limits: false
-    max_acceleration: 0
-  right_finger_joint:
+    max_velocity: 18.53
+    has_acceleration_limits: true
+    max_acceleration: 40.
+  joint7:
     has_velocity_limits: true
-    max_velocity: 3.772
-    has_acceleration_limits: false
-    max_acceleration: 0
+    max_velocity: 50.
+    has_acceleration_limits: true
+    max_acceleration: 150.
diff --git a/engineer_arm_config/config/ros_controllers.yaml b/engineer_arm_config/config/ros_controllers.yaml
@@ -9,16 +9,13 @@ generic_hw_control_loop:
 # Settings for ros_control hardware interface
 hardware_interface:
   joints:
-    - back_left_wheel_joint
-    - back_right_wheel_joint
-    - front_left_wheel_joint
-    - front_right_wheel_joint
     - joint1
     - joint2
     - joint3
     - joint4
     - joint5
-    - right_finger_joint
+    - joint6
+    - joint7
   sim_control_mode: 1  # 0: position, 1: velocity
 
 controller_list:
@@ -32,6 +29,8 @@ controller_list:
       - joint3
       - joint4
       - joint5
+      - joint6
+      - joint7
 
 controllers:
   robot_state_controller:
@@ -48,25 +47,36 @@ controllers:
       - joint3
       - joint4
       - joint5
+      - joint6
+      - joint7
     gains:
-      joint1: { p: 300, i: 0.0, d: 10, i_clamp_max: 2, i_clamp_min: -2, antiwindup: true, publish_state: true }
-      joint2: { p: 1000.0, i: 10, d: 10.0, i_clamp_max: 15, i_clamp_min: -15, antiwindup: true, publish_state: true }
-      joint3: { p: 700, i: 2, d: 6, i_clamp_max: 0.2, i_clamp_min: -0.2, antiwindup: true, publish_state: true }
-      joint4: { p: 105.0, i: 5.0, d: 6.5, i_clamp_max: 5, i_clamp_min: -5, antiwindup: true, publish_state: true }
-      joint5: { p: 100, i: 0.0, d: 5, i_clamp_max: 2, i_clamp_min: -2, antiwindup: true, publish_state: true }
-  hand_controller:
-    type: effort_controllers/JointPositionController
-    joint: right_finger_joint
-    pid: { p: 50000.0, i: 0.0, d: 1000, i_clamp_max: 2, i_clamp_min: -2, antiwindup: true, publish_state: true }
-  mast_controller:
-    type: effort_controllers/JointPositionController
-    joint: mast_joint
-    pid: { p: 50000.0, i: 0.0, d: 1000.0 }
+      joint1: { p: 10000.0, i: 120, d: 1200, i_clamp_max: 150, i_clamp_min: -150, antiwindup: true, publish_state: true }
+      joint2: { p: 8000.0, i: 0, d: 500, i_clamp_max: 0, i_clamp_min: 0, antiwindup: true, publish_state: true }
+      joint3: { p: 6000.0, i: 0, d: 300, i_clamp_max: 0, i_clamp_min: 0, antiwindup: true, publish_state: true }
+      joint4: { p: 8000.0, i: 0, d: 300, i_clamp_max: 0, i_clamp_min: 0, antiwindup: true, publish_state: true }
+      joint5: { p: 300.0, i: 0, d: 3, i_clamp_max: 0, i_clamp_min: 0, antiwindup: true, publish_state: true }
+      joint6: { p: 100.0, i: 0, d: 3, i_clamp_max: 0, i_clamp_min: 0, antiwindup: true, publish_state: true }
+      joint7: { p: 50.0, i: 0, d: 0.6, i_clamp_max: 0, i_clamp_min: 0, antiwindup: true, publish_state: true }
+  mimic_joint1_controller:
+    type: mimic_joint_controller/MimicJointController
+    target_joint_name: "joint1"
+    joint: "mimic_joint1"
+    pid: { p: 14000.0, i: 50, d: 1000.0, i_clamp_max: 150, i_clamp_min: -150, antiwindup: true, publish_state: true }
+  mimic_joint2_controller:
+    type: mimic_joint_controller/MimicJointController
+    target_joint_name: "joint2"
+    joint: "mimic_joint2"
+    pid: { p: 8000.0, i: 0, d: 500, i_clamp_max: 0, i_clamp_min: 0, antiwindup: true, publish_state: true }
+  mimic_joint3_controller:
+    type: mimic_joint_controller/MimicJointController
+    target_joint_name: "joint3"
+    joint: "mimic_joint3"
+    pid: { p: 6000.0, i: 0, d: 300, i_clamp_max: 0, i_clamp_min: 0, antiwindup: true, publish_state: true }
   card_controller:
     type: effort_controllers/JointPositionController
     joint: card_joint
-    pid: { p: 10000.0, i: 0.0, d: 1000.0 }
-  stone_platform_controller:
+    pid: { p: 3000.0, i: 0.0, d: 200.0 }
+  drag_controller:
     type: effort_controllers/JointPositionController
-    joint: stone_platform_joint
-    pid: { p: 50000, i: 0.0, d: 1000.0 }
+    joint: drag_joint
+    pid: { p: 12.0, i: 0.0, d: 3 }
diff --git a/...rm_config/config/ur_simulated_config.yaml → engineer_arm_config/config/servo.yaml b/...rm_config/config/ur_simulated_config.yaml → engineer_arm_config/config/servo.yaml
@@ -1,7 +1,7 @@
 ###############################################
 # Modify all parameters related to servoing here
 ###############################################
-use_gazebo: true # Whether the robot is started in a Gazebo simulation environment
+use_gazebo: false # Whether the robot is started in a Gazebo simulation environment
 
 ## Properties of incoming commands
 command_in_type: "speed_units" # "unitless"> in the range [-1:1], as if from joystick. "speed_units"> cmds are in m/s and rad/s
@@ -14,26 +14,26 @@ scale:
 low_pass_filter_coeff: 2.  # Larger --> trust the filtered data more, trust the measurements less.
 
 ## Properties of outgoing commands
-publish_period: 0.008  # 1/Nominal publish rate [seconds]
+publish_period: 0.5  # 1/Nominal publish rate [seconds]
 low_latency_mode: true  # Set this to true to publish as soon as an incoming Twist command is received (publish_period is ignored)
 
 # What type of topic does your robot driver expect?
 # Currently supported are std_msgs/Float64MultiArray (for ros_control JointGroupVelocityController or JointGroupPositionController)
 # or trajectory_msgs/JointTrajectory (for Universal Robots and other non-ros_control robots)
-command_out_type: std_msgs/Float64MultiArray
+command_out_type: trajectory_msgs/JointTrajectory
 
 # What to publish? Can save some bandwidth as most robots only require positions or velocities
 publish_joint_positions: true
-publish_joint_velocities: false
+publish_joint_velocities: true
 publish_joint_accelerations: false
 
 ## MoveIt properties
 move_group_name: engineer_arm  # Often 'manipulator' or 'arm'
 planning_frame: base_link  # The MoveIt planning frame. Often 'base_link' or 'world'
 
 ## Other frames
-ee_frame_name: link5  # The name of the end effector link, used to return the EE pose
-robot_link_command_frame: base_link  # commands must be given in the frame of a robot link. Usually either the base or end effector
+ee_frame_name: link6  # The name of the end effector link, used to return the EE pose
+robot_link_command_frame: tools_link  # commands must be given in the frame of a robot link. Usually either the base or end effector
 
 ## Stopping behaviour
 incoming_command_timeout: 0.1  # Stop servoing if X seconds elapse without a new command
@@ -44,25 +44,25 @@ num_outgoing_halt_msgs_to_publish: 4
 ## Configure handling of singularities and joint limits
 lower_singularity_threshold: 17  # Start decelerating when the condition number hits this (close to singularity)
 hard_stop_singularity_threshold: 30 # Stop when the condition number hits this
-joint_limit_margin: 0.1 # added as a buffer to joint limits [radians]. If moving quickly, make this larger.
+joint_limit_margin: 0.001 # added as a buffer to joint limits [radians]. If moving quickly, make this larger.
 
 ## Topic names
 cartesian_command_in_topic: delta_twist_cmds  # Topic for incoming Cartesian twist commands
 joint_command_in_topic: delta_joint_cmds # Topic for incoming joint angle commands
 joint_topic: joint_states
 status_topic: status # Publish status to this topic
-command_out_topic: /controllers/arm_position_controller/command # Publish outgoing commands here
+command_out_topic: /controllers/arm_trajectory_controller/command # Publish outgoing commands here
 
 ## Collision checking for the entire robot body
-check_collisions: false # Check collisions?
+check_collisions: true # Check collisions?
 collision_check_rate: 50 # [Hz] Collision-checking can easily bog down a CPU if done too often.
 # Two collision check algorithms are available:
 # "threshold_distance" begins slowing down when nearer than a specified distance. Good if you want to tune collision thresholds manually.
 # "stop_distance" stops if a collision is nearer than the worst-case stopping distance and the distance is decreasing. Requires joint acceleration limits
 collision_check_type: threshold_distance
 # Parameters for "threshold_distance"-type collision checking
-self_collision_proximity_threshold: 0.01 # Start decelerating when a self-collision is this far [m]
-scene_collision_proximity_threshold: 0.05 # Start decelerating when a scene collision is this far [m]
+self_collision_proximity_threshold: 0.0002 # Start decelerating when a self-collision is this far [m]
+scene_collision_proximity_threshold: 0.0005 # Start decelerating when a scene collision is this far [m]
 # Parameters for "stop_distance"-type collision checking
 collision_distance_safety_factor: 1000 # Must be >= 1. A large safety factor is recommended to account for latency
-min_allowable_collision_distance: 0.01 # Stop if a collision is closer than this [m]
+min_allowable_collision_distance: 0.001 # Stop if a collision is closer than this [m]
diff --git a/engineer_arm_config/launch/load_controllers.launch b/engineer_arm_config/launch/load_controllers.launch
@@ -3,21 +3,40 @@
     <!-- By default, we are not load controllers -->
     <arg name="spawn_controllers" default="true"/>
 
-    <rosparam file="$(find rm_chassis_controllers)/config/engineer.yaml" command="load"/>
-    <rosparam file="$(find rm_gimbal_controllers)/config/engineer.yaml" command="load"/>
+    <rosparam file="$(find rm_config)/config/rm_controllers/engineer.yaml" command="load"/>
 
     <!-- Load joint controller configurations from YAML file to parameter server -->
+
     <rosparam file="$(find engineer_arm_config)/config/ros_controllers.yaml" command="load"/>
 
     <!-- Load the controllers -->
-    <node if="$(arg spawn_controllers)" name="controller_spawner" pkg="controller_manager" type="spawner"
-          respawn="false" output="screen" args="
-          controllers/robot_state_controller
-          controllers/joint_state_controller
-          controllers/chassis_controller
-          controllers/arm_trajectory_controller
-          controllers/hand_controller
-          controllers/card_controller"
+    <node name="controller_loader" pkg="controller_manager" type="controller_manager" output="screen"
+          args="load
+                controllers/robot_state_controller
+                controllers/joint_state_controller
+                controllers/arm_trajectory_controller
+                controllers/joint1_calibration_controller
+                controllers/mimic_joint1_calibration_controller
+                controllers/joint2_calibration_controller
+                controllers/mimic_joint2_calibration_controller
+                controllers/joint3_calibration_controller
+                controllers/mimic_joint3_calibration_controller
+                controllers/joint4_calibration_controller
+                controllers/joint5_calibration_controller
+                controllers/joint6_calibration_controller
+                controllers/joint7_calibration_controller
+                controllers/mimic_joint1_controller
+                controllers/mimic_joint2_controller
+                controllers/mimic_joint3_controller
+                controllers/yaw_calibration_controller
+                controllers/pitch_calibration_controller
+                controllers/card_calibration_controller
+                controllers/drag_calibration_controller
+                controllers/card_controller
+                controllers/drag_controller
+                controllers/chassis_controller
+                controllers/gpio_controller
+                controllers/gimbal_controller
+                "
     />
-
 </launch>