ggould-tri
commented
3 years ago The python code that brought about this error was:
import math
import time
from pydrake.systems.framework import (DiagramBuilder,)
from pydrake.systems.primitives import (ConstantVectorSource,)
from pydrake.geometry import (Box,
DrakeVisualizer)
from pydrake.math import (RigidTransform, RollPitchYaw)
from pydrake.multibody.plant import (AddMultibodyPlantSceneGraph, CoulombFriction, Propeller, PropellerInfo)
from pydrake.multibody.tree import (SpatialInertia, RotationalInertia, PlanarJoint)
from pydrake.systems.analysis import Simulator
def make_box(mbp, name):
inertia = SpatialInertia.MakeFromCentralInertia(1, [0, 0, 0], RotationalInertia(1/600, 1/120, 1/120))
body = mbp.AddRigidBody(name, inertia)
shape = Box(1, 0.1, 0.1)
mbp.RegisterVisualGeometry(
body=body, X_BG=RigidTransform(), shape=shape, name=f"{name}_visual",
diffuse_color=[1., 0.64, 0.0, 0.5])
body_friction = CoulombFriction(static_friction=0.6,
dynamic_friction=0.5)
mbp.RegisterCollisionGeometry(
body=body, X_BG=RigidTransform(), shape=shape,
name="{name}_collision", coulomb_friction=body_friction)
return body
def make_mbp(builder):
mbp, sg = AddMultibodyPlantSceneGraph(builder, time_step=0.001)
robot_body = make_box(mbp, "robot")
planar_joint = mbp.AddJoint(
PlanarJoint(name="robot_planar_joint",
frame_on_parent=mbp.world_frame(),
frame_on_child=robot_body.body_frame(),
damping=[1,1,0.9]))
blocker1 = make_box(mbp, "blocker1")
mbp.WeldFrames(B=mbp.world_frame(), A=blocker1.body_frame(), X_AB=RigidTransform([1,0.5,0]))
return mbp, sg
def add_thrusters(builder, mbp):
robot_body_index = mbp.GetBodyByName("robot").index()
robot_tail_forward = RigidTransform(p=[-0.5, 0, 0], rpy=RollPitchYaw([math.pi/2, 0, 0]))
robot_tail_clockwise = RigidTransform(p=[-0.5, 0, 0], rpy=RollPitchYaw([math.pi/2, 0, math.pi/2]))
thrusters = builder.AddSystem(
Propeller([PropellerInfo(robot_body_index, X_BP=robot_tail_forward),
PropellerInfo(robot_body_index, X_BP=robot_tail_clockwise)]))
builder.Connect(thrusters.get_spatial_forces_output_port(), mbp.get_applied_spatial_force_input_port())
builder.Connect(mbp.get_body_poses_output_port(), thrusters.get_body_poses_input_port())
return thrusters
def run(sleeptime=0.001, thrust = [10, 2]):
builder = DiagramBuilder()
mbp, sg = make_mbp(builder)
mbp.Finalize()
DrakeVisualizer.AddToBuilder(builder, sg)
thrusters = add_thrusters(builder, mbp)
cmd = builder.AddSystem(ConstantVectorSource(thrust))
builder.Connect(cmd.get_output_port(), thrusters.get_command_input_port())
diagram = builder.Build()
simulator = Simulator(diagram)
for step in range(1000):
simulator.AdvanceTo(0.001 * step)
mbp_context = diagram.GetSubsystemContext(mbp, simulator.get_context())
time.sleep(sleeptime)
run()
sherm1
rpoyner-tri
amcastro-tri
The parent/child direction of a weld matters (which is itself odd, since its arguments are "A" and "B" rather than "parent" and "child"). Worse, if you apply a weld backward (welding world to object instead of object to world) then the error message you get is the cryptic and decidedly non-weldlike
(Python code that demonstrates this in the comment below, along with a stack trace)
See also: https://app.slack.com/client/T0JNB2DS4/threads/thread/C2WBPQDB7-1610670547.003800
The best outcome would be for welds to just adopt the unique correct direction automatically. A reasonable outcome would be an error message that names the two frames and two mobilizers (however, see comment below, the mobilizer name is in fact unhelpful craziness) plus a rename of the parameters of the AddWeld method.