add VFC example to mujoco config (#319)

src/picknik_ur_mujoco_config/config/config.yaml

@@ -73,6 +73,8 @@ ros2_control:
   # Load but do not start these controllers so they can be activated later if needed.
   controllers_inactive_at_startup:
     - "joint_trajectory_controller"
+    - "joint_trajectory_admittance_controller"
+    - "velocity_force_controller"
   # Any controllers here will not be spawned by MoveIt Pro.
   # [Optional, default=[]]
   controllers_not_managed: []

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

@@ -9,9 +9,12 @@ controller_manager:
       type: position_controllers/GripperActionController
     servo_controller:
       type: joint_trajectory_controller/JointTrajectoryController
-
     force_torque_sensor_broadcaster:
       type: force_torque_sensor_broadcaster/ForceTorqueSensorBroadcaster
+    joint_trajectory_admittance_controller:
+      type: joint_trajectory_admittance_controller/JointTrajectoryAdmittanceController
+    velocity_force_controller:
+      type: velocity_force_controller/VelocityForceController
 
 joint_state_broadcaster:
   ros__parameters:
@@ -151,3 +154,75 @@ force_torque_sensor_broadcaster:
       - torque.y
       - torque.z
     frame_id: fts_link
+
+
+joint_trajectory_admittance_controller:
+  ros__parameters:
+    joints:
+      - linear_rail_joint
+      - shoulder_pan_joint
+      - shoulder_lift_joint
+      - elbow_joint
+      - wrist_1_joint
+      - wrist_2_joint
+      - wrist_3_joint
+    base_frame: base_link
+    sensor_frame: tool0
+    ee_frame: grasp_link
+    ft_sensor_name: robotiq_ft_sensor
+    # Joint accelerations chosen to match MoveIt configs.
+    stop_accelerations:
+      - 30.0
+      - 30.0
+      - 30.0
+      - 30.0
+      - 30.0
+      - 30.0
+      - 30.0
+
+velocity_force_controller:
+  ros__parameters:
+    joints:
+      - linear_rail_joint
+      - shoulder_pan_joint
+      - shoulder_lift_joint
+      - elbow_joint
+      - wrist_1_joint
+      - wrist_2_joint
+      - wrist_3_joint
+    base_frame: base_link
+    sensor_frame: tool0
+    ee_frame: grasp_link
+    ft_sensor_name: robotiq_ft_sensor
+    ft_force_deadband: 2.0
+    ft_torque_deadband: 1.0
+    max_joint_velocity:
+      - 0.524
+      - 0.524
+      - 0.524
+      - 0.524
+      - 1.047
+      - 1.047
+      - 1.047
+    max_joint_acceleration:
+      - 0.524
+      - 0.524
+      - 0.524
+      - 0.524
+      - 0.524
+      - 0.524
+      - 0.524
+    max_cartesian_velocity:
+      - 0.25
+      - 0.25
+      - 0.25
+      - 1.5707
+      - 1.5707
+      - 1.5707
+    max_cartesian_acceleration:
+      - 2.0
+      - 2.0
+      - 2.0
+      - 4.0
+      - 4.0
+      - 4.0

src/picknik_ur_mujoco_config/description/picknik_ur.xacro

@@ -790,12 +790,6 @@
     <limit effort="28.0" lower="-6.283185307179586" upper="6.283185307179586" velocity="1.0471975511965976"/>
     <dynamics damping="0" friction="0"/>
   </joint>
-  <link name="ft_frame"/>
-  <joint name="wrist_3_link-ft_frame" type="fixed">
-    <parent link="wrist_3_link"/>
-    <child link="ft_frame"/>
-    <origin rpy="3.141592653589793 0 0" xyz="0 0 0"/>
-  </joint>
   <!-- ROS-Industrial 'base' frame - base_link to UR 'Base' Coordinates transform -->
   <link name="base"/>
   <joint name="base_link-base_fixed_joint" type="fixed">
@@ -880,20 +874,20 @@
     <visual>
       <origin rpy="0 0 0" xyz="0 0 0"/>
       <geometry>
-        <mesh filename="package://picknik_accessories/descriptions/furniture/generic_table/wood_block.dae" scale="0.125 0.125 0.00625"/>
+        <mesh filename="package://picknik_accessories/descriptions/furniture/generic_table/wood_block.dae" scale="0.125 0.125 0.05"/>
       </geometry>
     </visual>
     <collision>
       <origin rpy="0 0 0" xyz="0 0 0"/>
       <geometry>
-        <mesh filename="package://picknik_accessories/descriptions/furniture/generic_table/wood_block.dae" scale="0.125 0.125 0.00625"/>
+        <mesh filename="package://picknik_accessories/descriptions/furniture/generic_table/wood_block.dae" scale="0.125 0.125 0.05"/>
       </geometry>
     </collision>
   </link>
   <joint name="table_joint" type="fixed">
     <parent link="world"/>
     <child link="table"/>
-    <origin rpy="0 0 0" xyz="1 0 0.5"/>
+    <origin rpy="0 0 0" xyz="1 0 0.45"/>
   </joint>
   <ros2_control name="ur_mujoco_control" type="system">
     <hardware>

src/picknik_ur_mujoco_config/description/scene.xml

@@ -43,24 +43,22 @@
     <light pos="0 0 1.5" dir="0 0 -1" directional="true"/>
     <geom name="floor" pos="0 0 -1.626" size="0 0 0.05" type="plane" material="groundplane"/>   
     <!-- Define the table -->
-    <body name="table" pos="1 0 0.5">
-        <geom type="box" size="0.5 0.5 0.025" pos="0 0 0.025" material="wood"/>
+    <body name="table" pos="1 0 0.45">
+        <geom type="box" size="0.5 0.5 0.025" material="wood"/>
+    </body>
+    <!-- Define the blocks with AprilTags on each face -->
+    <body name="block1" pos="0.75 0.2 0.51">
+        <freejoint name="block1"/>
+        <geom type="box" size="0.025 0.025 0.025" rgba="1 0 0 1" material="apriltag_material1"/>
+    </body>
+    <body name="block2" pos="0.75 0.0 0.51">
+        <freejoint name="block2"/>
+        <geom type="box" size="0.025 0.025 0.025" rgba="0 1 0 1" material="apriltag_material2"/>
+    </body>
+    <body name="block3" pos="0.75 -0.2 0.51">
+        <freejoint name="block3"/>
+        <geom type="box" size="0.025 0.025 0.025" rgba="0 0 1 1" material="apriltag_material3"/>
     </body>
-        <!-- Define the blocks with AprilTags on each face -->
-
-        <!-- Define the block with AprilTags on each face -->
-        <body name="block1" pos="0.75 0.2 0.575">
-            <freejoint name="block1"/>
-            <geom type="box" size="0.025 0.025 0.025" rgba="1 0 0 1" material="apriltag_material1"/>
-        </body>
-        <body name="block2" pos="0.75 0.0 0.575">
-            <freejoint name="block2"/>
-            <geom type="box" size="0.025 0.025 0.025" rgba="0 1 0 1" material="apriltag_material2"/>
-        </body>
-        <body name="block3" pos="0.75 -0.2 0.575">
-            <freejoint name="block3"/>
-            <geom type="box" size="0.025 0.025 0.025" rgba="0 0 1 1" material="apriltag_material3"/>
-        </body>
 
   </worldbody>
 </mujoco>

src/picknik_ur_mujoco_config/description/ur5e.xml

@@ -1,5 +1,5 @@
 <mujoco model="ur5e">
-  <compiler angle="radian" meshdir="assets" autolimits="true"/>
+  <compiler angle="radian" autolimits="true"/>
 
   <option integrator="implicitfast"/>
 
@@ -68,7 +68,7 @@
     <default class="robot">
         <material specular="0.5" shininess="0.25"/>
         <joint armature="0.1" damping="1" axis="0 0 1" range="-2.8973 2.8973"/>
-        <general dyntype="none" biastype="affine" ctrlrange="-3.14159 3.14159" forcerange="-5000 5000"/>
+        <general dyntype="none" biastype="affine" ctrlrange="-3.14159 3.14159" forcerange="-50000 50000"/>
         <default class="limitedJoints">
             <joint limited="true"/>
         </default>
@@ -178,9 +178,11 @@
                     <geom material="linkgray" mesh="wrist3" class="visual"/>
                     <!-- <geom class="eef_collision" pos="0 0.08 0" quat="1 1 0 0" size="0.04 0.02"/> -->
                     <site name="attachment_site" pos="0 0.1 0" quat="-1 1 0 0"/>
+                    <!-- site for end effector's f/t sensor right at the base of the robotiq gripper -->
+                    <site name="robotiq_ft_sensor" size="0.01" pos="0 0.1 0" quat="-1 1 0 0"/>
                     <camera name="wrist_camera" pos="0.0 0.2 0.05" fovy="58" mode="fixed" resolution="640 480" euler="1.57 0.0 0.0"/>
                     <site name="wrist_camera_optical_frame" pos="0.0 0.2 0.05" euler="-1.57 0 0"/>
-                    <body childclass="2f85" >                    <!-- site for end effector's f/t sensor right at the base of the robotiq gripper -->                    <site name="robotiq_ft_sensor" size="0.01" pos="0.0 0.0 -0.0615"/>1
+                    <body childclass="2f85" >
                       <!-- Right-hand side 4-bar linkage -->
                       <body name="robotiq_85_base_link" gravcomp="1" pos="0.0 0.1441 0" quat="0.5 -0.5 0.5 0.5">
                         <inertial mass="0.777441" pos="0 -2.70394e-05 0.0354675" quat="1 -0.00152849 0 0"
@@ -290,12 +292,12 @@
   </actuator>
 
 
-    <contact>
-        <exclude body1="base_platform" body2="base"/>
-    </contact>
+  <contact>
+    <exclude body1="base_platform" body2="base"/>
+  </contact>
 
-    <actuator>
-        <general class="robot" name="linear_rail_joint" joint="linear_rail_joint" gainprm="2500" biasprm="0 -2500 -2000"/>
+  <actuator>
+    <position class="robot" name="linear_rail_joint" joint="linear_rail_joint" kp="75000" dampratio="1.0"/>
   </actuator>
 
     <!-- Robotiq gripper contact -->

src/picknik_ur_mujoco_config/objectives/velocity_force_control_example.xml

@@ -0,0 +1,31 @@
+<?xml version='1.0' encoding='UTF-8'?>
+<root BTCPP_format="4" main_tree_to_execute="Velocity Force Control Example">
+  <!--//////////-->
+  <BehaviorTree ID="Velocity Force Control Example" _description="An example showing how to use the Velocity Force Controller (VFC). Moves sideways on a table while applying a force." _favorite="false">
+    <Control ID="Sequence" name="TopLevelSequence">
+      <!--Move to the starting configuration.-->
+      <SubTree ID="Move to Waypoint" _collapsed="true" waypoint_name="VFC Start" joint_group_name="manipulator" planner_interface="moveit_default" controller_names="/joint_trajectory_controller"/>
+      <!--Activate the Velocity Force Controller.-->
+      <Action ID="ActivateControllers" controller_names="velocity_force_controller"/>
+      <!--Tare the force/torque sensor right before starting motion.-->
+      <Action ID="CallTriggerService" service_name="/velocity_force_controller/zero_fts"/>
+      <!--Create a StampedWrench message with the force/torque to apply.-->
+      <Action ID="CreateStampedWrench" stamped_wrench="{desired_wrench}" reference_frame="grasp_link" torque_xyz="0;0;0" force_xyz="0;0;-2"/>
+      <!--Create a StampedTwist message with the Cartesian velocity to move with.-->
+      <Action ID="CreateStampedTwist" linear_velocity_xyz="0.05;0.0;0.0" reference_frame="grasp_link" stamped_twist="{desired_twist}" angular_velocity_xyz="0;0;0"/>
+      <!--Publish the controller command in a loop during 5s.-->
+      <Decorator ID="ForceSuccess">
+        <Decorator ID="Timeout" msec="5000">
+          <Action ID="PublishVelocityForceCommand" velocity_controlled_axes="1;0;0;0;0;0" force_controlled_axes="0;0;1;0;0;0" wrench_gain="0.01"/>
+        </Decorator>
+      </Decorator>
+      <!-- Move up away from the table for 1 second.-->
+      <Action ID="CreateStampedTwist" stamped_twist="{move_up_twist}" linear_velocity_xyz="0;0;-0.1" reference_frame="grasp_link"/>
+      <Decorator ID="ForceSuccess">
+        <Decorator ID="Timeout" msec="1000">
+          <Action ID="PublishVelocityForceCommand" twist_stamped="{move_up_twist}" velocity_controlled_axes="0;0;1;0;0;0" wrench_gain="0.01"/>
+        </Decorator>
+      </Decorator>
+    </Control>
+  </BehaviorTree>
+</root>

src/picknik_ur_mujoco_config/package.xml

@@ -12,6 +12,7 @@
   <buildtool_depend>ament_cmake</buildtool_depend>
 
   <exec_depend>admittance_controller</exec_depend>
+  <exec_depend>joint_trajectory_admittance_controller</exec_depend>
   <exec_depend>moveit_planners_stomp</exec_depend>
   <exec_depend>moveit_ros_perception</exec_depend>
   <exec_depend>moveit_studio_agent</exec_depend>
@@ -26,6 +27,7 @@
   <exec_depend>ur_description</exec_depend>
   <exec_depend>ur_robot_driver</exec_depend>
   <exec_depend>picknik_mujoco_ros</exec_depend>
+  <exec_depend>velocity_force_controller</exec_depend>
 
   <test_depend>ament_lint_auto</test_depend>
 

src/picknik_ur_mujoco_config/waypoints/ur_waypoints.yaml

@@ -21,14 +21,14 @@
       - wrist_2_joint
       - wrist_3_joint
     position:
-      - 0.0
+      - 0
       - 0.29608248868337067
-      - 0.0
+      - 0
       - -1.5708
       - 0.785398
       - -1.2948195437205516
       - -1.5708
-      - 0.0
+      - 0
     velocity: []
   multi_dof_joint_state:
     header:
@@ -41,3 +41,46 @@
     twist: []
     wrench: []
   name: Look at Table
+- description: ''
+  favorite: false
+  joint_group_names:
+    - gripper
+    - linear_actuator
+    - manipulator
+  joint_state:
+    effort: []
+    header:
+      frame_id: ''
+      stamp:
+        nanosec: 0
+        sec: 0
+    name:
+      - robotiq_85_left_knuckle_joint
+      - linear_rail_joint
+      - shoulder_pan_joint
+      - shoulder_lift_joint
+      - elbow_joint
+      - wrist_1_joint
+      - wrist_2_joint
+      - wrist_3_joint
+    position:
+      - 0.020916781481167455
+      - -0.22610599344464546
+      - 0.38969451825539325
+      - -0.7463048667584851
+      - 1.4104637547514023
+      - -2.2350037021105393
+      - -1.5646945626735103
+      - 0.3796920004270002
+    velocity: []
+  multi_dof_joint_state:
+    header:
+      frame_id: ''
+      stamp:
+        nanosec: 0
+        sec: 0
+    joint_names: []
+    transforms: []
+    twist: []
+    wrench: []
+  name: VFC Start