CODEWITHSUNDEEP
pythondrake
L Wall
L Wall

Reputation: 59

pydrake - How to get cartesian pose of end effector for connections: plant.get_body_poses_output_port()?

Ubuntu 20.04 | Python 3.7 | drake 0.38.0 | PyCharm | Scripting in pydrake:

I need to generate a connection between the current pose in Cartesian space of a robotic end-effector and a controller's input port.

get_body_poses_output_port() seems the obvious solution, with some manipulation to:

(1) obtain only the RigidTranform for the last body (the end-effector) (2) convert this RigidTransform into it's rotational and translational components (3) stack those together for input to a homebrew controller that expects that input.

I am trying to do (1) with: (where the demultiplexer should take in n_dof+1 bodies that each have 9 components and split them up into n_dof+1 outputs) demux = builder.AddSystem(Demultiplexer(np.full(n_dof+1,9))) builder.Connect(plant.get_body_poses_output_port(), demux.get_input_port())

but I get the following error: RuntimeError: DiagramBuilder::Connect: Cannot mix vector-valued and abstract-valued ports while connecting output port body_poses of System plant to input port u0 of System drake/systems/Demultiplexer@00000000040505c0

When I look at the data_type of the get_body_poses_output_port() it's a combination of vector-valued and abstract_valued ports.

Is there another way to retrieve the port I need?

Upvotes: 1

Views: 179

Answers (1)

Russ Tedrake
Russ Tedrake

Reputation: 5533

It's true that the Demultiplexer doesn't support abstract types (and that the body poses output port is abstract-typed). You can write a tiny leaf system to extract the single body pose that you want. I have one example here.

Upvotes: 1

Related Questions