ODE_World.cpp
Go to the documentation of this file.
1 /*
2  * Copyright (c) 2008, AIST, the University of Tokyo and General Robotix Inc.
3  * All rights reserved. This program is made available under the terms of the
4  * Eclipse Public License v1.0 which accompanies this distribution, and is
5  * available at http://www.eclipse.org/legal/epl-v10.html
6  * Contributors:
7  * National Institute of Advanced Industrial Science and Technology (AIST)
8  */
9 
10 #include "ODE_World.h"
11 #include "ODE_ModelLoaderUtil.h"
12 
13 static const dReal DEFAULT_GRAVITY_ACCELERATION = -9.80665;
14 
16 {
17  dInitODE();
18  worldId = dWorldCreate();
19  spaceId = dHashSpaceCreate(0);
20  contactgroupId = dJointGroupCreate(0);
21 
22  dWorldSetCFM(worldId, CFM);
23  dWorldSetERP(worldId, ERP);
24  dWorldSetContactMaxCorrectingVel(worldId, CONTACT_MAX_CORRECTING_VEL);
25  dWorldSetContactSurfaceLayer(worldId, CONTACT_SURFACE_LAYER);
26  if(USE_QUICKSTEP)
27  dWorldSetQuickStepNumIterations(worldId, QUICKSTEP_NUM_ITERATIONS);
28 
29  dWorldSetGravity(worldId, 0, 0, DEFAULT_GRAVITY_ACCELERATION);
30 }
31 
33 {
34  for(int i=0; i<numBodies(); i++){
35  hrp::BodyPtr b = body(i);
36  ODE_Link* link = (ODE_Link*)(b->rootLink());
37  link->destroy();
38  }
39  dJointGroupDestroy(contactgroupId);
40  dSpaceDestroy(spaceId);
41  dWorldDestroy(worldId);
42  dCloseODE();
43 }
44 
45 void ODE_World::setGravityAcceleration(const dVector3& gravity)
46 {
47  dWorldSetGravity(worldId, gravity[0], gravity[1], gravity[2]);
48 }
49 
50 void ODE_World::getGravityAcceleration(dVector3& gravity)
51 {
52  dWorldGetGravity(worldId, gravity);
53 }
54 
55 void ODE_World::addBody(OpenHRP::BodyInfo_ptr bodyInfo, const char *name)
56 {
58  ODE_loadBodyFromBodyInfo(body, this, bodyInfo);
59  body->setName(name);
60 
62 }
63 
64 void ODE_World::addCollisionPair(OpenHRP::LinkPair& linkPair){
65  const char* bodyName[2];
66  bodyName[0] = linkPair.charName1;
67  bodyName[1] = linkPair.charName2;
68 
69  const char* linkName[2];
70  linkName[0] = linkPair.linkName1;
71  linkName[1] = linkPair.linkName2;
72 
73  hrp::BodyPtr body1 = this->body(bodyName[0]);
74  if(body1 == NULL){
75  std::cout << "ODE_World::addCollisionPair : Body ";
76  std::cout << bodyName[0] << " is not found." << std::endl;
77  return;
78  }
79  hrp::BodyPtr body2 = this->body(bodyName[1]);
80  if(body2 == NULL){
81  std::cout << "ODE_World::addCollisionPair : Body ";
82  std::cout << bodyName[1] << " is not found." << std::endl;
83  return;
84  }
85  ODE_Link* link1 = (ODE_Link*)body1->link(linkName[0]);
86  if(link1 == NULL){
87  std::cout << "ODE_World::addCollisionPair : Link ";
88  std::cout << linkName[0] << " is not found." << std::endl;
89  return;
90  }
91  ODE_Link* link2 = (ODE_Link*)body2->link(linkName[1]);
92  if(link2 == NULL){
93  std::cout << "ODE_World::addCollisionPair : Link ";
94  std::cout << linkName[1] << " is not found." << std::endl;
95  return;
96  }
97 
98  LinkPair _linkPair;
99  _linkPair.bodyId1 = link1->bodyId;
100  _linkPair.bodyId2 = link2->bodyId;
101  linkPairs.push_back(_linkPair);
102 }
103 
104 void ODE_World::calcNextState(OpenHRP::CollisionSequence& corbaCollisionSequence){
106  int n = linkPairs.size();
107  collisions.length(n);
108  for(int i=0; i<n; i++)
109  collisions[i].points.length(0);
110  dSpaceCollide(spaceId, (void *)this, &ODE_collideCallback);
111  corbaCollisionSequence = collisions;
112  }else{
113  for(int i=0; i<corbaCollisionSequence.length(); i++){
114  OpenHRP::Collision& _collision = corbaCollisionSequence[i];
115  std::string charName1 = (std::string)_collision.pair.charName1;
116  std::string linkName1 = (std::string)_collision.pair.linkName1;
117  ODE_Link* link1 = (ODE_Link*)body(charName1)->link(linkName1);
118  std::string charName2 = (std::string)_collision.pair.charName2;
119  std::string linkName2 = (std::string)_collision.pair.linkName2;
120  ODE_Link* link2 = (ODE_Link*)body(charName2)->link(linkName2);
121 
122  OpenHRP::CollisionPointSequence& points = _collision.points;
123  int n = points.length();
124  for(int j=0; j<n; j++){
125  dContact contact;
126  contact.geom.pos[0] = points[j].position[0];
127  contact.geom.pos[1] = points[j].position[1];
128  contact.geom.pos[2] = points[j].position[2];
129  contact.geom.normal[0] = -points[j].normal[0];
130  contact.geom.normal[1] = -points[j].normal[1];
131  contact.geom.normal[2] = -points[j].normal[2];
132  contact.geom.depth = points[j].idepth;
133  //TODO
134 
135  // contact.geom.g1 = link1->geomId;
136  // contact.geom.g2 = link2->geomId;
137  //std::cout << "out pos " << contact.geom.pos[0] << " " << contact.geom.pos[1] << " " <<contact.geom.pos[2] << std::endl;
138  //std::cout << "out normal " << contact.geom.normal[0] << " " << contact.geom.normal[1] << " " <<contact.geom.normal[2] << std::endl;
139  //std::cout << "out depth " << contact.geom.depth << std::endl;
140 
141  contact.surface.mode = SURFACE_MODE;
142  contact.surface.mu = SURFACE_MU;
143  contact.surface.mu2 = SURFACE_MU2;
144  contact.surface.bounce = SURFACE_BOUNCE;
145  contact.surface.bounce_vel = SURFACE_BOUNCE_VEL;
146  contact.surface.soft_erp = SURFACE_SOFT_ERP;
147  contact.surface.soft_cfm = SURFACE_SOFT_CFM;
148  contact.surface.motion1 = SURFACE_MOTION1;
149  contact.surface.motion2 = SURFACE_MOTION2;
150  contact.surface.slip1 = SURFACE_SLIP1;
151  contact.surface.slip2 = SURFACE_SLIP2;
152  contact.fdir1[0] = CONTACT_FDIR1_X;
153  contact.fdir1[1] = CONTACT_FDIR1_Y;
154  contact.fdir1[2] = CONTACT_FDIR1_Z;
155 
156  dJointID c = dJointCreateContact(worldId, contactgroupId, &contact);
157  dJointAttach(c, link1->bodyId, link2->bodyId);
158  }
159  //std::cout << std::endl;
160  }
161  }
162 
163  if(USE_QUICKSTEP)
164  dWorldQuickStep(worldId, timeStep_);
165  else
166  dWorldStep(worldId, timeStep_);
167 
168  updateSensors();
169 
171 }
172 
174  for(int i=0; i<numBodies(); i++){
175  hrp::BodyPtr b = body(i);
176  for(int j=0; j<b->numLinks(); j++){
177  ODE_Link* link = (ODE_Link*)b->link(j);
179  dJointSetFixed(link->odeJointId);
180  }
181 
182  // for sensor
184  info.forwardDynamics.reset(new ODE_ForwardDynamics(b));
185  info.forwardDynamics->setTimeStep(timeStep_);
186  info.forwardDynamics->enableSensors(sensorsAreEnabled);
187  info.forwardDynamics->initialize();
188 
189  }
190 
191 }
192 
194  dJointGroupEmpty(contactgroupId);
195 }
196 
198  for(int i=0; i<numBodies(); i++){
200  ODE_ForwardDynamicsPtr forwardDynamics = boost::dynamic_pointer_cast<ODE_ForwardDynamics>(info.forwardDynamics);
201  forwardDynamics->updateSensors();
202  }
203 }
204 
205 void ODE_collideCallback(void *data, dGeomID o1, dGeomID o2){
206 
207  dContact contact[COLLISION_MAX_POINT];
208  ODE_World* world = (ODE_World*)data;
209  OpenHRP::CollisionSequence& collisions = world->collisions;
210 
211  ODE_World::LinkPairArray& linkPairs = world->linkPairs;
212  int collisionIndex = -1;
213  dBodyID body1 = dGeomGetBody(o1);
214  dBodyID body2 = dGeomGetBody(o2);
215 
216  for(int i=0; i<linkPairs.size(); i++){
217  if( (linkPairs[i].bodyId1 == body1 && linkPairs[i].bodyId2 == body2) ||
218  (linkPairs[i].bodyId1 == body2 && linkPairs[i].bodyId2 == body1) ){
219  collisionIndex = i;
220  break;
221  }
222  }
223  if(collisionIndex == -1)
224  return;
225 
226  int n= dCollide(o1, o2, COLLISION_MAX_POINT, &contact[0].geom, sizeof(dContact));
227  collisions[collisionIndex].points.length(n);
228  for(int i=0; i<n; i++){
229  collisions[collisionIndex].points[i].position[0] = contact[i].geom.pos[0];
230  collisions[collisionIndex].points[i].position[1] = contact[i].geom.pos[1];
231  collisions[collisionIndex].points[i].position[2] = contact[i].geom.pos[2];
232  collisions[collisionIndex].points[i].normal[0] = contact[i].geom.normal[0];
233  collisions[collisionIndex].points[i].normal[1] = contact[i].geom.normal[1];
234  collisions[collisionIndex].points[i].normal[2] = contact[i].geom.normal[2];
235  collisions[collisionIndex].points[i].idepth = contact[i].geom.depth;
236 
237  contact[i].surface.mode = SURFACE_MODE;
238  contact[i].surface.mu = SURFACE_MU;
239  contact[i].surface.mu2 = SURFACE_MU2;
240  contact[i].surface.bounce = SURFACE_BOUNCE;
241  contact[i].surface.bounce_vel = SURFACE_BOUNCE_VEL;
242  contact[i].surface.soft_erp = SURFACE_SOFT_ERP;
243  contact[i].surface.soft_cfm = SURFACE_SOFT_CFM;
244  contact[i].surface.motion1 = SURFACE_MOTION1;
245  contact[i].surface.motion2 = SURFACE_MOTION2;
246  contact[i].surface.slip1 = SURFACE_SLIP1;
247  contact[i].surface.slip2 = SURFACE_SLIP2;
248  contact[i].fdir1[0] = CONTACT_FDIR1_X;
249  contact[i].fdir1[1] = CONTACT_FDIR1_Y;
250  contact[i].fdir1[2] = CONTACT_FDIR1_Z;
251 
252  dJointID c = dJointCreateContact(world->getWorldID(), world->getJointGroupID(), &contact[i]);
253  dJointAttach(c, dGeomGetBody(contact[i].geom.g1), dGeomGetBody(contact[i].geom.g2));
254  }
255 
256 }
257 
259  ForwardDynamics(body)
260 {
261 }
262 
264 }
265 
268 }
269 
270 #ifndef M_2PI
271 #define M_2PI 6.28318530717958647692
272 #endif
274  for(int i=0; i<body->numLinks(); i++){
275  ODE_Link* link = (ODE_Link*)body->link(i);
276  const dReal* _w = link->getAngularVel();
277  link->w << _w[0], _w[1], _w[2];
278  hrp::Matrix33 R;
279  link->getTransform(link->p, R);
280  link->setSegmentAttitude(R);
281  link->getLinearVel(link->v);
282 
283  link->dq = link->getVelocity();
284  link->q += link->dq * timeStep;
285  dReal q =link->getAngle();
286 
287  if(link->jointType == ODE_Link::ROTATIONAL_JOINT && fabs(q-link->q) > M_PI ){
288  dReal oldq=link->q;
289  int k = link->q/M_2PI;
290  if( link->q>=0 )
291  if( q>=0 )
292  link->q = k * M_2PI + q;
293  else
294  link->q = (k+1) * M_2PI + q;
295  else
296  if( q<0 )
297  link->q = k * M_2PI + q;
298  else
299  link->q = (k-1) * M_2PI + q;
300  }else
301  link->q = q;
302  }
304 
305  int n = body->numSensors(hrp::Sensor::FORCE);
306  for(int i=0; i < n; ++i){
308  }
309 }
310 
312  ODE_Link* link = (ODE_Link*)sensor->link;
313  dJointFeedback* fb = dJointGetFeedback(link->odeJointId);
314  hrp::Vector3 f(fb->f2[0], fb->f2[1], fb->f2[2]);
315  hrp::Vector3 tau(fb->t2[0], fb->t2[1], fb->t2[2]);
316  hrp::Matrix33 sensorR(link->R * sensor->localR);
317  hrp::Vector3 fs(sensorR.transpose() * f);
318  //hrp::Vector3 sensorPos(link->p + link->R * sensor->localPos);
319  hrp::Vector3 sensorPos(link->R * (sensor->localPos - link->parent->c));
320  hrp::Vector3 ts(sensorR.transpose() * (tau - sensorPos.cross(f)));
321 
322  sensor->f = fs;
323  sensor->tau = ts;
324 }
hrp::ForwardDynamics::initializeSensors
virtual void initializeSensors()
Definition: ForwardDynamics.cpp:93
ODE_World::ODE_World
ODE_World()
Definition: ODE_World.cpp:15
hrp::ForwardDynamics::body
BodyPtr body
Definition: ForwardDynamics.h:71
ODE_World.h
ODE_World::setGravityAcceleration
void setGravityAcceleration(const dVector3 &gravity)
set gravity acceleration
Definition: ODE_World.cpp:45
hrp::Sensor::localR
Matrix33 localR
Definition: hrplib/hrpModel/Sensor.h:60
i
png_uint_32 i
Definition: png.h:2732
hrp::WorldBase::body
BodyPtr body(int index)
get body by index
Definition: hrplib/hrpModel/World.cpp:56
SURFACE_SOFT_CFM
static const dReal SURFACE_SOFT_CFM
Definition: ODE_World.h:59
ColladaUtil::dReal
double dReal
Definition: ColladaUtil.h:78
hrp::Body
Definition: Body.h:44
ODE_World::LinkPair
Definition: ODE_World.h:118
CFM
static const dReal CFM
Definition: ODE_World.h:30
hrp::WorldBase::sensorsAreEnabled
bool sensorsAreEnabled
Definition: hrplib/hrpModel/World.h:176
ODE_World::spaceId
dSpaceID spaceId
Definition: ODE_World.h:127
CONTACT_FDIR1_X
static const dReal CONTACT_FDIR1_X
Definition: ODE_World.h:64
M_PI
#define M_PI
Definition: hrplib/hrpModel/Sensor.cpp:14
autoplay.n
n
Definition: autoplay.py:12
ODE_World::linkPairs
LinkPairArray linkPairs
Definition: ODE_World.h:123
ODE_ForwardDynamicsPtr
boost::shared_ptr< ODE_ForwardDynamics > ODE_ForwardDynamicsPtr
Definition: ODE_World.h:148
SURFACE_MOTION2
static const dReal SURFACE_MOTION2
Definition: ODE_World.h:61
hrp::ForwardDynamics::updateSensorsFinal
virtual void updateSensorsFinal()
Definition: ForwardDynamics.cpp:103
ODE_ForwardDynamics::initialize
virtual void initialize()
Definition: ODE_World.cpp:266
hrp::WorldBase::currentTime_
double currentTime_
Definition: hrplib/hrpModel/World.h:167
ODE_World::updateSensors
void updateSensors()
Definition: ODE_World.cpp:197
swingTest.f
f
Definition: swingTest.py:6
hrp::ForceSensor::f
Vector3 f
Definition: hrplib/hrpModel/Sensor.h:74
ODE_ForceSensor
Definition: ODE_Link.h:50
hrp::ForceSensor
Definition: hrplib/hrpModel/Sensor.h:68
ODE_World::worldId
dWorldID worldId
Definition: ODE_World.h:126
QUICKSTEP_NUM_ITERATIONS
static const int QUICKSTEP_NUM_ITERATIONS
Definition: ODE_World.h:25
b
long b
Definition: jpegint.h:371
hrp::Sensor::link
Link * link
Definition: hrplib/hrpModel/Sensor.h:59
hrp::WorldBase::numBodies
unsigned int numBodies()
get the number of bodies in this world
Definition: hrplib/hrpModel/World.h:44
COLLISION_MAX_POINT
static const int COLLISION_MAX_POINT
Definition: ODE_World.h:36
SURFACE_MU2
static const dReal SURFACE_MU2
Definition: ODE_World.h:55
hrp::ForceSensor::tau
Vector3 tau
Definition: hrplib/hrpModel/Sensor.h:75
ODE_World::LinkPair::bodyId1
dBodyID bodyId1
Definition: ODE_World.h:119
USE_QUICKSTEP
static const bool USE_QUICKSTEP
Definition: ODE_World.h:24
hrp::Vector3
Eigen::Vector3d Vector3
Definition: EigenTypes.h:11
hrp::ForwardDynamics::timeStep
double timeStep
Definition: ForwardDynamics.h:73
CONTACT_SURFACE_LAYER
static const dReal CONTACT_SURFACE_LAYER
Definition: ODE_World.h:34
ODE_ForwardDynamics::updateForceSensor
void updateForceSensor(ODE_ForceSensor *sensor)
Definition: ODE_World.cpp:311
ODE_World::useInternalCollisionDetector_
bool useInternalCollisionDetector_
Definition: ODE_World.h:130
ODE_World::collisions
OpenHRP::CollisionSequence collisions
Definition: ODE_World.h:116
SURFACE_BOUNCE
static const dReal SURFACE_BOUNCE
Definition: ODE_World.h:56
hrp::Sensor::localPos
Vector3 localPos
Definition: hrplib/hrpModel/Sensor.h:61
ODE_World::clearExternalForces
void clearExternalForces()
Definition: ODE_World.cpp:193
ODE_World::contactgroupId
dJointGroupID contactgroupId
Definition: ODE_World.h:128
hrp::WorldBase::calcNextState
virtual void calcNextState()
compute forward dynamics and update current state
Definition: hrplib/hrpModel/World.cpp:135
ODE_World::getJointGroupID
dJointGroupID getJointGroupID()
Definition: ODE_World.h:114
ODE_loadBodyFromBodyInfo
bool ODE_loadBodyFromBodyInfo(BodyPtr body, ODE_World *world, OpenHRP::BodyInfo_ptr bodyInfo)
Definition: ODE_ModelLoaderUtil.cpp:404
ODE_ForwardDynamics
Definition: ODE_World.h:137
SURFACE_SOFT_ERP
static const dReal SURFACE_SOFT_ERP
Definition: ODE_World.h:58
hrp::WorldBase::bodyInfoArray
std::vector< BodyInfo > bodyInfoArray
Definition: hrplib/hrpModel/World.h:174
hrp::BodyPtr
boost::shared_ptr< Body > BodyPtr
Definition: Body.h:315
SURFACE_MODE
static const int SURFACE_MODE
Definition: ODE_World.h:38
name
png_infop png_charpp name
Definition: png.h:2379
hrp::WorldBase::addBody
int addBody(BodyPtr body)
add body to this world
Definition: hrplib/hrpModel/World.cpp:153
SURFACE_SLIP2
static const dReal SURFACE_SLIP2
Definition: ODE_World.h:63
hrp::WorldBase::timeStep_
double timeStep_
Definition: hrplib/hrpModel/World.h:168
ODE_ForwardDynamics::calcNextState
virtual void calcNextState()
Definition: ODE_World.cpp:263
data
JSAMPIMAGE data
Definition: jpeglib.h:945
SURFACE_SLIP1
static const dReal SURFACE_SLIP1
Definition: ODE_World.h:62
ODE_World::LinkPairArray
std::vector< LinkPair > LinkPairArray
Definition: ODE_World.h:122
hrp::Sensor::FORCE
@ FORCE
Definition: hrplib/hrpModel/Sensor.h:33
ODE_World::LinkPair::bodyId2
dBodyID bodyId2
Definition: ODE_World.h:120
CONTACT_FDIR1_Z
static const dReal CONTACT_FDIR1_Z
Definition: ODE_World.h:66
hrp::WorldBase::getGravityAcceleration
const Vector3 & getGravityAcceleration()
get gravity acceleration
Definition: hrplib/hrpModel/World.h:128
ERP
static const dReal ERP
Definition: ODE_World.h:32
ODE_World::initialize
void initialize()
initialize this world. This must be called after all bodies are registered.
Definition: ODE_World.cpp:173
autoplay.c
int c
Definition: autoplay.py:16
ODE_World::addBody
void addBody(OpenHRP::BodyInfo_ptr body, const char *name)
add body to this world
Definition: ODE_World.cpp:55
hrp::WorldBase::forwardDynamics
ForwardDynamicsPtr forwardDynamics(int index)
get forward dynamics computation method for body
Definition: hrplib/hrpModel/World.h:65
hrp::WorldBase::BodyInfo
Definition: hrplib/hrpModel/World.h:170
SURFACE_MOTION1
static const dReal SURFACE_MOTION1
Definition: ODE_World.h:60
CONTACT_MAX_CORRECTING_VEL
static const dReal CONTACT_MAX_CORRECTING_VEL
Definition: ODE_World.h:33
SURFACE_BOUNCE_VEL
static const dReal SURFACE_BOUNCE_VEL
Definition: ODE_World.h:57
ODE_ModelLoaderUtil.h
hrp::Matrix33
Eigen::Matrix3d Matrix33
Definition: EigenTypes.h:12
ODE_World::~ODE_World
~ODE_World()
Definition: ODE_World.cpp:32
M_2PI
#define M_2PI
Definition: ODE_World.cpp:271
ODE_World
Definition: ODE_World.h:68
ODE_World::addCollisionPair
void addCollisionPair(OpenHRP::LinkPair &linkPair)
Definition: ODE_World.cpp:64
CONTACT_FDIR1_Y
static const dReal CONTACT_FDIR1_Y
Definition: ODE_World.h:65
info
backing_store_ptr info
Definition: jmemsys.h:181
ODE_World::getWorldID
dWorldID getWorldID()
Definition: ODE_World.h:112
ODE_ForwardDynamics::updateSensors
void updateSensors()
Definition: ODE_World.cpp:273
SURFACE_MU
static const dReal SURFACE_MU
Definition: ODE_World.h:54
DEFAULT_GRAVITY_ACCELERATION
static const dReal DEFAULT_GRAVITY_ACCELERATION
Definition: ODE_World.cpp:13
ODE_ForwardDynamics::ODE_ForwardDynamics
ODE_ForwardDynamics(hrp::BodyPtr body)
Definition: ODE_World.cpp:258
ODE_collideCallback
void ODE_collideCallback(void *data, dGeomID o1, dGeomID o2)
Definition: ODE_World.cpp:205


openhrp3
Author(s): AIST, General Robotix Inc., Nakamura Lab of Dept. of Mechano Informatics at University of Tokyo
autogenerated on Wed Sep 7 2022 02:51:03