Computational Embodied Neuroscience Simulator  1.1
3D simulation library
 All Classes Namespaces Files Functions Variables Typedefs Enumerations Enumerator Friends
cens_serialized_engine.cpp
Go to the documentation of this file.
1 // Computational Embodied Neuroscience Simulator (CENS) Library
2 // Copyright (c) 2013 Francesco Mannella
3 //
4 // cens_engine.cpp
5 // Copyright (c) 2013 Francesco Mannella
6 //
7 // This file is part of CENS library.
8 //
9 // CENS library is free software: you can redistribute it and/or modify
10 // it under the terms of the GNU General Public License as published by
11 // the Free Software Foundation, either version 3 of the License, or
12 // (at your option) any later version.
13 //
14 // CENS library is distributed in the hope that it will be useful,
15 // but WITHOUT ANY WARRANTY; without even the implied warranty of
16 // MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
17 // GNU General Public License for more details.
18 //
19 // You should have received a copy of the GNU General Public License
20 // along with CENS library. If not, see <http://www.gnu.org/licenses/>.
21 
22 #include "cens_utils.h"
23 #include "cens_serialized_engine.h"
24 
25 namespace cens
26 {
27 
32 
33 
35  std::string hingeName, CENSHingeConstraint *_hinge )
36  {
37  // initialize the data struct
38  hinges[hingeName] = CENSHingeData();
39  // link the pointer of the hinge
40  hinges[hingeName].hinge = _hinge;
41 
42  // set Parameters to be read and written from/to
43  // the file
44  addParameter(std::string(hingeName)
45  .append("_ENABLED"),
46  hinges[hingeName].enabled );
47  addParameter(std::string(hingeName)
48  .append("_MOTOR_TYPE"),
49  hinges[hingeName].motor_type );
50  addParameter(std::string(hingeName)
51  .append("_KP"),
52  hinges[hingeName].kp );
53  addParameter(std::string(hingeName)
54  .append("_KI"),
55  hinges[hingeName].ki );
56  addParameter(std::string(hingeName)
57  .append("_KD"),
58  hinges[hingeName].kd );
59  addParameter(std::string(hingeName)
60  .append("_MAX_MOTOR_IMPULSE"),
61  hinges[hingeName].max_motor_impulse );
62  addParameter(std::string(hingeName)
63  .append("_MOTOR_DT"),
64  hinges[hingeName].motor_dt );
65  addParameter(std::string(hingeName)
66  .append("_CURRENT_ANGLE"),
67  hinges[hingeName].current_angle );
68  addParameter(std::string(hingeName)
69  .append("_LOWER_LIMIT"),
70  hinges[hingeName].lower_limit );
71  addParameter(std::string(hingeName)
72  .append("_UPPER_LIMIT"),
73  hinges[hingeName].upper_limit );
74  addParameter(std::string(hingeName)
75  .append("_ENABLE_COLLISION"),
76  hinges[hingeName].enable_collision );
77  }
78 
80 
81  void CENSSerializedRobot::addGenericConstraint( std::string constraintName,
82  btTypedConstraint *_constraint )
83  {
84  constraints[constraintName] = _constraint;
85  }
86 
88 
89  void CENSSerializedRobot::addBody( std::string bodyName, btRigidBody *_body )
90  {
91  bodies[bodyName] = _body;
92  }
93 
95 
97  std::string transformName, btTransform _transform )
98  {
99  transforms[transformName] = _transform;
100  }
101 
103 
105  {
106  bool ret = loadParameters();
107  for(
108  Hinges::iterator it = hinges.begin();
109  it!=hinges.end();
110  ++it )
111  {
112  CENSHingeData &hdata = it->second;
113  hdata.hinge->setLimit(hdata.lower_limit, hdata.upper_limit);
114  hdata.hinge->setMaxMotorImpulse(hdata.max_motor_impulse);
115  hdata.hinge->enableMotor(hdata.enabled=="TRUE");
116  if (hdata.enable_collision=="FALSE")
117  {
118  CENSNonCollidingTable table;
119  table[std::make_pair(
120  &(hdata.hinge->getRigidBodyA()),
121  &(hdata.hinge->getRigidBodyB()))] = false;
122  engine->setCollisionFilter(table);
123  }
124  else
125  {
126  CENSNonCollidingTable table;
127  table[std::make_pair(
128  &(hdata.hinge->getRigidBodyA()),
129  &(hdata.hinge->getRigidBodyB()))] = true;
130  engine->setCollisionFilter(table);
131  }
132 
133  }
134 
135  saveParameters();
136  return ret;
137  }
138 
140 
142  {
143  for(Bodies::iterator it = bodies.begin(); it!= bodies.end(); ++it)
144  {
145  btRigidBody *body = it->second;
146  std::string name = it->first;
147  body->setActivationState(DISABLE_SIMULATION);
148  body->setCenterOfMassTransform(transforms[name]);
149  body->setLinearVelocity(btVector3(0,0,0));
150  body->setAngularVelocity(btVector3(0,0,0));
151  body->setGravity(btVector3(0,0,0));
152  }
153  for(Hinges::iterator it = hinges.begin(); it!= hinges.end(); ++it)
154  {
155  std::string hname = it->first;
156  move(hname,0,0);
157  }
158  }
159 
161 
163  {
164  for(Bodies::iterator it = bodies.begin(); it!= bodies.end(); ++it)
165  {
166  btRigidBody *body = it->second;
167  std::string name = it->first;
168  body->forceActivationState(DISABLE_DEACTIVATION);
169  body->setGravity(eigen2btVec(engine->getGravity()));
170  }
171  }
172 
174 
176  {
177 
178  for(
179  Hinges::iterator it = hinges.begin();
180  it!=hinges.end();
181  ++it )
182  {
183  CENSHingeData &hdata = it->second;
184 
185  btDynamicsWorld *world = engine->getDynamicsWorld();
186  bool is_in_world = false;
187  for(int x =0; x<world->getNumConstraints();x++)
188  if(world->getConstraint(x) == hdata.hinge)
189  {
190  is_in_world = true;
191  break;
192  }
193 
194  if(is_in_world)
195  {
196  hdata.hinge->setLimit(hdata.lower_limit, hdata.upper_limit );
197 
198 
199  if( hdata.enabled=="TRUE")
200  {
201  hdata.hinge->setMaxMotorImpulse(hdata.max_motor_impulse);
202  hdata.hinge->enableMotor(hdata.enabled=="TRUE");
203  if(hdata.motor_type == CENS_DEFAULT_CONTROL)
204  hdata.hinge->setMotorTarget(
205  hdata.current_angle,
206  hdata.motor_dt);
207  else if(hdata.motor_type == CENS_PD_CONTROL)
208  hdata.hinge->setPDMotorTarget(
209  hdata.current_angle,
210  hdata.kp,
211  hdata.ki,
212  hdata.dyn_kd,
213  hdata.motor_dt);
214  }
215  }
216  }
217  }
218 
220 
222  const std::string hingeName, double target_angle,
223  double impulse)
224  {
225  if (impulse != 0)
226  {
227  if(hinges[hingeName].motor_type == CENS_DEFAULT_CONTROL)
228  {
229  hinges[hingeName].max_motor_impulse = impulse;
230  }
231  else if (hinges[hingeName].motor_type == CENS_PD_CONTROL)
232  {
233  hinges[hingeName].max_motor_impulse = impulse;
234  hinges[hingeName].dyn_kd = hinges[hingeName].kd/impulse;
235  }
236  }
237  hinges[hingeName].current_angle = target_angle;
238  }
239 
244 
245 void CENSSerializedEngine::step(int timestep)
246 {
247  // update hinge positions of all hinge groups on every step
248  for(Robots::iterator it = m_eRobots.begin(); it!=m_eRobots.end(); ++it)
249  {
250  it->second->update();
251  }
252  CENSEngine::step(timestep);
253 }
254 
256 
258  const std::string &robotName,
259  const std::string &file)
260  {
261 
262  // build the hinge group for this bullet file
263  CENSSerializedRobot *robot = new CENSSerializedRobot(robotName, this);
264 
265  // build the file importer
266  CENSImporter *fileLoader =
268  fileLoader->loadFile(file.c_str());
269 
270  // get the map of bodies to theri names
272  bhm &bm = (bhm &)(fileLoader->getBodyMap());
273 
274  // link bodies to CENS cicle
275  for(int index=0; index<bm.size(); index++ )
276  {
277  btRigidBody *body = *(bm.getAtIndex(index));
278  localImportRigidBody(body);
279  m_eBodies[ bm.getKeyAtIndex(index).m_string ] = body;
280  robot->addBody( bm.getKeyAtIndex(index).m_string, body );
281  robot->addTransform( bm.getKeyAtIndex(index).m_string,
282  body->getCenterOfMassTransform() );
283  }
284 
285  // link constraints to CENS cicle
286  for(int index=0; index<fileLoader->getNumConstraints(); index++ )
287  {
288  // get a constraint pointer
289  btTypedConstraint *constraint = fileLoader->getConstraintByIndex(index);
290 
291  // Create the constraint name from the names of the bodies
292  std::stringstream constr_name_stream;
293  constr_name_stream <<
294  fileLoader->getNameForPointer(&(constraint->getRigidBodyA()))
295  << "_to_" <<
296  fileLoader->getNameForPointer(&(constraint->getRigidBodyB()));
297 
298  // if hinge serialize
299  if(constraint->getConstraintType() == HINGE_CONSTRAINT_TYPE )
300  {
301  // migrade from a btHingeconstraint to a CENSHingeConstraint
302  btHingeConstraint *hinge = dynamic_cast< btHingeConstraint *>(constraint);
303  CENSHingeConstraint *cens_constraint = new CENSHingeConstraint(
304  hinge->getRigidBodyA(),
305  hinge->getRigidBodyB(),
306  hinge->getFrameOffsetA(),
307  hinge->getFrameOffsetB() );
308  hinge->getRigidBodyA().addConstraintRef(cens_constraint);
309  hinge->getRigidBodyB().addConstraintRef(cens_constraint);
310  // substitute the constraint in the dynamics
311  m_phDynamicsWorld->removeConstraint(constraint);
312  m_phDynamicsWorld->addConstraint(cens_constraint);
313  delete constraint;
314  constraint = cens_constraint;
315 
316  // add the constraint to the serialization group
317  robot->addHinge(constr_name_stream.str(), cens_constraint);
318  }
319  else
320  {
321  robot->addGenericConstraint(constr_name_stream.str(), constraint);
322  }
323 
324  // add the constraint to the CENS constraints' map
325  m_eConstraints[constr_name_stream.str()] = constraint;
326  }
327 
328  // delete data in the importer
329  delete fileLoader;
330 
331  // reset gravity as from CENS parameters
332  m_phDynamicsWorld->setGravity(
333  btVector3(
334  m_phGravity.x(),
335  m_phGravity.y(),
336  m_phGravity.z()) );
337  m_phSoftBodyWorldInfo.m_gravity.setValue(
338  m_phGravity.x(),
339  m_phGravity.y(),
340  m_phGravity.z() );
341 
342  // initialize and return the serialized hinge group
343  robot->init();
344  return robot;
345  }
346 
347 }
A custom HingeConstraint Class.
Definition: cens_physics.h:374
btSoftBodyWorldInfo m_phSoftBodyWorldInfo
Definition: cens_physics.h:336
Computational Embodied Neuroscience Simulator library.
Definition: cens_engine.cpp:29
btSoftRigidDynamicsWorld * m_phDynamicsWorld
Definition: cens_physics.h:322
virtual btDynamicsWorld * getDynamicsWorld()
Definition: cens_physics.h:232
btHashMap< btHashString, btRigidBody * > & getBodyMap()
const std::string CENS_DEFAULT_CONTROL
Definition: cens_types.cpp:32
void addParameter(std::string parname, std::string &par)
btRigidBody * localImportRigidBody(btRigidBody *body, const btVector3 &color=eigen2btVec(CENS_NULL_COLOR), const TexCoords &texCoords=CENS_NULL_TEXCOORDS, CENSPixelMap &pixmap=CENS_NULL_PIXMAP)
void addBody(std::string bodyName, btRigidBody *_body)
Vector3f m_phGravity
Definition: cens_physics.h:342
virtual void step(int timestep)
Vector3f getGravity()
Definition: cens_physics.h:291
CENSHingeConstraint * hinge
void addHinge(std::string hingeName, CENSHingeConstraint *_hinge)
void addTransform(std::string transformName, btTransform _transform)
const std::string CENS_PD_CONTROL
Definition: cens_types.cpp:33
btVector3 eigen2btVec(const Eigen::Vector3f &v)
virtual void step(int value)
Definition: cens_engine.cpp:63
virtual CENSSerializedRobot * loadBulletFile(const std::string &robotName, const std::string &file)
void addGenericConstraint(std::string constraintName, btTypedConstraint *_constraint)
void move(const std::string hingeName, double target_angle, double impulse=0)
void setPDMotorTarget(float targetAngle, float kp=3, float ki=0, float kd=.5, float dt=.05)
virtual void setCollisionFilter(CENSNonCollidingTable _non_colliding_table)
std::map< CENSNCPair, bool > CENSNonCollidingTable
Definition: cens_physics.h:77