Joint between Rod and RigidBody

[mstoelzle]

Describe the bug According to the documentation, a numerous number of joints (and particularly the FixedJoint) should have compatibility for connecting a CosseratRod and a RigidBody (for example a Cylinder). However in practice, there are some bugs when trying to use this feature:

Error message for call simulator.connect(rigid_body, rod, first_connect_idx=0, second_connect_idx=-1):

File "/Users/maximilianstoelzle/opt/miniconda3/envs/hsa-control/lib/python3.10/site-packages/elastica/wrappers/connections.py", line 99, in _call_connections
    connection.apply_torques(
  File "/Users/maximilianstoelzle/opt/miniconda3/envs/hsa-control/lib/python3.10/site-packages/elastica/joint.py", line 250, in apply_torques
    + rod_two.rest_lengths[index_two] * rod_one.tangents[..., index_one]
AttributeError: 'Cylinder' object has no attribute 'tangents'

Error message for call simulator.connect(rod, rigid_body, first_connect_idx=0, second_connect_idx=0):

 File "/Users/maximilianstoelzle/opt/miniconda3/envs/hsa-control/lib/python3.10/site-packages/elastica/wrappers/connections.py", line 99, in _call_connections
    connection.apply_torques(
  File "/Users/maximilianstoelzle/opt/miniconda3/envs/hsa-control/lib/python3.10/site-packages/elastica/joint.py", line 250, in apply_torques
    + rod_two.rest_lengths[index_two] * rod_one.tangents[..., index_one]
AttributeError: 'Cylinder' object has no attribute 'rest_lengths'

Error message for call simulator.connect(rod, rigid_body, first_connect_idx=-1, second_connect_idx=0):

 File "/Users/maximilianstoelzle/opt/miniconda3/envs/hsa-control/lib/python3.10/site-packages/elastica/wrappers/connections.py", line 99, in _call_connections
    connection.apply_torques(
  File "/Users/maximilianstoelzle/opt/miniconda3/envs/hsa-control/lib/python3.10/site-packages/elastica/joint.py", line 239, in apply_torques
    rod_two.position_collection[..., index_two + 1]
IndexError: index 1 is out of bounds for axis 1 with size 1

To Reproduce

from elastica.joint import FixedJoint
from elastica.rod.cosserat_rod import CosseratRod
from elastica.rigidbody import Cylinder
rod = CosseratRod.straight_rod(
                n_elements=50,  # number of elements
                start=rod_base_position,  # Starting position of first node in rod
                direction=np.array([0.0, 1.0, 0.0]),  # Direction the rod extends
                normal=np.array([0.0, 0.0, 1.0]),  # normal vector of rod
                base_length=1.,  # original length of rod (m)
                base_radius=0.05,  # original radius of rod (m)
                density=1e3,  # density of rod (kg/m^3)
                nu=1e-3,  # Energy dissipation of rod
                youngs_modulus=youngs_modulus,
                shear_modulus=shear_modulus,
            )
rigid_body =  Cylinder(start=np.array[0., 0., 1.),
                            normal=np.array([0., 0., 1.]),
                            direction=np.array([1., 0., 0.]),
                            base_length=platform_thickness,
                            base_radius=platform_radius,
                            density=1.,  # TODO: replace with actual density of platform
                        )
simulator.connect(rigid_body, rod, first_connect_idx=0, second_connect_idx=-1).using(
                                    FixedJoint,
                                    k=1e8, kt=1e6, nu=1,
                                )

Environment

• Python=3.10.5, pip=22.1.2
• Numpy/numba version: 22.1.2
• PyElastica version: 0.2.3
• OS, Device: macOS / macBook Pro 15'' 2019

Expected behavior I expect the be able to connect a RigidBody and a CosseratRod via a (Fixed)Joint, no matter which is referenced as rod_1 or rod_2 in the connect method.

Screenshots If applicable, add screenshots to help explain your problem.

Additional context Add any other context about the problem here.

[armantekinalp]

Hi @mstoelzle

Thanks for reporting the bug. Fixed joint can only connect two rods, not rigid body and a rod. So documentation was misleading and I will correct that.

However, you can still write your joint class to connect rigid body and rod. The Joint class you need to write is similar to the fixed joint. First you need to compute the tip position of rigid body to find the connection point of rigid-body and rod. Second you need to write a torque spring to fix relative orientation of rigid body and rod.

Here are some hints for you to get starting:

• Tip position of cylinder is can be calculated as follows,

  cylinder_tip_position = cylinder_two.position_collection[..., 0]
              - 0.5 * cylinder_two.length * cylinder_two.director_collection[2, :, 0]

• Assuming rod_one is cylinder, you also need to correct this line instead of rod_one.tangents[..., index_one] you need to write rod_one.director_collection[2, :, 0]

Also after implementing joint class, you are welcome to do a PR and contribute to the PyElastica . Let me know if you have any other questions.

[mstoelzle]

Hi @mstoelzle

Thanks for reporting the bug. Fixed joint can only connect two rods, not rigid body and a rod. So documentation was misleading and I will correct that.

However, you can still write your joint class to connect rigid body and rod. The Joint class you need to write is similar to the fixed joint. First you need to compute the tip position of rigid body to find the connection point of rigid-body and rod. Second you need to write a torque spring to fix relative orientation of rigid body and rod.

Here are some hints for you to get starting:

• Tip position of cylinder is can be calculated as follows,

cylinder_tip_position = cylinder_two.position_collection[..., 0]
                - 0.5 * cylinder_two.length * cylinder_two.director_collection[2, :, 0]

• Assuming rod_one is cylinder, you also need to correct this line instead of rod_one.tangents[..., index_one] you need to write rod_one.director_collection[2, :, 0]

Also after implementing joint class, you are welcome to do a PR and contribute to the PyElastica . Let me know if you have any other questions.

@armantekinalp Ok, thank you for the hints on how to start implementing this feature. I was thinking of adding a third, rigid link (e.g. a cylinder) to the examples/JointCases to show-off how to use the joints to connect rods and rigid bodies together. Are you fine with this strategy or would you prefer something else?

[armantekinalp]

@mstoelzle it sounds good. After adding it please create a PR and we can go from there.

[mstoelzle]

@armantekinalp I have implemented the required functionality to connect rigid bodies and cosserat rods using a spherical joint. In the attached video, a robotic arm consists of two cosserat rods (red and blue) at the proximal end (also connected using a spherical joint) and then a cylinder (green) attached at the distal end. The external forces / torques act on the second rod. At least qualitatively, things look alright I think.

https://user-images.githubusercontent.com/2360366/178212355-9cce34ee-3909-416d-9058-d4f2dd50e4e8.mp4

https://user-images.githubusercontent.com/2360366/178212390-120dd4e3-67de-4784-abd4-2446dbf15f63.mp4

https://user-images.githubusercontent.com/2360366/178212407-3b4f68ab-b760-4083-ae18-fd862fd43604.mp4

To continue the implementation also for hinge and fixed joints, I would like to ask you the following question about the current implementation

• For a fixed joint, the contact torques are calculated by considering the error between a desired and the actual position of the second node of the second rod system. Basically, the director of the last node of the first rod is used to compute the desired position of the second node of the second rod by following the director of the first rod:

  def apply_torques(self, system_one, index_one, system_two, index_two):
      # current direction of the first element of link two
      # also NOTE: - rod two is fixed at first element
      link_direction = (
              system_two.position_collection[..., index_two + 1]
              - system_two.position_collection[..., index_two]
      )
  
      # To constrain the orientation of link two, the second node of link two should align with
      # the direction of link one. Thus, we compute the desired position of the second node of link two
      # as check1, and the current position of the second node of link two as check2. Check1 and check2
      # should overlap.
  
      tgt_destination = (
              system_one.position_collection[..., index_one]
              + system_two.rest_lengths[index_two] * system_one.tangents[..., index_one]
      )  # dl of rod 2 can be different than rod 1 so use rest length of rod 2
  
      curr_destination = system_two.position_collection[
          ..., index_two + 1
      ]  # second element of rod2
  
      # Compute the restoring torque
      force_direction = -self.kt * (
              curr_destination - tgt_destination
      )  # force direction is between rod2 2nd element and rod1
      torque = np.cross(link_direction, force_direction)
  
      # The opposite torque will be applied on link one
      system_one.external_torques[..., index_one] -= (
              system_one.director_collection[..., index_one] @ torque
      )
      system_two.external_torques[..., index_two] += (
              system_two.director_collection[..., index_two] @ torque
      )

  @armantekinalp I am curious about this method. Why is this approach followed instead of directly using the directors of both rods and compute the error / distance between the two directors?

[armantekinalp]

Hi @mstoelzle

Spherical joint results looks good. For you question I will let @xzhan139 to respond, whom implemented these connections in his Nature Communication paper.

[xzhan139]

@mstoelzle Thanks for the question - Yes either way worked - we chose to implement “moment_arm x F” in the beginning by thinking of the coefficient may carry over from the linear connection case — but it was actually not the case, the coefficient are very different anyway.. We tested out the torsional springs kind of connection (as you mentioned) for both fixed and hinge joint, and it worked well too.

[mstoelzle]

I will close this issue for now as #149 is merged now into the v0.3.0 branch.