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

admin管理员组

文章数量:1277902

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:

• BallRpyJoint with damping
• LinearBushingRollPitchYaw with large force stiffness
• LinearBushingRollPitchYaw + ball constraints between links

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", ";)
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!

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:

• BallRpyJoint with damping
• LinearBushingRollPitchYaw with large force stiffness
• LinearBushingRollPitchYaw + ball constraints between links

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!

• drake

Share Improve this question Follow asked Feb 24 at 23:18 Krishna SureshKrishna Suresh 2322 bronze badges

Add a comment  | 

1 Answer 1

Sorted by: Reset to default 0

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.

本文标签： How to speed up performance of simulating a rope in DrakeStack Overflow