I have been trying to setup a basic Pick n Place task using the template tutorial code provided. I am able to load the nextage robot by trying to load an existing urdf
and mesh file. I am able to visualise the loaded scenario (robot, table and a cube) on meshcat. However, when I plug in all the necessary systems in a diagram for Pick'n Place(along with the Inverse Dynamics Controller, Trajectory Source etc.) I get the following error.
Failure at systems/controllers/inverse_dynamics_controller.cc:41 in SetUp(): condition 'num_positions == num_actuators' failed.
I had initially tried the same setup with the PidController
class (instead of the inverse dynamics controller). My code had silently failed at the same point. There was a runtime error of the input/output port size mismatch between controller output and plant actuation input.
Which leads me to believe that there some crucial information in my URDF (num of actuators I guess) that drake
is unable to find.
Any idea of what I may be missing?
EDIT: Please find the concerned urdf
file being loaded under this gist
The URDF file you posted does not have any http://wiki.ros.org/urdf/XML/Transmission elements defined with an <actuator>
sub-element. Drake's URDF parser only adds joint actuators when a transmission is defined.
The original here does have transmissions and actuators, so somewhere along the way it looks like they got deleted.
See https://github.com/RobotLocomotion/drake/blob/master/manipulation/models/franka_description/urdf/panda_arm.urdf an example with transmissions. (There's a lot of other sample URDFs elsewhere in ROS too.)
I don't know yet whether this is a bug in Drake (incorrect parsing assumptions) or a bug in the URDF file (missing data). I will try to figure out whether URDF joints without a transmission are supposed to be actuated or not.
See https://github.com/RobotLocomotion/drake/issues/19136 for tracking and updates.