Design Brainstorming

Simulator Design

In order to fulfill our SVD demonstration requirements, a simulator was required to test out bimanual manipulation due to the lack of a second physical arm. While an out-of-the-box solution for a Gazebo Classic simulator is already provided by the xarm-ros repository, which additionally provided the option of attaching one of their officially supported end effectors to the end effector of a single simulated arm, several functionalities were required that needed to be addressed:

  • The simulation must include a detachable mechanism, which enables the pepper peduncle to be cut, freeing the pepper from the anchor in world position.
  • The end effectors should be attached to the last joint of both arms. The control interface of either end effector should not affect the implementation of the motor control module (and should behave as if the real motors are attached). The planner should take the end effectors into account when planning.
  • The pepper should be robustly grasped by the gripper end effector and not cause physics engine glitches or randomly fall from the hand. 

Our team had several choices to start exploring:

  • Using a completely new simulator (such as Mujoco). This is discouraged since a partially working solution was already implemented in Gazebo Classic, and creating a simulation from scratch would result in significantly more work than previously expected, causing major hiccups in progress.
  • Migrating the existing simulation stack to Ignition Gazebo, which replaces the Gazebo Classic engine. This was an option to be considered due to the fact that the Ignition Gazebo simulator officially features a world in which objects may be detached when a message is published (https://gazebosim.org/api/sim/9/detachablejoints.html). This is explored briefly, but was later abandoned due to 1) the amount of work potentially required to convert all files currently working with Gazebo Classic into Ignition Gazebo standards, and 2) the additional time and logistics required to create a custom plugin that senses the force being applied to the peduncle/fruit, and publishing a message based on the force reaching a limit. Additionally, due to the fact that we are using ROS 1 (Noetic), Ignition Gazebo is only somewhat supported (see https://gazebosim.org/docs/latest/ros_installation/). 
  • Using Gazebo Classic. While the engine is due for deprecation around January 2025, the community support for Gazebo Classic is overwhelmingly more comprehensive than other options, and the xarm_ros repository files already work with Gazebo Classic. Furthermore, with the files associated with their officially supported gripper, development can be significantly improved by referring to existing known solutions for end effectors and grasping. This was the option our team ended up pursuing.

First of all, the end effectors had to be included in the scene and attached to the arms, and to do this, we leveraged the Solidworks to URDF extension (https://wiki.ros.org/sw_urdf_exporter) to export our CAD assembly into an URDF file. The documentation to do this is available at (https://www.notion.so/Turning-Assemblies-into-URDF-for-RViz-Gazebo-1b6cedb4810780728bdec28add6a1891?pvs=4). In short, the assembly was significantly simplified, a base link (where the end effector attaches to the arm) is specified, revolute joints were defined and tested, and the files were exported (URDF and STL).

When the URDF files are exported and work as expected, the file structures in xarm_ros were then mimicked with the newly created gripper/cutter URDFs. This included defining several xacro files, including transmission, gazebo, urdf definitions, as well as an overall xacro file importing other xacro files. Additionally, joint controllers were defined in xarm_controller (PID position controllers) in order to move the joints inside Gazebo using gazebo_ros_control. This was verified by publishing to the joint controller channels with desired joint angles and verifying that the joints move in rviz and gazebo (since this infrastructure was already set up).

To address the other two points regarding the simulator design, two plugins were found to solve 1) detachable joints and 2) object grasping stability. 

After some research, a native plugin called libBreakableJointPlugin was found, which allows the definition of a breaking force threshold (beyond which the fixed joint breaks). Some manual tuning was done to ensure when the cutter blades collide into the peduncle the force threshold was met, and to ensure that other forces such as small bumps into the plant or gravity did not trigger this threshold.

    <joint name="breakable" type="revolute">
      <parent>peduncle</parent>
      <child>fruit</child>
      <axis>
        <xyz>0 0 1</xyz>
        <limit>
          <lower>0.0</lower>
          <upper>0.0</upper>
        </limit>
        <use_parent_model_frame>true</use_parent_model_frame>
      </axis>
      <physics>
        <ode>
          <erp>1</erp>
          <cfm>1</cfm>
        </ode>
      </physics>
      <sensor name="force_torque" type="force_torque">
        <always_on>true</always_on>
        <update_rate>1000</update_rate>
        <plugin name="breakable" filename="libBreakableJointPlugin.so">
          <breaking_force_N>10</breaking_force_N>
        </plugin>
      </sensor>
    </joint>

 Secondly, referring to the existing code in xarm_ros, a third-party library called libgazebo_grasp_fix was found (https://github.com/JenniferBuehler/gazebo-pkgs/wiki/The-Gazebo-grasp-fix-plugin), which fixes the object to the gripper given threshold conditions and lets go of the object when the force applied on the object is less than a threshold. This allowed the specification of multiple links (in our case, the gripper fingers and base) to be considered, along with some other options:

    <xacro:macro name="vader_gripper_grasp_fix" params="prefix:='' arm_name:='xarm' palm_link:='link_base'">
    <gazebo>
      <plugin name="gazebo_grasp_fix" filename="libgazebo_grasp_fix.so">
        <arm>
          <arm_name>${arm_name}</arm_name>
          <palm_link>${palm_link}</palm_link>
          <gripper_link>thumb_1</gripper_link>
          <gripper_link>fing_1</gripper_link>
          <gripper_link>fing_2</gripper_link>
        </arm>
        <forces_angle_tolerance>110</forces_angle_tolerance>
        <update_rate>30</update_rate>
        <grip_count_threshold>2</grip_count_threshold>
        <max_grip_count>3</max_grip_count>
        <release_tolerance>0.01</release_tolerance>
        <disable_collisions_on_attach>false</disable_collisions_on_attach>
        <contact_topic>__default_topic__</contact_topic>
      </plugin>
    </gazebo>
  </xacro:macro>

With these plugins and a large amount of infrastructure work, Gazebo Classic was successfully configured to simulate a breakable pepper, import URDFs of both end effectors, and allow stable grasp of the pepper in the gripper hand. See our teaser (https://youtu.be/Uqwh7hMD_l8?si=i2ckLbr7Mejvx8_g&t=135) for a demonstration of the results!


Planning Design

To reliably grasp a pepper with a robotic gripper, two major challenges must be solved. First, the robot needs to find a precise pregrasp position that places it close enough to the pepper to reach it easily. Second, it must orient the gripper’s camera so that it has a clear view of the pepper’s peduncle—the small stem that helps determine where to grasp. Without a clear view of the peduncle, the system cannot obtain an accurate pose estimate, which is critical for success.

To address both these challenges, we developed a novel and intuitive approach we call the parametric circle method. It models the entire planning process in a geometric and deterministic way that greatly improves grasp accuracy.

The process begins by modeling the pepper as a simple cylinder, using a coarse pose estimate of the fruit. The cylinder’s radius is approximated based on an average-sized pepper, and its central axis aligns with the long axis of the actual pepper. Around this virtual cylinder, we create an invisible circular plane. This circle is carefully placed so that its center matches the centroid of the pepper, and its normal vector matches the direction in which the pepper is pointing.

Now comes the key idea. The robot’s planner begins by adjusting the radius of this virtual circle and sampling candidate approach points along its perimeter. It starts with the point on the circle that is closest to the robot’s base, making the approach more energy-efficient and practical. From there, it tests additional points at various angular offsets along the circle’s edge. For each point, the system checks whether a valid inverse kinematics (IK) solution exists and whether the resulting joint positions fall within the robot’s mechanical limits.

Once a suitable candidate is found, the robot generates a motion plan that brings the gripper to that specific location. Importantly, the end effector is aligned so that its vertical axis points directly at the center of the circle—right where the pepper is located. This ensures that the gripper is not only in the right position, but also looking in the right direction.

From this optimal pregrasp pose, the robot executes a simple and deterministic grasp. It moves straight along the radial vector of the circle toward the pepper, stopping once it has covered a distance equal to the circle’s radius. This method guarantees a smooth and repeatable grasp, every time.

We use a similar strategy for cutting the pepper’s peduncle. By treating the peduncle as the key target and aligning everything relative to the cylinder axis, we establish a clear and consistent relationship between the fine pose estimation and the actual geometry of the fruit.

This approach not only simplifies the analysis of the grasping problem but also provides a robust and elegant solution that is both efficient and highly effective in real-world conditions.

 

CAD Design

To improve the versatility of our robotic gripper, we wanted to add a finger retraction mechanism that would allow the fingers to move out of the way when not needed—for example, during navigation or tool switching. However, modifying the existing motor mount proved challenging due to space constraints and design limitations.

Instead of redesigning the entire assembly, we took a more resourceful approach. We identified an unused fourth Dynamixel motor already present in the system and repurposed it to drive the new mechanism. By adding an extra tendon linked to this motor, we were able to control the retraction independently.

To make the integration seamless, we designed a small cavity in the gripper mount that routes the tendon cleanly to the back of the thumb. This allowed for a compact and efficient layout without interfering with the existing actuation.

This retraction feature is currently being tested and refined, and will be improved in FVD