ProjectUtil.cpp
Go to the documentation of this file.
1 #include <iostream>
2 #include <rtm/Manager.h>
3 #include <rtm/RTObject.h>
4 #include <rtm/DataFlowComponentBase.h>
5 #include <hrpModel/Link.h>
6 #include "ProjectUtil.h"
7 #include "BVutil.h"
8 
9 void initWorld(Project& prj, BodyFactory &factory,
11  std::vector<hrp::ColdetLinkPairPtr> &pairs)
12 {
13  world.clearBodies();
15  world.setCurrentTime(0.0);
16 
17  world.setTimeStep(prj.timeStep());
18  if(prj.isEuler()){
19  world.setEulerMethod();
20  } else {
21  world.setRungeKuttaMethod();
22  }
23 
24  // add bodies
25  for (std::map<std::string, ModelItem>::iterator it=prj.models().begin();
26  it != prj.models().end(); it++){
27  const std::string name
28  = it->second.rtcName == "" ? it->first : it->second.rtcName;
29  hrp::BodyPtr body = factory(name, it->second);
30  if (body){
31  body->setName(name);
32  world.addBody(body);
33  // <-- for OpenRTM-1.0.0 bug
35  = dynamic_cast<RTC::DataFlowComponentBase *>(body.get());
36  if (rtc && rtc->getInstanceName() != name){
37  rtc->setInstanceName(name.c_str());
38  }
39  // -->
40  }
41  }
42 
43  for (unsigned int i=0; i<prj.collisionPairs().size(); i++){
44  const CollisionPairItem &cpi = prj.collisionPairs()[i];
45  int bodyIndex1 = world.bodyIndex(cpi.objectName1);
46  if (bodyIndex1 < 0){
47  // different name is used for RTC
48  if (prj.models().find(cpi.objectName1) != prj.models().end()){
49  bodyIndex1
50  = world.bodyIndex(prj.models()[cpi.objectName1].rtcName);
51  }
52  }
53  int bodyIndex2 = world.bodyIndex(cpi.objectName2);
54  if (bodyIndex2 < 0){
55  // different name is used for RTC
56  if (prj.models().find(cpi.objectName2) != prj.models().end()){
57  bodyIndex2
58  = world.bodyIndex(prj.models()[cpi.objectName2].rtcName);
59  }
60  }
61 
62  if(bodyIndex1 >= 0 && bodyIndex2 >= 0){
63  hrp::BodyPtr bodyPtr1 = world.body(bodyIndex1);
64  hrp::BodyPtr bodyPtr2 = world.body(bodyIndex2);
65 
66  std::vector<hrp::Link*> links1;
67  if(cpi.jointName1.empty()){
68  const hrp::LinkTraverse& traverse = bodyPtr1->linkTraverse();
69  links1.resize(traverse.numLinks());
70  std::copy(traverse.begin(), traverse.end(), links1.begin());
71  } else {
72  links1.push_back(bodyPtr1->link(cpi.jointName1));
73  }
74 
75  std::vector<hrp::Link*> links2;
76  if(cpi.jointName2.empty()){
77  const hrp::LinkTraverse& traverse = bodyPtr2->linkTraverse();
78  links2.resize(traverse.numLinks());
79  std::copy(traverse.begin(), traverse.end(), links2.begin());
80  } else {
81  links2.push_back(bodyPtr2->link(cpi.jointName2));
82  }
83 
84  for(size_t j=0; j < links1.size(); ++j){
85  for(size_t k=0; k < links2.size(); ++k){
86  hrp::Link* link1 = links1[j];
87  hrp::Link* link2 = links2[k];
88 
89  if(link1 && link2 && link1 != link2
90  && link1->parent != link2 && link1 != link2->parent){
92  (bodyIndex1, link1, bodyIndex2, link2,
94  cpi.cullingThresh, cpi.restitution, 0.0);
95  pairs.push_back(new hrp::ColdetLinkPair(link1, link2));
96  }
97  }
98  }
99  }
100  }
101 
102  for (unsigned int i=0; i<prj.extraJoints().size(); i++){
103  const ExtraJointItem &ej = prj.extraJoints()[i];
104  int bodyIndex1 = world.bodyIndex(ej.object1Name);
105  if (bodyIndex1 < 0){
106  // different name is used for RTC
107  if (prj.models().find(ej.object1Name) != prj.models().end()){
108  bodyIndex1
109  = world.bodyIndex(prj.models()[ej.object1Name].rtcName);
110  }
111  }
112  int bodyIndex2 = world.bodyIndex(ej.object2Name);
113  if (bodyIndex2 < 0){
114  // different name is used for RTC
115  if (prj.models().find(ej.object2Name) != prj.models().end()){
116  bodyIndex2
117  = world.bodyIndex(prj.models()[ej.object2Name].rtcName);
118  }
119  }
120 
121  if(bodyIndex1 >= 0 && bodyIndex2 >= 0){
122  hrp::BodyPtr bodyPtr1 = world.body(bodyIndex1);
123  hrp::BodyPtr bodyPtr2 = world.body(bodyIndex2);
124 
125  hrp::Link *link1 = bodyPtr1->link(ej.link1Name);
126  hrp::Link *link2 = bodyPtr2->link(ej.link2Name);
127 
128  int jtype;
129  if (ej.jointType == "xyz"){
130  jtype = 0;
131  }else if (ej.jointType == "xy"){
132  jtype = 1;
133  }else if (ej.jointType == "z"){
134  jtype = 2;
135  }
136 
137  if (link1 && link2){
139  bodyIndex1, link1, bodyIndex2, link2,
140  ej.link1LocalPos.data(), ej.link2LocalPos.data(),
141  jtype, ej.jointAxis.data());
142  }
143  }
144  }
145 
146  world.enableSensors(true);
147 
148  int nBodies = world.numBodies();
149  for(int i=0; i < nBodies; ++i){
150  hrp::BodyPtr bodyPtr = world.body(i);
151  bodyPtr->initializeConfiguration();
152  }
153 
154  for (std::map<std::string, ModelItem>::iterator it=prj.models().begin();
155  it != prj.models().end(); it++){
157  if (it->second.rtcName != "") body = world.body(it->second.rtcName);
158  if (!body) body = world.body(it->first);
159  if (!body){
160  std::cerr << "can't find a body named " << it->first << " or "
161  << it->second.rtcName << std::endl;
162  continue;
163  }
164  for (std::map<std::string, JointItem>::iterator it2=it->second.joint.begin();
165  it2 != it->second.joint.end(); it2++){
166  hrp::Link *link = body->link(it2->first);
167  if (!link) continue;
168  if (link->isRoot()){
169  link->p = it2->second.translation;
170  link->setAttitude(it2->second.rotation);
171  link->v = it2->second.linearVelocity;
172  link->w = it2->second.angularVelocity;
173  link->vo = link->v - link->w.cross(link->p);
174  }else{
175  link->q = it2->second.angle;
176  }
177  }
178  body->calcForwardKinematics();
179  body->setDefaultRootPosition(body->rootLink()->p, body->rootLink()->attitude());
180  }
181  world.initialize();
182 #if 0
184 #endif
185 }
186 
187 void initRTS(Project &prj, std::vector<ClockReceiver>& receivers)
188 {
190 
191  RTSItem& rts = prj.RTS();
192  // load factories
193  for (std::map<std::string, RTSItem::rtc>::iterator it
194  = rts.components.begin(); it != rts.components.end(); it++){
195  std::string path = it->second.path;
196  if (path == "") continue;
197 #ifdef __APPLE__
198  path += ".dylib";
199 #else
200  path += ".so";
201 #endif
202  std::cout << "loading " << path << std::endl;
203  std::string initfunc = it->second.name + "Init";
204  manager.load(path.c_str(), initfunc.c_str());
205  }
206  // create components
207  for (std::map<std::string, RTSItem::rtc>::iterator it
208  = rts.components.begin(); it != rts.components.end(); it++){
209  RTC::RTObject_impl *rtc = manager.getComponent(it->first.c_str());
210  if (!rtc){
211  if (it->second.name == ""){
212  std::cerr << "factory name for " << it->first << " is not defined" << std::endl;
213  continue;
214  }
215  std::cout << "creating " << it->first << std::endl;
216  std::string args = it->second.name + "?instance_name=" + it->first;
217  rtc = manager.createComponent(args.c_str());
218  }
219  if (!rtc){
220  std::cerr << "failed to create RTC(" << it->first << ")" << std::endl;
221  continue;
222  }
223  RTC::ExecutionContextList_var eclist = rtc->get_owned_contexts();
224  for(CORBA::ULong i=0; i < eclist->length(); ++i){
225  if(!CORBA::is_nil(eclist[i])){
226  OpenRTM::ExtTrigExecutionContextService_var execContext = OpenRTM::ExtTrigExecutionContextService::_narrow(eclist[i]);
227  if(!CORBA::is_nil(execContext)){
228  std::cout << it->first << ":" << it->second.period << std::endl;
229  receivers.push_back(ClockReceiver(execContext, it->second.period));
230  execContext->activate_component(rtc->getObjRef());
231  }
232  }
233  }
234  // set configuration
235  {
236  RTC::RTObject_var rtc = findRTC(it->first);
237  for (size_t i=0; i<it->second.configuration.size(); i++){
238  setConfiguration(rtc, it->second.configuration[i].first,
239  it->second.configuration[i].second);
240  }
241  }
242  }
243  // make connections
244  for (std::vector<std::pair<std::string, std::string> >::iterator it
245  = rts.connections.begin(); it != rts.connections.end(); it++){
246  std::cout << "making a connection between "
247  << it->first << " and " << it->second << std::endl;
248  int pos1 = it->first.find('.');
249  std::string comp1 = it->first.substr(0, pos1);
250  std::string port1 = it->first;
251  int pos2 = it->second.find('.');
252  std::string comp2 = it->second.substr(0, pos2);
253  std::string port2 = it->second;
254 
255  RTC::RTObject_var rtc1 = findRTC(comp1);
256  if (!rtc1){
257  std::cerr << "can't find a component named " << comp1 << std::endl;
258  return;
259  }
260  RTC::RTObject_var rtc2 = findRTC(comp2);
261  if (!rtc2){
262  std::cerr << "can't find a component named " << comp2 << std::endl;
263  return;
264  }
265  RTC::PortServiceList_var ports1 = rtc1->get_ports();
266  RTC::PortServiceList_var ports2 = rtc2->get_ports();
267 
268  RTC::PortService_ptr portObj1=NULL, portObj2=NULL;
269  for(CORBA::ULong i = 0; i < ports1->length(); ++i ){
270  RTC::PortProfile_var profile = ports1[i]->get_port_profile();
271  std::string portName(profile->name);
272  if (portName == port1){
273  portObj1 = ports1[i];
274  break;
275  }
276  }
277  if (!portObj1) {
278  std::cerr << "can't find a port named " << port1 << std::endl;
279  return;
280  }
281  for(CORBA::ULong i = 0; i < ports2->length(); ++i ){
282  RTC::PortProfile_var profile = ports2[i]->get_port_profile();
283  std::string portName(profile->name);
284  if (portName == port2){
285  portObj2 = ports2[i];
286  break;
287  }
288  }
289  if (!portObj2) {
290  std::cerr << "can't find a port named " << port2 << std::endl;
291  return;
292  }
293  connectPorts(portObj1, portObj2);
294  }
295 }
std::map< std::string, rtc > components
Definition: Project.h:79
png_infop png_charpp int png_charpp profile
std::string jointType
Definition: Project.h:57
std::vector< ExtraJointItem > & extraJoints()
Definition: Project.h:116
RTObject_impl * createComponent(const char *comp_args)
bool addExtraJoint(int bodyIndex1, Link *link1, int bodyIndex2, Link *link2, const double *link1LocalPos, const double *link2LocalPos, const short jointType, const double *jointAxis)
int bodyIndex(const std::string &name)
void setEulerMethod()
int addBody(BodyPtr body)
void useBuiltinCollisionDetector(bool on)
def findRTC(name, rnc=None)
get RT component
Definition: jython/rtm.py:341
virtual void initialize()
double slidingFriction
Definition: Project.h:15
RTObject_impl * getComponent(const char *instance_name)
png_infop png_charpp name
void setCurrentTime(double tm)
void load(const char *fname, const char *initfunc)
manager
std::string objectName1
Definition: Project.h:11
png_uint_32 i
std::string link1Name
Definition: Project.h:57
static Manager & instance()
void setRungeKuttaMethod()
std::string jointName2
Definition: Project.h:14
BodyPtr body(int index)
std::string object1Name
Definition: Project.h:57
def setConfiguration(rtc, nvlist)
update default configuration set
Definition: jython/rtm.py:683
double restitution
Definition: Project.h:18
double staticFriction
Definition: Project.h:16
std::map< std::string, ModelItem > & models()
Definition: Project.h:114
std::string link2Name
Definition: Project.h:57
def connectPorts(outP, inPs, subscription="flush", dataflow="Push", bufferlength=1, rate=1000)
connect ports
Definition: jython/rtm.py:433
RTSItem & RTS()
Definition: Project.h:117
RTObject_ptr getObjRef() const
bool addCollisionCheckLinkPair(int bodyIndex1, Link *link1, int bodyIndex2, Link *link2, double muStatic, double muDynamic, double culling_thresh, double restitution, double epsilon)
path
const char * getInstanceName()
std::vector< Link *>::const_iterator begin() const
std::string jointName1
Definition: Project.h:13
virtual ExecutionContextList * get_owned_contexts()
hrp::Vector3 link2LocalPos
Definition: Project.h:56
void initRTS(Project &prj, std::vector< ClockReceiver > &receivers)
std::string objectName2
Definition: Project.h:12
TConstraintForceSolver constraintForceSolver
bool isEuler()
Definition: Project.h:110
std::vector< std::pair< std::string, std::string > > connections
Definition: Project.h:80
double cullingThresh
Definition: Project.h:17
void setTimeStep(double dt)
std::string object2Name
Definition: Project.h:57
void setInstanceName(const char *instance_name)
hrp::Vector3 jointAxis
Definition: Project.h:55
double timeStep()
Definition: Project.h:105
void clearBodies()
hrp::Vector3 link1LocalPos
Definition: Project.h:56
unsigned int numBodies()
std::vector< Link *>::const_iterator end() const
void enableSensors(bool on)
boost::function2< hrp::BodyPtr, const std::string &, const ModelItem & > BodyFactory
Definition: ProjectUtil.h:9
void initWorld(Project &prj, BodyFactory &factory, hrp::World< hrp::ConstraintForceSolver > &world, std::vector< hrp::ColdetLinkPairPtr > &pairs)
Definition: ProjectUtil.cpp:9
std::vector< CollisionPairItem > & collisionPairs()
Definition: Project.h:115
unsigned int numLinks() const


hrpsys
Author(s): AIST, Fumio Kanehiro
autogenerated on Sat Dec 17 2022 03:52:20