TimeSteppingRigidBodyManipulator frame handling causes errors for sensorless manipulators about drake HOT 9 CLOSED

I don't know how to assign this issue to myself, but I'm going to fix it.

from drake.

As of starting work on this bug:

The following tests FAILED:
     21 - systems/plants/test/coordinateSystemTest (Failed)
     23 - systems/plants/test/fallingBrickLCP (Failed)
     26 - systems/plants/test/massSpringDamperThrust (Failed)
     27 - systems/plants/test/momentumTest (Failed)
     28 - systems/plants/test/multiRobotTest (Failed)
     30 - systems/plants/test/terrainInterpTest (Failed)
     31 - systems/plants/test/terrainTest (Failed)
     32 - systems/plants/test/testApproximateIK (Failed)
     34 - systems/plants/test/testCOM (Failed)
     35 - systems/plants/test/testFloatingBaseDynamics (Failed)
     36 - systems/plants/test/testForwardJacDot (Failed)
     40 - systems/plants/test/testIK (Failed)
     44 - systems/plants/test/testThrust (Failed)
     95 - examples/Acrobot/test/testJointLimits (Failed)
     96 - examples/Acrobot/test/testKinematics (Failed)
     98 - examples/Acrobot/test/testTSRB (Failed)
    108 - examples/Atlas/runAtlasDynamics (Failed)
    109 - examples/Atlas/runAtlasFrontalDynamics (Failed)
    110 - examples/Atlas/runAtlasSagittalDynamics (Failed)
    112 - examples/Atlas/runPD (Failed)
    113 - examples/Atlas/runStandingDemo (Failed)
    176 - examples/RimlessWheel/runPassiveLCP (Failed)
    188 - examples/SimpleFourBar/runPassiveLCP (Failed)
    199 - examples/ZMP/CartTable.run (Failed)
    200 - examples/ZMP/CartTable2D.run (Failed)

from drake.

Not sure how to re-open this issue, but I don't believe this is working correctly. Why is it only looking at cv{1}?

I'm not entirely sure what MultiCoordinateFrames are, but this produces the wrong result when called with a compound TimeSteppingRigidBodyManipulator. For example (in the systems/plants/test directory):

r = TimeSteppingRigidBodyManipulator('ball.urdf',.01,struct('floating',true));
r = addRobotFromURDF(r,'ball.urdf',zeros(3,1),zeros(3,1),struct('floating',true));
x0 = zeros(24,1);
x0(1:6) = randn(6,1);
x0(7:12) = x0(1:6) + 1;
traj = r.simulate([0 5],x0);

In this example, the state vector is arranged: [q1 q2 qd1 qd2] Where the two balls are 1 and 2.

The output function here ends being [q1 qd1], but the ball 2 state is lost (even though r.getNumOutputs is 24). Somehow when this is turned into the simulink output, it ends up as something different altogether.

from drake.

Yeah. This is on me. I totally forgot about the multi-robot case when I implemented this. What we need is a way tell which frames in the TSRBM state-frame (which is a MultiCoordinateFrame) correspond to the sensor states. I could add a property to TSRBM called sensor_frame_indices or loop through the frames in the state multi-frame and check names against the state frames of the sensors. I'd rather do the former. Other suggestions are welcome!

from drake.

Or, I could just rely on the fact that the RigidBodyManipulator state frames come first in the TSRBM multi-frame and replace lines 79-84 of TimeSteppingRigidBodyManipulator with

      if isa(obj.getStateFrame(),'MultiCoordinateFrame')
        cv = obj.getStateFrame().splitCoordinates(x);
        x_manip = cv{1:length(obj.name)};
      else
        x_manip = x;
      end

from drake.

another option would be to have nested multicoordinateframes. so actually have each sensor output it’s own coordinate frame. don’t know if this would incur some short-term inefficiency, but it would be the cleanest long-term solution.

On Apr 1, 2014, at 12:32 PM, Andres Valenzuela [email protected] wrote:

Yeah. This is on me. I totally forgot about the multi-robot case when I implemented this. What we need is a way tell which frames in the TSRBM state-frame (which is a MultiCoordinateFrame) correspond to the sensor states. I could add a property to TSRBM called sensor_frame_indices or loop through the frames in the state multi-frame and check names against the state frames of the sensors. I'd rather do the former. Other suggestions are welcome!

—
Reply to this email directly or view it on GitHub.

from drake.

I don't think I can nest multi-coordinate frames, as per this comment in the MultiCoordinateFrame constructor:

% if coordinate_frame contains multi-frames, then extract them here
% (don't allow recursive multi-frames)

from drake.

doh. :)

On Apr 2, 2014, at 10:04 PM, Andres Valenzuela [email protected] wrote:

I don't think I can nest multi-coordinate frames, as per this comment in the MultiCoordinateFrame constructor:

% if coordinate_frame contains multi-frames, then extract them here
% (don't allow recursive multi-frames)
—
Reply to this email directly or view it on GitHub.

from drake.

This was resolved by 22b41ba.

from drake.