40 hinges[hingeName].hinge = _hinge;
46 hinges[hingeName].enabled );
48 .append(
"_MOTOR_TYPE"),
49 hinges[hingeName].motor_type );
60 .append(
"_MAX_MOTOR_IMPULSE"),
61 hinges[hingeName].max_motor_impulse );
64 hinges[hingeName].motor_dt );
66 .append(
"_CURRENT_ANGLE"),
67 hinges[hingeName].current_angle );
69 .append(
"_LOWER_LIMIT"),
70 hinges[hingeName].lower_limit );
72 .append(
"_UPPER_LIMIT"),
73 hinges[hingeName].upper_limit );
75 .append(
"_ENABLE_COLLISION"),
76 hinges[hingeName].enable_collision );
82 btTypedConstraint *_constraint )
97 std::string transformName, btTransform _transform )
108 Hinges::iterator it =
hinges.begin();
119 table[std::make_pair(
120 &(hdata.
hinge->getRigidBodyA()),
121 &(hdata.
hinge->getRigidBodyB()))] =
false;
127 table[std::make_pair(
128 &(hdata.
hinge->getRigidBodyA()),
129 &(hdata.
hinge->getRigidBodyB()))] =
true;
143 for(Bodies::iterator it =
bodies.begin(); it!=
bodies.end(); ++it)
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));
153 for(Hinges::iterator it =
hinges.begin(); it!=
hinges.end(); ++it)
155 std::string hname = it->first;
164 for(Bodies::iterator it =
bodies.begin(); it!=
bodies.end(); ++it)
166 btRigidBody *body = it->second;
167 std::string name = it->first;
168 body->forceActivationState(DISABLE_DEACTIVATION);
179 Hinges::iterator it =
hinges.begin();
186 bool is_in_world =
false;
187 for(
int x =0; x<world->getNumConstraints();x++)
188 if(world->getConstraint(x) == hdata.
hinge)
204 hdata.
hinge->setMotorTarget(
222 const std::string hingeName,
double target_angle,
229 hinges[hingeName].max_motor_impulse = impulse;
233 hinges[hingeName].max_motor_impulse = impulse;
234 hinges[hingeName].dyn_kd =
hinges[hingeName].kd/impulse;
237 hinges[hingeName].current_angle = target_angle;
250 it->second->update();
258 const std::string &robotName,
259 const std::string &file)
268 fileLoader->loadFile(file.c_str());
275 for(
int index=0; index<bm.size(); index++ )
277 btRigidBody *body = *(bm.getAtIndex(index));
279 m_eBodies[ bm.getKeyAtIndex(index).m_string ] = body;
280 robot->
addBody( bm.getKeyAtIndex(index).m_string, body );
282 body->getCenterOfMassTransform() );
286 for(
int index=0; index<fileLoader->getNumConstraints(); index++ )
289 btTypedConstraint *constraint = fileLoader->getConstraintByIndex(index);
292 std::stringstream constr_name_stream;
293 constr_name_stream <<
294 fileLoader->getNameForPointer(&(constraint->getRigidBodyA()))
296 fileLoader->getNameForPointer(&(constraint->getRigidBodyB()));
299 if(constraint->getConstraintType() == HINGE_CONSTRAINT_TYPE )
302 btHingeConstraint *hinge =
dynamic_cast< btHingeConstraint *
>(constraint);
304 hinge->getRigidBodyA(),
305 hinge->getRigidBodyB(),
306 hinge->getFrameOffsetA(),
307 hinge->getFrameOffsetB() );
308 hinge->getRigidBodyA().addConstraintRef(cens_constraint);
309 hinge->getRigidBodyB().addConstraintRef(cens_constraint);
314 constraint = cens_constraint;
317 robot->
addHinge(constr_name_stream.str(), cens_constraint);
A custom HingeConstraint Class.
btSoftBodyWorldInfo m_phSoftBodyWorldInfo
Computational Embodied Neuroscience Simulator library.
btSoftRigidDynamicsWorld * m_phDynamicsWorld
virtual btDynamicsWorld * getDynamicsWorld()
btHashMap< btHashString, btRigidBody * > & getBodyMap()
const std::string CENS_DEFAULT_CONTROL
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)
GenericConstraints constraints
virtual void step(int timestep)
CENSHingeConstraint * hinge
std::string enable_collision
void addHinge(std::string hingeName, CENSHingeConstraint *_hinge)
void addTransform(std::string transformName, btTransform _transform)
const std::string CENS_PD_CONTROL
btVector3 eigen2btVec(const Eigen::Vector3f &v)
virtual void step(int value)
virtual CENSSerializedRobot * loadBulletFile(const std::string &robotName, const std::string &file)
Constraints m_eConstraints
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