openrave.org

 All Classes Namespaces Files Functions Variables Typedefs Enumerations Enumerator Friends Macros Groups Pages
iksolver.cpp
Go to the documentation of this file.
1 // -*- coding: utf-8 -*-
2 // Copyright (C) 2006-2011 Rosen Diankov (rosen.diankov@gmail.com)
3 //
4 // This file is part of OpenRAVE.
5 // OpenRAVE is free software: you can redistribute it and/or modify
6 // it under the terms of the GNU Lesser General Public License as published by
7 // the Free Software Foundation, either version 3 of the License, or
8 // at your option) any later version.
9 //
10 // This program is distributed in the hope that it will be useful,
11 // but WITHOUT ANY WARRANTY; without even the implied warranty of
12 // MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
13 // GNU Lesser General Public License for more details.
14 //
15 // You should have received a copy of the GNU Lesser General Public License
16 // along with this program. If not, see <http://www.gnu.org/licenses/>.
17 #include "libopenrave.h"
18 
19 namespace OpenRAVE {
20 
21 bool IkReturn::Append(const IkReturn& r)
22 {
23  bool bclashing = false;
24  if( !!r._userdata ) {
25  if( !!_userdata ) {
26  RAVELOG_WARN("IkReturn already has _userdata set, but overwriting anyway\n");
27  bclashing = true;
28  }
29  _userdata = r._userdata;
30  }
31  if( _mapdata.size() == 0 ) {
32  _mapdata = r._mapdata;
33  }
34  else {
35  FOREACHC(itr,r._mapdata) {
36  if( !_mapdata.insert(*itr).second ) {
37  RAVELOG_WARN(str(boost::format("IkReturn _mapdata %s overwritten")%itr->first));
38  bclashing = true;
39  }
40  }
41  }
42  if( r._vsolution.size() > 0 ) {
43  if( _vsolution.size() > 0 ) {
44  RAVELOG_WARN("IkReturn already has _vsolution set, but overwriting anyway\n");
45  bclashing = true;
46  }
48  }
49  return bclashing;
50 }
51 
53 {
54  _mapdata.clear();
55  _userdata.reset();
56  _vsolution.resize(0);
57 }
58 
59 class CustomIkSolverFilterData : public boost::enable_shared_from_this<CustomIkSolverFilterData>, public UserData
60 {
61 public:
62  CustomIkSolverFilterData(int priority, const IkSolverBase::IkFilterCallbackFn& filterfn, IkSolverBasePtr iksolver) : _priority(priority), _filterfn(filterfn), _iksolverweak(iksolver) {
63  }
65  IkSolverBasePtr iksolver = _iksolverweak.lock();
66  if( !!iksolver ) {
67  iksolver->__listRegisteredFilters.erase(_iterator);
68  }
69  }
70 
71  int _priority;
74  std::list<UserDataWeakPtr>::iterator _iterator;
75 };
76 
77 typedef boost::shared_ptr<CustomIkSolverFilterData> CustomIkSolverFilterDataPtr;
78 
80 {
81  return boost::dynamic_pointer_cast<CustomIkSolverFilterData>(data0)->_priority > boost::dynamic_pointer_cast<CustomIkSolverFilterData>(data1)->_priority;
82 }
83 
84 bool IkSolverBase::Solve(const IkParameterization& param, const std::vector<dReal>& q0, int filteroptions, IkReturnPtr ikreturn)
85 {
86  if( !ikreturn ) {
87  return Solve(param,q0,filteroptions,boost::shared_ptr< vector<dReal> >());
88  }
89  ikreturn->Clear();
90  boost::shared_ptr< vector<dReal> > psolution(&ikreturn->_vsolution, utils::null_deleter());
91  if( !Solve(param,q0,filteroptions,psolution) ) {
92  ikreturn->_action = IKRA_Reject;
93  return false;
94  }
95  ikreturn->_action = IKRA_Success;
96  return true;
97 }
98 
99 bool IkSolverBase::SolveAll(const IkParameterization& param, int filteroptions, std::vector<IkReturnPtr>& ikreturns)
100 {
101  ikreturns.resize(0);
102  std::vector< std::vector<dReal> > vsolutions;
103  if( !SolveAll(param,filteroptions,vsolutions) ) {
104  return false;
105  }
106  ikreturns.resize(vsolutions.size());
107  for(size_t i = 0; i < ikreturns.size(); ++i) {
108  ikreturns[i].reset(new IkReturn(IKRA_Success));
109  ikreturns[i]->_vsolution = vsolutions[i];
110  ikreturns[i]->_action = IKRA_Success;
111  }
112  return vsolutions.size() > 0;
113 }
114 
115 bool IkSolverBase::Solve(const IkParameterization& param, const std::vector<dReal>& q0, const std::vector<dReal>& vFreeParameters, int filteroptions, IkReturnPtr ikreturn)
116 {
117  if( !ikreturn ) {
118  return Solve(param,q0,vFreeParameters,filteroptions,boost::shared_ptr< vector<dReal> >());
119  }
120  ikreturn->Clear();
121  boost::shared_ptr< vector<dReal> > psolution(&ikreturn->_vsolution, utils::null_deleter());
122  if( !Solve(param,q0,vFreeParameters,filteroptions,psolution) ) {
123  ikreturn->_action = IKRA_Reject;
124  return false;
125  }
126  ikreturn->_action = IKRA_Success;
127  return true;
128 }
129 
130 bool IkSolverBase::SolveAll(const IkParameterization& param, const std::vector<dReal>& vFreeParameters, int filteroptions, std::vector<IkReturnPtr>& ikreturns)
131 {
132  ikreturns.resize(0);
133  std::vector< std::vector<dReal> > vsolutions;
134  if( !SolveAll(param,vFreeParameters,filteroptions,vsolutions) ) {
135  return false;
136  }
137  ikreturns.resize(vsolutions.size());
138  for(size_t i = 0; i < ikreturns.size(); ++i) {
139  ikreturns[i].reset(new IkReturn(IKRA_Success));
140  ikreturns[i]->_vsolution = vsolutions[i];
141  ikreturns[i]->_action = IKRA_Success;
142  }
143  return vsolutions.size() > 0;
144 }
145 
147 {
149  std::list<UserDataWeakPtr>::iterator it;
150  FORIT(it, __listRegisteredFilters) {
151  CustomIkSolverFilterDataPtr pitdata = boost::dynamic_pointer_cast<CustomIkSolverFilterData>(it->lock());
152  if( !!pitdata && pdata->_priority > pitdata->_priority ) {
153  break;
154  }
155  }
156  pdata->_iterator = __listRegisteredFilters.insert(it,pdata);
157  return pdata;
158 }
159 
160 IkReturnAction IkSolverBase::_CallFilters(std::vector<dReal>& solution, RobotBase::ManipulatorPtr manipulator, const IkParameterization& param, IkReturnPtr filterreturn)
161 {
162  vector<dReal> vtestsolution,vtestsolution2;
164  RobotBasePtr robot = manipulator->GetRobot();
165  robot->GetConfigurationValues(vtestsolution);
166  for(size_t i = 0; i < manipulator->GetArmIndices().size(); ++i) {
167  int dofindex = manipulator->GetArmIndices()[i];
168  dReal fdiff = 0;
169  KinBody::JointPtr pjoint = robot->GetJointFromDOFIndex(dofindex);
170  fdiff = pjoint->SubtractValue(vtestsolution.at(dofindex), solution.at(i), dofindex-pjoint->GetDOFIndex());
171  if( fdiff > g_fEpsilonJointLimit ) {
172  throw OPENRAVE_EXCEPTION_FORMAT("_CallFilters on robot %s manip %s need to start with robot configuration set to the solution. manip dof %d (%f != %f)",manipulator->GetRobot()->GetName()%manipulator->GetName()%dofindex%vtestsolution.at(dofindex)%solution.at(i), ORE_InconsistentConstraints);
173  }
174  }
175  }
176 
177  FOREACHC(it,__listRegisteredFilters) {
178  CustomIkSolverFilterDataPtr pitdata = boost::dynamic_pointer_cast<CustomIkSolverFilterData>(it->lock());
179  if( !!pitdata) {
180  IkReturn ret = pitdata->_filterfn(solution,manipulator,param);
181  if( ret != IKRA_Success ) {
182  return ret._action; // just return the action
183  }
184  if( vtestsolution.size() > 0 ) {
185  // check that the robot is set to solution
186  vector<dReal> vtestsolution2;
187  manipulator->GetRobot()->GetConfigurationValues(vtestsolution2);
188  for(size_t i = 0; i < manipulator->GetArmIndices().size(); ++i) {
189  vtestsolution.at(manipulator->GetArmIndices()[i]) = solution.at(i);
190  }
191  for(size_t i = 0; i < vtestsolution.size(); ++i) {
192  if( RaveFabs(vtestsolution.at(i)-vtestsolution2.at(i)) > g_fEpsilonJointLimit ) {
193  throw OPENRAVE_EXCEPTION_FORMAT("one of the filters set on robot %s manip %s did not restore the robot configuraiton. config dof %d (%f -> %f)",manipulator->GetRobot()->GetName()%manipulator->GetName()%i%vtestsolution.at(i)%vtestsolution2.at(i), ORE_InconsistentConstraints);
194  }
195  }
196  }
197  if( !!filterreturn ) {
198  filterreturn->Append(ret);
199  }
200  }
201  }
202  if( !!filterreturn ) {
203  filterreturn->_action = IKRA_Success;
204  }
205  return IKRA_Success;
206 }
207 
208 }