16 #include <boost/format.hpp>
22 using namespace OpenRAVE;
25 namespace cppexamples {
33 dReal facos = min((t1.rot-t2.rot).lengthsqr4(),(t1.rot+t2.rot).
lengthsqr4());
34 return (t1.trans-t2.trans).lengthsqr3() + frotweight*facos;
40 _probot = pmanip->GetRobot();
41 _ptarget = RaveInterfaceCast<RobotBase>(pdoorjoint->GetParent());
42 _pdoorlink = _pdoorjoint->GetHierarchyChildLink();
43 _tgrasp =
Transform(
Vector(0.65839364, 0.68616871, -0.22320624, -0.21417118),
Vector(0.23126595, -0.01218956, 0.1084143));
44 BOOST_ASSERT(_pdoorjoint->GetDOF()==1);
51 return (
int)_pmanip->GetArmIndices().size()+1;
55 dReal d1 = _robotdistmetric->Eval(c0,c1);
56 std::vector<dReal> door0(1), door1(1);
59 dReal d2 = _doordistmetric->Eval(door0,door1);
64 for(
int i = 0; i < 50; ++i) {
65 _robotsamplefn->Sample(v);
67 _doorsamplefn->Sample(vdoor);
70 v.back() = vdoor.at(0);
83 vector<dReal> vdoor(1); vdoor[0] = v.back();
84 _ptarget->SetActiveDOFValues(vdoor);
85 _probot->SetActiveDOFValues(v);
87 vector<dReal> vsolution;
88 bool bsuccess = _pmanip->FindIKSolution(_pdoorlink->GetTransform() * _tgrasp, vsolution, filteroptions);
92 _probot->SetActiveDOFValues(vsolution);
93 std::copy(vsolution.begin(),vsolution.end(),v.begin());
94 _ptarget->SetActiveDOFValues(vdoor);
99 void SetState(
const std::vector<dReal>& v)
101 vector<dReal> vtemp = v;
102 if( !_SetState(vtemp) ) {
107 void GetState(std::vector<dReal>& v)
109 _probot->GetActiveDOFValues(v);
111 v.back() = _pdoorjoint->GetValue(0);
114 void DiffState(std::vector<dReal>& v1,
const std::vector<dReal>& v2)
116 _probot->SubtractActiveDOFValues(v1,v2);
117 v1.back() -= v2.back();
120 bool NeightState(std::vector<dReal>& v,
const std::vector<dReal>& vdelta,
int fromgoal)
124 _tmanipprev = _pmanip->GetTransform();
125 Transform tdoorprev = _pdoorlink->GetTransform();
126 BOOST_ASSERT( TransformDistance2(tdoorprev*_tgrasp,_tmanipprev) <=
g_fEpsilon );
130 vector<dReal> vdoor(1);
131 vdoor[0] = v.back()+0.5*vdelta.back();
132 _ptarget->SetActiveDOFValues(vdoor);
133 _tmanipmidreal = _pdoorlink->GetTransform()*_tgrasp;
136 for(
int i = 0; i < GetDOF(); ++i) {
137 v.at(i) += vdelta.at(i);
148 std::vector<dReal> vmidsolution(_probot->GetActiveDOF());
149 dReal realdist2 = TransformDistance2(_tmanipprev, tmanipnew);
150 const dReal ikmidpointmaxdist2mult = 0.5;
153 for(
int i = 0; i < _probot->GetActiveDOF(); ++i) {
154 vmidsolution.at(i) = 0.5*(_vprevsolution.at(i)+vsolution.at(i));
156 _probot->SetActiveDOFValues(vmidsolution);
158 Transform tmanipmid = _pmanip->GetTransform();
159 dReal middist2 = TransformDistance2(tmanipmid, _tmanipmidreal);
160 if( middist2 >
g_fEpsilon && middist2 > ikmidpointmaxdist2mult*realdist2 ) {
161 RAVELOG_VERBOSE(str(boost::format(
"rejected due to discontinuity at mid-point %e > %e")%middist2%(ikmidpointmaxdist2mult*realdist2)));
169 _probot->SetActiveDOFs(_pmanip->GetArmIndices());
170 vector<int> v(1); v[0] = _pdoorjoint->GetDOFIndex();
171 _ptarget->SetActiveDOFs(v);
173 params->_configurationspecification = _probot->GetActiveConfigurationSpecification() + _ptarget->GetActiveConfigurationSpecification();
174 _probot->GetActiveDOFLimits(params->_vConfigLowerLimit,params->_vConfigUpperLimit);
175 _pdoorjoint->GetLimits(params->_vConfigLowerLimit,params->_vConfigUpperLimit,
true);
177 _probot->GetActiveDOFVelocityLimits(params->_vConfigVelocityLimit);
178 params->_vConfigVelocityLimit.push_back(100);
180 _probot->GetActiveDOFAccelerationLimits(params->_vConfigAccelerationLimit);
181 params->_vConfigAccelerationLimit.push_back(100);
183 _probot->GetActiveDOFResolutions(params->_vConfigResolution);
184 params->_vConfigResolution.push_back(0.05);
188 params->_distmetricfn = boost::bind(&DoorConfiguration::ComputeDistance,shared_from_this(),_1,_2);
194 params->_samplefn = boost::bind(&DoorConfiguration::Sample,shared_from_this(),_1);
195 params->_sampleneighfn.clear();
197 params->_setstatefn = boost::bind(&DoorConfiguration::SetState,shared_from_this(),_1);
198 params->_getstatefn = boost::bind(&DoorConfiguration::GetState,shared_from_this(),_1);
199 params->_diffstatefn = boost::bind(&DoorConfiguration::DiffState,shared_from_this(),_1,_2);
200 params->_neighstatefn = boost::bind(&DoorConfiguration::NeightState,shared_from_this(),_1,_2,_3);
202 std::list<KinBodyPtr> listCheckCollisions;
203 listCheckCollisions.push_back(_probot);
207 _ikfilter = _pmanip->GetIkSolver()->RegisterCustomFilter(0, boost::bind(&DoorConfiguration::_CheckContinuityFilter, shared_from_this(), _1, _2, _3));
220 boost::shared_ptr<planningutils::SimpleNeighborhoodSampler>
_robotsamplefn, _doorsamplefn;
221 boost::shared_ptr<planningutils::LineCollisionConstraint>
_collision;
230 string scenefilename =
"data/wam_cabinet.env.xml";
232 penv->Load(scenefilename);
237 for(
size_t i = 1; i < probot->GetManipulators().size(); ++i) {
238 if( pmanip->GetArmIndices().size() < probot->GetManipulators()[i]->GetArmIndices().size() ) {
239 pmanip = probot->GetManipulators()[i];
242 RAVELOG_INFO(str(boost::format(
"planning with manipulator %s\n")%pmanip->GetName()));
247 std::vector<dReal> vpreshape(4);
248 vpreshape[0] = 2.3; vpreshape[1] = 2.3; vpreshape[2] = 0.8; vpreshape[3] = 0;
255 EnvironmentMutex::scoped_lock lock(penv->GetMutex());
257 probot->SetActiveDOFs(pmanip->GetGripperIndices());
258 probot->SetActiveDOFValues(vpreshape);
260 doorconfig->SetPlannerParameters(params);
261 params->_nMaxIterations = 150;
263 trobotorig = probot->GetTransform();
274 EnvironmentMutex::scoped_lock lock(penv->GetMutex());
276 if( (iter%5) == 0 ) {
278 for(
int i = 0; i < 100; ++i) {
282 probot->SetTransform(tnew);
285 params->_getstatefn(params->vinitialconfig);
286 params->_setstatefn(params->vinitialconfig);
287 params->_getstatefn(params->vinitialconfig);
289 params->vgoalconfig = params->vinitialconfig;
291 params->_setstatefn(params->vgoalconfig);
292 params->_getstatefn(params->vgoalconfig);
296 probot->SetTransform(trobotorig);
301 params->_getstatefn(params->vinitialconfig);
302 params->_setstatefn(params->vinitialconfig);
303 params->_getstatefn(params->vinitialconfig);
305 params->vgoalconfig = params->vinitialconfig;
307 params->_setstatefn(params->vgoalconfig);
308 params->_getstatefn(params->vgoalconfig);
313 if( !planner->InitPlan(probot,params) ) {
319 if( !planner->PlanPath(ptraj) ) {
327 vector<RaveVector<float> > vpoints;
328 vector<dReal> vtrajdata;
329 for(
dReal ftime = 0; ftime <= ptraj->GetDuration(); ftime += 0.01) {
330 ptraj->Sample(vtrajdata,ftime,probot->GetActiveConfigurationSpecification());
331 probot->SetActiveDOFValues(vtrajdata);
332 vpoints.push_back(pmanip->GetEndEffectorTransform().trans);
334 pgraph = penv->drawlinestrip(&vpoints[0].x,vpoints.size(),
sizeof(vpoints[0]),1.0f);
338 probot->GetController()->SetPath(ptraj);
339 target->GetController()->SetPath(ptraj);
343 while(!probot->GetController()->IsDone() && IsOk()) {
344 boost::this_thread::sleep(boost::posix_time::milliseconds(1));
352 int main(
int argc,
char ** argv)
355 return example.
main(argc,argv);