openrave.org

 All Classes Namespaces Files Functions Variables Typedefs Enumerations Enumerator Friends Macros Groups Pages
orplanning_door.cpp
Go to the documentation of this file.
1 
13 #include <openrave-core.h>
14 #include <vector>
15 #include <sstream>
16 #include <boost/format.hpp>
17 
18 #include <openrave/planningutils.h>
19 
20 #include "orexample.h"
21 
22 using namespace OpenRAVE;
23 using namespace std;
24 
25 namespace cppexamples {
26 
28 class DoorConfiguration : public boost::enable_shared_from_this<DoorConfiguration>
29 {
30  static dReal TransformDistance2(const Transform& t1, const Transform& t2, dReal frotweight=1, dReal ftransweight=1)
31  {
32  //dReal facos = RaveAcos(min(dReal(1),RaveFabs(dot4(t1.rot,t2.rot))));
33  dReal facos = min((t1.rot-t2.rot).lengthsqr4(),(t1.rot+t2.rot).lengthsqr4());
34  return (t1.trans-t2.trans).lengthsqr3() + frotweight*facos; //*facos;
35  }
36 
37 
38 public:
39  DoorConfiguration(RobotBase::ManipulatorPtr pmanip, KinBody::JointPtr pdoorjoint) : _pmanip(pmanip), _pdoorjoint(pdoorjoint) {
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);
45  }
46 
47  virtual ~DoorConfiguration() {
48  }
49 
50  int GetDOF() {
51  return (int)_pmanip->GetArmIndices().size()+1;
52  }
53 
54  dReal ComputeDistance(const std::vector<dReal>& c0, const std::vector<dReal>& c1) {
55  dReal d1 = _robotdistmetric->Eval(c0,c1);
56  std::vector<dReal> door0(1), door1(1);
57  door0[0] = c0.back();
58  door1[0] = c1.back();
59  dReal d2 = _doordistmetric->Eval(door0,door1);
60  return RaveSqrt(d1*d1+d2*d2);
61  }
62 
63  bool Sample(std::vector<dReal>&v) {
64  for(int i = 0; i < 50; ++i) {
65  _robotsamplefn->Sample(v);
66  vector<dReal> vdoor;
67  _doorsamplefn->Sample(vdoor);
68 
69  v.resize(GetDOF());
70  v.back() = vdoor.at(0);
71  if( _SetState(v) ) {
72  return true;
73  }
74  }
75  return false;
76  }
77 
78  bool _SetState(std::vector<dReal>& v, int filteroptions=IKFO_CheckEnvCollisions|IKFO_IgnoreCustomFilters) {
79  // save state before modifying it
82 
83  vector<dReal> vdoor(1); vdoor[0] = v.back();
84  _ptarget->SetActiveDOFValues(vdoor);
85  _probot->SetActiveDOFValues(v);
86 
87  vector<dReal> vsolution;
88  bool bsuccess = _pmanip->FindIKSolution(_pdoorlink->GetTransform() * _tgrasp, vsolution, filteroptions);
89  if( bsuccess ) {
90  savestate1.reset();
91  savestate2.reset();
92  _probot->SetActiveDOFValues(vsolution);
93  std::copy(vsolution.begin(),vsolution.end(),v.begin());
94  _ptarget->SetActiveDOFValues(vdoor);
95  }
96  return bsuccess;
97  }
98 
99  void SetState(const std::vector<dReal>& v)
100  {
101  vector<dReal> vtemp = v;
102  if( !_SetState(vtemp) ) {
103  throw OPENRAVE_EXCEPTION_FORMAT0("could not set state",ORE_InvalidArguments);
104  }
105  }
106 
107  void GetState(std::vector<dReal>& v)
108  {
109  _probot->GetActiveDOFValues(v);
110  v.resize(GetDOF());
111  v.back() = _pdoorjoint->GetValue(0);
112  }
113 
114  void DiffState(std::vector<dReal>& v1,const std::vector<dReal>& v2)
115  {
116  _probot->SubtractActiveDOFValues(v1,v2);
117  v1.back() -= v2.back();
118  }
119 
120  bool NeightState(std::vector<dReal>& v,const std::vector<dReal>& vdelta, int fromgoal)
121  {
122  _vprevsolution = v;
123  // the previous solution should already be set on the robot, so do a sanity check
124  _tmanipprev = _pmanip->GetTransform();
125  Transform tdoorprev = _pdoorlink->GetTransform();
126  BOOST_ASSERT( TransformDistance2(tdoorprev*_tgrasp,_tmanipprev) <= g_fEpsilon );
127 
128  {
129  KinBody::KinBodyStateSaver statesaver(_ptarget);
130  vector<dReal> vdoor(1);
131  vdoor[0] = v.back()+0.5*vdelta.back();
132  _ptarget->SetActiveDOFValues(vdoor);
133  _tmanipmidreal = _pdoorlink->GetTransform()*_tgrasp;
134  }
135 
136  for(int i = 0; i < GetDOF(); ++i) {
137  v.at(i) += vdelta.at(i);
138  }
139 
140  return _SetState(v,IKFO_CheckEnvCollisions);
141  }
142 
143  // due to discontinues check that the robot midpoint is also along the door's expected trajectory
144  // take the midpoint of the solutions and ikparameterization and see if they are close
145  IkReturn _CheckContinuityFilter(std::vector<dReal>& vsolution, RobotBase::ManipulatorConstPtr pmanip, const IkParameterization& ikp)
146  {
147  Transform tmanipnew = ikp.GetTransform6D();
148  std::vector<dReal> vmidsolution(_probot->GetActiveDOF());
149  dReal realdist2 = TransformDistance2(_tmanipprev, tmanipnew);
150  const dReal ikmidpointmaxdist2mult = 0.5;
151 
152  RobotBase::RobotStateSaver savestate(_probot);
153  for(int i = 0; i < _probot->GetActiveDOF(); ++i) {
154  vmidsolution.at(i) = 0.5*(_vprevsolution.at(i)+vsolution.at(i));
155  }
156  _probot->SetActiveDOFValues(vmidsolution);
157 
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)));
162  return IKRA_Reject;
163  }
164  return IKRA_Success;
165  }
166 
167  void SetPlannerParameters(PlannerBase::PlannerParametersPtr params)
168  {
169  _probot->SetActiveDOFs(_pmanip->GetArmIndices());
170  vector<int> v(1); v[0] = _pdoorjoint->GetDOFIndex();
171  _ptarget->SetActiveDOFs(v);
172 
173  params->_configurationspecification = _probot->GetActiveConfigurationSpecification() + _ptarget->GetActiveConfigurationSpecification();
174  _probot->GetActiveDOFLimits(params->_vConfigLowerLimit,params->_vConfigUpperLimit);
175  _pdoorjoint->GetLimits(params->_vConfigLowerLimit,params->_vConfigUpperLimit,true);
176 
177  _probot->GetActiveDOFVelocityLimits(params->_vConfigVelocityLimit);
178  params->_vConfigVelocityLimit.push_back(100);
179 
180  _probot->GetActiveDOFAccelerationLimits(params->_vConfigAccelerationLimit);
181  params->_vConfigAccelerationLimit.push_back(100);
182 
183  _probot->GetActiveDOFResolutions(params->_vConfigResolution);
184  params->_vConfigResolution.push_back(0.05);
185 
186  _robotdistmetric.reset(new planningutils::SimpleDistanceMetric(_probot));
187  _doordistmetric.reset(new planningutils::SimpleDistanceMetric(_ptarget));
188  params->_distmetricfn = boost::bind(&DoorConfiguration::ComputeDistance,shared_from_this(),_1,_2);
189 
190  SpaceSamplerBasePtr pconfigsampler1 = RaveCreateSpaceSampler(_probot->GetEnv(),str(boost::format("robotconfiguration %s")%_probot->GetName()));
191  _robotsamplefn.reset(new planningutils::SimpleNeighborhoodSampler(pconfigsampler1,boost::bind(&planningutils::SimpleDistanceMetric::Eval,_robotdistmetric,_1,_2)));
192  SpaceSamplerBasePtr pconfigsampler2 = RaveCreateSpaceSampler(_probot->GetEnv(),str(boost::format("robotconfiguration %s")%_ptarget->GetName()));
193  _doorsamplefn.reset(new planningutils::SimpleNeighborhoodSampler(pconfigsampler2,boost::bind(&planningutils::SimpleDistanceMetric::Eval,_doordistmetric,_1,_2)));
194  params->_samplefn = boost::bind(&DoorConfiguration::Sample,shared_from_this(),_1);
195  params->_sampleneighfn.clear(); // won't be using it
196 
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);
201 
202  std::list<KinBodyPtr> listCheckCollisions;
203  listCheckCollisions.push_back(_probot);
204  _collision.reset(new planningutils::LineCollisionConstraint(listCheckCollisions));
205  params->_checkpathconstraintsfn = boost::bind(&planningutils::LineCollisionConstraint::Check,_collision,params, _probot, _1, _2, _3, _4);
206 
207  _ikfilter = _pmanip->GetIkSolver()->RegisterCustomFilter(0, boost::bind(&DoorConfiguration::_CheckContinuityFilter, shared_from_this(), _1, _2, _3));
208  }
209 
215  vector<dReal> _vprevsolution;
216  Transform _tmanipprev, _tmanipmidreal;
218 
219  boost::shared_ptr<planningutils::SimpleDistanceMetric> _robotdistmetric, _doordistmetric;
220  boost::shared_ptr<planningutils::SimpleNeighborhoodSampler> _robotsamplefn, _doorsamplefn;
221  boost::shared_ptr<planningutils::LineCollisionConstraint> _collision;
222 };
223 
224 typedef boost::shared_ptr<DoorConfiguration> DoorConfigurationPtr;
225 
227 {
228 public:
229  virtual void demothread(int argc, char ** argv) {
230  string scenefilename = "data/wam_cabinet.env.xml";
232  penv->Load(scenefilename);
233 
234  RobotBasePtr probot = penv->GetRobot("BarrettWAM");
235  // find the longest manipulator chain to move
236  RobotBase::ManipulatorPtr pmanip = probot->GetManipulators().at(0);
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];
240  }
241  }
242  RAVELOG_INFO(str(boost::format("planning with manipulator %s\n")%pmanip->GetName()));
243 
244  RobotBasePtr target = penv->GetRobot("Cabinet");
245  KinBody::JointPtr pdoorjoint = target->GetJoint("J_right");
246 
247  std::vector<dReal> vpreshape(4);
248  vpreshape[0] = 2.3; vpreshape[1] = 2.3; vpreshape[2] = 0.8; vpreshape[3] = 0;
249 
250  // create the configuration space
251  DoorConfigurationPtr doorconfig(new DoorConfiguration(pmanip,pdoorjoint));
253  Transform trobotorig;
254  {
255  EnvironmentMutex::scoped_lock lock(penv->GetMutex()); // lock environment
256 
257  probot->SetActiveDOFs(pmanip->GetGripperIndices());
258  probot->SetActiveDOFValues(vpreshape);
259 
260  doorconfig->SetPlannerParameters(params);
261  params->_nMaxIterations = 150; // max iterations before failure
262 
263  trobotorig = probot->GetTransform();
264  }
265 
266  PlannerBasePtr planner = RaveCreatePlanner(penv,"birrt");
267  TrajectoryBasePtr ptraj;
268  int iter = 0;
269 
270  while(IsOk()) {
271  iter += 1;
272  GraphHandlePtr pgraph;
273  {
274  EnvironmentMutex::scoped_lock lock(penv->GetMutex()); // lock environment
275 
276  if( (iter%5) == 0 ) {
277  RAVELOG_INFO("find a new position for the robot\n");
278  for(int i = 0; i < 100; ++i) {
279  Transform tnew = trobotorig;
280  tnew.trans.x += 0.5*(RaveRandomFloat()-0.5);
281  tnew.trans.y += 0.5*(RaveRandomFloat()-0.5);
282  probot->SetTransform(tnew);
283 
284  try {
285  params->_getstatefn(params->vinitialconfig);
286  params->_setstatefn(params->vinitialconfig);
287  params->_getstatefn(params->vinitialconfig);
288 
289  params->vgoalconfig = params->vinitialconfig;
290  params->vgoalconfig.back() = RaveRandomFloat()*1.5; // in radians
291  params->_setstatefn(params->vgoalconfig);
292  params->_getstatefn(params->vgoalconfig);
293  break;
294  }
295  catch(const openrave_exception& ex) {
296  probot->SetTransform(trobotorig);
297  }
298  }
299  }
300  else {
301  params->_getstatefn(params->vinitialconfig);
302  params->_setstatefn(params->vinitialconfig);
303  params->_getstatefn(params->vinitialconfig);
304 
305  params->vgoalconfig = params->vinitialconfig;
306  params->vgoalconfig.back() = RaveRandomFloat()*1.5; // in radians
307  params->_setstatefn(params->vgoalconfig);
308  params->_getstatefn(params->vgoalconfig);
309  }
310 
311  //params->_sPostProcessingPlanner = "lineartrajectoryretimer";
312  ptraj = RaveCreateTrajectory(penv,"");
313  if( !planner->InitPlan(probot,params) ) {
314  RAVELOG_WARN("plan failed to init\n");
315  continue;
316  }
317 
318  // create a new output trajectory
319  if( !planner->PlanPath(ptraj) ) {
320  RAVELOG_WARN("plan failed, trying again\n");
321  continue;
322  }
323 
324  // draw the end effector of the trajectory
325  {
326  RobotBase::RobotStateSaver saver(probot); // save the state of the robot since will be setting joint values
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);
333  }
334  pgraph = penv->drawlinestrip(&vpoints[0].x,vpoints.size(),sizeof(vpoints[0]),1.0f);
335  }
336 
337  // send the trajectory to the robot
338  probot->GetController()->SetPath(ptraj);
339  target->GetController()->SetPath(ptraj);
340  }
341 
342  // wait for the robot to finish
343  while(!probot->GetController()->IsDone() && IsOk()) {
344  boost::this_thread::sleep(boost::posix_time::milliseconds(1));
345  }
346  }
347  }
348 };
349 
350 } // end namespace cppexamples
351 
352 int main(int argc, char ** argv)
353 {
355  return example.main(argc,argv);
356 }