Merge branch 'main' into document-imarker-joint-jog-config

Dockerfile

@@ -6,10 +6,7 @@
 #
 
 # Specify the MoveIt Pro release to build on top of.
-ARG MOVEIT_STUDIO_BASE_IMAGE
-ARG USERNAME=studio-user
-ARG USER_UID=1000
-ARG USER_GID=1000
+ARG MOVEIT_STUDIO_BASE_IMAGE=main
 
 ##################################################
 # Starting from the specified MoveIt Pro release #
@@ -22,7 +19,6 @@ FROM ${MOVEIT_STUDIO_BASE_IMAGE} AS base
 ARG USERNAME
 ARG USER_UID
 ARG USER_GID
-
 # Copy source code from the workspace's ROS 2 packages to a workspace inside the container
 ARG USER_WS=/home/${USERNAME}/user_ws
 ENV USER_WS=${USER_WS}
@@ -119,10 +115,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"]

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

@@ -30,10 +30,6 @@
             <!--Joint sliders interpolate to joint state-->
             <Decorator ID="ForceSuccess" _while="teleop_mode == 5">
               <Control ID="Sequence">
-                <Action
-                  ID="SwitchUIPrimaryView"
-                  primary_view_name="Visualization"
-                />
                 <Control ID="Fallback" name="root">
                   <Control ID="Sequence">
                     <Action
@@ -153,10 +149,6 @@
               />
             </Control>
             <Control ID="Sequence" _while="teleop_mode == 1">
-              <Action
-                ID="SwitchUIPrimaryView"
-                primary_view_name="Visualization"
-              />
               <Action
                 ID="TeleoperateJointJog"
                 controller_name="servo_controller"
@@ -167,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: