17 #include "libopenrave.h"
23 bool bclashing =
false;
26 RAVELOG_WARN(
"IkReturn already has _userdata set, but overwriting anyway\n");
36 if( !
_mapdata.insert(*itr).second ) {
37 RAVELOG_WARN(str(boost::format(
"IkReturn _mapdata %s overwritten")%itr->first));
44 RAVELOG_WARN(
"IkReturn already has _vsolution set, but overwriting anyway\n");
67 iksolver->__listRegisteredFilters.erase(
_iterator);
87 return Solve(param,q0,filteroptions,boost::shared_ptr< vector<dReal> >());
91 if( !
Solve(param,q0,filteroptions,psolution) ) {
102 std::vector< std::vector<dReal> > vsolutions;
103 if( !
SolveAll(param,filteroptions,vsolutions) ) {
106 ikreturns.resize(vsolutions.size());
107 for(
size_t i = 0; i < ikreturns.size(); ++i) {
109 ikreturns[i]->_vsolution = vsolutions[i];
112 return vsolutions.size() > 0;
118 return Solve(param,q0,vFreeParameters,filteroptions,boost::shared_ptr< vector<dReal> >());
121 boost::shared_ptr< vector<dReal> > psolution(&ikreturn->_vsolution,
utils::null_deleter());
122 if( !
Solve(param,q0,vFreeParameters,filteroptions,psolution) ) {
133 std::vector< std::vector<dReal> > vsolutions;
134 if( !
SolveAll(param,vFreeParameters,filteroptions,vsolutions) ) {
137 ikreturns.resize(vsolutions.size());
138 for(
size_t i = 0; i < ikreturns.size(); ++i) {
140 ikreturns[i]->_vsolution = vsolutions[i];
143 return vsolutions.size() > 0;
149 std::list<UserDataWeakPtr>::iterator it;
150 FORIT(it, __listRegisteredFilters) {
152 if( !!pitdata && pdata->_priority > pitdata->_priority ) {
156 pdata->
_iterator = __listRegisteredFilters.insert(it,pdata);
162 vector<dReal> vtestsolution,vtestsolution2;
165 robot->GetConfigurationValues(vtestsolution);
166 for(
size_t i = 0; i < manipulator->GetArmIndices().size(); ++i) {
167 int dofindex = manipulator->GetArmIndices()[i];
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);
177 FOREACHC(it,__listRegisteredFilters) {
180 IkReturn ret = pitdata->_filterfn(solution,manipulator,param);
184 if( vtestsolution.size() > 0 ) {
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);
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);
197 if( !!filterreturn ) {
198 filterreturn->Append(ret);
202 if( !!filterreturn ) {