Closed DJGCrusader closed 9 years ago
Daniel,
You shouldn't be using the terrain objects for this. As you say, they are intended for 2.5 "terrains" only. You can simply be adding geometries to the RigidBodyManipulator / TSRBM which have collision geometries. You should take a look at the quadrotor folder for an example. Most of the code there tries to avoid collisions, but here is a quick version that will show you that the collision dynamics work just fine:
function testCollisions
tmp = addpathTemporary(fullfile(pwd,'..'));
r = Quadrotor();
r = r.setTerrain([]);
r = addTrees(r, 25);
% The important trees to create swerving path
r = addTree(r, [.8,.45,1.25], [.20;2.5], pi/4);
r = addTree(r, [.5,.35,1.65], [-.25;5], -pi/6);
r = addTree(r, [.55,.65,1.5], [.25;7.5], pi/4);
r = addTree(r, [.55,.85,1.6], [-1.35;8.5], pi/3.7);
r = addTree(r, [.85,.95,1.65], [-1.85;5.2], -pi/3.7);
r = addTree(r, [.75,.9,1.75], [2;4.4], -pi/5);
x0 = Point(getStateFrame(r)); % initial conditions: all-zeros
x0.base_y = -1.5;
x0.base_z = .8;
x0.base_ydot = 5;
u0 = double(nominalThrust(r));
v = constructVisualizer(r);%,struct('use_collision_geometry',true));
v.draw(0,double(x0));
utraj = setOutputFrame(ConstantTrajectory(u0),r.getInputFrame);
xtraj = simulate(cascade(utraj,TimeSteppingRigidBodyManipulator(r,.01)),[0 5],x0);
v.playback(xtraj);
Sadly, although the different coefficient of frictions are very easy to implement, we haven't actually done it yet. See the discussion / recommendations in issue #57 .
On Dec 15, 2014, at 3:32 AM, DJGCrusader notifications@github.com wrote:
Hi all,
I am a student in 6.832 and am working on planning for a quadruped climbing robot, climbing up a shaft for my final project.
I am trying to generate a vertical shaft to use as my environment for my quadruped climber robot, but the RigidBodyManipulator framework only supports a single terrain polygon. It'd be great to simply have a custom terrain object contain a list of RigidBodyBox objects for the terrain, but that can't work either. I already have the URDF with collisions all working (the urdf tag contact_coefficients, which contains friction coeffs and collision compliance/impedance, seems to do nothing, though) with a planar simulation and self-collisions, collisions with the RigidBodyFlatTerrain, etc.
I have code that creates a random cave environment that I've plotted below, which now just outputs a list of line segment lengths, endpoints, the centroids, and the angles, with which I can use the RigidBodyBox geometry to create terrain, but I'd first like to test my system with two simple straight parallel walls, 20m high, 1m thick, 4m apart. Pictured above is the current situation, with collisions, \only one wall. This was made by modifying the width, depth, and T in the constructRigidBodyGeometry function of the RigidBodyFlatTerrain object. It collides with the wall due to the geometry, and collides with the "ground" because I've left the getHeight() function is untouched.
The issue is, it seems, that the RigidBodyManipulator assumes only a single RigidBodyTerrain object, and the terrain objects only assume a single polygon. Digging deeper into RigidBodyGeometry objects, multiple polygons will only be combined because it keeps only the convex hull of the points rather than custom mazes and cavesetc. Am I on the right track in trying to make my own custom "options.terrain" within the Drake RigidBodyManipulator framework? Should I be making something other than terrain, like just including another dummy object? I'd like to randomly generate my vertical shaft rather than have a single URDF that represents the environment. The closest example simulation with objects I could find was Airplane2D, but it follows a completely different and much simpler model framework than the RigidBodyManipulator. Right now I'm leaning towards making a custom copy of RigidBodyBox and hacking it to have multiple floating polygons in the geometry. I'm using the latest rigid-body build of Drake on Ubuntu with mosek installed.
I've looked into the rt.getManipulator().setTerrain() and .addTerrainGeometries, but it all only seems to be written for only a single terrain object, and terrain objects seem to be only written for a single geometry object. I tried creating my own RigidBodySimpleShaftTerrain.m and RigidBodySimpleShaft geometry. The output of geometry.getPoints() is:
pts =
2 3 3 2 2 3 3 2 -3 -2 -2 -3 -3 -2 -2 -3 1 1 1 1 -1 -1 -1 -1 1 1 1 1 -1 -1 -1 -1 20 20 0 0 0 0 20 20 20 20 0 0 0 0 20 20 Which are the 3D corner points of both the left and the right box. And the resultant playback looks like:
Here it is peeking out through the left wall after dropping it with an initial leftward x velocity. If it were simulating correctly but drawing incorrectly, it would still be hidden. The right side definitely contacts the wall, I've verified it by dropping it with a rightward x velocity plotting the xy trajectory and noting a "bounce". Perhaps TSRBM.getTerrain.getRigidBodyContactGeometry() and the associated RigidBodyGeometry objects isn't the best way to go for anything but the ground.
I tried modifying the URDF to have two additional links which are the walls (since robot self-collisions are woking for me), no frills, but the URDF won't load.
Anywhere I can look next?
Also, I would really like to specify custom coefficients of friction between objects, but the urdf tag contact_coefficients, which contains friction coeffs and collision compliance/impedance, seems to do nothing in drake when it loads the urdf. Does the LCP solver called during TSRBM.update(t,x,u) support static and kinetic friction?
Thanks so much!
-Daniel J. Gonzalez dgonz@mit.edu
— Reply to this email directly or view it on GitHub.
Just to follow up on Russ's comment, the method of RigidBodyManipulator
that you'll want to use is addGeometryToBody
. The following code adds a one meter cube at the origin of the world (assuming a RigidBodyManipulator
named r
is present in the workspace):
edge_lengths_cube = [1; 1; 1]
xyz_cube = [0; 0; 0];
rpy_cube = [0; 0; 0];
cube = RigidBodyBox(edge_lengths_cube, xyz_cube, rpy_cube);
r = r.addGeometryToBody('world', cube);
r = r.compile();
Note the call to compile
in the last line - that's where the new geometry gets passed down into Bullet for collision detection.
Yes, what avalenzu showed is exactly what I need, thanks. Except I don't have that function in any of my manipulators classes!
A quick ctrlF through TSRBM, PRBM, RBM, and M turned up cold, the function doesn't exist, but the addContactShapeToBody doesn't show up with the "depreciated" error as shown here:https://github.com/RobotLocomotion/drake/search?utf8=%E2%9C%93&q=addGeometryToBody
I'll try and get the latest ones that are on github, but it seems that the rigidbody distro, which I cloned and compiled from source about a week ago, doesn't have those updates added. I was following the instructions to the letter for compiling drake distro rigidbody from source on ubuntu from drake.mit.edu, but it strangely doesn't contain the updated manipulator classes.
edit: just tried "dgonz@dgonz-W530:~/projects/6832/drake-distro/drake/systems/plants$ git checkout *Manipulator.m" which didn't change anything. I'm going to manually update my TSRBM and RBM.
edit2: updated just TSRBM and RBM, throws error that there's no function getCollisionGeometry for class RigidBodyFlatTerrain, which means the whole way geometry versus shape and contact geometry is implemented in my version, distro rigidbody, doesn't work for the the function addGeometryToBody().
Is there some git command that I can use to update everything that is necessary for the above function to work to the latest stable version rather than what is used in the rigidbody distro? (novice git user here, I've never had do this kind of thing before...)
This may also be why my version of drake can still never find pod snopt, even though it's been added in...
Thanks!
To update drake to the current head of RobotLocomotion/master:
$ cd ~/projects/6832/drake-distro/drake
$ git pull
This will make your checkout of drake be out of sync from the submodule in drake-distro, which is still pointing to a revision from early November, but it should give you access to all of the methods you need.
After "git stash"-ing my changes and running a "git pull origin master", that seems to have updated everything appropriately. However, now when I run I get a segfault -_-.
MATLAB crash file:/home/dgonz/matlab_crash_dump.4766-1:
------------------------------------------------------------------------
Segmentation violation detected at Mon Dec 15 17:05:43 2014
------------------------------------------------------------------------
Configuration:
Crash Decoding : Disabled
Current Visual : None
Default Encoding : UTF-8
GNU C Library : 2.15 stable
MATLAB Architecture: glnxa64
MATLAB Root : /usr/local/MATLAB/R2014b
MATLAB Version : 8.4.0.150421 (R2014b)
Operating System : Linux 3.8.0-44-generic #66~precise1-Ubuntu SMP Tue Jul 15 04:01:04 UTC 2014 x86_64
Processor ID : x86 Family 6 Model 58 Stepping 9, GenuineIntel
Software OpenGL : 0
Virtual Machine : Java 1.7.0_11-b21 with Oracle Corporation Java HotSpot(TM) 64-Bit Server VM mixed mode
Window System : No active display
Fault Count: 1
Abnormal termination:
Segmentation violation
Register State (from fault):
RAX = 0000000000000000 RBX = 00007f85e2741830
RCX = 00007f8604000048 RDX = 00007f86055b7600
RSP = 00007f86138e4ae0 RBP = 00007f86138e4dc0
RSI = 0000000000000000 RDI = 0000000000000000
R8 = 0000000000000001 R9 = 0000000000000001
R10 = 0000000000000000 R11 = 00007f8628942570
R12 = 00007f84c8c0d330 R13 = 00007f84c8cfb7f0
R14 = 0000000000000000 R15 = 00007f84c8cafe80
RIP = 00007f8629f06cf1 EFL = 0000000000010206
CS = 0033 FS = 0000 GS = 0000
Stack Trace (from fault):
[ 0] 0x00007f8629f06cf1 /usr/local/MATLAB/R2014b/bin/glnxa64/libmx.so+00761073 _ZN6matrix6detail10noninlined12mx_array_api9mxIsEmptyEPK11mxArray_tag+00000001
[ 1] 0x00007f84d20da862 /home/dgonz/projects/6832/drake-distro/drake/systems/plants/constructModelmex.mexa64+00010338 mexFunction+00003554
[ 2] 0x00007f861e9c9c0a /usr/local/MATLAB/R2014b/bin/glnxa64/libmex.so+00150538 mexRunMexFile+00000090
[ 3] 0x00007f861e9c65c4 /usr/local/MATLAB/R2014b/bin/glnxa64/libmex.so+00136644
[ 4] 0x00007f861e9c7414 /usr/local/MATLAB/R2014b/bin/glnxa64/libmex.so+00140308
...And a lot more but it's all irrelevant internal MATLAB files. The culprit seems to be the latest version of "constructModelmex.mexa64". Ends in saying that the error was detected when using an unofficial mex file.
This occurs when I create my object: rt = TimeSteppingRigidBodyManipulator('QuadClimber.urdf',TIMESTEP, options);
both with and without adding terrain to the options.
Not sure what to pry into now, perhaps use an older constructModelmex.mexa64?
Thank you so much for the help by the way, the final project is due Wednesday and I've had this issue for weeks! -Dan
Did you run make in the drake directory after pulling?
On Mon, Dec 15, 2014, 5:17 PM DJGCrusader notifications@github.com wrote:
After "git stash"-ing my changes and running a "git pull origin master", that seems to have updated everything appropriately. However, now when I run I get a segfault -_-.
MATLAB crash file:/home/dgonz/matlab_crash_dump.4766-1:
Segmentation violation detected at Mon Dec 15 17:05:43 2014
Configuration: Crash Decoding : Disabled Current Visual : None Default Encoding : UTF-8 GNU C Library : 2.15 stable MATLAB Architecture: glnxa64 MATLAB Root : /usr/local/MATLAB/R2014b MATLAB Version : 8.4.0.150421 (R2014b) Operating System : Linux 3.8.0-44-generic #66~precise1-Ubuntu SMP Tue Jul 15 04:01:04 UTC 2014 x86_64 Processor ID : x86 Family 6 Model 58 Stepping 9, GenuineIntel Software OpenGL : 0 Virtual Machine : Java 1.7.0_11-b21 with Oracle Corporation Java HotSpot(TM) 64-Bit Server VM mixed mode Window System : No active display
Fault Count: 1
Abnormal termination: Segmentation violation
Register State (from fault): RAX = 0000000000000000 RBX = 00007f85e2741830 RCX = 00007f8604000048 RDX = 00007f86055b7600 RSP = 00007f86138e4ae0 RBP = 00007f86138e4dc0 RSI = 0000000000000000 RDI = 0000000000000000
R8 = 0000000000000001 R9 = 0000000000000001 R10 = 0000000000000000 R11 = 00007f8628942570 R12 = 00007f84c8c0d330 R13 = 00007f84c8cfb7f0 R14 = 0000000000000000 R15 = 00007f84c8cafe80
RIP = 00007f8629f06cf1 EFL = 0000000000010206
CS = 0033 FS = 0000 GS = 0000
Stack Trace (from fault): [ 0] 0x00007f8629f06cf1 /usr/local/MATLAB/R2014b/bin/glnxa64/libmx.so+00761073 _ZN6matrix6detail10noninlined12mx_array_api9mxIsEmptyEPK11mxArray_tag+00000001 [ 1] 0x00007f84d20da862 /home/dgonz/projects/6832/drake-distro/drake/systems/plants/constructModelmex.mexa64+00010338 mexFunction+00003554 [ 2] 0x00007f861e9c9c0a /usr/local/MATLAB/R2014b/bin/glnxa64/libmex.so+00150538 mexRunMexFile+00000090 [ 3] 0x00007f861e9c65c4 /usr/local/MATLAB/R2014b/bin/glnxa64/libmex.so+00136644 [ 4] 0x00007f861e9c7414 /usr/local/MATLAB/R2014b/bin/glnxa64/libmex.so+00140308
...And a lot more but it's all irrelevant internal MATLAB files. The culprit seems to be the latest version of "constructModelmex.mexa64". Ends in saying that the error was detected when using an unofficial mex file.
This occurs when I create my object: rt = TimeSteppingRigidBodyManipulator('QuadClimber.urdf',TIMESTEP, options); both with and without adding terrain to the options.
Not sure what to pry into now, perhaps use an older constructModelmex.mexa64?
Thank you so much for the help by the way, the final project is due Wednesday and I've had this issue for weeks! -Dan
— Reply to this email directly or view it on GitHub https://github.com/RobotLocomotion/drake/issues/617#issuecomment-67076799 .
No I did not make it, that fixed it!
Here it is stopping itself from a fall by forcing its legs out and maintaining a centered body position with Hybrid Force/Position control!
Thanks so much again for the help! Thanks a million! -Daniel
Hi all,
I am a student in 6.832 and am working on planning for a quadruped climbing robot, climbing up a shaft for my final project.
I am trying to generate a vertical shaft to use as my environment for my quadruped climber robot, but the RigidBodyManipulator framework only supports a single terrain polygon. It'd be great to simply have a custom terrain object contain a list of RigidBodyBox objects for the terrain, but that can't work either. I already have the URDF with collisions all working (the urdf tag contact_coefficients, which contains friction coeffs and collision compliance/impedance, seems to do nothing, though) with a planar simulation and self-collisions, collisions with the RigidBodyFlatTerrain, etc.
I have code that creates a random cave environment that I've plotted below, which now just outputs a list of line segment lengths, endpoints, the centroids, and the angles, with which I can use the RigidBodyBox geometry to create terrain, but I'd first like to test my system with two simple straight parallel walls, 20m high, 1m thick, 4m apart. Pictured above is the current situation, with collisions, \only one wall. This was made by modifying the width, depth, and T in the constructRigidBodyGeometry function of the RigidBodyFlatTerrain object. It collides with the wall due to the geometry, and collides with the "ground" because I've left the getHeight() function is untouched.
The issue is, it seems, that the RigidBodyManipulator assumes only a single RigidBodyTerrain object, and the terrain objects only assume a single polygon. Digging deeper into RigidBodyGeometry objects, multiple polygons will only be combined because it keeps only the convex hull of the points rather than custom mazes and cavesetc. Am I on the right track in trying to make my own custom "options.terrain" within the Drake RigidBodyManipulator framework? Should I be making something other than terrain, like just including another dummy object? I'd like to randomly generate my vertical shaft rather than have a single URDF that represents the environment. The closest example simulation with objects I could find was Airplane2D, but it follows a completely different and much simpler model framework than the RigidBodyManipulator. Right now I'm leaning towards making a custom copy of RigidBodyBox and hacking it to have multiple floating polygons in the geometry. I'm using the latest rigid-body build of Drake on Ubuntu with mosek installed.
I've looked into the rt.getManipulator().setTerrain() and .addTerrainGeometries, but it all only seems to be written for only a single terrain object, and terrain objects seem to be only written for a single geometry object. I tried creating my own RigidBodySimpleShaftTerrain.m and RigidBodySimpleShaft geometry. The output of geometry.getPoints() is:
pts =
Which are the 3D corner points of both the left and the right box. And the resultant playback looks like:
Here it is peeking out through the left wall after dropping it with an initial leftward x velocity. If it were simulating correctly but drawing incorrectly, it would still be hidden. The right side definitely contacts the wall, I've verified it by dropping it with a rightward x velocity plotting the xy trajectory and noting a "bounce". Perhaps TSRBM.getTerrain.getRigidBodyContactGeometry() and the associated RigidBodyGeometry objects isn't the best way to go for anything but the ground.
I tried modifying the URDF to have two additional links which are the walls (since robot self-collisions are woking for me), no frills, but the URDF won't load.
Anywhere I can look next?
Also, I would really like to specify custom coefficients of friction between objects, but the urdf tag contact_coefficients, which contains friction coeffs and collision compliance/impedance, seems to do nothing in drake when it loads the urdf. Does the LCP solver called during TSRBM.update(t,x,u) support static and kinetic friction?
Thanks so much!
-Daniel J. Gonzalez dgonz@mit.edu