Menu
▾
▴
[Personalrobots-commit] SF.net SVN: personalrobots:[6969] pkg/trunk
|
From: <rdi...@us...> - 2008-11-19 03:37:56
|
Revision: 6969
http://personalrobots.svn.sourceforge.net/personalrobots/?rev=6969&view=rev
Author: rdiankov
Date: 2008-11-19 03:37:49 +0000 (Wed, 19 Nov 2008)
Log Message:
-----------
updated openrave ik solver and robots tests
Modified Paths:
--------------
pkg/trunk/3rdparty/openrave/Makefile
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/testrosik.m
pkg/trunk/robot_descriptions/openrave_robot_description/test/test_robots.cpp
Modified: pkg/trunk/3rdparty/openrave/Makefile
===================================================================
--- pkg/trunk/3rdparty/openrave/Makefile 2008-11-19 03:27:59 UTC (rev 6968)
+++ pkg/trunk/3rdparty/openrave/Makefile 2008-11-19 03:37:49 UTC (rev 6969)
@@ -2,7 +2,7 @@
SVN_DIR = openrave_svn
# Should really specify a revision
-SVN_REVISION = -r 474
+SVN_REVISION = -r 475
SVN_URL = https://openrave.svn.sourceforge.net/svnroot/openrave
include $(shell rospack find mk)/svn_checkout.mk
Modified: pkg/trunk/openrave_planning/or_rosplanning/src/rosarmik.cpp
===================================================================
--- pkg/trunk/openrave_planning/or_rosplanning/src/rosarmik.cpp 2008-11-19 03:27:59 UTC (rev 6968)
+++ pkg/trunk/openrave_planning/or_rosplanning/src/rosarmik.cpp 2008-11-19 03:37:49 UTC (rev 6969)
@@ -68,46 +68,53 @@
joint_type.resize(7);
// Shoulder pan
- aj << 0 << 0 << -1.0;
+ 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;
+ 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;
+ 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;
+ 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");
@@ -189,11 +196,14 @@
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
wstringstream ss;
int j;
for(j = 0; j < (int)pmanip->_vecarmjoints.size(); ++j) {
- if( sol[j] < _qlower[j] || sol[j] > _qupper[j] )
+ if( vravesol[j] < _qlower[j] || vravesol[j] > _qupper[j] )
break;
}
@@ -201,12 +211,11 @@
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() )
+ if( _probot->CheckSelfCollision() ) {
continue;
+ }
COLLISIONREPORT report;
if( bCheckEnvCollision && g_pEnviron->CheckCollision(_probot, &report) ) {
@@ -221,7 +230,7 @@
dReal d = 0;
for(int k = 0; k < (int)pmanip->_vecarmjoints.size(); ++k)
- d += SQR(sol[k]-q0[k]);
+ d += SQR(vravesol[k]-q0[k]);
if( bestdist > d ) {
pbest = /
@@ -240,7 +249,7 @@
if( qResult != NULL ) {
for(int i = 0; i < (int)pbest->size(); ++i)
- qResult[i] = (*pbest)[i];
+ qResult[i] = (*pbest)[i] * _vjointmult[i];
}
bsuccess = true;
break;
@@ -307,11 +316,14 @@
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
int j;
for(j = 0; j < (int)pmanip->_vecarmjoints.size(); ++j) {
- if( sol[j] < _qlower[j] || sol[j] > _qupper[j] )
+ if( vravesol[j] < _qlower[j] || vravesol[j] > _qupper[j] )
break;
}
@@ -319,8 +331,6 @@
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() )
@@ -329,10 +339,7 @@
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];
+ qSolutions.push_back(vravesol);
}
}
@@ -369,10 +376,13 @@
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
int j;
for(j = 0; j < (int)pmanip->_vecarmjoints.size(); ++j) {
- if( sol[j] < _qlower[j] || sol[j] > _qupper[j] )
+ if( vravesol[j] < _qlower[j] || vravesol[j] > _qupper[j] )
break;
}
@@ -380,8 +390,6 @@
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() )
@@ -419,7 +427,7 @@
if( pbest != NULL ) {
if( qResult != NULL ) {
for(int i = 0; i < (int)pbest->size(); ++i)
- qResult[i] = (*pbest)[i];
+ qResult[i] = (*pbest)[i]*_vjointmult[i];
}
return true;
}
@@ -456,10 +464,13 @@
vector<double>& sol = *itsol;
assert( (int)sol.size() == _probot->GetActiveDOF());
+ for(int i = 0; i < (int)sol.size(); ++i)
+ vravesol[i] = sol[i];
+
// 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] )
+ if( vravesol[j] < _qlower[j] || vravesol[j] > _qupper[j] )
break;
}
@@ -467,8 +478,6 @@
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() )
Modified: pkg/trunk/openrave_planning/or_rosplanning/src/rosarmik.h
===================================================================
--- pkg/trunk/openrave_planning/or_rosplanning/src/rosarmik.h 2008-11-19 03:27:59 UTC (rev 6968)
+++ pkg/trunk/openrave_planning/or_rosplanning/src/rosarmik.h 2008-11-19 03:37:49 UTC (rev 6969)
@@ -54,7 +54,7 @@
inline dReal GetPhiInc() { return 0.04f; }
RobotBase* _probot;
- std::vector<dReal> _qlower, _qupper;
+ std::vector<dReal> _qlower, _qupper, _vjointmult;
Vector voffset;
dReal fiFreeParam;
boost::shared_ptr<kinematics::arm7DOF> iksolver;
Modified: pkg/trunk/openrave_planning/or_rosplanning/test/testrosik.m
===================================================================
--- pkg/trunk/openrave_planning/or_rosplanning/test/testrosik.m 2008-11-19 03:27:59 UTC (rev 6968)
+++ pkg/trunk/openrave_planning/or_rosplanning/test/testrosik.m 2008-11-19 03:37:49 UTC (rev 6969)
@@ -19,7 +19,9 @@
%% left arm
orProblemSendCommand('SetActiveManip 0')
-orProblemSendCommand('debugik numtests 10',probid);
+tic;
+orProblemSendCommand('debugik numtests 100',probid);
+toc
%% right arm
% orProblemSendCommand('SetActiveManip 1')
Modified: pkg/trunk/robot_descriptions/openrave_robot_description/test/test_robots.cpp
===================================================================
--- pkg/trunk/robot_descriptions/openrave_robot_description/test/test_robots.cpp 2008-11-19 03:27:59 UTC (rev 6968)
+++ pkg/trunk/robot_descriptions/openrave_robot_description/test/test_robots.cpp 2008-11-19 03:37:49 UTC (rev 6969)
@@ -31,7 +31,7 @@
vector<string> files = tokenizer(ROBOT_FILES,"; \r\n");
for(vector<string>::iterator it = files.begin(); it != files.end(); ++it) {
cout << "testing: " << *it << "... ";
- int result = runExternalProcess(OPENRAVE_EXECUTABLE, *it + string(" -testscene"));
+ int result = runExternalProcess(OPENRAVE_EXECUTABLE, *it + string(" -testscene -nogui"));
if( result != 0 ) {
bSuccess = false;
cout << "fail." << endl;
This was sent by the SourceForge.net collaborative development platform, the world's largest Open Source development site.
|
