RussTedrake commented 8 years ago

[ ] parse the heightmap field from sdf (http://sdformat.org/spec?ver=1.5&elem=geometry#geometry_heightmap)
[ ] populate bHeightfieldTerrainShape in the bullet wrapper (http://bulletphysics.com/Bullet/BulletFull/classbtHeightfieldTerrainShape.html)
[ ] import heightmap images from @crogers

crogers3 commented 8 years ago

So, as a first step, should I implement a Drake::Heightmap in Geometry.h, which will hold onto the bullet object and use it to answer to getPoints() and getBoundingBoxPoints()?

RussTedrake commented 8 years ago

Yes. Although answering getPoints() and getBoundingBox() with a runtime_error(“not implemented yet”) is ok for the first pass. The parts we really need are for the collisionDetect to work with that type (the object needs to get added to the collision world, etc).

crogers3 commented 8 years ago

I'm having trouble debugging a segfault that I keep getting when using the btHeightfieldTerrainShape. The segfault happens when the simulation runs and btHeightfieldTerrainShape tries to access the data array that holds the raw heights. I know that this data array needs to be kept alive in memory by something other than btHeightfieldTerrainShape, as per its documentation, so @psiorx and I were working yesterday to find the best class to hold onto this data. But after debugging today, I found that the problem actually comes from the index that btHeightfieldTerrainShape is trying to access. The x and y are completely wrong (too large of an x and a negative y), and the values I had originally supplied for width and height are completely different. This leads me to believe that somehow, the btHeightfieldTerrainShape is being corrupted or misaligned.

Is there anything you know about the storage of these Bullet Shapes within BulletModel.cpp that may pose a problem when storing a btHeightfieldTerrainShape? I'm completely lost at this point. I have confirmed that the btHeightfieldTerrainShape object is being initialized correctly with the correct values, but somewhere along the line, these values get mutated/corrupted. Does this sound familiar at all to you?

crogers3 commented 8 years ago

Something I noticed (which may or may not he helpful) is that the code calling for the height is of the class btConvexShape, which delegates a call to btHeightfieldTerrainShape which indexes for the height. But my problem is, btHeightfieldShape is a btConcave shape, it shouldn't be associated with btConvexShape as this. Is there something in the code that assumes that all Bullet Shapes we deal with are convex shapes?

crogers3 commented 8 years ago

Upon further investigation, we are casting the btHeightfieldTerrainShape as a btConvexShape from the generic btCollisionShape, which may cause our data corruption, since btHeightfieldTerrainShape is a btConcaveShape. We do this casting in BulletModel.cpp. So my guess is: either this cast is misaligning our data or the btGjkPairDetector we are passing the heightfield shape to is mistreating the shape, since it expects the shape to be convex.

Do you think this is the problem? If so, what is the best course of action to get around this?

crogers3 commented 8 years ago

I found one potential solution. Rather than using btHeightfieldTerrainShape, I can create a btConvexHullShape and add points for every pixel in the height map, similarly to what is done in BulletModel for Mesh. This seems to be working in the simulator, although higher resolution heightmaps cause a slowdown in simulation speed.

I could cleanly integrate this solution into DrakeShapes by making another constructor for the MeshPoints class, which produces a MeshPoints object by parsing a heightmap PNG and calculating the mesh points based on the pixels in the image.

Let me know if this is the route you want to go in. I will start looking into adding visuals for heightmaps.

RussTedrake commented 8 years ago

It seems like we should prefer the heightmap to the convex hull shape. it’s should definitely more efficient…. (and could be obviously be non-convex)

RussTedrake commented 8 years ago

from @avalenzu:

Have you tried using the potentialCollisionPoints method? That uses Bullet’s dispatch directly rather than looping over convex collision elements like we do when we want a constant number of collision pairs. I think that if you added a new DrakeCollision::Shape class for heightmaps and configured DrakeCollision::BulletModel to add a btHeightfieldTerrainShape to the collision world when passed an object of that class, it might work.

amcastro-tri commented 8 years ago

Some Progress

height_field

I was able to include a height field withing a simulation in bullet. Bullet2 used to provide a TerrainDemo with their code now available at an old google archive repo.

However the new Bullet3 does not provide any examples. I was able to modify one of the examples provided, API->BasicExample, to load a height field successfully. The most difficult task actually was how to render the height field to the screen.

The result can be seen in this movie.

Now that I know a little bit about bullet's inner workings the next task is to incorporate this into Drake. Wish me luck!

amcastro-tri commented 8 years ago

I was able to create a terrain in an sdf file and open it with Gazebo. For this example I used a png file however I saw online there are dem files. Which one should we use? is it correct using png files to describe our terrains? I wonder if there is any standard out there to describe terrains in engineering applications (besides the graphics community).

RussTedrake commented 8 years ago

Based on this discussion: https://github.com/RobotLocomotion/drake/issues/1803 it seems like we might have a preference for png?

jpieper-tri commented 8 years ago

For what it is worth, it is relatively easy to convert DEM files into png using GDAL and other tools. The only downside to png is that it has no geo-referencing information within the file. Typically, you would use an auxiliary pngw to provide a coordinate system for the .png (in the same manner as a TIFF "world" file).

RussTedrake commented 8 years ago

@jpieper-tri -- I would think that the pose in the urdf/sdf is probably the most natural place to contain that information?

jpieper-tri commented 8 years ago

Sure you can do that. It just won't inter-operate with any other GIS tool, so checking export accuracy may be tough.

RussTedrake commented 8 years ago

the urdf/sdf will impose a coordinate system in either case (whether it is in the image file or not). so i don't see a way around the accuracy check?

jpieper-tri commented 8 years ago

You could argue that is a deficiency in the sdf/urdf spec, but otherwise I agree.

amcastro-tri commented 8 years ago

I loaded the height map into Drake. Now I need to visualize it with Director. The code actually doesn't even run because BotVisualizer doesn't know the new shape yet.

I am trying to pass Director my height map. So in the initialization of BotVisualizer in BotVisualizer::publishLoadRobot() I am trying to do something like (this is just a draft, I still need to pass the terrain size, but the idea is the same):

      case DrakeShapes::HEIGHT_MAP_TERRAIN: {
            gdata.type = gdata.HEIGHT_MAP_TERRAIN;
            auto terrain = dynamic_cast<const DrakeShapes::HeightMapTerrain &>(geometry);
            gdata.num_float_data = terrain.nTotCells();            
            for(int i=0;i<terrain.nTotCells();i++)
              gdata.float_data.push_back(static_cast<float>(terrain.cellValue(i)));
            break;
      }

However gdata, declared here of course doesn't know what a HEIGHT_MAP_TERRAIN is.

Is this the right approach for this problem? if so, where do I find the definition for drake::lcmt_viewer_geometry_data? I figure I would then need to modify director?

I can easily create a triangular mesh if that helps...

amcastro-tri commented 8 years ago

I guess an easy hack would be to write an grid file when the HeightMapTerrain object is instantiated and then procede as in case DrakeShapes::MESH. Would this work?

RussTedrake commented 8 years ago

Yes. that's what we do in matlab.

amcastro-tri commented 8 years ago

Ok, thanks. Good to know. I'll give it a try.

patmarion commented 8 years ago

If it's helpful, I just added support to drake-visualizer for receiving mesh data over lcm, rather than receiving mesh filenames. For more details, see the pull request is here:

https://github.com/RobotLocomotion/director/pull/226

I have not added support to the drake BotVisualizer publisher, yet. But at least the support is in drake-visualizer in case someone would like to add the drake publishing feature. The implementation is very simple, but it seems to perform ok for now. We could discuss possible improvements.

amcastro-tri commented 8 years ago

So, I am getting a strange segmentation fault. When track it I end at this point where you can see that the collision shape is cast to btConvexShape*. However, a btHeightfieldTerrainShape is a btConcaveShape. I thinkg this is causing the segmentation fault. How should I proceed here? how much should I change findClosestPointsBtwElements in order to support the height maps?

amcastro-tri commented 8 years ago

by the way, thanks @patmarion for the post. I'll look into it once I get away with this new seg-fault.

liangfok commented 8 years ago

Clearly, Drake's BulletModel.cpp:461 assumes the collision shape is convex. Thankfully, btCollisionObject provides methods like isConvex() for querying whether this is true.

amcastro-tri commented 8 years ago

This is the backtrace I am getting using gdb. As you can see the last call on Drake's side happens at BulletModel.cpp:532, frame _#_4. After that all calls are on Bullet's side.

At frame #1 within Bullet's method btConvexShape::localGetSupportVertexWithoutMarginNonVirtual the code ends up in the default case within a switch when it fails to find the appropriate convex shape since we passed a concave shape instead cast to a convex one. We get the segmentation fault at frame _#_0 when we attempt to access a convex shape method not available in our concave height field map.

#0  0x00007ffff10c111e in btHeightfieldTerrainShape::getRawHeightFieldValue (this=0x7fffffffba80, x=8563104, y=-17856)
    at /home/alejandro/Drake/drake-distro/externals/bullet/pod-build/bullet3-prefix/src/bullet3/src/BulletCollision/CollisionShapes/btHeightfieldTerrainShape.cpp:156
#1  0x00007ffff10bc00f in btConvexShape::localGetSupportVertexWithoutMarginNonVirtual (this=0x82a9a0, localDir=...)
    at /home/alejandro/Drake/drake-distro/externals/bullet/pod-build/bullet3-prefix/src/bullet3/src/BulletCollision/CollisionShapes/btConvexShape.cpp:296
#2  0x00007ffff10f3587 in btGjkPairDetector::getClosestPointsNonVirtual (this=0x7fffffffc280, input=..., output=..., debugDraw=0x0)
    at /home/alejandro/Drake/drake-distro/externals/bullet/pod-build/bullet3-prefix/src/bullet3/src/BulletCollision/NarrowPhaseCollision/btGjkPairDetector.cpp:141
#3  0x00007ffff10f31c8 in btGjkPairDetector::getClosestPoints (this=0x7fffffffc280, input=..., output=..., debugDraw=0x0, swapResults=false)
    at /home/alejandro/Drake/drake-distro/externals/bullet/pod-build/bullet3-prefix/src/bullet3/src/BulletCollision/NarrowPhaseCollision/btGjkPairDetector.cpp:77
#4  0x00007ffff4a0b93e in DrakeCollision::BulletModel::findClosestPointsBtwElements (this=0x8132e0, idA=8504080, idB=8506720, use_margins=true, 
    c=std::unique_ptr<DrakeCollision::ResultCollector> containing 0x81b1e0) at /home/alejandro/Drake/drake-distro/drake/systems/plants/collision/BulletModel.cpp:532
#5  0x00007ffff4a0d091 in DrakeCollision::BulletModel::closestPointsPairwise (this=0x8132e0, id_pairs=std::vector of length 1, capacity 1 = {...}, use_margins=true, 
    closest_points=std::vector of length 0, capacity 0) at /home/alejandro/Drake/drake-distro/drake/systems/plants/collision/BulletModel.cpp:731
#6  0x00007ffff4a0cf9e in DrakeCollision::BulletModel::closestPointsAllToAll (this=0x8132e0, ids_to_check=std::vector of length 2, capacity 2 = {...}, use_margins=true, 
    closest_points=std::vector of length 0, capacity 0) at /home/alejandro/Drake/drake-distro/drake/systems/plants/collision/BulletModel.cpp:721
#7  0x00007ffff621aa32 in RigidBodyTree::collisionDetect (this=0x813190, cache=..., phi=..., normal=..., xA=..., xB=..., bodyA_idx=std::vector of length 0, capacity 0, 
    bodyB_idx=std::vector of length 0, capacity 0, ids_to_check=std::vector of length 2, capacity 2 = {...}, use_margins=true) at /home/alejandro/Drake/drake-distro/drake/systems/plants/RigidBodyTree.cpp:414
#8  0x00007ffff621b3e8 in RigidBodyTree::collisionDetect (this=0x813190, cache=..., phi=..., normal=..., xA=..., xB=..., bodyA_idx=std::vector of length 0, capacity 0, 
    bodyB_idx=std::vector of length 0, capacity 0, use_margins=true) at /home/alejandro/Drake/drake-distro/drake/systems/plants/RigidBodyTree.cpp:524
#9  0x00007ffff7a060e2 in Drake::RigidBodySystem::dynamics (this=0x8130d8, t=@0x7fffffffd2f8: 0, x=..., u=...) at /home/alejandro/Drake/drake-distro/drake/systems/plants/RigidBodySystem.cpp:124
#10 0x0000000000577cb3 in Drake::CascadeSystem<Drake::RigidBodySystem, Drake::BotVisualizer<Drake::RigidBodySystem::StateVector> >::dynamics<double> (this=0x81c248, t=@0x7fffffffd2f8: 0, x=..., u=...)
    at /home/alejandro/Drake/drake-distro/drake/../drake/systems/System.h:409
#11 0x0000000000574305 in Drake::CascadeSystem<Drake::CascadeSystem<Drake::RigidBodySystem, Drake::BotVisualizer<Drake::RigidBodySystem::StateVector> >, Drake::internal::LCMOutputSystem<Drake::CascadeSystem<Drake::RigidBodySystem, Drake::BotVisualizer<Drake::RigidBodySystem::StateVector> >::OutputVector, void> >::dynamics<double> (this=0x81e468, t=@0x7fffffffd2f8: 0, x=..., u=...)
    at /home/alejandro/Drake/drake-distro/drake/../drake/systems/System.h:409
#12 0x0000000000570a8c in Drake::CascadeSystem<Drake::internal::LCMInputSystem<Drake::CascadeSystem<Drake::RigidBodySystem, Drake::BotVisualizer<Drake::RigidBodySystem::StateVector> >::InputVector, void>, Drake::CascadeSystem<Drake::CascadeSystem<Drake::RigidBodySystem, Drake::BotVisualizer<Drake::RigidBodySystem::StateVector> >, Drake::internal::LCMOutputSystem<Drake::CascadeSystem<Drake::RigidBodySystem, Drake::BotVisualizer<Drake::RigidBodySystem::StateVector> >::OutputVector, void> > >::dynamics<double> (this=0x81e4a8, t=@0x7fffffffd2f8: 0, x=..., u=...)
    at /home/alejandro/Drake/drake-distro/drake/../drake/systems/System.h:409
#13 0x000000000056d42e in Drake::simulate<Drake::CascadeSystem<Drake::internal::LCMInputSystem<Drake::CascadeSystem<Drake::RigidBodySystem, Drake::BotVisualizer<Drake::RigidBodySystem::StateVector> >::InputVector, void>, Drake::CascadeSystem<Drake::CascadeSystem<Drake::RigidBodySystem, Drake::BotVisualizer<Drake::RigidBodySystem::StateVector> >, Drake::internal::LCMOutputSystem<Drake::CascadeSystem<Drake::RigidBodySystem, Drake::BotVisualizer<Drake::RigidBodySystem::StateVector> >::OutputVector, void> > > > (sys=..., t0=0, tf=inf, x0=..., options=...)
    at /home/alejandro/Drake/drake-distro/drake/../drake/systems/Simulation.h:90
#14 0x000000000056a163 in Drake::runLCM<Drake::CascadeSystem<Drake::RigidBodySystem, Drake::BotVisualizer<Drake::RigidBodySystem::StateVector> > > (sys=std::shared_ptr (count 2, weak 0) 0x81c248, 
    lcm=std::shared_ptr (count 2, weak 0) 0x82acc8, t0=0, tf=inf, x0=..., options=...) at /home/alejandro/Drake/drake-distro/drake/systems/LCMSystem.h:268
#15 0x0000000000564ca9 in main (argc=2, argv=0x7fffffffda48) at /home/alejandro/Drake/drake-distro/drake/examples/BouncingBall++/simulateLCM.cpp:174

amcastro-tri commented 8 years ago

My plan now is to study the method BulletModel::findClosestPointsBtwElements to evaluate how it can be extended to cases with concave shapes like the height map terrains.

My question at this point is, is there anywhere else within Drake where we assume convex shapes are used?

sherm1 commented 8 years ago

Closest point for non-convex is completely different and much harder than closed point for convex. Might be worth asking Erwin Coumans (Bullet author) if he has a suggestion before tackling this separately.

amcastro-tri commented 8 years ago

Thank you for the input @sherm1. As far as I understand Bullet is capable of dealing with the more general case of non-convex shapes though with the additional cost in performance. To deal with this for the very commonly occurring case of terrains they have a Height Map Terrain class that is supposed to be more efficient than the general non-convex shape. See the chart below on how to select your shape taken from Bullet's manual:

Bullet seems to have a collision dispatcher that calls the proper (most efficient) algorithm between two shapes. However it seems like Drake is not using this dispatcher but it implements its own? I am still studying the code in this regard...

avalenzu commented 8 years ago

Just a bit of historical context: The collision code in drake was originally written for use in planning problems where a fixed number of differentiable (mostly) closest distance constraints were required. The closestPointsAllBodies function was written to do just that. It's not a dispatcher really, since it always calls GJK/EPA. The collision geometries used for this were all convex, since the gradients of separation/penetration distances for non convex shapes are ill defined in many cases. The potentialCollisions function is much more along the lines of general purpose collision detection. It does use bullet's dispatcher. Hope that helps!

On Mon, Mar 21, 2016, 4:20 PM Alejandro Castro notifications@github.com wrote:

Thank you for the input @sherm1 https://github.com/sherm1. As far as I understand Bullet is capable of dealing with the more general case of non-convex shapes though with the additional cost in performance. To deal with this for the very commonly occurring case of terrains they have a Height Map Terrain http://bulletphysics.com/Bullet/BulletFull/classbtHeightfieldTerrainShape.html class that is supposed to be more efficient than the general non-convex shape. See the chart below on how to select your shape taken from Bullet's manual https://github.com/bulletphysics/bullet3/blob/master/docs/Bullet_User_Manual.pdf :

[image: image] https://cloud.githubusercontent.com/assets/17601461/13932729/f65cc5ea-ef7f-11e5-95bc-667d894f597e.png

Bullet seems to have a collision dispatcher that calls the proper (most efficient) algorithm between two shapes. However it seems like Drake is not using this dispatcher but it implements its own? I am still studying the code in this regard...

— You are receiving this because you were mentioned.

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

amcastro-tri commented 8 years ago

Thank you for this information @avalenzu, really helpful. I tried the potentialCollisions method today and that got rid of the segmentation fault. Also, I am being able to successfully display the height map in director. However my objects go through the height map as if it wasn't there. I am working to determine the cause of this problem.

@avalenzu, is there any material you would recommend for me to read regarding this methodology with a fixed number of differentiable constraints? My background is on simulation tools though I am not a roboticist and I would like to learn more about these tools.

amcastro-tri commented 8 years ago

I created a simple test consisting of a sphere bouncing on a height field. The height field for this test is a sine function in x. However my sphere still goes through the height field and it seems to bounce as it reaches z=0. See this video.

amcastro-tri commented 8 years ago

I think I finally got this right, at least in what concerns to the height map implementation. I think there might be a problem with how the elastic forces between objects is applied once a collision is detected, see videos bellow.

Essentially, after fighting with my repository for the whole day, I fixed the following issues:

Drake side was using float's and Bullet side was using double's (biggest bug here)
Updated the URDF file defining the ball gemetry. I think the problem here is that <visual> and <collision> elements are positioned by <origin> with respect to their parent joint. Therefore in this case, with a base QUATERNION, the sphere is ALWAYS at xyz=[0,0,0]. If I am correct then <origin> only moves the visual and collision elements with respect to this origin (which in this case I do not want). Is this correct @liangfok ?
Fixed a bug in the initial conditions. I think this problem appeared before in the Cars example and I think @liangfok fixed it there. I had already started from an old example with the bug.
Reimplemented clone method of the HeightMap class. Not really a bug, but cleaned up the code.

Now, for testing, I created an example of a ball bouncing against a height map that has the shape of a sine wave. Damping and friction are zero. I am using the latest RK2 (second order) from the repository. As seen in the video here the ball successfully bounces on the height map and energy seems to be preserved.

In this next example the only change I make is to move the height map to the left. Now the problem is not symmetric anymore and the ball should bounce off in an angle. However the ball gains angular momentum (nonphysical in this case with zero friction) and gains considerable linear momentum. Notice also that the ball sinks quite a bit into the height map.

I think the problem might be on how forces are applied after a collision is detected. To be honest I still don't understand fully how this is done. However, the following piece of code caught my attention. If I understand correctly R should be a rotation matrix. However tangent2 doesn't seem to generate a right handed coordinate system. I think tangent2 should be pointing in the other direction and then tangent2 = this_normal.cross(tangent1). Is this reasoning correct @RussTedrake?

Another question I have, the position vectors xA and xB computed here seem to be in the local system of the corresponding rigid body?

I think the height map implementation is ready now. I'll clean up the code and submit a pull request tomorrow. I'll work on the contact forces as a separate issue.

liangfok commented 8 years ago

Great work! Yes, in URDF, the joints and links form a tree structure and the <origin> of a link's <visual> and <collision> elements are defined relative to its parent joint. Here two illustrations obtained from the URDF joint webpage and the URDF link webpage:

joint

inertial

In your case, you have a sphere that's connected to the world via a 6-DOF floating joint. According to the URDF link specification, the origin of the sphere's geometry is at its center. Thus, it makes sense for the <origin> of both the <visual> and <collision> elements to be xyz = "0 0 0" and rpy = "0 0 0". However, I don't think it has to be the case. I believe non-zero origins should be OK as long as all three elements (<visual>, <collision>, and <inertial>) are consistent.

liangfok commented 8 years ago

Fixed a bug in the initial conditions. I think this problem appeared before in the Cars example and I think @liangfok fixed it there. I had already started from an old example with the bug.

Can you point me to the relevant commit or lines of code? Just want to make sure I made the same change in the car sim.

amcastro-tri commented 8 years ago

Sorry @liang, I was in lazy mode last night. I found the commit with Blame (great tool!). Here is the fix to Cars/simulateLCM.cpp but it wasn't you but @psiorx.

liangfok commented 8 years ago

No problem! Thanks for the info!

avalenzu commented 8 years ago

"Drake side was using float's and Bullet side was using double's (biggest bug here)"

What in Drake was using floats?

On Thu, Mar 24, 2016, 8:08 AM Chien-Liang Fok notifications@github.com wrote:

No problem! Thanks for the info!

— You are receiving this because you were mentioned.

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

amcastro-tri commented 8 years ago

Sorry @avalenzu. I misspoke, it was the other way around. Drake was using doubles while I was using floats on Bullet's side.

avalenzu commented 8 years ago

Yeah, I spent quite a lot of time fighting with that too. That's why the version of Bullet in that Drake pulls in by default is configured to use doubles. Just wanted to make sure we weren't fighting the same battle in the other direction now :)

On Mon, Mar 28, 2016 at 8:13 PM Alejandro Castro notifications@github.com wrote:

Sorry @avalenzu https://github.com/avalenzu. I misspoke, it was the other way around. Drake was using doubles while I was using floats on Bullet's side.

— You are receiving this because you were mentioned.

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

liangfok commented 8 years ago

@amcastro-tri has temporarily placed this on the back burner while he focuses on System 2.0 development.

amcastro-tri commented 8 years ago

I actually have the self-driving Prius driving on a highway demo in a System 1.0 shape. It is not clean to push yet and most importantly I'd need to have a "better" version of a highway for this example. In any case, I was hoping to have enough capability to land in System 2.0 (which it's very close to land) and then just PR the System 2.0 version of that demo (which I already have in a branch in my fork).

What already IS in master: the capability to work with static terrains discretized by triangle meshes. There is an example unit test in model_test.cc in ModelTest::StaticMeshes.

liangfok commented 7 years ago

Is height map a type of Digital Elevation Model (DEM)?

amcastro-tri commented 7 years ago

Not, at the time is was an obj file.

liangfok commented 7 years ago

Thanks. Perhaps a DEM could be used to create a height map?

RussTedrake commented 6 years ago

closing this as "won't fix"

RobotLocomotion / drake

drive on heightmap terrain #1616

Some Progress