CODEWITHSUNDEEP
drake
popcornelves
popcornelves

Reputation: 31

How to implement joint limits for discrete multibodyplant simulation

I'm implementing a two-link acrobot simulation using pydrake and would like to enforce joint limits to prevent the lower link from colliding with the upper link. I've added the joint limits to the URDF and am parsing this URDF to generate an acrobot multibodyplant object. I've used functions to successfully verify that the joint limit is being parsed (position_lower_limits(), position_upper_limits()), but my simulation is not responding to those limits. I'd really appreciate some help in identifying why this may be.

I'm creating and simulating the acrobot multibodyplant as follows: 

plt_is_interactive = SetupMatplotlibBackend()
builder = DiagramBuilder()

# instantiate the pendulum and the scene graph
double_pendulum, scene_graph = AddMultibodyPlantSceneGraph(
    builder,
    time_step=0.01 # discrete update period 
)

# parse the urdf 
urdf_path = 'double_pendulum_no_shoulder.urdf'
Parser(double_pendulum).AddModelFromFile(urdf_path)
double_pendulum.Finalize()

#Simulation Set-Up

# Set the constant torque 
torque_system = builder.AddSystem(ConstantVectorSource([[0.]]))
builder.Connect(torque_system.get_output_port(0), double_pendulum.get_input_port(3))
visualizer = builder.AddSystem(
    PlanarSceneGraphVisualizer(scene_graph, xlim=[-10., 10.], ylim=[-10., 10.], show=plt_is_interactive))
builder.Connect(scene_graph.get_pose_bundle_output_port(),
                visualizer.get_input_port(0))

diagram = builder.Build()
simulator = Simulator(diagram)
context = simulator.get_mutable_context()
plant_context = diagram.GetMutableSubsystemContext(double_pendulum, context)
double_pendulum.SetPositionsAndVelocities(plant_context, [2, 0, 0, 0])

# Simulate
duration = 10.0 if get_ipython() else 0.1 # sets a shorter duration during testing
context.SetTime(0.0)

AdvanceToAndVisualize(simulator, visualizer, duration)

and my URDF contains the following line within the associated joint to attempt to enforce joint limits. 

<limit lower="-2.35" upper="2.35"/>

Upvotes: 3

Views: 198

Answers (3)

Alejandro
Alejandro

Reputation: 1099

We just were talking with @Russ Tedrake. As I mentioned, this is a "corner" case for MultibodyPlant that unfortunately it does not handle well, see Drake's issue #12775.

The "corner" case here is that the rotational inertia of the upper arm about elbow's axis is zero. That is because the upper arm is being modeled as a point mass concentrated at the joint's axis.

A quick solution we can handle today is if you instead model the upper arm as a solid rod, which has non-zero moments of inertia about the joint's axis.

That is, in your URDF you'd need to update the inertia values in <inertia> to be those of a rod of length 1.0 and mass 1.0 (in your case) about its center of mass.

For your case I verified that the following change works:

<origin xyz="0 0 -0.5" rpy="0 0 0" />
<inertia ixx="0.08333333" ixy="0" ixz="0" iyy="0.08333333" iyz="0" izz="0"/>

Which effectively replaces your point mass by a thin rod of length 1.0 and mass 1.0.

Upvotes: 1

Russ Tedrake
Russ Tedrake

Reputation: 5533

I just tried to reproduce, and was also surprised that it doesn't appear to work.
Here is a runnable reproduction that violates the joint lower limit at the elbow, which is set to 0.0.

It also prints out the limit from the joint, confirming that the parsing worked. https://www.dropbox.com/s/2m12ws0g88t5uei/joint_limits_test.py?dl=0 https://www.dropbox.com/s/fdjb4uv4m2i17i8/double_pendulum.urdf?dl=0

Upvotes: 3

Alejandro
Alejandro

Reputation: 1099

...but my simulation is not responding to those limits

what do you mean exactly?.

First, you should know that our joint limits are "soft", meaning that they are not constraints but more like stiff springs. MultibodyPlant computes the stiffness of these springs automatically for you to ensure the stability of our solvers.

It's a function of the time step. The smaller the time step, the stiffer the joint limits are; they converge to the rigid limit quadratically with time step.

That being said, I see your time step is pretty large (10 ms). It might be your limits are just too soft and that's what you observe in the simulation results.

Given you verified the correct parsing (I will trust you on that one), I'd suggest you run with a smaller time steps first as to verify this hypothesis. You should observe the quadratic convergence with time step.

Upvotes: 1

Related Questions