Merge branch 'main' into removing-primary-view-switch-in-Teleop-Joint…

…-Jog

Dockerfile

@@ -119,10 +119,6 @@ ENV USER_WS=${USER_WS}
 
 # Compile the workspace
 WORKDIR $USER_WS
-# hadolint ignore=SC1091
-RUN --mount=type=cache,target=/home/${USERNAME}/.ccache \
-    . /opt/overlay_ws/install/setup.sh && \
-    colcon build
 
 # Set up the user's .bashrc file and shell.
 CMD ["/usr/bin/bash"]

docker-compose.yaml

@@ -14,7 +14,7 @@ services:
       target: user-overlay
       args:
         - USER_UID=${STUDIO_USER_UID:-1000}
-        - USER_GID=${STUDIO_USER_UID:-1000}
+        - USER_GID=${STUDIO_USER_GID:-1000}
         - USERNAME=${STUDIO_USERNAME:-studio-user}
         - MOVEIT_STUDIO_BASE_IMAGE=picknikciuser/moveit-studio:${STUDIO_DOCKER_TAG:-main}
 

src/arm_on_rail_sim/config/moveit/hard_joint_limits.yaml

@@ -15,7 +15,7 @@ joint_limits:
     has_effort_limits: true
     has_position_limits: true
     has_velocity_limits: true
-    max_acceleration: 0.1
+    max_acceleration: 10.0
     max_effort: 150.0
     max_position: 2.9
     max_velocity: 0.175
@@ -25,7 +25,7 @@ joint_limits:
     has_effort_limits: true
     has_position_limits: true
     has_velocity_limits: true
-    max_acceleration: !degrees 90.0
+    max_acceleration: !degrees 180.0
     max_effort: 150.0
     max_position: !degrees  180.0
     max_velocity: !degrees  30.0
@@ -35,7 +35,7 @@ joint_limits:
     has_effort_limits: true
     has_position_limits: true
     has_velocity_limits: true
-    max_acceleration: !degrees 90.0
+    max_acceleration: !degrees 180.0
     max_effort: 150.0
     max_position: !degrees  90.0
     max_velocity: !degrees  30.0
@@ -45,7 +45,7 @@ joint_limits:
     has_effort_limits: true
     has_position_limits: true
     has_velocity_limits: true
-    max_acceleration: !degrees 90.0
+    max_acceleration: !degrees 180.0
     max_effort: 150.0
     # we artificially limit this joint to half its actual joint position limit
     # to avoid (MoveIt/OMPL) planning problems, as due to the physical
@@ -65,7 +65,7 @@ joint_limits:
     has_effort_limits: true
     has_position_limits: true
     has_velocity_limits: true
-    max_acceleration: !degrees 90.0
+    max_acceleration: !degrees 180.0
     max_effort: 28.0
     max_position: !degrees  180.0
     max_velocity: !degrees  60.0

src/arm_on_rail_sim/description/ur5e.xml

@@ -11,8 +11,8 @@
         gaintype="fixed"
         biastype="affine"
         ctrlrange="-6.2831 6.2831"
-        gainprm="2000"
-        biasprm="0 -2000 -400"
+        gainprm="20000"
+        biasprm="0 -20000 -1000"
         forcerange="-150 150"
       />
       <default class="size3">
@@ -22,7 +22,11 @@
         </default>
       </default>
       <default class="size1">
-        <general gainprm="500" biasprm="0 -500 -100" forcerange="-28 28" />
+        <general
+          gainprm="20000"
+          biasprm="0 -20000 -1000"
+          forcerange="-150 150"
+        />
       </default>
       <default class="visual">
         <geom type="mesh" contype="0" conaffinity="0" group="2" />

src/picknik_ur_base_config/launch/pro_rs_launch.py

@@ -56,5 +56,14 @@ def generate_launch_description():
     )
     return LaunchDescription(
         rs_launch.declare_configurable_parameters(rs_launch.configurable_parameters)
-        + [OpaqueFunction(function=rs_launch.launch_setup)]
+        + [
+            OpaqueFunction(
+                function=rs_launch.launch_setup,
+                kwargs={
+                    "params": rs_launch.set_configurable_parameters(
+                        rs_launch.configurable_parameters
+                    )
+                },
+            )
+        ]
     )

src/picknik_ur_base_config/objectives/request_teleoperation.xml

@@ -159,4 +159,11 @@
       </Control>
     </Control>
   </BehaviorTree>
+  <TreeNodesModel>
+    <SubTree ID="Request Teleoperation">
+      <input_port name="enable_user_interaction" default="false" />
+      <input_port name="user_interaction_prompt" default="" />
+      <input_port name="initial_teleop_mode" default="3" />
+    </SubTree>
+  </TreeNodesModel>
 </root>

src/picknik_ur_site_config/config/config.yaml

@@ -32,8 +32,6 @@ ros2_control:
   # [Optional, default=[]]
   controllers_inactive_at_startup:
     - "joint_trajectory_controller"
-    - "admittance_controller_open_door"
-    - "joint_trajectory_controller_chained_open_door"
     - "joint_trajectory_admittance_controller"
     - "velocity_force_controller"
   # Any controllers here will not be spawned by MoveIt Pro.

src/picknik_ur_site_config/config/control/picknik_ur.ros2_control.yaml

@@ -15,10 +15,6 @@ controller_manager:
       type: robotiq_controllers/RobotiqActivationController
     servo_controller:
       type: joint_trajectory_controller/JointTrajectoryController
-    admittance_controller_open_door:
-      type: admittance_controller/AdmittanceController
-    joint_trajectory_controller_chained_open_door:
-      type: joint_trajectory_controller/JointTrajectoryController
     joint_trajectory_admittance_controller:
       type: joint_trajectory_admittance_controller/JointTrajectoryAdmittanceController
     velocity_force_controller:
@@ -81,47 +77,6 @@ joint_trajectory_controller:
       wrist_3_joint:
         goal: 0.05
 
-joint_trajectory_controller_chained_open_door:
-  ros__parameters:
-    joints:
-      - shoulder_pan_joint
-      - shoulder_lift_joint
-      - elbow_joint
-      - wrist_1_joint
-      - wrist_2_joint
-      - wrist_3_joint
-    command_interfaces:
-      - position
-      - velocity
-    state_interfaces:
-      - position
-      - velocity
-    command_joints:
-      - admittance_controller_open_door/shoulder_pan_joint
-      - admittance_controller_open_door/shoulder_lift_joint
-      - admittance_controller_open_door/elbow_joint
-      - admittance_controller_open_door/wrist_1_joint
-      - admittance_controller_open_door/wrist_2_joint
-      - admittance_controller_open_door/wrist_3_joint
-    state_publish_rate: 100.0
-    action_monitor_rate: 20.0
-    allow_partial_joints_goal: false
-    constraints:
-      stopped_velocity_tolerance: 0.0
-      goal_time: 0.0
-      shoulder_pan_joint:
-        goal: 0.25
-      shoulder_lift_joint:
-        goal: 0.25
-      elbow_joint:
-        goal: 0.25
-      wrist_1_joint:
-        goal: 0.25
-      wrist_2_joint:
-        goal: 0.25
-      wrist_3_joint:
-        goal: 0.25
-
 servo_controller:
   ros__parameters:
     joints:
@@ -172,97 +127,6 @@ robotiq_activation_controller:
   ros__parameters:
     default: true
 
-admittance_controller_open_door:
-  ros__parameters:
-    joints:
-      - shoulder_pan_joint
-      - shoulder_lift_joint
-      - elbow_joint
-      - wrist_1_joint
-      - wrist_2_joint
-      - wrist_3_joint
-
-    command_interfaces:
-      - position
-
-    state_interfaces:
-      - position
-      - velocity
-
-    chainable_command_interfaces:
-      - position
-      - velocity
-
-    kinematics:
-      plugin_name: kinematics_interface_kdl/KinematicsInterfaceKDL
-      plugin_package: kinematics_interface
-      base: base_link  # Assumed to be stationary
-      tip: grasp_link  # The end effector frame
-      alpha: 0.05
-
-    ft_sensor:
-      name: tcp_fts_sensor
-      frame:
-        id: wrist_3_link  # Wrench measurements are in this frame
-      filter_coefficient: 0.1
-
-    control:
-      frame:
-        id: grasp_link  # Admittance calcs (displacement etc) are done in this frame. Usually the tool or end-effector
-
-    fixed_world_frame:
-      frame:            # Gravity points down (neg. Z) in this frame (Usually: world or base_link)
-        id: base_link   # Admittance calcs (displacement etc) are done in this frame. Usually the tool or end-effector
-
-    gravity_compensation:
-      frame:
-        id: wrist_3_link
-
-      CoG:  # specifies the center of gravity of the end effector
-        pos:
-          - 0.1  # x
-          - 0.0  # y
-          - 0.0  # z
-        force: 0.0  # mass * 9.81
-
-    admittance:
-      selected_axes:
-        - true  # x
-        - true  # y
-        - true  # z
-        - true  # rx
-        - true  # ry
-        - true  # rz
-
-      # Having ".0" at the end is MUST, otherwise there is a loading error
-      # F = M*a + D*v + S*(x - x_d)
-      mass:
-        - 10.0  # x
-        - 10.0  # y
-        - 10.0  # z
-        - 5.0  # rx
-        - 5.0  # ry
-        - 5.0  # rz
-
-      damping_ratio:  # damping can be used instead: zeta = D / (2 * sqrt( M * S ))
-        - 5.0  # x
-        - 5.0  # y
-        - 5.0  # z
-        - 5.0  # rx
-        - 5.0  # ry
-        - 5.0  # rz
-
-      stiffness:
-        - 500.0  # x
-        - 500.0  # y
-        - 500.0  # z
-        - 100.0  # rx
-        - 100.0  # ry
-        - 100.0  # rz
-
-    # general settings
-    enable_parameter_update_without_reactivation: true
-
 joint_trajectory_admittance_controller:
   ros__parameters:
     joints:

src/picknik_ur_site_config/objectives/close_cabinet_door.xml

@@ -6,7 +6,7 @@
   >
     <Control ID="Sequence">
       <SubTree ID="Re-Zero Force-Torque Sensors" />
-      <SubTree ID="CloseGripper" />
+      <SubTree ID="Close Gripper" />
       <Action ID="SaveCurrentState" saved_robot_state="{initial_robot_state}" />
       <Control ID="Sequence">
         <Action
@@ -116,67 +116,10 @@
         </Control>
       </Control>
       <SubTree
-        ID="RetreatToInitialPose"
+        ID="Retreat To Initial Pose"
         pre_approach_robot_state="{pre_approach_robot_state}"
         initial_robot_state="{initial_robot_state}"
       />
     </Control>
   </BehaviorTree>
-  <BehaviorTree ID="CloseGripper">
-    <Control ID="Sequence" name="close_gripper_main">
-      <Action
-        ID="MoveGripperAction"
-        gripper_command_action_name="/robotiq_gripper_controller/gripper_cmd"
-        position="0.7929"
-      />
-    </Control>
-  </BehaviorTree>
-  <BehaviorTree ID="RetreatToInitialPose">
-    <Control ID="Sequence">
-      <Action
-        ID="InitializeMTCTask"
-        task_id="close_cabinet_door_retreat"
-        controller_names="/joint_trajectory_controller"
-        task="{retreat_task}"
-      />
-      <Action ID="SetupMTCCurrentState" task="{retreat_task}" />
-      <Action
-        ID="SetupMTCUpdateGroupCollisionRule"
-        name="AllowGripperCollisionWithOctomap"
-        group_name="gripper"
-        object_name="&lt;octomap&gt;"
-        allow_collision="true"
-        task="{retreat_task}"
-      />
-      <Action
-        ID="SetupMTCCartesianMoveToJointState"
-        joint_state="{pre_approach_robot_state}"
-        planning_group_name="manipulator"
-        task="{retreat_task}"
-      />
-      <Action
-        ID="SetupMTCUpdateGroupCollisionRule"
-        name="ForbidGripperCollisionWithOctomap"
-        group_name="gripper"
-        object_name="&lt;octomap&gt;"
-        allow_collision="false"
-        task="{retreat_task}"
-      />
-      <Action
-        ID="SetupMTCMoveToJointState"
-        joint_state="{initial_robot_state}"
-        planning_group_name="manipulator"
-        task="{retreat_task}"
-      />
-      <Action
-        ID="PlanMTCTask"
-        solution="{return_to_initial_waypoint_solution}"
-        task="{retreat_task}"
-      />
-      <Action
-        ID="ExecuteMTCTask"
-        solution="{return_to_initial_waypoint_solution}"
-      />
-    </Control>
-  </BehaviorTree>
 </root>