Menu
▾
▴
[Personalrobots-commit] SF.net SVN: personalrobots:[6924] pkg/trunk
|
From: <rdi...@us...> - 2008-11-18 23:33:54
|
Revision: 6924
http://personalrobots.svn.sourceforge.net/personalrobots/?rev=6924&view=rev
Author: rdiankov
Date: 2008-11-18 23:33:48 +0000 (Tue, 18 Nov 2008)
Log Message:
-----------
added openrave planning structure and an IK solver plugin that wraps libKinematics
Modified Paths:
--------------
pkg/trunk/3rdparty/openrave/Makefile
pkg/trunk/robot_descriptions/openrave_robot_description/manifest.xml
Added Paths:
-----------
pkg/trunk/openrave_planning/
pkg/trunk/openrave_planning/or_plugins/
pkg/trunk/openrave_planning/or_plugins/CMakeLists.txt
pkg/trunk/openrave_planning/or_plugins/Makefile
pkg/trunk/openrave_planning/or_plugins/manifest.xml
pkg/trunk/openrave_planning/or_rosplanning/
pkg/trunk/openrave_planning/or_rosplanning/CMakeLists.txt
pkg/trunk/openrave_planning/or_rosplanning/Makefile
pkg/trunk/openrave_planning/or_rosplanning/manifest.xml
pkg/trunk/openrave_planning/or_rosplanning/src/
pkg/trunk/openrave_planning/or_rosplanning/src/or_rosplanningmain.cpp
pkg/trunk/openrave_planning/or_rosplanning/src/plugindefs.h
pkg/trunk/openrave_planning/or_rosplanning/src/rosarmik.cpp
pkg/trunk/openrave_planning/or_rosplanning/src/rosarmik.h
pkg/trunk/openrave_planning/or_rosplanning/test/
pkg/trunk/openrave_planning/or_rosplanning/test/addopenrave.m
pkg/trunk/openrave_planning/or_rosplanning/test/testrosik.m
pkg/trunk/openrave_planning/setopenrave.sh
pkg/trunk/robot_descriptions/openrave_robot_description/robots/pr2full.robot.xml
Modified: pkg/trunk/3rdparty/openrave/Makefile
===================================================================
--- pkg/trunk/3rdparty/openrave/Makefile 2008-11-18 23:32:52 UTC (rev 6923)
+++ pkg/trunk/3rdparty/openrave/Makefile 2008-11-18 23:33:48 UTC (rev 6924)
@@ -2,7 +2,7 @@
SVN_DIR = openrave_svn
# Should really specify a revision
-SVN_REVISION = -r 465
+SVN_REVISION = -r 472
SVN_URL = https://openrave.svn.sourceforge.net/svnroot/openrave
include $(shell rospack find mk)/svn_checkout.mk
Added: pkg/trunk/openrave_planning/or_plugins/CMakeLists.txt
===================================================================
--- pkg/trunk/openrave_planning/or_plugins/CMakeLists.txt (rev 0)
+++ pkg/trunk/openrave_planning/or_plugins/CMakeLists.txt 2008-11-18 23:33:48 UTC (rev 6924)
@@ -0,0 +1,21 @@
+cmake_minimum_required(VERSION 2.6)
+include(rosbuild)
+rospack(or_plugins)
+
+find_ros_package(or_rosplanning)
+
+file(GLOB or_rosplanning_files ${or_rosplanning_PACKAGE_PATH}/lib/*)
+
+set(openrave_plugins )
+foreach(it ${or_rosplanning_files})
+get_filename_component(basename ${it} NAME)
+add_custom_command(
+ OUTPUT ${CMAKE_SOURCE_DIR}/${basename}
+ COMMAND ${CMAKE_COMMAND} -E create_symlink
+ ARGS ${it} ${CMAKE_SOURCE_DIR}/${basename}
+ DEPENDS ${it})
+
+ set(openrave_plugins ${openrave_plugins} ${CMAKE_SOURCE_DIR}/${basename})
+endforeach(it)
+
+add_custom_target(or_plugins ALL DEPENDS ${openrave_plugins})
Added: pkg/trunk/openrave_planning/or_plugins/Makefile
===================================================================
--- pkg/trunk/openrave_planning/or_plugins/Makefile (rev 0)
+++ pkg/trunk/openrave_planning/or_plugins/Makefile 2008-11-18 23:33:48 UTC (rev 6924)
@@ -0,0 +1 @@
+include $(shell rospack find mk)/cmake.mk
\ No newline at end of file
Added: pkg/trunk/openrave_planning/or_plugins/manifest.xml
===================================================================
--- pkg/trunk/openrave_planning/or_plugins/manifest.xml (rev 0)
+++ pkg/trunk/openrave_planning/or_plugins/manifest.xml 2008-11-18 23:33:48 UTC (rev 6924)
@@ -0,0 +1,8 @@
+<package>
+ <description brief="OpenRAVE Plugins Maintainer">
+ This package depends on all OpenRAVE plugins. Once the plugins are compiled, they will be installed to this package. All an OpenRAVE user has to do is add this path to the OPENRAVE_PLUGINS environment variable.
+ </description>
+ <author>Rosen Diankov (rdi...@cs...)</author>
+ <license>BSD</license>
+ <depend package="or_rosplanning"/>
+</package>
Added: pkg/trunk/openrave_planning/or_rosplanning/CMakeLists.txt
===================================================================
--- pkg/trunk/openrave_planning/or_rosplanning/CMakeLists.txt (rev 0)
+++ pkg/trunk/openrave_planning/or_rosplanning/CMakeLists.txt 2008-11-18 23:33:48 UTC (rev 6924)
@@ -0,0 +1,5 @@
+cmake_minimum_required(VERSION 2.6)
+include(rosbuild)
+add_definitions(-Wall)
+rospack(or_rosplanning)
+rospack_add_library(or_rosplanning src/or_rosplanningmain.cpp src/rosarmik.cpp)
Added: pkg/trunk/openrave_planning/or_rosplanning/Makefile
===================================================================
--- pkg/trunk/openrave_planning/or_rosplanning/Makefile (rev 0)
+++ pkg/trunk/openrave_planning/or_rosplanning/Makefile 2008-11-18 23:33:48 UTC (rev 6924)
@@ -0,0 +1 @@
+include $(shell rospack find mk)/cmake.mk
\ No newline at end of file
Added: pkg/trunk/openrave_planning/or_rosplanning/manifest.xml
===================================================================
--- pkg/trunk/openrave_planning/or_rosplanning/manifest.xml (rev 0)
+++ pkg/trunk/openrave_planning/or_rosplanning/manifest.xml 2008-11-18 23:33:48 UTC (rev 6924)
@@ -0,0 +1,12 @@
+<package>
+ <description brief="OpenRAVE Plugin for ROS Planning">
+ Contains robot ik solvers, planners, and commonly used functions that integrate with the ROS framework.
+ </description>
+ <author>Rosen Diankov (rdi...@cs...)</author>
+ <license>BSD</license>
+ <depend package="roscpp"/>
+ <depend package="openrave"/>
+ <depend package="libKinematics"/>
+ <sysdepend os="ubuntu" version="7.04-feisty" package="libboost-dev"/>
+ <sysdepend os="ubuntu" version="8.04-hardy" package="libboost-dev"/>
+</package>
Added: pkg/trunk/openrave_planning/or_rosplanning/src/or_rosplanningmain.cpp
===================================================================
--- pkg/trunk/openrave_planning/or_rosplanning/src/or_rosplanningmain.cpp (rev 0)
+++ pkg/trunk/openrave_planning/or_rosplanning/src/or_rosplanningmain.cpp 2008-11-18 23:33:48 UTC (rev 6924)
@@ -0,0 +1,68 @@
+// Software License Agreement (BSD License)
+// Copyright (c) 2008, Willow Garage, Inc.
+// 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 Stanford University 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 "rosarmik.h"
+
+typedef void (*CREATECALLBACK)(PluginType type, const wchar_t* pname);
+EnvironmentBase* g_pEnviron = NULL;
+
+// need c linkage
+extern "C" {
+
+InterfaceBase* DECL_STDCALL(ORCreate, (PluginType type, wchar_t* name))
+{
+ switch(type) {
+ case PT_InverseKinematicsSolver:
+ if( wcsicmp(name, L"ROSArmIK") == 0 )
+ return new ROSArmIK();
+ break;
+
+ default:
+ break;
+ }
+
+ return NULL;
+}
+
+bool DECL_STDCALL(GetPluginAttributes, (PLUGININFO* pinfo, int size))
+{
+ if( pinfo == NULL ) return false;
+ if( size != sizeof(PLUGININFO) ) {
+ printf("bad plugin info sizes %d != %d\n", size, sizeof(PLUGININFO));
+ return false;
+ }
+
+ pinfo->iksolvers.push_back(L"ROSArmIK");
+ return true;
+}
+
+void DECL_STDCALL(OpenPlugin, (void* pcallback, EnvironmentBase* penv))
+{
+ RaveSetEnvironment(penv);
+ g_pEnviron = penv;
+}
+
+}
Added: pkg/trunk/openrave_planning/or_rosplanning/src/plugindefs.h
===================================================================
--- pkg/trunk/openrave_planning/or_rosplanning/src/plugindefs.h (rev 0)
+++ pkg/trunk/openrave_planning/or_rosplanning/src/plugindefs.h 2008-11-18 23:33:48 UTC (rev 6924)
@@ -0,0 +1,165 @@
+// Copyright (C) 2006-2008 Rosen Diankov (rdi...@cs...)
+//
+// This program is free software: you can redistribute it and/or modify
+// it under the terms of the GNU General Public License as published by
+// the Free Software Foundation, either version 3 of the License, or
+// at your option) any later version.
+//
+// This program is distributed in the hope that it will be useful,
+// but WITHOUT ANY WARRANTY; without even the implied warranty of
+// MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
+// GNU General Public License for more details.
+//
+// You should have received a copy of the GNU General Public License
+// along with this program. If not, see <http://www.gnu.org/licenses/>.
+#ifndef RRT_PLANNER_STDAFX
+#define RRT_PLANNER_STDAFX
+
+#define _CRT_SECURE_NO_WARNINGS
+#define _CRT_SECURE_NO_DEPRECATE
+
+#include <assert.h>
+#include <cstdio>
+#include <cmath>
+#include <cstdlib>
+
+#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 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
+
+#endif
+
+#include <fstream>
+#include <iostream>
+#include <sstream>
+
+using namespace std;
+typedef unsigned int u32;
+
+#include <sys/timeb.h> // ftime(), struct timeb
+
+// 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
+
+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 unsigned long timeGetTime()
+{
+#ifdef _WIN32
+ _timeb t;
+ _ftime(&t);
+#else
+ timeb t;
+ ftime(&t);
+#endif
+
+ return (unsigned long)(t.time*1000+t.millitm);
+}
+
+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 <rave/rave.h>
+using namespace OpenRAVE;
+
+extern EnvironmentBase* g_pEnviron;
+
+#endif
Added: pkg/trunk/openrave_planning/or_rosplanning/src/rosarmik.cpp
===================================================================
--- pkg/trunk/openrave_planning/or_rosplanning/src/rosarmik.cpp (rev 0)
+++ pkg/trunk/openrave_planning/or_rosplanning/src/rosarmik.cpp 2008-11-18 23:33:48 UTC (rev 6924)
@@ -0,0 +1,510 @@
+// Software License Agreement (BSD License)
+// Copyright (c) 2008, Willow Garage, Inc.
+// 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 Stanford University 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 "rosarmik.h"
+
+#ifndef SQR
+template <class T>
+inline T SQR(T x) { return x * x; }
+#endif
+
+ROSArmIK::ROSArmIK() : IkSolverBase()
+{
+}
+
+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[0]-_qlower[0]);
+ 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);
+
+ // Shoulder pitch
+ aj << 0 << -1 << 0;
+ axis.push_back(aj);
+ an << 0.1 << 0 << 0;
+ anchor.push_back(an);
+
+ // Shoulder roll
+ aj << -1 << 0 << 0;
+ axis.push_back(aj);
+ an << 0.1 << 0 << 0;
+ anchor.push_back(an);
+
+ // Elbow flex
+ aj << 0 << 1 << 0;
+ axis.push_back(aj);
+ an << 0.5 << 0 << 0;
+ anchor.push_back(an);
+
+ // Forearm roll
+ aj << 1 << 0 << 0;
+ axis.push_back(aj);
+ an << 0.5 << 0 << 0;
+ anchor.push_back(an);
+
+ // Wrist flex
+ aj << 0 << 1 << 0;
+ axis.push_back(aj);
+ an << 0.82025 << 0 << 0;
+ anchor.push_back(an);
+
+ // Gripper roll
+ aj << -1 << 0 << 0;
+ axis.push_back(aj);
+ an << 0.82025 << 0 << 0;
+ anchor.push_back(an);
+
+ 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[0] : 0;
+ dReal upperphi = _qupper[0], lowerphi = _qlower[0], 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->ComputeIK(nmT,curphi);
+ vector<dReal> vravesol(_probot->GetActiveDOF());
+ vector<double>* pbest = NULL;
+ FOREACH(itsol, iksolver->solution_ik_) {
+ vector<double>& sol = *itsol;
+ assert( (int)sol.size() == _probot->GetActiveDOF());
+
+ // find the first valid solution that satisfies joint constraints and collisions
+ wstringstream ss;
+ int j;
+ for(j = 0; j < (int)pmanip->_vecarmjoints.size(); ++j) {
+ if( sol[j] < _qlower[j] || sol[j] > _qupper[j] )
+ break;
+ }
+
+ if( j < (int)pmanip->_vecarmjoints.size() )
+ continue; // out of bounds
+
+ // check for self collisions
+ for(int i = 0; i < (int)sol.size(); ++i)
+ vravesol[i] = sol[i];
+ _probot->SetActiveDOFValues(NULL, &vravesol[0]);
+
+ if( _probot->CheckSelfCollision() )
+ continue;
+
+ COLLISIONREPORT report;
+ if( bCheckEnvCollision && g_pEnviron->CheckCollision(_probot, &report) ) {
+ if( report.plink1 != NULL && report.plink2 != NULL ) {
+ RAVELOG(L"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(sol[k]-q0[k]);
+
+ if( bestdist > d ) {
+ pbest = /
+ bestdist = d;
+ }
+ }
+ else {
+ pbest = /
+ 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( pbest != NULL ) {
+
+ if( qResult != NULL ) {
+ for(int i = 0; i < (int)pbest->size(); ++i)
+ qResult[i] = (*pbest)[i];
+ }
+ 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[0], lowerphi = _qlower[0], 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->ComputeIK(nmT,curphi);
+ vector<dReal> vravesol(_probot->GetActiveDOF());
+
+ FOREACH(itsol, iksolver->solution_ik_) {
+ vector<double>& sol = *itsol;
+ assert( (int)sol.size() == _probot->GetActiveDOF());
+
+ // find the first valid solutino that satisfies joint constraints and collisions
+ int j;
+ for(j = 0; j < (int)pmanip->_vecarmjoints.size(); ++j) {
+ if( sol[j] < _qlower[j] || sol[j] > _qupper[j] )
+ break;
+ }
+
+ if( j < (int)pmanip->_vecarmjoints.size() )
+ continue; // out of bounds
+
+ // check for self collisions
+ for(int i = 0; i < (int)sol.size(); ++i)
+ vravesol[i] = sol[i];
+ _probot->SetActiveDOFValues(NULL, &vravesol[0]);
+
+ if( _probot->CheckSelfCollision() )
+ continue;
+
+ if( bCheckEnvCollision && g_pEnviron->CheckCollision(_probot) )
+ continue;
+
+ qSolutions.push_back(vector<dReal>());
+ qSolutions.back().resize(pmanip->_vecarmjoints.size());
+ for(int i = 0; i < (int)sol.size(); ++i)
+ qSolutions.back()[i] = sol[i];
+ }
+ }
+
+ 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->ComputeIK(nmT,_qlower[0] + (_qupper[0]-_qlower[0])*pFreeParameters[0]);
+
+ vector<dReal> vravesol(_probot->GetActiveDOF());
+ vector<double>* pbest = NULL;
+ FOREACH(itsol, iksolver->solution_ik_) {
+ vector<double>& sol = *itsol;
+ assert( (int)sol.size() == _probot->GetActiveDOF());
+
+ // find the first valid solutino that satisfies joint constraints and collisions
+ int j;
+ for(j = 0; j < (int)pmanip->_vecarmjoints.size(); ++j) {
+ if( sol[j] < _qlower[j] || sol[j] > _qupper[j] )
+ break;
+ }
+
+ if( j < (int)pmanip->_vecarmjoints.size() )
+ continue; // out of bounds
+
+ // check for self collisions (does WAM ever self-collide?)
+ for(int i = 0; i < (int)sol.size(); ++i)
+ vravesol[i] = sol[i];
+ _probot->SetActiveDOFValues(NULL, &vravesol[0]);
+
+ if( _probot->CheckSelfCollision() )
+ continue;
+
+ COLLISIONREPORT report;
+ if( bCheckEnvCollision && g_pEnviron->CheckCollision(_probot, &report) ) {
+ if( report.plink1 != NULL && report.plink2 != NULL ) {
+ RAVELOG(L"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 ) {
+ pbest = /
+ bestdist = d;
+ }
+ }
+ else {
+ pbest = /
+ 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( pbest != NULL ) {
+ if( qResult != NULL ) {
+ for(int i = 0; i < (int)pbest->size(); ++i)
+ qResult[i] = (*pbest)[i];
+ }
+ 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->ComputeIK(nmT,_qlower[0] + (_qupper[0]-_qlower[0])*pFreeParameters[0]);
+
+ FOREACH(itsol, iksolver->solution_ik_) {
+ vector<double>& sol = *itsol;
+ assert( (int)sol.size() == _probot->GetActiveDOF());
+
+ // find the first valid solutino that satisfies joint constraints and collisions
+ int j;
+ for(j = 0; j < (int)pmanip->_vecarmjoints.size(); ++j) {
+ if( sol[j] < _qlower[j] || sol[j] > _qupper[j] )
+ break;
+ }
+
+ if( j < (int)pmanip->_vecarmjoints.size() )
+ continue; // out of bounds
+
+ // check for self collisions
+ for(int i = 0; i < (int)sol.size(); ++i)
+ vravesol[i] = sol[i];
+ _probot->SetActiveDOFValues(NULL, &vravesol[0]);
+
+ if( _probot->CheckSelfCollision() )
+ continue;
+
+ if( bCheckEnvCollision && g_pEnviron->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( pmanip->_vecarmjoints.size() > 0 && pmanip->_vecarmjoints[0] < _probot->GetDOF());
+ assert( _qlower.size() > 0 && _qupper.size() > 0 );
+
+ dReal values[3];
+ _probot->GetJointFromDOFIndex(pmanip->_vecarmjoints[0])->GetValues(values);
+ pFreeParameters[0] = (values[0]-_qlower[0])*fiFreeParam;
+ 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;
+}
Added: pkg/trunk/openrave_planning/or_rosplanning/src/rosarmik.h
===================================================================
--- pkg/trunk/openrave_planning/or_rosplanning/src/rosarmik.h (rev 0)
+++ pkg/trunk/openrave_planning/or_rosplanning/src/rosarmik.h 2008-11-18 23:33:48 UTC (rev 6924)
@@ -0,0 +1,63 @@
+// Software License Agreement (BSD License)
+// Copyright (c) 2008, Willow Garage, Inc.
+// 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 Stanford University 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_ROSARMIK_H
+#define OPENRAVE_ROSARMIK_H
+
+#include <libKinematics/pr2_ik.h>
+#include <boost/shared_ptr.hpp>
+
+class ROSArmIK : public IkSolverBase
+{
+public:
+ ROSArmIK();
+
+ 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.04f; }
+
+ RobotBase* _probot;
+ std::vector<dReal> _qlower, _qupper;
+ Vector voffset;
+ dReal fiFreeParam;
+ boost::shared_ptr<kinematics::arm7DOF> iksolver;
+};
+
+#endif
Added: pkg/trunk/openrave_planning/or_rosplanning/test/addopenrave.m
===================================================================
--- pkg/trunk/openrave_planning/or_rosplanning/test/addopenrave.m (rev 0)
+++ pkg/trunk/openrave_planning/or_rosplanning/test/addopenrave.m 2008-11-18 23:33:48 UTC (rev 6924)
@@ -0,0 +1,9 @@
+function addopenrave()
+
+[status,orprefix] = system('openrave-config --prefix');
+orprefix = strtrim(orprefix);
+if( exist('OCTAVE_VERSION') ~= 0 )
+ addpath(fullfile(orprefix,'share','openrave','octave'));
+else
+ addpath(fullfile(orprefix,'share','openrave','matlab'));
+end
Added: pkg/trunk/openrave_planning/or_rosplanning/test/testrosik.m
===================================================================
--- pkg/trunk/openrave_planning/or_rosplanning/test/testrosik.m (rev 0)
+++ pkg/trunk/openrave_planning/or_rosplanning/test/testrosik.m 2008-11-18 23:33:48 UTC (rev 6924)
@@ -0,0 +1,26 @@
+function testrosik()
+
+addopenrave();
+
+orEnvLoadScene('',1);
+robotid = orEnvCreateRobot('pr2','robots/pr2full.robot.xml');
+probid = orEnvCreateProblem('basemanipulation','pr2');
+
+manips = orRobotGetManipulators(robotid);
+
+% orBodySetJointValues(robotid,[ -0.21 0.754256 0.40712 0.323661 1.28898 0.175237 2.13528],manips{1}.armjoints);
+% links = orBodyGetLinks(robotid);
+% Thand = reshape(links(:,manips{1}.eelink),[3 4]);
+% s = orProblemSendCommand(['iktest matrix ' sprintf('%f ',Thand(:))]);
+% s
+% if( isempty(s) )
+% return;
+% end
+
+%% left arm
+orProblemSendCommand('SetActiveManip 0')
+orProblemSendCommand('debugik numtests 10',probid);
+
+%% right arm
+% orProblemSendCommand('SetActiveManip 1')
+% orProblemSendCommand('debugik numtests 1',probid);
Added: pkg/trunk/openrave_planning/setopenrave.sh
===================================================================
--- pkg/trunk/openrave_planning/setopenrave.sh (rev 0)
+++ pkg/trunk/openrave_planning/setopenrave.sh 2008-11-18 23:33:48 UTC (rev 6924)
@@ -0,0 +1,5 @@
+# sets the openrave environment variables to recognize the ROS directories
+# for bash, use source setopenrave.sh
+export PATH=`rospack find openrave`/bin:$PATH
+export OPENRAVE_DATA=`rospack find openrave_robot_description`:`openrave-config --prefix`/share/openrave/:$OPENRAVE_DATA
+export OPENRAVE_PLUGINS=`rospack find or_plugins`:`openrave-config --prefix`/share/openrave/plugins:$OPENRAVE_PLUGINS
Modified: pkg/trunk/robot_descriptions/openrave_robot_description/manifest.xml
===================================================================
--- pkg/trunk/robot_descriptions/openrave_robot_description/manifest.xml 2008-11-18 23:32:52 UTC (rev 6923)
+++ pkg/trunk/robot_descriptions/openrave_robot_description/manifest.xml 2008-11-18 23:33:48 UTC (rev 6924)
@@ -1,6 +1,9 @@
<package>
- <description>Openrave robot file generator from wg_robot_description</description>
- <author>Rosen Diankov (from gazebo_robot_description)</author>
+ <description brief="Openrave Robot Description Manager">
+ Base robot files auto-generated from wg_robot_description package.
+ To use in openrave, add this path to the OPENRAVE_DATA environment variable.
+ </description>
+ <author>Rosen Diankov (rdi...@cs...)</author>
<license>BSD</license>
<review status="unreviewed" notes=""/>
<url>http://pr.willowgarage.com/wiki/FIXME</url>
Added: pkg/trunk/robot_descriptions/openrave_robot_description/robots/pr2full.robot.xml
===================================================================
--- pkg/trunk/robot_descriptions/openrave_robot_description/robots/pr2full.robot.xml (rev 0)
+++ pkg/trunk/robot_descriptions/openrave_robot_description/robots/pr2full.robot.xml 2008-11-18 23:33:48 UTC (rev 6924)
@@ -0,0 +1,24 @@
+<!-- uses the base kinematics from pr2.robot.xml and includes other definitions -->
+<Robot file="pr2.robot.xml">
+ <!-- left gripper -->
+ <Manipulator>
+ <base>torso</base>
+ <effector>left_gripper_palm</effector>
+ <armjoints>left_shoulder_pan_joint left_shoulder_pitch_joint left_upper_arm_roll_joint left_elbow_flex_joint left_forearm_roll_joint left_wrist_flex_joint left_wrist_roll_joint</armjoints>
+ <joints>left_gripper_l_finger_joint</joints>
+ <closed>0</closed>
+ <opened>0.8</opened>
+ <iksolver>rosarmik</iksolver>
+ </Manipulator>
+
+ <!-- right gripper -->
+ <Manipulator>
+ <base>torso</base>
+ <effector>right_gripper_palm</effector>
+ <armjoints>right_shoulder_pan_joint right_shoulder_pitch_joint right_upper_arm_roll_joint right_elbow_flex_joint right_forearm_roll_joint right_wrist_flex_joint right_wrist_roll_joint</armjoints>
+ <joints>right_gripper_l_finger_joint</joints>
+ <closed>0</closed>
+ <opened>0.8</opened>
+ <iksolver>rosarmik</iksolver>
+ </Manipulator>
+</Robot>
This was sent by the SourceForge.net collaborative development platform, the world's largest Open Source development site.
|
