CODEWITHSUNDEEP
drake
Krishna Suresh
Krishna Suresh

Reputation: 23

How to speed up performance of simulating a rope in Drake?

Drake Version v1.38.0

Platform M4 Mac Pro

I am simulating a rope in Drake as a chain of rigid bodies connected in a couple of different ways:

I currently have in the range of 15-30 links in the chain and am including collision checking between all non-neighbor links. For reference, I'm generating the rope as an .sdf using a generator script as follows:

import xml.etree.ElementTree as ET
import numpy as np
import os
from enum import Enum

from pydrake.all import UnitInertia, SpatialInertia
        

class LinkConnectionType(Enum):
    kBallRpy = 1
    kLinearBushingBallConstraint = 2
    kLinearBushingRpy = 3


kLinkConnectionType = LinkConnectionType.kBallRpy
kRopeMassPerLen = 0.0390909091 # kg/m of rope
kRopeRadius = 0.009/2 # [m]
kRopeLinkLength = 0.05 # [m]
kNumOfLinks = 20
kLinkMass = kRopeMassPerLen*kRopeLinkLength
print(f"Rope Length {kNumOfLinks*kRopeLinkLength}")
kLinkExtraScale = 1.0

unit_inertia = SpatialInertia.SolidCylinderWithMass(kLinkMass, kRopeRadius, kRopeLinkLength, np.array([1,0,0])).get_unit_inertia()
kLinkInertiaX = unit_inertia[0,0]
kLinkInertiaY = unit_inertia[1,1]
kLinkInertiaZ = unit_inertia[2,2]

torque_stiffness = np.ones(3)*0
torque_damping = np.ones(3)*0
force_stiffness = [0,0,0]
force_damping = np.ones(3)*0
kBallRpyDamping = 0.01

tree = ET.parse("rope_template.sdf")
root = tree.getroot()
root.set("xmlns:drake", "http://drake.mit.edu")
model = root.find(".//model")
model.append(ET.Comment("**BELOW AUTOGENERATED** by rope_gen.py"))
model.append(ET.Comment("modifying by hand not recommended."))
# print(list(model.iter()))
i = 0
for i in range(kNumOfLinks):
    link_name = f"link_{i}"
    link = ET.SubElement(model, "link")
    link.set("name", link_name)
    pose = ET.SubElement(link, "pose")
    pose.set("relative_to", "rope_base" if i==0 else f"link_{i-1}")
    pose.text = f"{kRopeLinkLength/2 if i==0 else kRopeLinkLength} 0 0 0 0 0"

    link_inertial = ET.SubElement(link, "inertial")
    mass = ET.SubElement(link_inertial, "mass")
    mass.text = str(kLinkMass)
    link_inertia = ET.SubElement(link_inertial, "inertia")
    ixx = ET.SubElement(link_inertia, "ixx")
    ixx.text = str(kLinkInertiaX)
    ixy = ET.SubElement(link_inertia, "ixy")
    ixy.text = "0"
    ixz = ET.SubElement(link_inertia, "ixz")
    ixz.text = "0"
    iyy = ET.SubElement(link_inertia, "iyy")
    iyy.text = str(kLinkInertiaY)
    iyz = ET.SubElement(link_inertia, "iyz")
    iyz.text = "0"
    izz = ET.SubElement(link_inertia, "izz")
    izz.text = str(kLinkInertiaZ)

    link_visual = ET.SubElement(link, "visual")
    link_visual.set("name", f"{link_name}_visual")
    pose = ET.SubElement(link_visual, "pose")
    pose.set("degrees", "True")
    pose.text = f"0 0 0 0 90 0"
    visual_geometry = ET.SubElement(link_visual, "geometry")
    visual_capsule = ET.SubElement(visual_geometry, "capsule")
    ET.SubElement(visual_capsule, "radius").text = str(kRopeRadius)
    ET.SubElement(visual_capsule, "length").text = str(kRopeLinkLength*kLinkExtraScale)

    link_collision = ET.SubElement(link, "collision")
    link_collision.set("name", f"{link_name}_collision")
    pose = ET.SubElement(link_collision, "pose")
    pose.set("degrees", "True")
    pose.text = f"0 0 0 0 90 0"
    collision_geometry = ET.SubElement(link_collision, "geometry")
    collision_capsule = ET.SubElement(collision_geometry, "capsule")
    ET.SubElement(collision_capsule, "radius").text = str(kRopeRadius)
    ET.SubElement(collision_capsule, "length").text = str(kRopeLinkLength*kLinkExtraScale)

    A_frame = ET.SubElement(model, "frame")
    A_frame.set("name", f"{link_name}_A")
    A_frame.set("attached_to", link_name)
    pose = ET.SubElement(A_frame, "pose")
    pose.text = f"-{kRopeLinkLength/2} 0 0 0 0 0"
    B_frame = ET.SubElement(model, "frame")
    B_frame.set("name", f"{link_name}_B")
    B_frame.set("attached_to", link_name)
    pose = ET.SubElement(B_frame, "pose")
    pose.text = f"{kRopeLinkLength/2} 0 0 0 0 0"

for i in range(kNumOfLinks-1):
    if kLinkConnectionType == LinkConnectionType.kBallRpy:
        joint = ET.SubElement(model, "joint")
        joint.set("name", f"joint_{i}_{i+1}")
        joint.set("type", f"ball")
        ET.SubElement(joint, "parent").text = f"link_{i}_B"
        ET.SubElement(joint, "child").text = f"link_{i+1}_A"
        axis = ET.SubElement(joint, "axis")
        dynamics = ET.SubElement(axis, "dynamics")
        ET.SubElement(dynamics, "damping").text = str(kBallRpyDamping)
    elif kLinkConnectionType == LinkConnectionType.kLinearBushingRpy or kLinkConnectionType == LinkConnectionType.kLinearBushingBallConstraint:
        linear_bushing = ET.SubElement(model, "drake:linear_bushing_rpy")
        ET.SubElement(linear_bushing, "drake:bushing_frameA").text = f"link_{i}_B"
        ET.SubElement(linear_bushing, "drake:bushing_frameC").text = f"link_{i+1}_A"
        ET.SubElement(linear_bushing, "drake:bushing_torque_stiffness").text = " ".join(str(x) for x in torque_stiffness)
        ET.SubElement(linear_bushing, "drake:bushing_torque_damping").text = " ".join(str(x) for x in torque_damping)
        ET.SubElement(linear_bushing, "drake:bushing_force_stiffness").text = " ".join(str(x) for x in force_stiffness)
        ET.SubElement(linear_bushing, "drake:bushing_force_damping").text = " ".join(str(x) for x in force_damping)

        if kLinkConnectionType == LinkConnectionType.kLinearBushingBallConstraint:
            ball_constraint = ET.SubElement(model, "drake:ball_constraint")
            ET.SubElement(ball_constraint, "drake:ball_constraint_body_A").text = f"link_{i}"
            ET.SubElement(ball_constraint, "drake:ball_constraint_p_AP").text = f"{kRopeLinkLength/2} 0 0"
            ET.SubElement(ball_constraint, "drake:ball_constraint_body_B").text = f"link_{i+1}"
            ET.SubElement(ball_constraint, "drake:ball_constraint_p_BQ").text = f"-{kRopeLinkLength/2} 0 0"

    collision_group = ET.SubElement(model, "drake:collision_filter_group")
    collision_group.set("name", f"link_{i}_{i+1}_group")
    ET.SubElement(collision_group, "drake:member").text = f"link_{i}"
    ET.SubElement(collision_group, "drake:member").text = f"link_{i+1}"
    ET.SubElement(collision_group, "drake:ignored_collision_filter_group").text = f"link_{i}_{i+1}_group"



indent(root)

tree = ET.ElementTree(root)
tree.write(
    f"rope_generated.sdf",
    encoding="utf-8",
    xml_declaration=True,
)

I'm then simulating the rope with a standard multi-body plant and getting fast simulation (8-9x realtime) for ball joints since I can increase the timesteps to 0.005. But when I try the same test with a LinearBushingRollPitchYaw+ball constraint with 0 force damping/stiffness and 0 torque stiffness (effectively the same joint as a ball joint with damping), the performance is slower since I'm required to decrease the timestep size to 0.0005 meaning it is only 1.8x realtime and decrease the timestep further (0.00005) to allow for rope/object collisions which results in 0.19x realtime. The error for too large of a timestep size for no collision and for collision is:

RuntimeError: The initial guess for line search is NaN. The typical root cause for this failure is usually outside the solver, when there are not enough checks to catch it earlier. In this case, a previous (valid) simulation result led to the generation of NaN values in a controller, that are then fed as actuation through MultibodyPlant's ports. If you don't believe this is the root cause of your problem, please contact the Drake developers and/or open a Drake issue with a minimal reproduction example to help debug your problem.

I would expect similar performance between a BallRpyJoint and LinearBushingRollPitchYaw+BallConstraint with the same damping properties but am not sure if I'm missing something else.

I primarily want to use the bushing to model the rope's rotational stiffness but don't want to slow the simulation down too much by using the LinearBushingRollPitchYaw. Does it make sense to implement a ForceElement for the BallRpyJoint similar to the RevoluteSpring?

Thanks in advance!

Upvotes: 1

Views: 30

Answers (1)

Mitiguy
Mitiguy

Reputation: 191

Modeling: A rope bends relatively easy about its two transverse axes whereas it is much stiffer to twisting along the rope. One way to speed simulation is for each rigid body to be connected to its neighbor by a 2 degree-of-freedom UniversalJoint (with appropriate stiffness and damping) so the highly-stiff twist is not a degree of freedom.

Upvotes: 0

Related Questions