Menu
▾
▴
[Personalrobots-commit] SF.net SVN: personalrobots:[11242] pkg/trunk
|
From: <rdi...@us...> - 2009-02-17 06:08:57
|
Revision: 11242
http://personalrobots.svn.sourceforge.net/personalrobots/?rev=11242&view=rev
Author: rdiankov
Date: 2009-02-17 06:08:51 +0000 (Tue, 17 Feb 2009)
Log Message:
-----------
fixed openrave robot kinematics, ormanipulation plugin works again. Added auto-generation of inverse kinematics for pr2 using ikfast, openrave_robot_description now uses auto-generated ik
Modified Paths:
--------------
pkg/trunk/3rdparty/openrave/Makefile
pkg/trunk/openrave_planning/ormanipulation/octave/grasp_pr2_ricebox.mat
pkg/trunk/openrave_planning/orplugins/CMakeLists.txt
pkg/trunk/openrave_planning/orplugins/manifest.xml
pkg/trunk/openrave_planning/orrosplanning/CMakeLists.txt
pkg/trunk/openrave_planning/orrosplanning/manifest.xml
pkg/trunk/openrave_planning/orrosplanning/src/orrosplanningmain.cpp
pkg/trunk/robot_descriptions/openrave_robot_description/CMakeLists.txt
pkg/trunk/robot_descriptions/openrave_robot_description/robots/pr2full.robot.xml
pkg/trunk/robot_descriptions/openrave_robot_description/src/urdf2openrave.cpp
Added Paths:
-----------
pkg/trunk/robot_descriptions/openrave_robot_description/src/ikbase.h
pkg/trunk/robot_descriptions/openrave_robot_description/src/orrosikmain.cpp
pkg/trunk/robot_descriptions/openrave_robot_description/src/plugindefs.h
Removed Paths:
-------------
pkg/trunk/openrave_planning/orrosplanning/src/rosarmik.cpp
pkg/trunk/openrave_planning/orrosplanning/src/rosarmik.h
Modified: pkg/trunk/3rdparty/openrave/Makefile
===================================================================
--- pkg/trunk/3rdparty/openrave/Makefile 2009-02-17 05:43:25 UTC (rev 11241)
+++ pkg/trunk/3rdparty/openrave/Makefile 2009-02-17 06:08:51 UTC (rev 11242)
@@ -2,7 +2,7 @@
SVN_DIR = openrave_svn
# Should really specify a revision
-SVN_REVISION = -r 660
+SVN_REVISION = -r 686
SVN_URL = https://openrave.svn.sourceforge.net/svnroot/openrave
#SVN_PATCH = fini_patch.patch
include $(shell rospack find mk)/svn_checkout.mk
Modified: pkg/trunk/openrave_planning/ormanipulation/octave/grasp_pr2_ricebox.mat
===================================================================
(Binary files differ)
Modified: pkg/trunk/openrave_planning/orplugins/CMakeLists.txt
===================================================================
--- pkg/trunk/openrave_planning/orplugins/CMakeLists.txt 2009-02-17 05:43:25 UTC (rev 11241)
+++ pkg/trunk/openrave_planning/orplugins/CMakeLists.txt 2009-02-17 06:08:51 UTC (rev 11242)
@@ -3,11 +3,13 @@
rospack(orplugins)
find_ros_package(orrosplanning)
+find_ros_package(openrave_robot_description)
file(GLOB orrosplanning_files ${orrosplanning_PACKAGE_PATH}/lib/*)
+file(GLOB openrave_robot_description_files ${openrave_robot_description_PACKAGE_PATH}/lib/*)
set(openrave_plugins )
-foreach(it ${orrosplanning_files})
+foreach(it ${orrosplanning_files} ${openrave_robot_description_files})
get_filename_component(basename ${it} NAME)
add_custom_command(
OUTPUT ${CMAKE_SOURCE_DIR}/${basename}
Modified: pkg/trunk/openrave_planning/orplugins/manifest.xml
===================================================================
--- pkg/trunk/openrave_planning/orplugins/manifest.xml 2009-02-17 05:43:25 UTC (rev 11241)
+++ pkg/trunk/openrave_planning/orplugins/manifest.xml 2009-02-17 06:08:51 UTC (rev 11242)
@@ -5,4 +5,5 @@
<author>Rosen Diankov (rdi...@cs...)</author>
<license>BSD</license>
<depend package="orrosplanning"/>
+ <depend package="openrave_robot_description"/>
</package>
Modified: pkg/trunk/openrave_planning/orrosplanning/CMakeLists.txt
===================================================================
--- pkg/trunk/openrave_planning/orrosplanning/CMakeLists.txt 2009-02-17 05:43:25 UTC (rev 11241)
+++ pkg/trunk/openrave_planning/orrosplanning/CMakeLists.txt 2009-02-17 06:08:51 UTC (rev 11242)
@@ -5,5 +5,5 @@
rospack_add_boost_directories()
-rospack_add_library(orrosplanning src/orrosplanningmain.cpp src/rosarmik.cpp)
+rospack_add_library(orrosplanning src/orrosplanningmain.cpp)
rospack_link_boost(orrosplanning thread)
Modified: pkg/trunk/openrave_planning/orrosplanning/manifest.xml
===================================================================
--- pkg/trunk/openrave_planning/orrosplanning/manifest.xml 2009-02-17 05:43:25 UTC (rev 11241)
+++ pkg/trunk/openrave_planning/orrosplanning/manifest.xml 2009-02-17 06:08:51 UTC (rev 11242)
@@ -7,7 +7,6 @@
<depend package="roscpp"/>
<depend package="openrave"/>
<depend package="openrave_robot_description"/>
- <depend package="libKinematics"/>
<depend package="checkerboard_detector"/>
<depend package="robot_msgs"/>
<depend package="pr2_mechanism_controllers"/>
Modified: pkg/trunk/openrave_planning/orrosplanning/src/orrosplanningmain.cpp
===================================================================
--- pkg/trunk/openrave_planning/orrosplanning/src/orrosplanningmain.cpp 2009-02-17 05:43:25 UTC (rev 11241)
+++ pkg/trunk/openrave_planning/orrosplanning/src/orrosplanningmain.cpp 2009-02-17 06:08:51 UTC (rev 11242)
@@ -25,7 +25,6 @@
// \author Rosen Diankov
#include "plugindefs.h"
-#include "rosarmik.h"
#include "mocapsystem.h"
#include "objecttransformsystem.h"
#include "collisionmapsystem.h"
@@ -58,10 +57,6 @@
if( wcsicmp(name, L"ROSRobot") == 0 )
return new ROSRobotController(penv);
break;
- case PT_InverseKinematicsSolver:
- if( wcsicmp(name, L"ROSArmIK") == 0 )
- return new ROSArmIK(penv);
- break;
case PT_ProblemInstance:
if( wcsicmp(name, L"ROSPlanning") == 0 )
return new ROSPlanningProblem(penv);
@@ -90,7 +85,6 @@
}
pinfo->controllers.push_back(L"ROSRobot");
- pinfo->iksolvers.push_back(L"ROSArmIK");
pinfo->problems.push_back(L"ROSPlanning");
pinfo->sensorsystems.push_back(L"ROSMocap");
pinfo->sensorsystems.push_back(L"ObjectTransform");
Deleted: pkg/trunk/openrave_planning/orrosplanning/src/rosarmik.cpp
===================================================================
--- pkg/trunk/openrave_planning/orrosplanning/src/rosarmik.cpp 2009-02-17 05:43:25 UTC (rev 11241)
+++ pkg/trunk/openrave_planning/orrosplanning/src/rosarmik.cpp 2009-02-17 06:08:51 UTC (rev 11242)
@@ -1,501 +0,0 @@
-// Software License Agreement (BSD License)
-// Copyright (c) 2008, Rosen Diankov
-// Redistribution and use in source and binary forms, with or without
-// modification, are permitted provided that the following conditions are met:
-// * Redistributions of source code must retain the above copyright notice,
-// this list of conditions and the following disclaimer.
-// * Redistributions in binary form must reproduce the above copyright
-// notice, this list of conditions and the following disclaimer in the
-// documentation and/or other materials provided with the distribution.
-// * The name of the author may not be used to endorse or promote products
-// derived from this software without specific prior written permission.
-//
-// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
-// AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
-// IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
-// ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE
-// LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
-// CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
-// SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
-// INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
-// CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
-// ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
-// POSSIBILITY OF SUCH DAMAGE.
-//
-// author: Rosen Diankov
-#include "plugindefs.h"
-
-#include "rosarmik.h"
-
-#ifndef SQR
-template <class T>
-inline T SQR(T x) { return x * x; }
-#endif
-
-ROSArmIK::ROSArmIK(EnvironmentBase* penv) : IkSolverBase(penv)
-{
-}
-
-bool ROSArmIK::Init(RobotBase* probot, const RobotBase::Manipulator* pmanip, int options)
-{
- assert( probot != NULL );
- _probot = probot;
- if( _probot == NULL || pmanip == NULL )
- return false;
-
- // get the joint limits
- const vector<int>& vjoints = pmanip->_vecarmjoints;
-
- vector<dReal> qlower, qupper;
- _probot->GetJointLimits(qlower, qupper);
- _qlower.resize(vjoints.size()); _qupper.resize(vjoints.size());
-
- for(int i = 0; i < (int)vjoints.size(); ++i) {
- _qlower[i] = qlower[vjoints[i]];
- _qupper[i] = qupper[vjoints[i]];
- }
-
- if( _qlower.size() > 0 )
- fiFreeParam = 1.0f / (_qupper[2]-_qlower[2]);
- else fiFreeParam = 1;
-
- std::vector<NEWMAT::Matrix> axis;
- std::vector<NEWMAT::Matrix> anchor;
- std::vector<std::string> joint_type;
-
- NEWMAT::Matrix aj(3,1);
- NEWMAT::Matrix an(3,1);
-
- joint_type.resize(7);
-
- // Shoulder pan
- aj << 0 << 0 << 1.0;
- axis.push_back(aj);
- an << 0 << 0 << 0;
- anchor.push_back(an);
- _vjointmult.push_back(1);
-
- // Shoulder pitch
- aj << 0 << 1 << 0;
- axis.push_back(aj);
- an << 0.1 << 0 << 0;
- anchor.push_back(an);
- _vjointmult.push_back(1);
-
- // Shoulder roll
- aj << 1 << 0 << 0;
- axis.push_back(aj);
- an << 0.1 << 0 << 0;
- anchor.push_back(an);
- _vjointmult.push_back(1);
-
- // Elbow flex
- aj << 0 << 1 << 0;
- axis.push_back(aj);
- an << 0.5 << 0 << 0;
- anchor.push_back(an);
- _vjointmult.push_back(1);
-
- // Forearm roll
- aj << 1 << 0 << 0;
- axis.push_back(aj);
- an << 0.5 << 0 << 0;
- anchor.push_back(an);
- _vjointmult.push_back(-1);
-
- // Wrist flex
- aj << 0 << 1 << 0;
- axis.push_back(aj);
- an << 0.82025 << 0 << 0;
- anchor.push_back(an);
- _vjointmult.push_back(-1);
-
- // Gripper roll
- aj << 1 << 0 << 0;
- axis.push_back(aj);
- an << 0.82025 << 0 << 0;
- anchor.push_back(an);
- _vjointmult.push_back(1);
-
- for(int i=0; i < 7; i++)
- joint_type[i] = std::string("ROTARY");
-
- iksolver.reset(new kinematics::arm7DOF(anchor,axis,joint_type));
-
- if( pmanip->_vecarmjoints.size() > 0 ) {
- RobotBase::RobotStateSaver saver(_probot);
- _probot->SetTransform(Transform());
- vector<dReal> vjoints(_probot->GetDOF(),0);
- _probot->SetJointValues(NULL,NULL,&vjoints[0]);
- Transform tbase = pmanip->pBase->GetTransform();
- KinBody::Joint* pjoint = _probot->GetJoint(pmanip->_vecarmjoints.front());
- KinBody::Link* pother = pjoint->GetFirstAttached() == pmanip->pBase ? pjoint->GetSecondAttached() : pjoint->GetFirstAttached();
- if( pother != NULL )
- voffset = tbase.trans - pother->GetTransform().trans;
- }
-
- return true;
-}
-
-// end eff transform is the transform of the wrist with respect to the base arm link
-bool ROSArmIK::Solve(const Transform &_T, const dReal* q0, bool bCheckEnvCollision, dReal* qResult)
-{
- assert( _probot != NULL );
-
- const RobotBase::Manipulator* pmanip = _probot->GetActiveManipulator();
- assert( pmanip != NULL );
-
- // the world coordinate system is at the origin of the intersection of the first 3 joint axes
- assert( pmanip->_vecarmjoints.size() == _qlower.size() );
-
- RobotBase::RobotStateSaver saver(_probot);
-
- _probot->SetActiveDOFs(pmanip->_vecarmjoints);
-
- // start searching for phi close to q0, as soon as a solution is found for the curphi, return it
- dReal startphi = q0 != NULL ? q0[2] : 0;
- dReal upperphi = _qupper[2], lowerphi = _qlower[2], deltaphi = 0;
- int iter = 0;
- bool bsuccess = false;
-
- dReal bestdist = 1000; // only valid if q0 != NULL
- NEWMAT::Matrix nmT = GetNewMat(_T);
-
- while(1) {
-
- dReal curphi = startphi;
- if( iter & 1 ) { // increment
- curphi += deltaphi;
- if( curphi > upperphi ) {
-
- if( startphi-deltaphi < lowerphi)
- break; // reached limit
- ++iter;
- continue;
- }
- }
- else { // decrement
- curphi -= deltaphi;
- if( curphi < lowerphi ) {
-
- if( startphi+deltaphi > upperphi )
- break; // reached limit
- deltaphi += GetPhiInc(); // increment
- ++iter;
- continue;
- }
-
- deltaphi += GetPhiInc(); // increment
- }
-
- iter++;
-
- iksolver->ComputeIKEfficientTheta3(nmT,curphi);
- vector<dReal> vravesol(_probot->GetActiveDOF());
- vector<dReal> vbest;
- FOREACH(itsol, iksolver->solution_ik_) {
- vector<double>& sol = *itsol;
- assert( (int)sol.size() == _probot->GetActiveDOF());
-
- for(int i = 0; i < (int)sol.size(); ++i)
- vravesol[i] = sol[i]*_vjointmult[i];
-
- // find the first valid solution that satisfies joint constraints and collisions
- if( !checkjointangles(vravesol) )
- continue;
-
- // check for self collisions
- _probot->SetActiveDOFValues(NULL, &vravesol[0]);
-
- if( _probot->CheckSelfCollision() )
- continue;
-
- COLLISIONREPORT report;
- if( bCheckEnvCollision && GetEnv()->CheckCollision(_probot, &report) ) {
- if( report.plink1 != NULL && report.plink2 != NULL ) {
- RAVELOG_VERBOSEA("WAMIK: collision %S:%S with %S:%S\n", report.plink1->GetParent()->GetName(), report.plink1->GetName(), report.plink2->GetParent()->GetName(), report.plink2->GetName());
- }
- continue;
- }
-
- // solution is valid, check with q0
- if( q0 != NULL ) {
-
- dReal d = 0;
- for(int k = 0; k < (int)pmanip->_vecarmjoints.size(); ++k)
- d += SQR(vravesol[k]-q0[k]);
-
- if( bestdist > d ) {
- vbest = vravesol;
- bestdist = d;
- }
- }
- else {
- vbest = vravesol;
- break;
- }
- }
-
- // return as soon as a solution is found, since we're visiting phis starting from q0[0], we are guaranteed
- // that the solution will be close (ie, phi's dominate in the search). This is to speed things up
- if( vbest.size() == _qlower.size() && qResult != NULL ) {
- memcpy(&qResult[0], &vbest[0], sizeof(dReal)*_qlower.size());
- bsuccess = true;
- break;
- }
- }
-
- return bsuccess;
-}
-
-bool ROSArmIK::Solve(const Transform &_T, bool bCheckEnvCollision, std::vector< std::vector<dReal> >& qSolutions)
-{
- assert( _probot != NULL );
- const RobotBase::Manipulator* pmanip = _probot->GetActiveManipulator();
-
- assert( pmanip != NULL );
-
- qSolutions.resize(0);
-
- // the world coordinate system is at the origin of the intersection of the first 3 joint axes
- assert( pmanip->_vecarmjoints.size() && _qlower.size() );
-
- RobotBase::RobotStateSaver saver(_probot);
-
- _probot->SetActiveDOFs(pmanip->_vecarmjoints);
-
- // start searching for phi close to q0, as soon as a solution is found for the curphi, return it
- double startphi = 0;
- double upperphi = _qupper[2], lowerphi = _qlower[2], deltaphi = 0;
- int iter = 0;
-
- NEWMAT::Matrix nmT = GetNewMat(_T);
-
- while(1) {
- double curphi = startphi;
- if( iter & 1 ) { // increment
- curphi += deltaphi;
- if( curphi > upperphi ) {
-
- if( startphi-deltaphi < lowerphi)
- break; // reached limit
- ++iter;
- continue;
- }
- }
- else { // decrement
- curphi -= deltaphi;
- if( curphi < lowerphi ) {
-
- if( startphi+deltaphi > upperphi )
- break; // reached limit
- ++iter;
- deltaphi += GetPhiInc(); // increment
- continue;
- }
-
- deltaphi += GetPhiInc(); // increment
- }
-
- iter++;
-
- iksolver->ComputeIKEfficientTheta3(nmT,curphi);
- vector<dReal> vravesol(_probot->GetActiveDOF());
-
- FOREACH(itsol, iksolver->solution_ik_) {
- vector<double>& sol = *itsol;
- assert( (int)sol.size() == _probot->GetActiveDOF());
-
- for(int i = 0; i < (int)sol.size(); ++i)
- vravesol[i] = sol[i]*_vjointmult[i];
-
- // find the first valid solutino that satisfies joint constraints and collisions
- if( !checkjointangles(vravesol) )
- continue;
-
- // check for self collisions
- _probot->SetActiveDOFValues(NULL, &vravesol[0]);
-
- if( _probot->CheckSelfCollision() )
- continue;
-
- if( bCheckEnvCollision && GetEnv()->CheckCollision(_probot) )
- continue;
-
- qSolutions.push_back(vravesol);
- }
- }
-
- return qSolutions.size()>0;
-}
-
-bool ROSArmIK::Solve(const Transform &_T, const dReal* q0, const dReal* pFreeParameters,
- bool bCheckEnvCollision, dReal* qResult)
-{
- if( pFreeParameters == NULL )
- return Solve(_T, q0, bCheckEnvCollision, qResult);
-
- assert( _probot != NULL );
-
- const RobotBase::Manipulator* pmanip = _probot->GetActiveManipulator();
- assert( pmanip != NULL );
-
- // the world coordinate system is at the origin of the intersection of the first 3 joint axes
- assert( pmanip->_vecarmjoints.size() == _qlower.size() );
-
- RobotBase::RobotStateSaver saver(_probot);
-
- _probot->SetActiveDOFs(pmanip->_vecarmjoints);
-
- // start searching for phi close to q0, as soon as a solution is found for the curphi, return it
- dReal bestdist = 1000; // only valid if q0 != NULL
-
- NEWMAT::Matrix nmT = GetNewMat(_T);
- iksolver->ComputeIKEfficientTheta3(nmT,_qlower[2] + (_qupper[2]-_qlower[2])*pFreeParameters[0]);
-
- vector<dReal> vravesol(_probot->GetActiveDOF());
- vector<dReal> vbest;
- FOREACH(itsol, iksolver->solution_ik_) {
- vector<double>& sol = *itsol;
- assert( (int)sol.size() == _probot->GetActiveDOF());
-
- for(int i = 0; i < (int)sol.size(); ++i)
- vravesol[i] = sol[i] * _vjointmult[i];
-
- // find the first valid solutino that satisfies joint constraints and collisions
- if( !checkjointangles(vravesol) )
- continue;
-
- // check for self collisions (does WAM ever self-collide?)
- _probot->SetActiveDOFValues(NULL, &vravesol[0]);
-
- if( _probot->CheckSelfCollision() )
- continue;
-
- COLLISIONREPORT report;
- if( bCheckEnvCollision && GetEnv()->CheckCollision(_probot, &report) ) {
- if( report.plink1 != NULL && report.plink2 != NULL ) {
- RAVELOG_VERBOSEA("WAMIK: collision %S:%S with %S:%S\n", report.plink1->GetParent()->GetName(), report.plink1->GetName(), report.plink2->GetParent()->GetName(), report.plink2->GetName());
- }
- continue;
- }
-
- // solution is valid, check with q0
- if( q0 != NULL ) {
-
- dReal d = 0;
-
- for(int k = 0; k < (int)pmanip->_vecarmjoints.size(); ++k)
- d += SQR(vravesol[k]-q0[k]);
-
- if( bestdist > d ) {
- vbest = vravesol;
- bestdist = d;
- }
- }
- else {
- vbest = vravesol;
- break;
- }
- }
-
- // return as soon as a solution is found, since we're visiting phis starting from q0[0], we are guaranteed
- // that the solution will be close (ie, phi's dominate in the search). This is to speed things up
- if( vbest.size() == _qlower.size() && qResult != NULL ) {
- memcpy(&qResult[0], &vbest[0], sizeof(dReal)*_qlower.size());
- return true;
- }
-
- return false;
-}
-
-bool ROSArmIK::Solve(const Transform &_T, const dReal* pFreeParameters,
- bool bCheckEnvCollision, std::vector< std::vector<dReal> >& qSolutions)
-{
- if( pFreeParameters == NULL )
- return Solve(_T, bCheckEnvCollision, qSolutions);
-
- assert( _probot != NULL );
- const RobotBase::Manipulator* pmanip = _probot->GetActiveManipulator();
-
- assert( pmanip != NULL );
-
- qSolutions.resize(0);
-
- // the world coordinate system is at the origin of the intersection of the first 3 joint axes
- assert( pmanip->_vecarmjoints.size() && _qlower.size() );
-
- RobotBase::RobotStateSaver saver(_probot);
-
- _probot->SetActiveDOFs(pmanip->_vecarmjoints);
- vector<dReal> vravesol(_probot->GetActiveDOF());
-
- // start searching for phi close to q0, as soon as a solution is found for the curphi, return it
- NEWMAT::Matrix nmT = GetNewMat(_T);
- iksolver->ComputeIKEfficientTheta3(nmT,_qlower[2] + (_qupper[2]-_qlower[2])*pFreeParameters[0]);
-
- FOREACH(itsol, iksolver->solution_ik_) {
- vector<double>& sol = *itsol;
- assert( (int)sol.size() == _probot->GetActiveDOF());
-
- for(int i = 0; i < (int)sol.size(); ++i)
- vravesol[i] = sol[i]*_vjointmult[i];
-
- // find the first valid solutino that satisfies joint constraints and collisions
- if( !checkjointangles(vravesol) )
- continue;
-
- // check for self collisions
- _probot->SetActiveDOFValues(NULL, &vravesol[0]);
-
- if( _probot->CheckSelfCollision() )
- continue;
-
- if( bCheckEnvCollision && GetEnv()->CheckCollision(_probot) )
- continue;
-
- qSolutions.push_back(vravesol);
- }
-
- return qSolutions.size()>0;
-}
-
-bool ROSArmIK::GetFreeParameters(dReal* pFreeParameters) const
-{
- assert( _probot != NULL && pFreeParameters != NULL );
-
- const RobotBase::Manipulator* pmanip = _probot->GetActiveManipulator();
- if( pmanip == NULL )
- return false;
- assert( _qlower.size() > 2 && _qupper.size() > 2 );
- assert( pmanip->_vecarmjoints.size() > 0 && pmanip->_vecarmjoints[2] < _probot->GetDOF());
-
- dReal values[3];
- _probot->GetJointFromDOFIndex(pmanip->_vecarmjoints[2])->GetValues(values);
- pFreeParameters[0] = (values[0]-_qlower[2])*fiFreeParam;
- return true;
-}
-
-bool ROSArmIK::checkjointangles(vector<dReal>& vravesol)
-{
- for(int j = 0; j < (int)_qlower.size(); ++j) {
- if( _qlower[j] < -PI && vravesol[j] > _qupper[j] )
- vravesol[j] -= 2*PI;
- if( _qupper[j] > PI && vravesol[j] < _qlower[j] )
- vravesol[j] += 2*PI;
- if( vravesol[j] < _qlower[j] || vravesol[j] > _qupper[j] )
- return false;
- }
-
- return true;
-}
-
-NEWMAT::Matrix ROSArmIK::GetNewMat(const TransformMatrix& tm)
-{
- NEWMAT::Matrix nmT(4,4);
- nmT(1,1) = tm.m[0]; nmT(1,2) = tm.m[1]; nmT(1,3) = tm.m[2]; nmT(1,4) = tm.trans.x+voffset.x;
- nmT(2,1) = tm.m[4]; nmT(2,2) = tm.m[5]; nmT(2,3) = tm.m[6]; nmT(2,4) = tm.trans.y+voffset.y;
- nmT(3,1) = tm.m[8]; nmT(3,2) = tm.m[9]; nmT(3,3) = tm.m[10]; nmT(3,4) = tm.trans.z+voffset.z;
- nmT(4,1) = 0; nmT(4,2) = 0; nmT(4,3) = 0; nmT(4,4) = 1;
- return nmT;
-}
Deleted: pkg/trunk/openrave_planning/orrosplanning/src/rosarmik.h
===================================================================
--- pkg/trunk/openrave_planning/orrosplanning/src/rosarmik.h 2009-02-17 05:43:25 UTC (rev 11241)
+++ pkg/trunk/openrave_planning/orrosplanning/src/rosarmik.h 2009-02-17 06:08:51 UTC (rev 11242)
@@ -1,64 +0,0 @@
-// Software License Agreement (BSD License)
-// Copyright (c) 2008, Rosen Diankov
-// Redistribution and use in source and binary forms, with or without
-// modification, are permitted provided that the following conditions are met:
-// * Redistributions of source code must retain the above copyright notice,
-// this list of conditions and the following disclaimer.
-// * Redistributions in binary form must reproduce the above copyright
-// notice, this list of conditions and the following disclaimer in the
-// documentation and/or other materials provided with the distribution.
-// * The name of the author may not be used to endorse or promote products
-// derived from this software without specific prior written permission.
-//
-// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
-// AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
-// IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
-// ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE
-// LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
-// CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
-// SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
-// INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
-// CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
-// ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
-// POSSIBILITY OF SUCH DAMAGE.
-//
-// author: Rosen Diankov
-#ifndef OPENRAVE_ROSARMIK_H
-#define OPENRAVE_ROSARMIK_H
-
-#include <libKinematics/pr2_ik.h>
-
-class ROSArmIK : public IkSolverBase
-{
-public:
- ROSArmIK(EnvironmentBase* penv);
-
- virtual bool Init(RobotBase* probot, const RobotBase::Manipulator* pmanip, int options);
-
- virtual bool Solve(const Transform &transEE, const dReal* q0, bool bCheckEnvCollision, dReal* qResult);
- virtual bool Solve(const Transform &transEE, bool bCheckEnvCollision, std::vector< std::vector<dReal> >& qSolutions);
-
- virtual bool Solve(const Transform &endEffTransform, const dReal* q0, const dReal* pFreeParameters,
- bool bCheckEnvCollision, dReal* qResult);
- virtual bool Solve(const Transform &endEffTransform, const dReal* pFreeParameters,
- bool bCheckEnvCollision, std::vector< std::vector<dReal> >& qSolutions);
-
- virtual int GetNumFreeParameters() const { return 1; }
- virtual bool GetFreeParameters(dReal* pFreeParameters) const;
- virtual RobotBase* GetRobot() const { return _probot; }
-
-private:
-
- NEWMAT::Matrix GetNewMat(const TransformMatrix& tm);
-
- inline dReal GetPhiInc() { return 0.03f; }
- bool checkjointangles(vector<dReal>& vravesol);
-
- RobotBase* _probot;
- std::vector<dReal> _qlower, _qupper, _vjointmult;
- Vector voffset;
- dReal fiFreeParam;
- boost::shared_ptr<kinematics::arm7DOF> iksolver;
-};
-
-#endif
Modified: pkg/trunk/robot_descriptions/openrave_robot_description/CMakeLists.txt
===================================================================
--- pkg/trunk/robot_descriptions/openrave_robot_description/CMakeLists.txt 2009-02-17 05:43:25 UTC (rev 11241)
+++ pkg/trunk/robot_descriptions/openrave_robot_description/CMakeLists.txt 2009-02-17 06:08:51 UTC (rev 11242)
@@ -42,6 +42,32 @@
add_custom_target(urdf_robots ALL DEPENDS ${robot_files})
add_dependencies(urdf_robots urdf2openrave)
+set(PR2_ROBOT "${CMAKE_SOURCE_DIR}/robots/pr2full.robot.xml")
+set(PR2_IK_LEFT_OUTPUT "${CMAKE_CURRENT_SOURCE_DIR}/src/ik_pr2left.h")
+add_custom_command(
+ OUTPUT ${PR2_IK_LEFT_OUTPUT}
+ # need -d 0 to suppress spurious warnings
+ COMMAND "${openrave_PACKAGE_PATH}/bin/openrave" -d 0 --generateik robot "${PR2_ROBOT}" freeparam l_shoulder_pan_joint manipulator 0 output "${PR2_IK_LEFT_OUTPUT}"
+ DEPENDS ${PR2_ROBOT}
+ )
+add_custom_target(pr2leftik ALL DEPENDS ${PR2_IK_LEFT_OUTPUT})
+add_dependencies(pr2leftik urdf_robots)
+
+set(PR2_IK_RIGHT_OUTPUT "${CMAKE_CURRENT_SOURCE_DIR}/src/ik_pr2right.h")
+add_custom_command(
+ OUTPUT ${PR2_IK_RIGHT_OUTPUT}
+ # need -d 0 to suppress spurious warnings
+ COMMAND "${openrave_PACKAGE_PATH}/bin/openrave" -d 0 --generateik robot "${PR2_ROBOT}" freeparam r_shoulder_pan_joint manipulator 1 output "${PR2_IK_RIGHT_OUTPUT}"
+ DEPENDS ${PR2_ROBOT}
+ )
+add_custom_target(pr2rightik ALL DEPENDS ${PR2_IK_RIGHT_OUTPUT})
+add_dependencies(pr2rightik urdf_robots)
+
+# add the plugin for the inverse kinematics
+rospack_add_library(orrosik src/orrosikmain.cpp)
+add_dependencies(orrosik pr2leftik pr2rightik)
+#rospack_link_boost(orrosik thread)
+
# add the test package
rospack_add_gtest(test_or_robots test/test_robots.cpp)
target_link_libraries (test_or_robots openrave-core)
Modified: pkg/trunk/robot_descriptions/openrave_robot_description/robots/pr2full.robot.xml
===================================================================
--- pkg/trunk/robot_descriptions/openrave_robot_description/robots/pr2full.robot.xml 2009-02-17 05:43:25 UTC (rev 11241)
+++ pkg/trunk/robot_descriptions/openrave_robot_description/robots/pr2full.robot.xml 2009-02-17 06:08:51 UTC (rev 11242)
@@ -6,10 +6,10 @@
<base>torso_lift_link</base>
<effector>l_gripper_palm_link</effector>
<armjoints>l_shoulder_pan_joint l_shoulder_lift_joint l_upper_arm_roll_joint l_elbow_flex_joint l_forearm_roll_joint l_wrist_flex_joint l_wrist_roll_joint</armjoints>
- <joints>l_gripper_palm_joint l_gripper_l_finger_joint</joints>
+ <joints>l_gripper_palm_joint l_gripper_l_finger_joint l_gripper_tool_joint</joints>
<closed>0 0</closed>
<opened>0 0.8</opened>
- <iksolver>rosarmik</iksolver>
+ <iksolver>PR2Leftikfast</iksolver>
</Manipulator>
<!-- right arm -->
@@ -18,10 +18,10 @@
<base>torso_lift_link</base>
<effector>r_gripper_palm_link</effector>
<armjoints>r_shoulder_pan_joint r_shoulder_lift_joint r_upper_arm_roll_joint r_elbow_flex_joint r_forearm_roll_joint r_wrist_flex_joint r_wrist_roll_joint</armjoints>
- <joints>r_gripper_palm_joint r_gripper_l_finger_joint</joints>
+ <joints>r_gripper_palm_joint r_gripper_l_finger_joint r_gripper_tool_joint</joints>
<closed>0 0</closed>
<opened>0 0.8</opened>
- <iksolver>rosarmik</iksolver>
+ <iksolver>PR2Rightikfast</iksolver>
</Manipulator>
<!-- head -->
Added: pkg/trunk/robot_descriptions/openrave_robot_description/src/ikbase.h
===================================================================
--- pkg/trunk/robot_descriptions/openrave_robot_description/src/ikbase.h (rev 0)
+++ pkg/trunk/robot_descriptions/openrave_robot_description/src/ikbase.h 2009-02-17 06:08:51 UTC (rev 11242)
@@ -0,0 +1,383 @@
+/*********************************************************************
+ * Software License Agreement (BSD License)
+ *
+ * Copyright (c) 2009, Rosen Diankov
+ * All rights reserved.
+ *
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions
+ * are met:
+ *
+ * * Redistributions of source code must retain the above copyright
+ * notice, this list of conditions and the following disclaimer.
+ * * Redistributions in binary form must reproduce the above
+ * copyright notice, this list of conditions and the following
+ * disclaimer in the documentation and/or other materials provided
+ * with the distribution.
+ * * Neither the name of the Willow Garage nor the names of its
+ * contributors may be used to endorse or promote products derived
+ * from this software without specific prior written permission.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+ * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+ * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+ * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+ * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+ * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+ * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
+ * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
+ * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+ * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+ * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+ * POSSIBILITY OF SUCH DAMAGE.
+ *********************************************************************/
+#ifndef IKFASTSOLVERBASE_H
+#define IKFASTSOLVERBASE_H
+
+#include <boost/bind.hpp>
+
+template <typename IKReal, typename Solution>
+class IkFastSolver : public IkSolverBase
+{
+public:
+ typedef bool (*IkFn)(const IKReal* eetrans, const IKReal* eerot, const IKReal* pfree, std::vector<Solution>& vsolutions);
+
+ IkFastSolver(IkFn pfnik, const std::vector<int>& vfreeparams, dReal fFreeInc, int nTotalJoints, EnvironmentBase* penv) : IkSolverBase(penv), _probot(NULL), _vfreeparams(vfreeparams), _pfnik(pfnik), _fFreeInc(fFreeInc)
+ {
+ _varmjoints.resize(nTotalJoints);
+ }
+
+ virtual bool Init(RobotBase* probot, const RobotBase::Manipulator* pmanip, int options)
+ {
+ assert( probot != NULL );
+ _probot = probot;
+ if( _probot == NULL || pmanip == NULL )
+ return false;
+
+ if( _varmjoints.size() != pmanip->_vecarmjoints.size() ) {
+ RAVELOG_ERRORA("ik %s configured with different number of joints than robot manipulator (%"PRIdS"!=%"PRIdS")\n", GetXMLId(), _varmjoints.size(), pmanip->_vecarmjoints.size());
+ return false;
+ }
+
+ _varmjoints = pmanip->_vecarmjoints;
+
+ // get the joint limits
+ std::vector<dReal> qlower, qupper;
+ _probot->GetJointLimits(qlower, qupper);
+ _qlower.resize(_varmjoints.size()); _qupper.resize(_varmjoints.size());
+
+ for(int i = 0; i < (int)_varmjoints.size(); ++i) {
+ _qlower[i] = qlower[_varmjoints[i]];
+ _qupper[i] = qupper[_varmjoints[i]];
+ }
+
+ _vfreeparamscales.clear();
+ FOREACH(itfree, _vfreeparams) {
+ if( *itfree < 0 || *itfree >= (int)_qlower.size() ) {
+ RAVELOG_ERRORA("free parameter idx %d out of bounds\n", *itfree);
+ return false;
+ }
+
+ if( _qupper[*itfree] > _qlower[*itfree] )
+ _vfreeparamscales.push_back(1.0f/(_qupper[*itfree]-_qlower[*itfree]));
+ else
+ _vfreeparamscales.push_back(0.0f);
+ }
+
+ return true;
+ }
+
+ virtual bool Solve(const Transform &transEE, const dReal* q0, bool bCheckEnvCollision, dReal* qResult)
+ {
+ assert( _probot != NULL );
+
+ RobotBase::RobotStateSaver saver(_probot);
+ _probot->SetActiveDOFs(_varmjoints);
+
+ std::vector<IKReal> vfree(_vfreeparams.size());
+
+ TransformMatrix t = transEE;
+ IKReal eetrans[3] = {t.trans.x, t.trans.y, t.trans.z};
+ IKReal eerot[9] = {t.m[0],t.m[1],t.m[2],t.m[4],t.m[5],t.m[6],t.m[8],t.m[9],t.m[10]};
+
+ return ComposeSolution(_vfreeparams, &vfree[0], 0, q0, boost::bind(&IkFastSolver::_SolveSingle,this, eerot,eetrans,&vfree[0],q0,bCheckEnvCollision,qResult));
+ }
+
+ virtual bool Solve(const Transform &transEE, bool bCheckEnvCollision, std::vector< std::vector<dReal> >& qSolutions)
+ {
+ assert( _probot != NULL );
+
+ RobotBase::RobotStateSaver saver(_probot);
+ _probot->SetActiveDOFs(_varmjoints);
+
+ std::vector<IKReal> vfree(_vfreeparams.size());
+
+ TransformMatrix t = transEE;
+ IKReal eetrans[3] = {t.trans.x, t.trans.y, t.trans.z};
+ IKReal eerot[9] = {t.m[0],t.m[1],t.m[2],t.m[4],t.m[5],t.m[6],t.m[8],t.m[9],t.m[10]};
+
+ qSolutions.resize(0);
+ ComposeSolution(_vfreeparams, &vfree[0], 0, NULL, boost::bind(&IkFastSolver::_SolveAll,this, eerot,eetrans,&vfree[0],bCheckEnvCollision,boost::ref(qSolutions)));
+
+ return qSolutions.size()>0;
+ }
+
+ virtual bool Solve(const Transform &transEE, const dReal* q0, const dReal* pFreeParameters, bool bCheckEnvCollision, dReal* qResult)
+ {
+ if( pFreeParameters == NULL )
+ return Solve(transEE,q0,bCheckEnvCollision,qResult);
+
+ assert( _probot != NULL );
+
+ RobotBase::RobotStateSaver saver(_probot);
+ _probot->SetActiveDOFs(_varmjoints);
+
+ std::vector<IKReal> vfree(_vfreeparams.size());
+ for(size_t i = 0; i < _vfreeparams.size(); ++i)
+ vfree[i] = pFreeParameters[i]*(_qupper[_vfreeparams[i]]-_qlower[_vfreeparams[i]]) + _qlower[_vfreeparams[i]];
+
+ TransformMatrix t = transEE;
+ IKReal eetrans[3] = {t.trans.x, t.trans.y, t.trans.z};
+ IKReal eerot[9] = {t.m[0],t.m[1],t.m[2],t.m[4],t.m[5],t.m[6],t.m[8],t.m[9],t.m[10]};
+
+ return _SolveSingle(eerot,eetrans,&vfree[0],q0,bCheckEnvCollision,qResult);
+ }
+
+ virtual bool Solve(const Transform &transEE, const dReal* pFreeParameters, bool bCheckEnvCollision, std::vector< std::vector<dReal> >& qSolutions)
+ {
+ if( pFreeParameters == NULL )
+ return Solve(transEE,bCheckEnvCollision,qSolutions);
+
+ assert( _probot != NULL );
+
+ RobotBase::RobotStateSaver saver(_probot);
+ _probot->SetActiveDOFs(_varmjoints);
+
+ std::vector<IKReal> vfree(_vfreeparams.size());
+ for(size_t i = 0; i < _vfreeparams.size(); ++i)
+ vfree[i] = pFreeParameters[i]*(_qupper[_vfreeparams[i]]-_qlower[_vfreeparams[i]]) + _qlower[_vfreeparams[i]];
+
+ TransformMatrix t = transEE;
+ IKReal eetrans[3] = {t.trans.x, t.trans.y, t.trans.z};
+ IKReal eerot[9] = {t.m[0],t.m[1],t.m[2],t.m[4],t.m[5],t.m[6],t.m[8],t.m[9],t.m[10]};
+
+ qSolutions.resize(0);
+ _SolveAll(eerot,eetrans,&vfree[0],bCheckEnvCollision,qSolutions);
+
+ return qSolutions.size()>0;
+ }
+
+ virtual int GetNumFreeParameters() const { return (int)_vfreeparams.size(); }
+ virtual bool GetFreeParameters(dReal* pFreeParameters) const
+ {
+ assert( _probot != NULL && pFreeParameters != NULL );
+
+ std::vector<dReal> values;
+ std::vector<dReal>::const_iterator itscale = _vfreeparamscales.begin();
+ _probot->GetJointValues(values);
+ FOREACHC(itfree, _vfreeparams)
+ *pFreeParameters++ = (values[_varmjoints[*itfree]]-_qlower[*itfree]) * *itscale++;
+
+ return true;
+ }
+ virtual RobotBase* GetRobot() const { return _probot; }
+
+private:
+ template <class SolveFn>
+ bool ComposeSolution(const std::vector<int>& vfreeparams, IKReal* pfree, int freeindex, const dReal* q0, const SolveFn& fn)
+ {
+ if( freeindex >= (int)vfreeparams.size())
+ return fn();
+
+ // start searching for phi close to q0, as soon as a solution is found for the curphi, return it
+ dReal startphi = q0 != NULL ? q0[vfreeparams[freeindex]] : 0;
+ dReal upperphi = _qupper[vfreeparams[freeindex]], lowerphi = _qlower[vfreeparams[freeindex]], deltaphi = 0;
+ int iter = 0;
+
+ while(1) {
+
+ dReal curphi = startphi;
+ if( iter & 1 ) { // increment
+ curphi += deltaphi;
+ if( curphi > upperphi ) {
+
+ if( startphi-deltaphi < lowerphi)
+ break; // reached limit
+ ++iter;
+ continue;
+ }
+ }
+ else { // decrement
+ curphi -= deltaphi;
+ if( curphi < lowerphi ) {
+
+ if( startphi+deltaphi > upperphi )
+ break; // reached limit
+ deltaphi += _fFreeInc; // increment
+ ++iter;
+ continue;
+ }
+
+ deltaphi += _fFreeInc; // increment
+ }
+
+ iter++;
+
+ pfree[freeindex] = curphi;
+ if( ComposeSolution(_vfreeparams, pfree, freeindex+1,q0, fn) )
+ return true;
+ }
+
+ return false;
+ }
+
+ bool _SolveSingle(const IKReal* eerot, const IKReal* eetrans, const IKReal* pfree, const dReal* q0, bool bCheckEnvCollision, dReal* qResult)
+ {
+ std::vector<Solution> vsolutions;
+ if( _pfnik(eetrans, eerot, pfree, vsolutions) ) {
+ vector<IKReal> vsolfree;
+
+ dReal bestdist = 1000; // only valid if q0 != NULL
+ std::vector<dReal> vravesol(_varmjoints.size());
+ std::vector<dReal> vbest;
+ std::vector<IKReal> sol(_varmjoints.size());
+ // find the first valid solution that satisfies joint constraints and collisions
+ FOREACH(itsol, vsolutions) {
+ if( itsol->GetFree().size() > 0 ) {
+ // have to search over all the free parameters of the solution!
+ vsolfree.resize(itsol->GetFree().size());
+
+ ComposeSolution(itsol->GetFree(), &vsolfree[0], 0, q0, boost::bind(&IkFastSolver::_ValidateSolutionSingle,this, boost::ref(*itsol), &vsolfree[0], q0, bCheckEnvCollision, boost::ref(sol), boost::ref(vravesol), boost::ref(vbest), boost::ref(bestdist)));
+ }
+ else {
+ if( _ValidateSolutionSingle(*itsol, NULL,q0,bCheckEnvCollision, sol, vravesol, vbest, bestdist) )
+ break;
+ }
+ }
+
+ // return as soon as a solution is found, since we're visiting phis starting from q0, we are guaranteed
+ // that the solution will be close (ie, phi's dominate in the search). This is to speed things up
+ if( vbest.size() == _qlower.size() && qResult != NULL ) {
+ memcpy(&qResult[0], &vbest[0], sizeof(dReal)*_qlower.size());
+ return true;
+ }
+ }
+
+ return false;
+ }
+
+ // validate a solution
+ bool _ValidateSolutionSingle(const Solution& iksol, const IKReal* pfree, const dReal* q0, bool bCheckEnvCollision, std::vector<IKReal>& sol, std::vector<dReal>& vravesol, std::vector<dReal>& vbest, dReal& bestdist)
+ {
+ iksol.GetSolution(&sol[0],pfree);
+
+ for(int i = 0; i < (int)sol.size(); ++i)
+ vravesol[i] = (dReal)sol[i];
+
+ if( !checkjointangles(vravesol) )
+ return false;
+
+ // check for self collisions
+ _probot->SetActiveDOFValues(NULL, &vravesol[0]);
+
+ if( _probot->CheckSelfCollision() )
+ return false;
+
+ COLLISIONREPORT report;
+ if( bCheckEnvCollision && GetEnv()->CheckCollision(_probot, &report) ) {
+ if( report.plink1 != NULL && report.plink2 != NULL ) {
+ RAVELOG_VERBOSEA("WAMIK: collision %S:%S with %S:%S\n", report.plink1->GetParent()->GetName(), report.plink1->GetName(), report.plink2->GetParent()->GetName(), report.plink2->GetName());
+ }
+ return false;
+ }
+
+ // solution is valid, check with q0
+ if( q0 != NULL ) {
+
+ dReal d = 0;
+ for(int k = 0; k < (int)_varmjoints.size(); ++k)
+ d += SQR(vravesol[k]-q0[k]);
+
+ if( bestdist > d ) {
+ vbest = vravesol;
+ bestdist = d;
+ }
+ }
+ else
+ vbest = vravesol;
+ return true;
+ }
+
+ bool _SolveAll(const IKReal* eerot, const IKReal* eetrans, const IKReal* pfree, bool bCheckEnvCollision, std::vector< std::vector<dReal> >& qSolutions)
+ {
+ std::vector<Solution> vsolutions;
+ if( _pfnik(eetrans, eerot, pfree, vsolutions) ) {
+ vector<IKReal> vsolfree;
+ vector<dReal> vravesol(_varmjoints.size());
+ std::vector<IKReal> sol(_varmjoints.size());
+ FOREACH(itsol, vsolutions) {
+ if( itsol->GetFree().size() > 0 ) {
+ // have to search over all the free parameters of the solution!
+ vsolfree.resize(itsol->GetFree().size());
+
+ ComposeSolution(itsol->GetFree(), &vsolfree[0], 0, NULL, boost::bind(&IkFastSolver::_ValidateSolutionAll,this, boost::ref(*itsol), &vsolfree[0], bCheckEnvCollision, boost::ref(sol), boost::ref(vravesol), boost::ref(qSolutions)));
+ }
+ else {
+ if( _ValidateSolutionAll(*itsol, NULL, bCheckEnvCollision, sol, vravesol, qSolutions) )
+ break;
+ }
+ }
+ }
+
+ return false;
+ }
+
+ bool _ValidateSolutionAll(const Solution& iksol, const IKReal* pfree, bool bCheckEnvCollision, std::vector<IKReal>& sol, std::vector<dReal>& vravesol, std::vector< std::vector<dReal> >& qSolutions)
+ {
+ iksol.GetSolution(&sol[0],pfree);
+
+ for(int i = 0; i < (int)sol.size(); ++i)
+ vravesol[i] = (dReal)sol[i];
+
+ // find the first valid solutino that satisfies joint constraints and collisions
+ if( !checkjointangles(vravesol) )
+ return false;
+
+ // check for self collisions
+ _probot->SetActiveDOFValues(NULL, &vravesol[0]);
+
+ if( _probot->CheckSelfCollision() )
+ return false;
+
+ if( bCheckEnvCollision && GetEnv()->CheckCollision(_probot) )
+ return false;
+
+ qSolutions.push_back(vravesol);
+ return false;
+ }
+
+ bool checkjointangles(std::vector<dReal>& vravesol)
+ {
+ for(int j = 0; j < (int)_qlower.size(); ++j) {
+ if( _qlower[j] < -PI && vravesol[j] > _qupper[j] )
+ vravesol[j] -= 2*PI;
+ if( _qupper[j] > PI && vravesol[j] < _qlower[j] )
+ vravesol[j] += 2*PI;
+ if( vravesol[j] < _qlower[j] || vravesol[j] > _qupper[j] )
+ return false;
+ }
+
+ return true;
+ }
+
+ template <typename U> U SQR(U t) { return t*t; }
+
+ RobotBase* _probot;
+ std::vector<int> _vfreeparams, _varmjoints;
+ std::vector<dReal> _vfreeparamscales;
+ IkFn _pfnik;
+ dReal _fFreeInc;
+ std::vector<dReal> _qlower, _qupper;
+};
+
+#endif
Added: pkg/trunk/robot_descriptions/openrave_robot_description/src/orrosikmain.cpp
===================================================================
--- pkg/trunk/robot_descriptions/openrave_robot_description/src/orrosikmain.cpp (rev 0)
+++ pkg/trunk/robot_descriptions/openrave_robot_description/src/orrosikmain.cpp 2009-02-17 06:08:51 UTC (rev 11242)
@@ -0,0 +1,107 @@
+/*********************************************************************
+ * Software License Agreement (BSD License)
+ *
+ * Copyright (c) 2009, Rosen Diankov
+ * All rights reserved.
+ *
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions
+ * are met:
+ *
+ * * Redistributions of source code must retain the above copyright
+ * notice, this list of conditions and the following disclaimer.
+ * * Redistributions in binary form must reproduce the above
+ * copyright notice, this list of conditions and the following
+ * disclaimer in the documentation and/or other materials provided
+ * with the distribution.
+ * * Neither the name of the Willow Garage nor the names of its
+ * contributors may be used to endorse or promote products derived
+ * from this software without specific prior written permission.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+ * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+ * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+ * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+ * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+ * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+ * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
+ * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
+ * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+ * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+ * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+ * POSSIBILITY OF SUCH DAMAGE.
+ *********************************************************************/
+#include "plugindefs.h"
+#include "ikbase.h"
+
+// declaring variables with stdcall can be a little complex
+#ifdef _MSC_VER
+
+#define PROT_STDCALL(name, paramlist) __stdcall name paramlist
+#define DECL_STDCALL(name, paramlist) __stdcall name paramlist
+
+#else
+
+#ifdef __x86_64__
+#define DECL_STDCALL(name, paramlist) name paramlist
+#else
+#define DECL_STDCALL(name, paramlist) __attribute__((stdcall)) name paramlist
+#endif
+
+#endif // _MSC_VER
+
+namespace pr2left {
+#include "ik_pr2left.h"
+}
+namespace pr2right {
+#include "ik_pr2right.h"
+}
+
+// need c linkage
+extern "C" {
+
+// for some reason windows complains when the prototypes are different
+InterfaceBase* DECL_STDCALL(ORCreate, (PluginType type, wchar_t* name, EnvironmentBase* penv))
+{
+ if( name == NULL ) return NULL;
+
+ switch(type) {
+ case PT_InverseKinematicsSolver:
+ if( wcsicmp(name,L"PR2Leftikfast") == 0 ) {
+ vector<int> vfree(pr2left::getNumFreeParameters());
+ for(size_t i = 0; i < vfree.size(); ++i)
+ vfree[i] = pr2left::getFreeParameters()[i];
+ return new IkFastSolver<pr2left::IKReal,pr2left::IKSolution>(pr2left::ik,vfree,0.02f,pr2left::getNumJoints(),penv);
+ }
+ else if( wcsicmp(name,L"PR2Rightikfast") == 0 ) {
+ vector<int> vfree(pr2right::getNumFreeParameters());
+ for(size_t i = 0; i < vfree.size(); ++i)
+ vfree[i] = pr2right::getFreeParameters()[i];
+ return new IkFastSolver<pr2right::IKReal,pr2right::IKSolution>(pr2right::ik,vfree,0.02f,pr2right::getNumJoints(),penv);
+ }
+ break;
+ default:
+ break;
+ }
+
+ return NULL;
+}
+
+bool DECL_STDCALL(GetPluginAttributes, (PLUGININFO* pinfo, int size))
+{
+ if( pinfo == NULL ) return false;
+ if( size != sizeof(PLUGININFO) ) {
+ RAVELOG_ERRORA("bad plugin info sizes %d != %d\n", size, sizeof(PLUGININFO));
+ return false;
+ }
+
+ pinfo->iksolvers.push_back(L"PR2Leftikfast");
+ pinfo->iksolvers.push_back(L"PR2Rightikfast");
+ return true;
+}
+
+void DECL_STDCALL(DestroyPlugin, ())
+{
+}
+
+}
Added: pkg/trunk/robot_descriptions/openrave_robot_description/src/plugindefs.h
===================================================================
--- pkg/trunk/robot_descriptions/openrave_robot_description/src/plugindefs.h (rev 0)
+++ pkg/trunk/robot_descriptions/openrave_robot_description/src/plugindefs.h 2009-02-17 06:08:51 UTC (rev 11242)
@@ -0,0 +1,203 @@
+/*********************************************************************
+ * Software License Agreement (BSD License)
+ *
+ * Copyright (c) 2009, Rosen Diankov
+ * All rights reserved.
+ *
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions
+ * are met:
+ *
+ * * Redistributions of source code must retain the above copyright
+ * notice, this list of conditions and the following disclaimer.
+ * * Redistributions in binary form must reproduce the above
+ * copyright notice, this list of conditions and the following
+ * disclaimer in the documentation and/or other materials provided
+ * with the distribution.
+ * * Neither the name of the Willow Garage nor the names of its
+ * contributors may be used to endorse or promote products derived
+ * from this software without specific prior written permission.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+ * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+ * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+ * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+ * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+ * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+ * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
+ * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
+ * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+ * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+ * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+ * POSSIBILITY OF SUCH DAMAGE.
+ *********************************************************************/
+#ifndef OPENRAVE_PLUGINDEFS_H
+#define OPENRAVE_PLUGINDEFS_H
+
+#define _CRT_SECURE_NO_WARNINGS
+
+#include <assert.h>
+#include <cstdio>
+#include <cmath>
+#include <cstdlib>
+
+// include boost for vc++ only (to get typeof working)
+#ifdef _MSC_VER
+#include <boost/typeof/std/string.hpp>
+#include <boost/typeof/std/vector.hpp>
+#include <boost/typeof/std/list.hpp>
+#include <boost/typeof/std/map.hpp>
+#include <boost/typeof/std/string.hpp>
+
+#define FOREACH(it, v) for(BOOST_TYPEOF(v)::iterator it = (v).begin(); it != (v).end(); (it)++)
+#define FOREACHC(it, v) for(BOOST_TYPEOF(v)::const_iterator it = (v).begin(); it != (v).end(); (it)++)
+#define TYPEOF BOOST_TYPEOF
+
+#define RAVE_REGISTER_BOOST
+#else
+
+#include <string>
+#include <vector>
+#include <list>
+#include <map>
+#include <string>
+
+#define FOREACH(it, v) for(typeof((v).begin()) it = (v).begin(); it != (v).end(); (it)++)
+#define FOREACHC FOREACH
+#define TYPEOF typeof
+
+#endif
+
+#include <stdint.h>
+#include <fstream>
+#include <iostream>
+
+using namespace std;
+
+#include <sys/timeb.h> // ftime(), struct timeb
+
+#ifdef _MSC_VER
+#include <float.h>
+#ifndef isnan
+#define isnan _isnan
+#endif
+#define PRIdS "Id"
+#else
+#define PRIdS "zd"
+#endif // _MSC_VER
+
+
+#ifndef _WIN32
+#include <sys/time.h>
+#define Sleep(milli) usleep(1000*milli)
+#else
+#define WIN32_LEAN_AND_MEAN
+#include <winsock2.h>
+#endif
+
+template<class T>
+inline T CLAMP_ON_RANGE(T value, T min, T max)
+{
+ if (value < min) return min;
+ if (value > max) return max;
+ return value;
+}
+
+inline uint32_t timeGetTime()
+{
+#ifdef _WIN32
+ _timeb t;
+ _ftime(&t);
+#else
+ timeb t;
+ ftime(&t);
+#endif
+
+ return (uint32_t)(t.time*1000+t.millitm);
+}
+
+inline uint64_t GetMicroTime()
+{
+#ifdef _WIN32
+ LARGE_INTEGER count, freq;
+ QueryPerformanceCounter(&count);
+ QueryPerformanceFrequency(&freq);
+ return (count.QuadPart * 1000000) / freq.QuadPart;
+#else
+ struct timeval t;
+ gettimeofday(&t, NULL);
+ return (uint64_t)t.tv_sec*1000000+t.tv_usec;
+#endif
+}
+
+inline float RANDOM_FLOAT()
+{
+#if defined(__IRIX__)
+ return drand48();
+#else
+ return rand()/((float)RAND_MAX);
+#endif
+}
+
+inline float RANDOM_FLOAT(float maximum)
+{
+#if defined(__IRIX__)
+ return (drand48() * maximum);
+#else
+ return (RANDOM_FLOAT() * maximum);
+#endif
+}
+
+inline int RANDOM_INT(int maximum)
+{
+#if defined(__IRIX__)
+ return (random() % maximum);
+#else
+ return (rand() % maximum);
+#endif
+}
+
+#ifndef ARRAYSIZE
+#define ARRAYSIZE(x) (sizeof(x)/(sizeof( (x)[0] )))
+#endif
+
+#define FORIT(it, v) for(it = (v).begin(); it != (v).end(); (it)++)
+
+#ifdef _WIN32
+
+#define WCSTOK(str, delim, ptr) wcstok(str, delim)
+
+// define wcsicmp for MAC OS X
+#elif defined(__APPLE_CC__)
+
+#define WCSTOK(str, delim, ptr) wcstok(str, delim, ptr);
+
+#define strnicmp strncasecmp
+#define stricmp strcasecmp
+
+inline int wcsicmp(const wchar_t* s1, const wchar_t* s2)
+{
+ char str1[128], str2[128];
+ sprintf(str1, "%S", s1);
+ sprintf(str2, "%S", s2);
+ return stricmp(str1, str2);
+}
+
+
+#else
+
+#define WCSTOK(str, delim, ptr) wcstok(str, delim, ptr)
+
+#define strnicmp strncasecmp
+#define stricmp strcasecmp
+#define wcsnicmp wcsncasecmp
+#define wcsicmp wcscasecmp
+
+#endif
+
+#include <pthread.h>
+
+#include <rave/rave.h>
+using namespace OpenRAVE;
+
+#endif
Modified: pkg/trunk/robot_descriptions/openrave_robot_description/src/urdf2openrave.cpp
===================================================================
--- pkg/trunk/robot_descriptions/openrave_robot_description/src/urdf2openrave.cpp 2009-02-17 05:43:25 UTC (rev 11241)
+++ pkg/trunk/robot_descriptions/openrave_robot_description/src/urdf2openrave.cpp 2009-02-17 06:08:51 UTC (rev 11242)
@@ -153,6 +153,8 @@
addKeyValue(geom, "extents", values2str(3,extents));
}
else {
+ setupTransform(vis, link->visual->xyz, link->visual->rpy);
+
switch(link->visual->geometry->type) {
case robot_desc::URDF::Link::Geometry::MESH: {
robot_desc::URDF::Link::Geometry::Mesh* mesh = static_cast<robot_desc::URDF::Link::Geometry::Mesh*>(link->visual->geometry->shape);
@@ -169,8 +171,9 @@
if( mesh->filename.empty() ) {
cerr << "mesh file is empty for link " << link->name << ", adding box" << endl;
type = "box";
- double extents[3] = {0.01,0.01,0.01};
+ double extents[3] = {0.001,0.001,0.001};
addKeyValue(geom, "extents", values2str(3,extents));
+ setupTransform(vis, link->collision->xyz, link->collision->rpy);
}
else {
type = "trimesh";
@@ -204,6 +207,8 @@
ss.str("");
ss << collisionfilename << " " << mesh->scale[0]; // don't use low!
addKeyValue(geom, "data", ss.str());
+
+ //setupTransform(vis, link->collision->xyz, link->collision->rpy);
}
break;
}
@@ -232,7 +237,6 @@
}
}
- setupTransform(vis, link->visual->xyz, link->visual->rpy);
copyOpenraveMap(link->visual->data, geom);
}
This was sent by the SourceForge.net collaborative development platform, the world's largest Open Source development site.
|
