prius.sdf does not match prius.urdf

[RussTedrake]

In #1711. i'd like for the dynamics to match exactly, so that we can use this as another unit test. But right now, in addition to not matching the prius.sdf model actually goes unstable when used for simulation. So there is something fundamentally wrong with either the sdf or the parser. This one is urgent. :)

[RussTedrake]

The new prius.sdf still doesn’t match (nor run), but is definitely closer. @Sproulx — you can see what I’m seeing by running

drake/pod-build/bin/compareRigidBodySystems full-path-to/prius.urdf full-path-to/prius.sdf 

that will print information on your screen about two files that it generated. on my mac, I then run

dot -Tpng -O /tmp/r1.dot; dot -Tpng -O /tmp/r2.dot; open /tmp/r*.dot.png

to open them and compare.

[liangfok]

The Prius SDF model is missing the following links relative to the URDF model:

1. front_right_contact_point
2. rear_right_contact_point
3. front_left_contact_point
4. rear_left_contact_point

This difference is resulting in RigidBodySystem::getNumStates() returning different values. The URDF has 35 states (18 positions, 17 velocities) while the SDF only has 27 states (14 positions, 13 velocities). This is why compareRigidBodySystems crashes. I'm not sure if it provides any insight into why the simulation crashes.

I also notice the SDF has a lidar link whereas the URDF does not.

[RussTedrake]

The fix is actually to simplify the urdf. Those joints are extra; we could have implemented the same dynamics with a spring element on the prismatic joint instead of the linear spring across the joints. And we can add the lidar link to the urdf, because having them match is valuable.

[liangfok]

It looks like the URDF is modeling a suspension system whereas the SDF is not. This is because the URDF contains prismatic joints with parallel spring elements between the wheels and the rest of the car. In contrast, the SDF does not contain any prismatic joints.

I believe modeling a suspension system is desirable and thus the SDF should be updated to match the URDF in terms of having prismatic joints between the wheels and the rest of the car. Is this correct?

[RussTedrake]

Suspension yes. But there was an extra, unnecessary joint in the urdf model, and the suspension can be written in better way using a joint spring instead of a linear spring (which acts more like a compliant four-bar linkage). I think you’re right that we still need to add the spring parameters to the suspension joint in the sdf. i’ll take a quick pass at updating the urdf now.

[RussTedrake]

@liangfok -- you were absolutely correct. the names of the links were just confusing me, because the contact points were originally used for something different. i've opened #1770 and consider the urdf to be in decent shape now. agree that the rest of the work is on the sdf. In SDF we can just have a spring tag on the actual prismatic joint (we could have done that in urdf, too, but it's probably not worth changing since they compute the same thing).

NB: the suspension is implemented right now as a prismatic joint hanging off the end of the tie rod assembly, which I'm not sure I love. Obviously the real system is much more complicated (http://www.gatesautomotiveservice.com/Pages/Suspension.aspx). Just putting it out there that we might want to revisit this later.

[liangfok]

I noticed prius.urdf has a "loop_joint", which I don't believe is supported by ROS/Gazebo. Is this a new type of joint introduced by Drake?

[liangfok]

Despite applying the SDF COM calculation fix, I am unable to simulate the Prius using its SDF. The error appears to depend on whether Drake was compiled in release vs. debug mode.

After compiling in debug mode, I get a AssertionptIndex >= 0'` error from within Bullet:

carSimLCM: /home/liang/dev/drake/externals/bullet/pod-build/bullet3-prefix/src/bullet3/src/BulletCollision/CollisionShapes/btConvexShape.cpp:123: btVector3 convexHullSupport(const btVector3&, const btVector3*, int, const btVector3&): Assertion `ptIndex >= 0' failed.

The gdb backtrace indicates the problem is stemming from the following call into bullet:

DrakeCollision::BulletModel::findClosestPointsBtwElements (this=0x87a2e0, idA=9039920, idB=8995456, use_margins=true, c=std::unique_ptr<DrakeCollision::ResultCollector> containing 0x8d6fb0)
    at /home/liang/dev/drake/drake/systems/plants/collision/BulletModel.cpp:473

After compiling in release mode, I get a "No closest point found between X and Y" error:

terminate called after throwing an instance of 'std::runtime_error'
  what():  In BulletModel::findClosestPointsBtwElements: No closest point found between 26488880 and 26444416

The two numbers in the error above change with each execution.

From @RussTedrake's suggestion, I tried increasing the real-time factor to 10 (10X slower than real-time), but the error persists, see: https://github.com/liangfok/drake/commit/fb98d49a84a1b4acad8f0b25b75fc1ea83ed8046.

Earlier @david-german-tri discovered that the above errors also change depending on whether a scene is loaded.

[sammy-tri]

Minor note, which is probably not relevant until we get a lot further into suspension modelling... The graphic linked above is for a double wishbone suspension. I'm about 99% certain that all Prius models produced have been MacPherson struts, which do have fewer moving parts...

https://en.wikipedia.org/wiki/MacPherson_strut or http://auto.howstuffworks.com/car-suspension4.htm

[liangfok]

Based on something @bmartin-tri mentioned in slack, I modified the Drake simulator to pause each cycle of the simulation loop and saw that the robot was starting very far away from the world origin, which I assume is in the middle of the grid.

The "No closest point found between X and Y" occurs after 10 cycles of the simulation loop when the car is re-positioned at the world origin. At this time, the car is sitting sideways on the ground and the wheels, axles, and chassis are all underground.

Looks like we need to fix the model and its initial configuration in the world.

[liangfok]

I'm fairly confident the SDF file itself is OK and the bug is in Drake (maybe RigidBodyTreeSDF.cpp?) because I can load the SDF model in Gazebo without a problem.

[psiorx]

I'm fairly sure this is another uninitialised memory bug that should be fairly straightforward to track down using valgrind if you can reliably reproduce it. Happy to help with it. On Mar 3, 2016 6:04 PM, "Chien-Liang Fok" notifications@github.com wrote:

I'm fairly confident the SDF file itself is OK and the bug is in Drake (maybe RigidBodyTreeSDF.cpp?) because I can load the SDF model in Gazebo http://gazebosim.org/ without a problem.

[image: drake_prius_sdf_in_gazebo] https://cloud.githubusercontent.com/assets/1388098/13512700/44769b26-e16a-11e5-9076-951c4182eae7.png

— Reply to this email directly or view it on GitHub https://github.com/RobotLocomotion/drake/issues/1714#issuecomment-192009922 .

[liangfok]

Yes it is reproduceable. I will seek your help if I cannot figure it out. Thanks for the tip!

[liangfok]

I haven't positively identified the SDF problem yet but there are three places I'm currently suspicious about:

1. drake/systems/plants/shapes/Element.h contains a member variable called T_element_to_world. In RigidBodyTreeSDF.cpp, method parseSDFVisual(), it doesn't look like this variable is initialized when a VisualElement is instantiated.
2. In RigidBodyTreeSDF.cpp, the last parameter to methods parseSDFVisual() and parseSDFCollision() is transform_parent_to_model. However it looks like a world-to-model transform is being passed in instead.
3. In drake/systems/plants/collision/Model.cpp, there is a comment saying "fixme: this is taking T_local_to_world, not T_elem_to_world. so this method name is wrong".

SDF includes a pose specification for a link plus a pose specification for each visualization element nested within the link. It looks like Drake's SDF parser is ignoring the pose of the link when drawing the nested visualizations.

[liangfok]

The pose element inside of the joint element appears to be modifying the reference frame in which child link poses are specified. Prior to commenting out these pose elements, loading prius.sdf resulted in all links at the zero position:

After commenting out the pose elements inside the joint elements, the car loads correctly:

Assuming Gazebo's SDF parser is correct, I believe the above observation indicates there is a problem in the way Drake's SDF parser interprets pose elements inside joint elements.

I got the idea of removing the pose elements from the joint elements because I noticed the URDF-to-SDF converter tool I was using omitted the pose element within joint elements. See for example:

• stickbot_lowerleg_3dof_urdf.urdf
• stickbot_lowerleg_3dof_sdf.sdf

The above SDF file was automatically generated by Gazebo's gz tool.

[liangfok]

With @RussTedrake's help, the problem was identified to be the fact that links in an SDF model can be defined in an arbitrary frame. This differs from URDF and Drake's internal model, which require that links be defined in the same frame as their "parent joints" (i.e., the joint that connects a link up the kinematic tree). This explains why the URDF-to-SDF converter results in the "pose" of joints to always be zero, since the joint frame is the same as the child link's frame.

Fixing the problem required modifying the reference frames of a link's inertia matrix, visualization elements, and collision elements to match that of its parent joint. The fix was applied in the following commits:

• https://github.com/liangfok/drake/commit/6a63b4df1fc660fb23bebfa2b4a71fec11ce1e63
• https://github.com/liangfok/drake/commit/46f15df42c977c233788af8e11ff45a5e01feed8

With this fix, I am now able to simulate the SDF version of the prius. Here is a screenshot after 5 seconds of simulation (1000 simulation cycles):

There are still some issues that need to be resolved prior to closing this ticket:

1. The correctness of the model appears to depend on the order of the links and joints inside of the SDF file.
2. Modify URDF to match SDF to allow for comparisons.
3. I'm unable to drive the car around. The axis of the wheels appear incorrect. Here's what the car looks like after 20,000 cycles of the simulation loop:

The axes of rotation appear OK in Gazebo:

The Drake Visualizer clearly shows a problem with the wheel joints. The SDF file specifies that the axis of rotation is the Y, which is pointing up.

[sherm1]

Some thoughts, not sure how relevant to this issue:

• In general the (parent,child) relationship in SDF defines the sense of the connecting joint but is independent of the kinematic tree because you can form loops with it. In Gazebo we used a model-building step to map the (parent,child) graph to a kinematic tree plus constraints for use with Simbody. When the distinction is important, I use the terms (inboard,outboard) for the cross-joint relation in the kinematic tree (inboard==closer to world in kinematic graph, outboard==further from world). The (parent,child) relation is user-meaningful while the (inboard,outboard) relation is an implementation detail.
• One way to stay out of trouble in a multibody system is to use the world frame for everything internally (I learned this trick from Abhi Jain at JPL and used it successfully in Simbody.) Every cross-body calculation has to be done in a common frame anyway; Abhi deals with that by mapping everything to the ground (==world) frame first rather than doing them pairwise. This gets rid of a lot of potential confusion and cleans up the notation considerably. We should consider this for Drake if it isn't being done that way currently.

[liangfok]

@sherm1: Thanks for the advice. Your reason for doing everything in the world frame may explain why SDF by default defines link poses in the model frame. I will start using the terms "inboard" and "outboard" links on the context of joints in the future. In your Gazebo+Simbody system, is a constraint used to form loops in a kinematic tree?

[RussTedrake]

@sherm1 -- that's interesting. we store (almost) everything in relative coordinates in drake. For instance, visual geometry in a link has a transform to the link frame, and the interia is in the link frame. Since we use minimal coordinates, the link frame is the same as the inbound joint frame. That seems like the right coordinate system for me. are you actually proposing that the internal representation in the link is in a world frame?

[liangfok]

I believe the joint axis of rotation problem was a simple fix:

• https://github.com/liangfok/drake/commit/3027a471eacdcb46e4dd87cde3d119fe100b4369

Currently handling a problem where the loop joint between the right_tie_rod_arm and tie_rod is not working. The following screenshot was taken after 64.93 simulation seconds (12,986 cycles):

[sherm1]

Your reason for doing everything in the world frame may explain why SDF by default defines link poses in the model frame.

No, they were doing that before I got involved. I did get them to add the ability to explicitly specify joint axes in the link frame because it is not always reasonable to require that the links are assembled when defined (that's very difficult for closed-topology mechanisms). The world frame computations I meant affect only internal calculations and shouldn't be user visible at all.

[sherm1]

I will start using the terms "inboard" and "outboard" links on the context of joints in the future.

It is still good to use "parent" and "child" whenever you mean something user-facing. Inboard and outboard refer to the topology of the multibody tree we use for the computational representation of a user's model. For a given joint it is possible that the user's parent/child sense will be reversed from the inboard/outboard sense. We found that the term "joint" was ambiguous (especially in a biomechanical context where everyone thinks they already know what a "knee joint" is!), so coined a term "mobilizer" that refers only to internal coordinate joints in a multibody tree. That makes it possible to talk about a joint (physical, meaningful concept) being implemented using a mobilizer (differential equations), or constraints (algebraic equations), or a bushing (force element).

Most people don't need to make those distinctions, but when you do it really helps shorten the discussion to have unambiguous terms to use! This paper introduced the term "mobilizer" and defines it.

[sherm1]

@RussTedrake --

we store (almost) everything in relative coordinates in drake.

Terminology clarification: "relative coordinates" to me means "joint space (minimal) coordinates", i.e. link's kinematics are known only relative to its parent (common usage among multibody geeks). But I think here you are using it to mean "in the link frame". Please correct me if I'm misunderstanding.

For instance, visual geometry in a link has a transform to the link frame, and the inertia is in the link frame.

Yes, that makes sense and that's how I do it too.

Since we use minimal coordinates, the link frame is the same as the inbound joint frame.

That is reasonable as a way to handle internal computations, but not as a way to define the system in general. Do you mean for internal use? There are several reasons why it doesn't work for definition (happy to discuss further) but as one example a link in a closed topology doesn't have a unique inboard joint until after a minimal spanning tree is chosen for it.

That seems like the right coordinate system for me. are you actually proposing that the internal representation in the link is in a world frame?

No. Just that when we perform internal computations, such as the tree recursion used to calculate accelerations, it is cleaner to re-express everything in World first than to re-express one step at a time from child to parent to grandparent while performing ugly computations. For example, working in World means an articulated body inertia never needs to be rotated -- only much cheaper shift operations are needed.

The transform to World is free anyway because it has to be done to go from minimal coordinates to Cartesian pose. So given a new state, all kinematics and mass props can be re-expressed in world (just instantaneously). After that all hard computations can assume World frame quantities. The code is easier to understand and debug that way.

[RussTedrake]

@sherm1 - In drake "link frame" = "joint frame". so i think we're on the same page. and yes, drake calculates everything into world coordinates once in the call to doKinematics, then all other queries use the results of that computation.

[sherm1]

In drake "link frame" = "joint frame". so i think we're on the same page.

I'm not so sure! In general a link can have many joints. But I'm guessing you mean what I would write as "link frame" = "mobilizer frame", meaning define a spanning tree for all the links and then work in the frame of the unique inboard joint (mobilizer) for each link. That's fine if it isn't user visible but often those choices leak out into the model definition. IMO the user should define a natural link frame, and that choice will often be distinct from all the joint frames. That may be exactly what you have; I'm sure this would be a short discussion with a whiteboard handy! Here is a typical picture I use for the user-visible body (link) and joint frames:

The parent and child bodies each have their own frames defined independent of any joint. Then every joint defines a pair of frames, one fixed on each of the two connected bodies. (From Simbody's doxygen.)

[RussTedrake]

Yes. This is all consistent. The sdf format provides this view to the user. Internally drake stores a kinematic tree with a unique parent for every link, therefore a unique joint (call it mobilizer frame) if you will. additional joints are added in as a special “RigidBodyLoop” object, which adds constraints to the dynamics that are solved at runtime. The biggest problem with the sdf format is lack of documentation on default frames. It was maddening. Because of the history (starting for me with the sdfast format, and continuing through urdf) of specifying all transforms relative to the parent, it took time and experimentation to realize that in sdf the link and joint frames are specified in the model coordinates by default, instead of in the parent coordinates. That has been the source of many of @liangfok’s frustrations.

[liangfok]

The broken loop joint in the front steering mechanism was fixed in the following commit (thanks @amcastro-tri for helping to find the solution):

• https://github.com/liangfok/drake/commit/f91b7df648bb8704de1b6759aa9eb5b2f5c040b4

It was caused by a bug in the way a joint's axis of rotation was being transformed from the model coordinate frame to the joint's coordinate frame.

Currently looking into why I can't drive the car around.

[liangfok]

The SDF-based Prius wasn't moving because PD controllers were not being attached to the actuators. It was fixed in this commit: https://github.com/liangfok/drake/commit/e9ae226d06c423d9e68c0e0c5f34960f63f1f2ed

Unfortunately, now that the actuators work, it's obvious the original loop joint problem was not solved. Here's what the front steering mechanism looks like when I try to drive backwards.

[liangfok]

The loop joint (right_pin) did not work because tie_rod's frame was internally modified by Drake's SDF parser to adhere to the convention that child link frames be equal to their parent joint frames. In this case, tie_rod is a child joint of left_pin meaning tie_rod's frame is at the left_pin as shown below. Note that the origin of tie_rod's frame's Y axis is at Y=0.4 in the model frame.

This is in contrast to the original (unmodified) model, which defines tie_rod's frame's Y-axis to be at Y=0 in the model frame.

Since the tie_rod's frame was shifted to the left, the pose of the right_pin joint, which also has tie_rod as a child link needs to be updated. The original pose assumed tie_rod's frame is centered at Y=0:

<pose>0 -0.408503 0 0 0 0</pose>

Now that tie_rod's frame at at the left_pin, the new pose of right_pin is:

<pose>0 -0.817006 0 0 0 0</pose>

This change, along with a fix on how to calculate the "loop point" in both the child and parent links' reference frames, resulted in the loop joint working.

Here is the commit: https://github.com/liangfok/drake/commit/2e74241c9c8708faad3b549499ee0d9b2b47ae12

Here is a video: https://youtu.be/IX8M8K6-zfY

Remaining issues:

1. Add support for joint position limits.
2. Update URDF to match SDF and verify they are the same according to the compareRigidBodySystems tester.
3. Verify that the model can load correctly regardless of the order in which the elements appear in the SDF file.
4. Add LIDAR sensor (I had to comment it out due to an error regarding the number of outputs of the RigidBodySystem component, see: https://github.com/liangfok/drake/blob/feature/debug_sdf_2/drake/examples/Cars/prius/prius_debug.sdf#L441

[liangfok]

Support for joint limits was fixed in this commit: https://github.com/liangfok/drake/commit/41cd8375192bb93734991976dfe8a412b5cb6b9c

[liangfok]

I created a URDF model based on the working SDF model but the simulation is unable to remain stable. Using the URDF, the wheels appear to spin as they sink into the ground causing the model to "blow up" within 10 simulation cycles. This is strange since the SDF model simulates just fine.

I verified that the RigidBodyTree objects created from both URDF and SDF model files are identical in terms of kinematics and dynamics. However, differences in the collision models may be causing the instability in the URDF. The screenshot below shows the differences in the collision models between the URDF (right) and SDF (left) models. Note that this is using simplified models that only contain the chassis floor, front axle, tie rod links, and the two front wheels.

Here are the model files:

• URDF
• SDF

The commit containing the debug statements is here: https://github.com/liangfok/drake/commit/5c3b40af183d3a6e36bb05aa633446cf50fb3a13

I will continue to investigate in the morning.

[sherm1]

@liangfok -- numbers with exponents in the -300s almost always indicate uninitialized memory. Strange to see those above.

[RussTedrake]

unless you are plotting the world transform after the simulation has already blown up...?

[liangfok]

Thanks I will look into whether this is a memory initialization issue. The collision model numbers shown above are from immediately after loading the models and prior to the first simulation cycle, so I am fairly sure the simulation hadn't blown up yet.

[liangfok]

The memory initialization issue was fixed (https://github.com/liangfok/drake/commit/fdc288e1878882382b2fea61a6a2878050133fee) but this did not solve the URDF-exploding problem. The problem is related to the wheel's inertia.

To investigate this problem, I pinned the robot 1m high in the air using a fixed joint. The screenshot below shows the left wheel's frame. It rotates along the negative Z (blue) axis.

The wheel has a mass of 14.28kg, a radius of 0.323342m, and a width of 0.215m. Using the formula for the moment of inertial tensor for a solid cylinder, I computed the inertia matrix to be:

<inertia ixx="0.42825" ixy="0" ixz="0" iyy="0.42825" iyz="0" izz="0.746487"/>

Using this value in the model, the left wheel would initially remain still but start spinning after about 40 simulation cycles. At this point, it would promptly spin out of control.

Disabling gravity results in the left wheel remaining still even after 350 simulation cycles. Setting the wheels mass to be zero also results in it remaining still after 350 simulation cycles. I believe these observations support the hypothesis that the problem is related to the wheel's inertia model.

[liangfok]

Both the Prius URDF and SDF simulations work. The aforementioned problem with the URDF-based simulation was caused by the following two issues:

First, the min/max position limits of the wheel joints were accidentally set to zero. According to the URDF specification, continuous joints should omit the limit specification. It appears that Drake goes unstable when a revolute joint has min/max position limits of zero.

Second, URDF does not specify the position and orientation of the model's root link in the world. This results in the robot being initialized with its chassis_floor link on the ground and the axles / wheels entirely / partially underground. Fixing this problem required modifying the URDF parser with a custom model-to-world transform. With these changes, the simulation of the URDF Prius works.

Below is a video showing the URDF version of the Prius working in simulation. At time 1:35, the video switches to the SDF version of the Prius to show that both work.

TODO:

1. Verify the URDF and SDF models match according to the compareRigidBodySystems tester.
2. Modify API of RigidBodyTreeURDF to take as input the position and orientation of the robot's root link.
3. Determine whether the ordering of elements in the URDF / SDF file do in fact matter and if so create a separate issue to track the resolution of that problem.
4. Create a separate issue to track addition of LIDAR sensor to the URDF / SDF models. More details here.
5. Modify RigidBodyTreeURDF to issue warning if a revolute joint has limits of min/max = 0.

[RussTedrake]

Great!

If joint limits are both then you will have constraint forces active. This is especially true if the initial conditions of the car violate the joint limits.

If you start the car under the ground, you will also have constraint forces active.

In both cases, the simulation should be stable again (but potentially exciting -- car flying into the air, etc) if you make the simulation dt small. This should always be the first test. Soon @sherm1 will bring online a variable step solver that will do this automatically.

We have logic for finding a feasible initial condition (which satisfies all of the constraints) at the beginning of simulation which i have temporarily disabled for the car simulation because it requires SNOPT. As soon as we have a SNOPT alternative in the optimization hierarchy, we can turn it back on and these things will get better.

While urdf does not have a tag for the mode relative to the world, the addRobotFromURDF can take an initial pose programmatically. That's how we placed the models in the world in the past.

[liangfok]

Thanks for the info. Can't wait for @sherm1's variable step solver to be integrated! For debugging purposes it would be nice to print or even better visualize all constraint and internal forces in the system. Is this capability already available in the C++ version of Drake? Can you point me to where in Drake these forces are being computed?

[RussTedrake]

They are in the dynamics method of RigidBodySystem.cpp

[amcastro-tri]

Great work! that's awesome news!. I'd really like to have your new sdf model to play around with my height fields.

Regarding better ways of visualization for debugging I completely agree with you @liangfok . @RussTedrake, can Director already visualize velocities (linear/angular), twists, frames, contact points?

[RussTedrake]

Director has a panel that can visualize triads and other widgets, but also supports rendering lcmgl -- it's easy to publish a message from your code to publish an openGL style draw command directly to the viewer. We use the java version of lcmgl quite a bit in the matlab code, but it has c bindings as well.

For example, the botvisualizer.m class has an option which will draw the "inertial ellipsoids" on top of the visual geometry for debugging inertial properties of a model.

If you explore it and have a simple C++ example, this would be a great thing to add to the drake sphinx documentation.

[amcastro-tri]

@RussTedrake. Where in the code is a good place to start? Besides debugging as @liangfok mentioned I am interested in visualizing the height fields. Where is a good place to start in the C++ code? where in the code lcmgl is used? (for instance to visualize triangulated surfaces).

I can get a triangulated surface from the height field (and I did in my example #1616, just for visualization). Is there a place for instance in the BotVisualizer where similar information is passed to Director?

[RussTedrake]

The inertial ellipsoids example is here: https://github.com/RobotLocomotion/drake/blob/master/drake/systems/plants/BotVisualizer.m#L132 drawing the heightmap terrain is handled differently. on load, i actually write a new .obj file to disk, then add a visual mesh geometry to the world element of the kinematic tree. then the existing BotVisualizer.h class will send all the draw commands for you. https://github.com/RobotLocomotion/drake/blob/master/drake/systems/plants/RigidBodyHeightMapTerrain.m#L40

[liangfok]

I've verified that the correctness of the model currently does depend on the order of elements in the SDF file. I created a new issue to track its resolution: https://github.com/RobotLocomotion/drake/issues/1856

[liangfok]

I'm running compareRigidBodySystems to verify URDF and SDF models of the Prius are identical. The test passes the part where it checks that the two models share the same number of states, inputs, and outputs. However, an exception is thrown the first time the following line of code is executed:

VectorXd x = getInitialState(*r1);

Here is the gdb backtrace:

terminate called after throwing an instance of 'std::runtime_error'
  what():  not implemented yet

Program received signal SIGABRT, Aborted.
0x00007ffff46a8cc9 in __GI_raise (sig=sig@entry=6) at ../nptl/sysdeps/unix/sysv/linux/raise.c:56
56  ../nptl/sysdeps/unix/sysv/linux/raise.c: No such file or directory.
(gdb) bt
#0  0x00007ffff46a8cc9 in __GI_raise (sig=sig@entry=6) at ../nptl/sysdeps/unix/sysv/linux/raise.c:56
#1  0x00007ffff46ac0d8 in __GI_abort () at abort.c:89
#2  0x00007ffff4fb3535 in __gnu_cxx::__verbose_terminate_handler() () from /usr/lib/x86_64-linux-gnu/libstdc++.so.6
#3  0x00007ffff4fb16d6 in ?? () from /usr/lib/x86_64-linux-gnu/libstdc++.so.6
#4  0x00007ffff4fb1703 in std::terminate() () from /usr/lib/x86_64-linux-gnu/libstdc++.so.6
#5  0x00007ffff4fb1922 in __cxa_throw () from /usr/lib/x86_64-linux-gnu/libstdc++.so.6
#6  0x00007ffff7a03c66 in Drake::OptimizationProblem::MathematicalProgram::solve (this=0x6bba80, prog=...)
    at /home/liang/dev/drake/drake/../drake/solvers/Optimization.h:709
#7  0x00007ffff7a03d92 in Drake::OptimizationProblem::NonlinearProgram::solve (this=0x6bba80, prog=...)
    at /home/liang/dev/drake/drake/../drake/solvers/Optimization.h:729
#8  0x00007ffff7a03b2e in Drake::OptimizationProblem::solve (this=0x7fffffffcaa0)
    at /home/liang/dev/drake/drake/../drake/solvers/Optimization.h:584
#9  0x00007ffff79fd97e in Drake::getInitialState (sys=...) at /home/liang/dev/drake/drake/systems/plants/RigidBodySystem.cpp:298
#10 0x000000000044d38a in main (argc=3, argv=0x7fffffffcd58) at /home/liang/dev/drake/drake/systems/plants/test/compareRigidBodySystems.cpp:37

Looking at Optimization.h:727, the problem is I do not have SNOPT installed. @RussTedrake, is there a workaround to this problem?

[RussTedrake]

Sorry if this sounds snarky: install SNOPT. :) But we should also print a more useful error message. This will also be resolved with #1627.

[liangfok]

No problem! Sorry I was confused about the earlier discussion in this thread about finding a SNOPT alternative:

We have logic for finding a feasible initial condition (which satisfies all of the constraints) at the beginning of simulation which i have temporarily disabled for the car simulation because it requires SNOPT. As soon as we have a SNOPT alternative in the optimization hierarchy, we can turn it back on and these things will get better.

My impression was that SNOPT was not available for some reason. Its website has a form for me to fill out to get a trial version. Will look into it.

[liangfok]

While urdf does not have a tag for the mode relative to the world, the addRobotFromURDF can take an initial pose programmatically. That's how we placed the models in the world in the past.

In RigidBodyTreeURDF.cpp, method addRobotFromURDF(...) includes a weld_to_frame parameter that allows a URDF model to be attached to an existing body, but not to the world since the world does not exist at the time the method is called. Also this parameter was not exposed through RigidBodySystem's API, meaning it could not be set by simulateLCM.cpp, which implements the car simulation's main() method. I modified all three to support programmatic specification of the URDF model's root link's location and orientation in the world. It's not pretty since the weld_to_frame parameter of RigidBodySystem::addRobotFromFile(...) is only used when the model is a URDF.

Here is the commit: https://github.com/liangfok/drake/commit/6f224b4ebb103e58bad043b35746df39249e84d4

[liangfok]

I manually tweaked prius_v1.urdf and prius_v1.sdf to be nearly identical but still cannot get the compareRigidBodySystems test to pass. Here are visualizations of the rigid body trees:

URDF:

SDF:

I'm seeing some very high velocity values. Maybe this is because random inputs are being fed into the system.

A:
       0
       0
       0
       0
       0
       0
       0
       0
       0
       0
       0
       0
       0
       0
 47.7606
 6.41334
 348.346
-18.0603
 378.801
-4.85224
-5435.36
 4323.85
-46.3032
-5595.89
 42.0212
-6.41334
-6.41334
B:
       0
       0
       0
       0
       0
       0
       0
       0
       0
       0
       0
       0
       0
       0
-127.143
-10.3149
-1159.41
-6.68444
-1050.45
-7.30237
   17109
-13750.8
 120.469
 16872.6
-110.807
 10.3149
 10.3149
A-B:
       0
       0
       0
       0
       0
       0
       0

       0
       0
       0
       0
       0
       0
       0
       0
       0
       0
       0
 174.904
 16.7283
 1507.75
-11.3759
 1429.25
 2.45014
-22544.4
 18074.7
-166.772
-22468.5
 152.829
-16.7283
-16.7283

I will continue to investigate this problem in the morning. I may need to resort to the technique of starting with a simple single-joint/link model and incrementally adding joints/links each time re-testing for equivalence until the whole vehicle is assembled.

[amcastro-tri]

I found very useful in the past to tell Eigen what to do on initialization as described here.

The preprocessor directive EIGEN_INITIALIZE_MATRICES_BY_ZERO would let you check if you can run both, your sdf and urdf cases successfully. If that's the case most likely something is not correctly initialized. You can also try EIGEN_INITIALIZE_MATRICES_BY_NAN which will make your uninitialized entries Nan and easier to pickup with a simple print. This however can be a bit confusing if there are many non initialized variables, though it'd be good to go through all of them at some point.

Notice that Eigen does not define either of those and therefore variables not initialized by the user most likely have garbage. I think this is the desired option for efficiency reasons.