41 typedef map<int, const char*> IdToLabelMap;
43 IdToLabelMap commandLabelMaps[2];
45 IdToLabelMap commandLabelMap;
47 IdLabel commandLabels[] = {
49 { DynamicsSimulator::POSITION_GIVEN,
"Position Given (High Gain Mode)" },
60 void initializeCommandLabelMaps()
62 if(commandLabelMap.empty()){
65 IdLabel& idLabel = commandLabels[
i++];
69 commandLabelMap[idLabel.id] = idLabel.label;
74 const char* getLabelOfLinkDataType(DynamicsSimulator::LinkDataType
type)
76 IdToLabelMap::iterator p = commandLabelMap.find(
type);
77 return (p != commandLabelMap.end()) ? p->second :
"Requesting Unknown Data Type";
82 template<
typename X,
typename X_ptr>
85 CosNaming::Name ncName;
87 ncName[0].id = CORBA::string_dup(
n.c_str());
88 ncName[0].kind = CORBA::string_dup(
"");
91 srv = X::_narrow(cxt->resolve(ncName));
93 std::cerr <<
n <<
" not found: ";
95 case CosNaming::NamingContext::missing_node:
96 std::cerr <<
"Missing Node" << std::endl;
97 case CosNaming::NamingContext::not_context:
98 std::cerr <<
"Not Context" << std::endl;
100 case CosNaming::NamingContext::not_object:
101 std::cerr <<
"Not Object" << std::endl;
105 }
catch(CosNaming::NamingContext::CannotProceed &exc) {
106 std::cerr <<
"Resolve " <<
n <<
" CannotProceed" << std::endl;
107 }
catch(CosNaming::NamingContext::AlreadyBound &exc) {
108 std::cerr <<
"Resolve " <<
n <<
" InvalidName" << std::endl;
115 : orb_(CORBA::ORB::_duplicate(orb))
117 world.setCurrentTime(0.0);
118 world.setRungeKuttaMethod();
121 CollisionDetectorFactory_var collisionDetectorFactory;
123 CORBA::Object_var nS = orb->resolve_initial_references(
"NameService");
124 CosNaming::NamingContext_var cxT;
125 cxT = CosNaming::NamingContext::_narrow(nS);
126 collisionDetectorFactory =
128 CollisionDetectorFactory_var>(
"CollisionDetectorFactory", cxT);
129 if (CORBA::is_nil(collisionDetectorFactory)) {
130 std::cerr <<
"CollisionDetectorFactory not found" << std::endl;
133 collisionDetector = collisionDetectorFactory->create();
135 collisions =
new CollisionSequence;
136 collidingLinkPairs =
new LinkPairSequence;
137 allCharacterPositions =
new CharacterPositionSequence;
138 allCharacterSensorStates =
new SensorStateSequence;
140 needToUpdatePositions =
true;
141 needToUpdateSensorStates =
true;
152 PortableServer::POA_var poa_ = _default_POA();
153 PortableServer::ObjectId_var
id = poa_->servant_to_id(
this);
154 poa_->deactivate_object(
id);
170 CORBA::Double _timestep,
171 OpenHRP::DynamicsSimulator::IntegrateMethod integrateOpt,
172 OpenHRP::DynamicsSimulator::SensorOption sensorOpt)
179 if(integrateOpt == OpenHRP::DynamicsSimulator::EULER){
201 jlist.push_back(cur);
214 const char *charName1,
215 const char *linkName1,
216 const char *charName2,
217 const char *linkName2,
218 const CORBA::Double staticFriction,
219 const CORBA::Double slipFriction,
220 const DblSequence6 & K,
221 const DblSequence6 & C,
222 const double culling_thresh,
223 const double restitution)
225 const double epsilon = 0.0;
228 std::string emptyString =
"";
229 std::vector<Joint*> joints1;
230 std::vector<Joint*> joints2;
233 if(emptyString == linkName1)
241 if(jnt1) joints1.push_back(jnt1);
243 if(emptyString == linkName2)
251 if(jnt2) joints2.push_back(jnt2);
254 for(
size_t i=0;
i < joints1.size(); ++
i)
257 for(
size_t j=0; j < joints2.size(); ++j)
259 Joint* j2 = joints2[j];
260 if(j1 && j2 && j1 != j2)
263 world.addCollisionCheckLinkPair(j1, j2, staticFriction, slipFriction, epsilon);
264 LinkPair_var linkPair =
new LinkPair();
265 linkPair->charName1 = CORBA::string_dup(charName1);
266 linkPair->linkName1 = CORBA::string_dup(j1->
basename);
267 linkPair->charName2 = CORBA::string_dup(charName2);
268 linkPair->linkName2 = CORBA::string_dup(j2->
basename);
269 linkPair->tolerance = 0;
277 const char *charName1,
278 const char *linkName1,
279 const char *charName2,
280 const char *linkName2,
281 const double tolerance)
285 std::string emptyString =
"";
286 std::vector<Joint*> joints1;
287 std::vector<Joint*> joints2;
290 if(emptyString == linkName1)
298 if(jnt1) joints1.push_back(jnt1);
300 if(emptyString == linkName2)
308 if(jnt2) joints2.push_back(jnt2);
311 for(
size_t i=0;
i < joints1.size(); ++
i)
314 for(
size_t j=0; j < joints2.size(); ++j)
316 Joint* j2 = joints2[j];
317 if(j1 && j2 && j1 != j2)
319 LinkPair_var linkPair =
new LinkPair();
320 linkPair->charName1 = CORBA::string_dup(charName1);
321 linkPair->linkName1 = CORBA::string_dup(j1->
basename);
322 linkPair->charName2 = CORBA::string_dup(charName2);
323 linkPair->linkName2 = CORBA::string_dup(j2->
basename);
324 linkPair->tolerance = tolerance;
333 const char* charName1,
334 const char* linkName1,
335 const char* charName2,
336 const char* linkName2,
337 const DblSequence3& link1LocalPos,
338 const DblSequence3& link2LocalPos,
339 const ExtraJointType jointType,
340 const DblSequence3& jointAxis,
341 const char* extraJointName)
347 const char * characterName,
348 const char * extraJointName,
349 DblSequence6_out contactForce)
356 seq[CORBA::ULong(offset++)] = vec(0);
357 seq[CORBA::ULong(offset++)] = vec(1);
358 seq[CORBA::ULong(offset)] = vec(2);
369 const char *characterName,
370 const char *sensorName,
371 DblSequence_out sensorOutput)
373 sensorOutput =
new DblSequence;
374 Sensor* sensor =
world.findSensor(sensorName, characterName);
383 sensorOutput->length(6);
388 sensorOutput[CORBA::ULong(0)] = forceSensor->
f(0);
389 sensorOutput[CORBA::ULong(1)] = forceSensor->
f(1);
390 sensorOutput[CORBA::ULong(2)] = forceSensor->
f(2);
391 sensorOutput[CORBA::ULong(3)] = forceSensor->
tau(0);
392 sensorOutput[CORBA::ULong(4)] = forceSensor->
tau(1);
393 sensorOutput[CORBA::ULong(5)] = forceSensor->
tau(2);
401 sensorOutput->length(3);
405 sensorOutput[CORBA::ULong(0)] = gyro->
w(0);
406 sensorOutput[CORBA::ULong(1)] = gyro->
w(1);
407 sensorOutput[CORBA::ULong(2)] = gyro->
w(2);
415 sensorOutput->length(3);
419 sensorOutput[CORBA::ULong(0)] = accelSensor->
dv(0);
420 sensorOutput[CORBA::ULong(1)] = accelSensor->
dv(1);
421 sensorOutput[CORBA::ULong(2)] = accelSensor->
dv(2);
477 const char* characterName,
478 const char* linkName,
479 OpenHRP::DynamicsSimulator::LinkDataType
type,
480 const DblSequence& wdata)
483 Joint* joint =
world.Chain()->FindJoint(linkName, characterName);
488 case OpenHRP::DynamicsSimulator::POSITION_GIVEN:
489 joint->
t_given = !(wdata[0] > 0.0);
510 fVec3 abs_pos(wdata[0], wdata[1], wdata[2]);
511 fMat33 abs_att(wdata[3], wdata[4], wdata[5], wdata[6], wdata[7], wdata[8], wdata[9], wdata[10], wdata[11]);
518 fVec3 abs_lin_vel(wdata[0], wdata[1], wdata[2]);
519 fVec3 abs_ang_vel(wdata[3], wdata[4], wdata[5]);
532 fVec3 n0(wdata[3], wdata[4], wdata[5]);
549 const char * characterName,
550 const char * linkName,
551 OpenHRP::DynamicsSimulator::LinkDataType
type,
552 DblSequence_out out_rdata)
555 Joint* joint =
world.Chain()->FindJoint(linkName, characterName);
558 DblSequence_var rdata =
new DblSequence;
569 rdata[0] = joint->
qd;
574 rdata[0] = joint->
qdd;
579 rdata[0] = joint->
tau;
591 rdata[4] = ep.
Axis()(0);
592 rdata[5] = ep.
Axis()(1);
593 rdata[6] = ep.
Axis()(2);
633 out_rdata = rdata._retn();
638 const char * characterName,
639 OpenHRP::DynamicsSimulator::LinkDataType
type,
640 DblSequence_out rdata)
643 rdata =
new DblSequence();
645 world.getAllCharacterData(characterName,
type, rdata);
650 const char * characterName,
651 OpenHRP::DynamicsSimulator::LinkDataType
type,
652 const DblSequence & wdata)
655 world.setAllCharacterData(characterName,
type, wdata);
660 const DblSequence3& wdata)
662 assert(wdata.length() == 3);
673 DblSequence3_out wdata)
684 const char * characterName,
685 OpenHRP::DynamicsSimulator::JointDriveMode jointMode)
688 bool isTorqueMode = (jointMode != OpenHRP::DynamicsSimulator::HIGH_GAIN_MODE);
690 world.Chain()->SetCharacterTorqueGiven(characterName, isTorqueMode);
696 const char * characterName,
697 const char * fromLink,
const char * toLink,
698 const LinkPosition& target)
709 const char * characterName)
712 world.Chain()->CalcPosition();
722 world.Chain()->CalcPosition();
734 wstate =
new WorldState;
746 int n_char =
world.numCharacter();
747 for(
int i=0;
i<n_char;
i++)
750 if(!strcmp(j->
CharName(), characterName))
766 sstate =
new SensorState;
773 int nchar =
world.numCharacter();
780 for(
int i=0;
i<nchar;
i++)
785 characterPosition.characterName = CORBA::string_dup(j->
CharName());
786 int n_links =
world.numLinks(
i);
788 LinkPositionSequence& linkPositions = characterPosition.linkPositions;
789 linkPositions.length(n_links);
791 int n_joints =
world.numJoints(
i);
793 sensorState.q.length(n_joints);
794 sensorState.dq.length(n_joints);
795 sensorState.u.length(n_joints);
810 for(
int i=0;
i<n_char;
i++)
813 int n_link = cpos.linkPositions.length();
814 for(
int j=0; j<n_link; j++)
816 LinkPosition& lpos = cpos.linkPositions[j];
834 const char *characterName,
835 LinkPairSequence_out pairs)
837 std::vector<unsigned int> locations;
841 if( (strcmp(characterName,
collisions[
i].pair.charName1) == 0) ||
842 (strcmp(characterName,
collisions[
i].pair.charName2) == 0))
843 locations.push_back(
i);
846 pairs->length(locations.size());
849 for(std::vector<unsigned int>::iterator iter = locations.begin();
850 iter != locations.end(); ++iter) {
852 strcpy(pairs[
n].charName1,
collisions[*iter].pair.charName1);
853 strcpy(pairs[
n].charName2,
collisions[*iter].pair.charName2);
854 strcpy(pairs[
n].linkName1,
collisions[*iter].pair.linkName1);
855 strcpy(pairs[
n].linkName2,
collisions[*iter].pair.linkName2);
863 const char *characterName,
864 const char *baseLink,
865 const char *targetLink,
866 DblSequence_out jacobian)
869 world.calcCharacterJacobian(characterName, baseLink, targetLink, J);
876 for(
int r=0; r <
height; ++r){
878 (*jacobian)[
i++] = J(r,
c);
898 DistanceSequence_var distances =
new DistanceSequence;
900 return distances._retn();
907 LinkPairSequence_var pairs =
new LinkPairSequence;
909 return pairs._retn();
917 orb_(CORBA::ORB::_duplicate(orb))
919 initializeCommandLabelMaps();
925 PortableServer::POA_var poa = _default_POA();
926 PortableServer::ObjectId_var
id = poa -> servant_to_id(
this);
927 poa -> deactivate_object(
id);
935 PortableServer::ServantBase_var integratorrServant = integratorImpl;
936 PortableServer::POA_var poa_ = _default_POA();
937 PortableServer::ObjectId_var
id =
938 poa_ -> activate_object(integratorImpl);
940 return integratorImpl -> _this();
946 orb_->shutdown(
false);