CODEWITHSUNDEEP
pythondrake
adamconkey
adamconkey

Reputation: 4745

How do I set the pose of an object in pydrake manually after creation?

I would like to create an object in pydrake that is loaded from a URDF (e.g. just a single-link URDF that has an object mesh), and be able to manually set the pose of the object as I wish after creation. What I've been doing so far is this:

urdf = FindResourceOrThrow(my_urdf_path)
parser.AddModelFromFile(urdf)
frame = plant.GetFrameByName("my_frame")
T = RigidTransform(p=pos, rpy=RollPitchYaw(rpy))
plant.WeldFrames(plant.world_frame(), frame, T)

That lets me set the pose on creation. However, I don't know how to change the pose once the plant has been finalized. I assume there's a way to do this but am struggling to find an example. Thanks in advance for your help.

Upvotes: 1

Views: 303

Answers (2)

jwnimmer-tri
jwnimmer-tri

Reputation: 2449

I believe you can do it as follows: (1) Attach a FixedOffsetFrame (e.g., "robot_base") to the world. The offset is a parameter of the context, not State dofs. (2) Weld the robot to robot_base. (3) Re-pose that frame in a Context.

(1) goes like:

robot_base = plant.AddFrame(frame=FixedOffsetFrame(
    name="robot_base",
    P=plant.world_frame(),
    X_PF=RigidTransform_[float].Identity(),
    model_instance=None))

(3) goes like:

robot_base.SetPoseInBodyFrame(context=context, X_PF=new_pose)

(Yes, Drake's examples should show this better; it's a bit obscure right now.)

Upvotes: 2

Sean Curtis
Sean Curtis

Reputation: 1873

I believe what you're looking for is SetFreeBodyPose (C++, Python). Do this instead of welding your frame to the world frame.

It's worth nothing that you need to do with a Context. I.e., something like:

urdf = FindResourceOrThrow(my_urdf_path)
parser.AddModelFromFile(urdf)
plant.Finalize()
body = plant.GetBodyByName("my_body")
T = RigidTransform(p=pos, rpy=RollPitchYaw(rpy))
context = plant.CreateDefaultContext()
plant.SetFreeBodyPose(context=context, body=body, X_WB=T)

(Note the change from grabbing the frame to the body. I assume the frame you grabbed is the body frame of a body.)

Upvotes: 2

Related Questions