本文整理汇总了C++中EnvironmentBasePtr::StepSimulation方法的典型用法代码示例。如果您正苦于以下问题：C++ EnvironmentBasePtr::StepSimulation方法的具体用法？C++ EnvironmentBasePtr::StepSimulation怎么用？C++ EnvironmentBasePtr::StepSimulation使用的例子？那么, 这里精选的方法代码示例或许可以为您提供帮助。您也可以进一步了解该方法所在类EnvironmentBasePtr的用法示例。
在下文中一共展示了EnvironmentBasePtr::StepSimulation方法的1个代码示例，这些例子默认根据受欢迎程度排序。您可以为喜欢或者感觉有用的代码点赞，您的评价将有助于系统推荐出更棒的C++代码示例。
示例1: main
//.........这里部分代码省略.........
penv->GetRobots(vbodies);
// get the first body
if( vbodies.size() == 0 ) {
RAVELOG_ERROR("no bodies loaded\n");
return -3;
}
//KinBodyPtr pbody = vbodies.at(0);
RobotBasePtr pbody = vbodies.at(0);
vector<dReal> values;
pbody->GetDOFValues(values);
// set new values
for(int i = 0; i < (int)vsetvalues.size() && i < (int)values.size(); ++i) {
values[i] = vsetvalues[i];
}
pbody->Enable(true);
//pbody->SetVisible(true);
//CollisionReportPtr report(new CollisionReport());
//bool runflag = true;
//while(runflag) {
/* Wait until next shot */
clock_nanosleep(0,TIMER_ABSTIME,&t, NULL);
/* Get updated joint info here */
r = ach_get( &chan_num, &H, sizeof(H), &fs, NULL, ACH_O_LAST );
assert( sizeof(H) == fs );
/* set all joints from ach */
int len = (int)(sizeof(openRAVEJointIndices)/sizeof(openRAVEJointIndices[0]));
/* set all joints from ach */
for( int ii = 0; ii < (int)values.size() ; ii++ ) {
values[ii] = 0.0;
}
for( int ii = 0; ii < len; ii++){
if(openRAVEJointIndices[ii] != -1){
values[openRAVEJointIndices[ii]] = H.ref[hubo_refJointIndices[ii]];
}
}
// values[RSY] = -1.0;
// values[REB] = 1.0;
pbody->SetDOFValues(values,true);
// penv->GetCollisionChecker()->SetCollisionOptions(CO_Contacts);
// if( pbody->CheckSelfCollision(report) ) {
// cflag = true;
// if(vflag | Vflag){
// RAVELOG_INFO("body not in collision\n");
// }
// if(Vflag) {
// contactpoints = (int)report->contacts.size();
// stringstream ss;
// ss << "body in self-collision "
// << (!!report->plink1 ? report->plink1->GetName() : "") << ":"
// << (!!report->plink2 ? report->plink2->GetName() : "") << " at "
// << contactpoints << "contacts" << endl;
// for(int i = 0; i < contactpoints; ++i) {
// CollisionReport::CONTACT& c = report->contacts[i];
// ss << "contact" << i << ": pos=("
// << c.pos.x << ", " << c.pos.y << ", " << c.pos.z << "), norm=("
// << c.norm.x << ", " << c.norm.y << ", " << c.norm.z << ")" << endl;
// }
// RAVELOG_INFOA(ss.str());
// }
// }
// else {
// cflag = false;
// if(vflag | Vflag) {
// RAVELOG_INFO("body not in collision\n");
// }
// }
// get the transformations of all the links
vector<Transform> vlinktransforms;
pbody->GetLinkTransformations(vlinktransforms);
//boost::this_thread::sleep(boost::posix_time::milliseconds(2000));
// boost::this_thread::sleep(boost::posix_time::milliseconds(1));
t.tv_nsec+=interval;
tsnorm(&t);
// runflag = false;
//pbody->Enable(true);
//pbody->SetVisible(true);
usleep(10000);
pbody->SimulationStep(0.01);
penv->StepSimulation(0.01);
}
}
pause();
RaveDestroy(); // destroy
return contactpoints;
}