Menu
▾
▴
[Personalrobots-commit] SF.net SVN: personalrobots:[8142] pkg/trunk
|
From: <rdi...@us...> - 2008-12-16 02:22:40
|
Revision: 8142
http://personalrobots.svn.sourceforge.net/personalrobots/?rev=8142&view=rev
Author: rdiankov
Date: 2008-12-16 02:00:14 +0000 (Tue, 16 Dec 2008)
Log Message:
-----------
fixed many problems with the openrave scripts, all example scripts from default openrave distro run with the ros backend now
Modified Paths:
--------------
pkg/trunk/3rdparty/openrave/Makefile
pkg/trunk/openrave_planning/openraveros/msg/RobotInfo.msg
pkg/trunk/openrave_planning/openraveros/octave/openraveros_getrobotinfo.m
pkg/trunk/openrave_planning/openraveros/octave/orBodyGetAABBs.m
pkg/trunk/openrave_planning/openraveros/octave/orBodyGetJointValues.m
pkg/trunk/openrave_planning/openraveros/octave/orBodySetJointValues.m
pkg/trunk/openrave_planning/openraveros/octave/orBodySetTransform.m
pkg/trunk/openrave_planning/openraveros/octave/orEnvCheckCollision.m
pkg/trunk/openrave_planning/openraveros/octave/orEnvClose.m
pkg/trunk/openrave_planning/openraveros/octave/orEnvGetBody.m
pkg/trunk/openrave_planning/openraveros/octave/orEnvPlot.m
pkg/trunk/openrave_planning/openraveros/octave/orEnvRayCollision.m
pkg/trunk/openrave_planning/openraveros/octave/orEnvSetOptions.m
pkg/trunk/openrave_planning/openraveros/octave/orEnvTriangulate.m
pkg/trunk/openrave_planning/openraveros/octave/orRobotGetActiveDOF.m
pkg/trunk/openrave_planning/openraveros/octave/orRobotGetAttachedSensors.m
pkg/trunk/openrave_planning/openraveros/octave/orRobotGetDOFLimits.m
pkg/trunk/openrave_planning/openraveros/octave/orRobotGetDOFValues.m
pkg/trunk/openrave_planning/openraveros/octave/orRobotGetManipulators.m
pkg/trunk/openrave_planning/openraveros/octave/orRobotSetActiveDOFs.m
pkg/trunk/openrave_planning/openraveros/octave/orRobotSetDOFValues.m
pkg/trunk/openrave_planning/openraveros/octave/orRobotStartActiveTrajectory.m
pkg/trunk/openrave_planning/openraveros/src/rosserver.h
pkg/trunk/openrave_planning/openraveros/src/session.h
pkg/trunk/openrave_planning/openraveros/test/testhanoi.m
Added Paths:
-----------
pkg/trunk/openrave_planning/openraveros/test/getexamplesdir.m
pkg/trunk/openrave_planning/openraveros/test/testgrasping.m
pkg/trunk/openrave_planning/openraveros/test/testrosplotting.m
Removed Paths:
-------------
pkg/trunk/openrave_planning/openraveros/test/addexamplesdir.m
Modified: pkg/trunk/3rdparty/openrave/Makefile
===================================================================
--- pkg/trunk/3rdparty/openrave/Makefile 2008-12-16 01:45:37 UTC (rev 8141)
+++ pkg/trunk/3rdparty/openrave/Makefile 2008-12-16 02:00:14 UTC (rev 8142)
@@ -2,7 +2,7 @@
SVN_DIR = openrave_svn
# Should really specify a revision
-SVN_REVISION = -r 554
+SVN_REVISION = -r 557
SVN_URL = https://openrave.svn.sourceforge.net/svnroot/openrave
include $(shell rospack find mk)/svn_checkout.mk
Modified: pkg/trunk/openrave_planning/openraveros/msg/RobotInfo.msg
===================================================================
--- pkg/trunk/openrave_planning/openraveros/msg/RobotInfo.msg 2008-12-16 01:45:37 UTC (rev 8141)
+++ pkg/trunk/openrave_planning/openraveros/msg/RobotInfo.msg 2008-12-16 02:00:14 UTC (rev 8142)
@@ -1,4 +1,4 @@
-# robot information
+ # robot information
BodyInfo bodyinfo
Manipulator[] manips
Modified: pkg/trunk/openrave_planning/openraveros/octave/openraveros_getrobotinfo.m
===================================================================
--- pkg/trunk/openrave_planning/openraveros/octave/openraveros_getrobotinfo.m 2008-12-16 01:45:37 UTC (rev 8141)
+++ pkg/trunk/openrave_planning/openraveros/octave/openraveros_getrobotinfo.m 2008-12-16 02:00:14 UTC (rev 8142)
@@ -9,7 +9,7 @@
for i = 1:length(robot.manips)
mi = robotinfo.manips{i};
robot.manips{i} = struct('baselink',mi.baselink+1,...
- 'eelink',eelink+1,...
+ 'eelink',mi.eelink+1,...
'Tgrasp',reshape(cell2mat(mi.tgrasp.m),[3 4]),...
'handjoints',cell2mat(mi.handjoints),...
'armjoints',cell2mat(mi.armjoints),...
@@ -31,6 +31,6 @@
robot.activejoints = cell2mat(robotinfo.active.joints);
robot.rotationaxis = cell2mat(robotinfo.active.rotationaxis);
-robot.activelowerlimit = cell2mat(bodyinfo.activelowerlimit);
-robot.activeupperlimit = cell2mat(bodyinfo.activeupperlimit);
+robot.activelowerlimit = cell2mat(robotinfo.activelowerlimit);
+robot.activeupperlimit = cell2mat(robotinfo.activeupperlimit);
Modified: pkg/trunk/openrave_planning/openraveros/octave/orBodyGetAABBs.m
===================================================================
--- pkg/trunk/openrave_planning/openraveros/octave/orBodyGetAABBs.m 2008-12-16 01:45:37 UTC (rev 8141)
+++ pkg/trunk/openrave_planning/openraveros/octave/orBodyGetAABBs.m 2008-12-16 02:00:14 UTC (rev 8142)
@@ -7,14 +7,14 @@
function aabbs = orBodyGetAABBs(bodyid)
session = openraveros_getglobalsession();
-req = openraveros_body_aabbs();
+req = openraveros_body_getaabbs();
req.bodyid = bodyid;
res = rosoct_session_call(session.id,'body_getaabbs',req);
if(~isempty(res))
aabbs = zeros(6,length(res.boxes));
for i = 1:length(res.boxes)
- aabbs(:,i) = [res.boxes{i}.center;res.boxes{i}.extents];
+ aabbs(:,i) = cell2mat([res.boxes{i}.center; res.boxes{i}.extents]);
end
else
aabbs = [];
Modified: pkg/trunk/openrave_planning/openraveros/octave/orBodyGetJointValues.m
===================================================================
--- pkg/trunk/openrave_planning/openraveros/octave/orBodyGetJointValues.m 2008-12-16 01:45:37 UTC (rev 8141)
+++ pkg/trunk/openrave_planning/openraveros/octave/orBodyGetJointValues.m 2008-12-16 02:00:14 UTC (rev 8142)
@@ -9,7 +9,7 @@
req = openraveros_body_getjointvalues();
req.bodyid = bodyid;
if( exist('indices','var') && length(indices)>0 )
- req.indices = mat2cell(indices,1,ones(1,length(indices)));
+ req.indices = mat2cell(indices(:)',1,ones(1,length(indices)));
end
res = rosoct_session_call(session.id,'body_getjointvalues',req);
Modified: pkg/trunk/openrave_planning/openraveros/octave/orBodySetJointValues.m
===================================================================
--- pkg/trunk/openrave_planning/openraveros/octave/orBodySetJointValues.m 2008-12-16 01:45:37 UTC (rev 8141)
+++ pkg/trunk/openrave_planning/openraveros/octave/orBodySetJointValues.m 2008-12-16 02:00:14 UTC (rev 8142)
@@ -10,9 +10,9 @@
session = openraveros_getglobalsession();
req = openraveros_body_setjointvalues();
req.bodyid = bodyid;
-req.jointvalues = mat2cell(values,1,ones(1,length(values)));
+req.jointvalues = mat2cell(values(:)',1,ones(1,length(values)));
if( exist('indices','var') )
- req.indices = mat2cell(indices,1,ones(1,length(indices)));
+ req.indices = mat2cell(indices(:)',1,ones(1,length(indices)));
end
res = rosoct_session_call(session.id,'body_setjointvalues',req);
success = ~isempty(res);
Modified: pkg/trunk/openrave_planning/openraveros/octave/orBodySetTransform.m
===================================================================
--- pkg/trunk/openrave_planning/openraveros/octave/orBodySetTransform.m 2008-12-16 01:45:37 UTC (rev 8141)
+++ pkg/trunk/openrave_planning/openraveros/octave/orBodySetTransform.m 2008-12-16 02:00:14 UTC (rev 8142)
@@ -12,11 +12,12 @@
if(nargin >= 3)
R = openraveros_rotfromquat(varargin{3});
- req.transform.m(1:9) = mat2cell(R(:));
- req.transform.m(10:12) = mat2cell(varargin{2});
+ req.transform.m(1:9) = mat2cell(R(:)',1,ones(9,1));
+ trans = varargin{2};
+ req.transform.m(10:12) = mat2cell(trans(:)',1,ones(3,1));
elseif(nargin == 2)
t = varargin{2};
- req.transform.m(1:12) = mat2cell(t(:));
+ req.transform.m(1:12) = mat2cell(t(:)',1,ones(12,1));
else
error('orBodySetTransform not enough arguments');
end
Modified: pkg/trunk/openrave_planning/openraveros/octave/orEnvCheckCollision.m
===================================================================
--- pkg/trunk/openrave_planning/openraveros/octave/orEnvCheckCollision.m 2008-12-16 01:45:37 UTC (rev 8141)
+++ pkg/trunk/openrave_planning/openraveros/octave/orEnvCheckCollision.m 2008-12-16 02:00:14 UTC (rev 8142)
@@ -1,10 +1,10 @@
-% [collision, colbodyid,contacts, mindist] = orEnvCheckCollision(bodyid,excludeid,req_contacts)
+% [collision, colbodyid,contacts,hitbodies,mindist] = orEnvCheckCollision(bodyid,excludeid,req_contacts,req_distance)
%
% Check collision of the robot with the environment. collision is 1 if the robot
% is colliding, colbodyid is the id of the object that body collided with
%% bodyid - the uid of the body, if size > 1, bodyidD(2) narrows collision down to specific body link (one-indexed)
-function [collision, colbodyid, contacts, mindist] = orEnvCheckCollision(bodyid,excludeids,req_contacts)
+function [collision, colbodyid, contacts, hitbodies, mindist] = orEnvCheckCollision(bodyid,excludeids,req_contacts,req_distance)
session = openraveros_getglobalsession();
req = openraveros_env_checkcollision();
@@ -16,12 +16,15 @@
req.linkid = -1; % all links of the body
end
-if( ~exist('excludeid', 'var') )
- req.excludeids = mat2cell(excludeids,1,ones(length(excludeids),1));
+if( exist('excludeid', 'var') )
+ req.excludeids = mat2cell(excludeids(:)',1,ones(length(excludeids),1));
end
if( exist('req_contacts','var') && req_contacts )
req.options = req.options + req.CO_Contacts();
end
+if( exist('req_distance','var') && req_distance )
+ req.options = req.options + req.CO_Distance();
+end
res = rosoct_session_call(session.id,'env_checkcollision',req);
@@ -30,19 +33,21 @@
colbodyid = res.collidingbodyid;
if( ~isempty(res.contacts) )
- contacts = zeros(6,numrays);
+ contacts = zeros(6,length(res.contacts));
for i = 1:length(res.contacts)
- colinfo(1:3,i) = cell2mat(res.contacts{i}.position);
- colinfo(4:6,i) = cell2mat(res.contacts{i}.normal);
+ contacts(1:3,i) = cell2mat(res.contacts{i}.position);
+ contacts(4:6,i) = cell2mat(res.contacts{i}.normal);
end
else
contacts = [];
end
- hitbodies = cell2mat(res.hitbodies);
+ hitbodies = [];%cell2mat(res.hitbodies);
+ mindist = res.mindist;
else
collision = [];
colbodyid = [];
contacts = [];
+ hitbodies = [];
mindist = [];
end
Modified: pkg/trunk/openrave_planning/openraveros/octave/orEnvClose.m
===================================================================
--- pkg/trunk/openrave_planning/openraveros/octave/orEnvClose.m 2008-12-16 01:45:37 UTC (rev 8141)
+++ pkg/trunk/openrave_planning/openraveros/octave/orEnvClose.m 2008-12-16 02:00:14 UTC (rev 8142)
@@ -6,7 +6,7 @@
session = openraveros_getglobalsession();
req = openraveros_env_closefigures();
if( exist('figureids','var') && ~isempty(figureids) )
- req.figureids = mat2cell(figureids,1,ones(1,length(figureids)));
+ req.figureids = mat2cell(figureids(:)',1,ones(1,length(figureids)));
end
res = rosoct_session_call(session.id,'env_closefigures',req);
Modified: pkg/trunk/openrave_planning/openraveros/octave/orEnvGetBody.m
===================================================================
--- pkg/trunk/openrave_planning/openraveros/octave/orEnvGetBody.m 2008-12-16 01:45:37 UTC (rev 8141)
+++ pkg/trunk/openrave_planning/openraveros/octave/orEnvGetBody.m 2008-12-16 02:00:14 UTC (rev 8142)
@@ -11,5 +11,5 @@
if(~isempty(res))
id = res.bodyid;
else
- id = [];
+ id = 0;
end
Modified: pkg/trunk/openrave_planning/openraveros/octave/orEnvPlot.m
===================================================================
--- pkg/trunk/openrave_planning/openraveros/octave/orEnvPlot.m 2008-12-16 01:45:37 UTC (rev 8141)
+++ pkg/trunk/openrave_planning/openraveros/octave/orEnvPlot.m 2008-12-16 02:00:14 UTC (rev 8142)
@@ -18,7 +18,7 @@
points = varargin{1}';
numpoints = size(points,2);
-req.points = mat2cell(points(:),1,ones(numel(points),1));
+req.points = mat2cell(points(:)',1,ones(numel(points),1));
req.size = 0.5;
req.drawtype = req.Draw_Point();
@@ -26,13 +26,13 @@
i = 2;
while(i <= nargin)
- if( strcmp(varargin{i},'size') )
+ if( strcmp(varargin{i},'color') )
i = i + 1;
- colors = varargin{i};
- req.size = mat2cell(colors(:),1,ones(numel(colors;),1));
- elseif( strcmp(varargin{i},'color') )
+ colors = varargin{i}';
+ req.colors = mat2cell(colors(:)',1,ones(numel(colors'),1));
+ elseif( strcmp(varargin{i},'size') )
i = i + 1;
- req.colors = varargin{i};
+ req.size = varargin{i};
elseif( strcmp(varargin{i},'line') | strcmp(varargin{i},'linestrip') )
req.drawtype = req.Draw_LineStrip();
elseif( strcmp(varargin{i},'linelist') )
@@ -52,7 +52,7 @@
res = rosoct_session_call(session.id,'env_plot',req);
if(~isempty(res))
- figureid = req.figureid;
+ figureid = res.figureid;
else
figureid = [];
end
Modified: pkg/trunk/openrave_planning/openraveros/octave/orEnvRayCollision.m
===================================================================
--- pkg/trunk/openrave_planning/openraveros/octave/orEnvRayCollision.m 2008-12-16 01:45:37 UTC (rev 8141)
+++ pkg/trunk/openrave_planning/openraveros/octave/orEnvRayCollision.m 2008-12-16 02:00:14 UTC (rev 8142)
@@ -19,16 +19,18 @@
numrays = size(rays,2);
req.rays = cell(numrays,1);
for i = 1:numrays
- req.rays{i}.position = {rays(1,i),rays(2,i),rays(3,i)};
- req.rays{i}.direction = {rays(4,i),rays(5,i),rays(6,i)};
+ req.rays{i} = openraveros_Ray();
+ req.rays{i}.position(1:3) = {rays(1,i),rays(2,i),rays(3,i)};
+ req.rays{i}.direction(1:3) = {rays(4,i),rays(5,i),rays(6,i)};
end
if( exist('bodyid','var') )
req.bodyid = bodyid;
end
-if( exist('req_contacts','var') && req_contacts )
- req.request_contacts = 1;
+if( ~exist('req_contacts','var') )
+ req_contacts = 1;
end
+req.request_contacts = req_contacts>0;
if( exist('req_bodyinfo','var') && req_bodyinfo )
req.request_bodies = req_bodyinfo;
end
Modified: pkg/trunk/openrave_planning/openraveros/octave/orEnvSetOptions.m
===================================================================
--- pkg/trunk/openrave_planning/openraveros/octave/orEnvSetOptions.m 2008-12-16 01:45:37 UTC (rev 8141)
+++ pkg/trunk/openrave_planning/openraveros/octave/orEnvSetOptions.m 2008-12-16 02:00:14 UTC (rev 8142)
@@ -55,7 +55,7 @@
req.viewer = strtrim(rem);
case 'gravity'
req.setmask = req.Set_Gravity();
- req.gravity(1:3) = mat2cell(str2num(rem),1,[1 1 1]);
+ req.gravity(1:3) = mat2cell(str2num(rem)',1,[1 1 1]);
case 'publishanytime'
req.setmask = req.Set_PublishAnytime();
req.publishanytime = str2num(rem);
Modified: pkg/trunk/openrave_planning/openraveros/octave/orEnvTriangulate.m
===================================================================
--- pkg/trunk/openrave_planning/openraveros/octave/orEnvTriangulate.m 2008-12-16 01:45:37 UTC (rev 8141)
+++ pkg/trunk/openrave_planning/openraveros/octave/orEnvTriangulate.m 2008-12-16 02:00:14 UTC (rev 8142)
@@ -15,9 +15,12 @@
session = openraveros_getglobalsession();
req = openraveros_env_triangulate();
-req.inclusive = inclusive;
+req.inclusive = 0;
+if( exist('inclusive','var') )
+ req.inclusive = inclusive;
+end
if( exist('ids','var') )
- req.bodyids = mat2cell(ids,1,ones(length(ids),1));
+ req.bodyids = mat2cell(ids(:)',1,ones(length(ids),1));
end
res = rosoct_session_call(session.id,'env_triangulate',req);
Modified: pkg/trunk/openrave_planning/openraveros/octave/orRobotGetActiveDOF.m
===================================================================
--- pkg/trunk/openrave_planning/openraveros/octave/orRobotGetActiveDOF.m 2008-12-16 01:45:37 UTC (rev 8141)
+++ pkg/trunk/openrave_planning/openraveros/octave/orRobotGetActiveDOF.m 2008-12-16 02:00:14 UTC (rev 8142)
@@ -8,8 +8,11 @@
req = openraveros_env_getrobots();
req.bodyid = robotid;
res = rosoct_session_call(session.id,'env_getrobots',req);
-if( ~isempty(resinfo) )
- dof = res.activedof;
+if( ~isempty(res) )
+ if( res.robots{1}.bodyinfo.bodyid ~= robotid )
+ error('wrong robot id');
+ end
+ dof = res.robots{1}.activedof;
else
dof = [];
end
Modified: pkg/trunk/openrave_planning/openraveros/octave/orRobotGetAttachedSensors.m
===================================================================
--- pkg/trunk/openrave_planning/openraveros/octave/orRobotGetAttachedSensors.m 2008-12-16 01:45:37 UTC (rev 8141)
+++ pkg/trunk/openrave_planning/openraveros/octave/orRobotGetAttachedSensors.m 2008-12-16 02:00:14 UTC (rev 8142)
@@ -9,5 +9,9 @@
% Tglobal = Tlink * Trelative
% type - the xml id of the sensor that is attached
function sensors = orRobotGetAttachedSensors(robotid)
-robotinfo = orEnvGetRobots(robotid);
-sensors = robotinfo.sensors;
+robots = orEnvGetRobots(robotid);
+if( isempty(robots) )
+ sensors = [];
+else
+ sensors = robots{1}.sensors;
+end
Modified: pkg/trunk/openrave_planning/openraveros/octave/orRobotGetDOFLimits.m
===================================================================
--- pkg/trunk/openrave_planning/openraveros/octave/orRobotGetDOFLimits.m 2008-12-16 01:45:37 UTC (rev 8141)
+++ pkg/trunk/openrave_planning/openraveros/octave/orRobotGetDOFLimits.m 2008-12-16 02:00:14 UTC (rev 8142)
@@ -12,7 +12,7 @@
req.options = openraveros_RobotInfo().Req_ActiveLimits();
res = rosoct_session_call(session.id,'env_getrobots',req);
if( ~isempty(res) )
- if( res.robots{1}.id ~= robotid )
+ if( res.robots{1}.bodyinfo.bodyid ~= robotid )
error('wrong robot id');
end
Modified: pkg/trunk/openrave_planning/openraveros/octave/orRobotGetDOFValues.m
===================================================================
--- pkg/trunk/openrave_planning/openraveros/octave/orRobotGetDOFValues.m 2008-12-16 01:45:37 UTC (rev 8141)
+++ pkg/trunk/openrave_planning/openraveros/octave/orRobotGetDOFValues.m 2008-12-16 02:00:14 UTC (rev 8142)
@@ -12,7 +12,7 @@
req = openraveros_robot_getactivevalues();
req.bodyid = robotid;
if( exist('indices','var') )
- req.indices = mat2cell(indices,1,ones(length(indices),1));
+ req.indices = mat2cell(indices(:)',1,ones(length(indices),1));
end
res = rosoct_session_call(session.id,'robot_getactivevalues',req);
Modified: pkg/trunk/openrave_planning/openraveros/octave/orRobotGetManipulators.m
===================================================================
--- pkg/trunk/openrave_planning/openraveros/octave/orRobotGetManipulators.m 2008-12-16 01:45:37 UTC (rev 8141)
+++ pkg/trunk/openrave_planning/openraveros/octave/orRobotGetManipulators.m 2008-12-16 02:00:14 UTC (rev 8142)
@@ -11,5 +11,9 @@
% effect on the end effector
% iksolvername - name of ik solver to use
function manipulators = orRobotGetManipulators(robotid)
-robotinfo = orEnvGetRobots(robotid);
-sensors = robotinfo.manips;
+robots = orEnvGetRobots(robotid);
+if( isempty(robots) )
+ manipulators = [];
+else
+ manipulators = robots{1}.manips;
+end
Modified: pkg/trunk/openrave_planning/openraveros/octave/orRobotSetActiveDOFs.m
===================================================================
--- pkg/trunk/openrave_planning/openraveros/octave/orRobotSetActiveDOFs.m 2008-12-16 01:45:37 UTC (rev 8141)
+++ pkg/trunk/openrave_planning/openraveros/octave/orRobotSetActiveDOFs.m 2008-12-16 02:00:14 UTC (rev 8142)
@@ -17,12 +17,14 @@
session = openraveros_getglobalsession();
req = openraveros_robot_setactivedofs();
req.bodyid = robotid;
-req.active.affine = affinedofs;
+if( exist('affinedofs','var') )
+ req.active.affine = affinedofs;
+end
if( exist('indices','var') )
- req.active.joints = mat2cell(indices,1,ones(length(indices),1));
+ req.active.joints = mat2cell(indices(:)',1,ones(length(indices),1));
end
if( exist('rotationaxis','var') )
- req.active.rotationaxis(1:3) = mat2cell(rotationaxis,1,[1 1 1]);
+ req.active.rotationaxis(1:3) = mat2cell(rotationaxis(:)',1,[1 1 1]);
end
res = rosoct_session_call(session.id,'robot_setactivedofs',req);
Modified: pkg/trunk/openrave_planning/openraveros/octave/orRobotSetDOFValues.m
===================================================================
--- pkg/trunk/openrave_planning/openraveros/octave/orRobotSetDOFValues.m 2008-12-16 01:45:37 UTC (rev 8141)
+++ pkg/trunk/openrave_planning/openraveros/octave/orRobotSetDOFValues.m 2008-12-16 02:00:14 UTC (rev 8142)
@@ -13,9 +13,9 @@
session = openraveros_getglobalsession();
req = openraveros_robot_setactivevalues();
req.bodyid = robotid;
-req.values = mat2cell(values,1,ones(1,length(values)));
+req.values = mat2cell(values(:)',1,ones(1,length(values)));
if( exist('indices','var') )
- req.indices = mat2cell(indices,1,ones(1,length(indices)));
+ req.indices = mat2cell(indices(:)',1,ones(1,length(indices)));
end
res = rosoct_session_call(session.id,'robot_setactivevalues',req);
success = ~isempty(res);
Modified: pkg/trunk/openrave_planning/openraveros/octave/orRobotStartActiveTrajectory.m
===================================================================
--- pkg/trunk/openrave_planning/openraveros/octave/orRobotStartActiveTrajectory.m 2008-12-16 01:45:37 UTC (rev 8141)
+++ pkg/trunk/openrave_planning/openraveros/octave/orRobotStartActiveTrajectory.m 2008-12-16 02:00:14 UTC (rev 8142)
@@ -37,7 +37,7 @@
req.trajectory.points = cell(numpts,1);
for i = 1:numpts
pt = openraveros_TrajectoryPoint();
- pt.position = mat2cell(jointvalues(:,i),1,arr);
+ pt.position = mat2cell(jointvalues(:,i)',1,arr);
req.trajectory.points{i} = pt;
end
@@ -54,11 +54,11 @@
%% convert from quaternions
for i = 1:numpts
R = openraveros_rotfromquat(transformations(1:4,i));
- req.trajectory.points{i}.transform.m(1:12) = mat2cell([R(:);transformations(5:7,i)],1,ones(9,1));
+ req.trajectory.points{i}.transform.m(1:12) = mat2cell([R(:);transformations(5:7,i)]',1,ones(12,1));
end
elseif( size(transformations,1) == 12 )
for i = 1:numpts
- req.trajectory.points{i}.transform.m(1:12) = mat2cell(transformations(:,i),1,ones(12,1));
+ req.trajectory.points{i}.transform.m(1:12) = mat2cell(transformations(:,i)',1,ones(12,1));
end
else
error('transformations wrong size');
Modified: pkg/trunk/openrave_planning/openraveros/src/rosserver.h
===================================================================
--- pkg/trunk/openrave_planning/openraveros/src/rosserver.h 2008-12-16 01:45:37 UTC (rev 8141)
+++ pkg/trunk/openrave_planning/openraveros/src/rosserver.h 2008-12-16 02:00:14 UTC (rev 8142)
@@ -41,13 +41,47 @@
class FIGURE
{
public:
- FIGURE(EnvironmentBase* penv, void* handle) : _handle(handle) {}
+ FIGURE(EnvironmentBase* penv, void* handle) : _penv(penv), _handle(handle) {}
~FIGURE() { _penv->closegraph(_handle); }
private:
+ FIGURE(const FIGURE& r ) { ROS_ASSERT(0); }
EnvironmentBase* _penv;
void* _handle;
};
+ class ServerWorker
+ {
+ public:
+ virtual void work() = 0;
+// ServerWorker(boost::condition& cond) : _cond(cond) {}
+// ~ServerWorker() { _cond.notify_all(); }
+// private:
+// boost::condition& _cond;
+ };
+
+ class WorkExecutor
+ {
+ public:
+ WorkExecutor(ServerWorker* pworker) : _worker(pworker) {}
+ ~WorkExecutor() { _worker->work(); }
+ private:
+ boost::shared_ptr<ServerWorker> _worker;
+ };
+
+ class SendProblemWorker : public ServerWorker
+ {
+ public:
+ SendProblemWorker(boost::shared_ptr<ProblemInstance> prob, const string& cmd, string& out) : _prob(prob), _cmd(cmd), _out(out) {}
+ virtual void work() {
+ _prob->SendCommand(_cmd.c_str(),_out);
+ }
+
+ private:
+ boost::shared_ptr<ProblemInstance> _prob;
+ const string& _cmd;
+ string& _out;
+ };
+
public:
ROSServer(SetViewerFunc* psetviewer, EnvironmentBase* penv, const string& physicsengine, const string& collisionchecker, const string& viewer)
: RaveServerBase(penv), _nNextFigureId(1), _nNextPlannerId(1), _nNextProblemId(1) {
@@ -84,6 +118,8 @@
virtual void Reset()
{
+ Worker();
+
_mapFigureIds.clear();
// destroy environment specific state: problems, planners, figures
@@ -99,8 +135,24 @@
/// worker thread called from the main environment thread
virtual void Worker()
{
+ list<boost::shared_ptr<WorkExecutor> > listlocalworkers;
+ {
+ boost::mutex::scoped_lock lock(_mutexWorker);
+ listlocalworkers.swap(_listWorkers);
+ }
+
+ listlocalworkers.clear();
+ _conditionWorkers.notify_all();
}
+ virtual void AddWorker(ServerWorker* pworker, bool bWait)
+ {
+ boost::mutex::scoped_lock lock(_mutexWorker);
+ _listWorkers.push_back(boost::shared_ptr<WorkExecutor>(new WorkExecutor(pworker)));
+ if( bWait )
+ _conditionWorkers.wait(lock);
+ }
+
virtual bool IsInit()
{
return true;
@@ -267,7 +319,7 @@
pbody->GetJointValues(vtemp);
res.values.resize(vtemp.size());
- for(size_t i = 0; i < req.indices.size(); ++i)
+ for(size_t i = 0; i < res.values.size(); ++i)
res.values[i] = vtemp[i];
}
@@ -526,7 +578,7 @@
void FillKinBodyInfo(KinBody* pbody, BodyInfo& info, uint32_t options)
{
- info.bodyid = pbody->GetDOF();
+ info.bodyid = pbody->GetNetworkId();
info.transform = GetAffineTransform(pbody->GetTransform());
info.enabled = pbody->IsEnabled();
@@ -619,6 +671,8 @@
{
FillKinBodyInfo(probot,info.bodyinfo,options);
+ info.activedof = probot->GetActiveDOF();
+
if( options & RobotInfo::Req_Manipulators ) {
info.manips.resize(probot->GetManipulators().size()); int index = 0;
FOREACHC(itmanip, probot->GetManipulators()) {
@@ -658,9 +712,8 @@
}
if( options & RobotInfo::Req_ActiveDOFs ) {
FillActiveDOFs(probot, info.active);
- info.activedof = probot->GetActiveDOF();
}
- if( options & BodyInfo::Req_JointLimits ) {
+ if( options & RobotInfo::Req_ActiveLimits ) {
vector<dReal> vlower, vupper;
probot->GetActiveDOFLimits(vlower,vupper);
ROS_ASSERT( vlower.size() == vupper.size() );
@@ -731,6 +784,8 @@
float falpha = max(0.0f, 1.0f-req.transparency);
falpha = min(1.0f,falpha);
RaveVector<float> vOneColor(1.0f,0.5f,0.5f,falpha);
+ if( req.colors.size() >= 3 )
+ vOneColor = RaveVector<float>(req.colors[0], req.colors[1], req.colors[2],falpha);
void* figure = NULL;
switch(req.drawtype) {
@@ -754,7 +809,7 @@
break;
case env_plot::request::Draw_TriList:
//if( bOneColor )
- figure = GetEnv()->drawtrimesh(&req.points[0],3*sizeof(req.points[0]), NULL, req.points.size()/3, vOneColor);
+ figure = GetEnv()->drawtrimesh(&req.points[0],3*sizeof(req.points[0]), NULL, req.points.size()/9, vOneColor);
//else
//figure = GetEnv()->plot3(&req.points[0],req.points.size()/3,3*sizeof(req.points[0]),req.size,&req.colors[0], 0);
break;
@@ -799,19 +854,29 @@
if( !GetEnv()->SetCollisionOptions(req.request_contacts ? CO_Contacts : 0) )
RAVELOG_WARNA("failed to set collision options\n");
+ int index = 0;
FOREACHC(itray, req.rays) {
RAY r;
r.pos = Vector(itray->position[0], itray->position[1], itray->position[2]);
r.dir = Vector(itray->direction[0], itray->direction[1], itray->direction[2]);
+
+ RAVELOG_WARN("%d: %f %f %f, %f %f %f\n", index++,r.pos.x,r.pos.y,r.pos.z,r.dir.x,r.dir.y,r.dir.z);
- uint8_t bCollision;
- if( pbody != NULL )
- bCollision = GetEnv()->CheckCollision(r,pbody,&report);
+ uint8_t bCollision = 0;
+
+ if( r.dir.lengthsqr3() > 1e-7 ) {
+ r.dir.normalize3();
+
+ if( pbody != NULL )
+ bCollision = GetEnv()->CheckCollision(r,pbody,&report);
+ else
+ bCollision = GetEnv()->CheckCollision(r,&report);
+ }
else
- bCollision = GetEnv()->CheckCollision(r,&report);
+ RAVELOG_WARN("ray has zero direction\n");
res.collision.push_back(bCollision);
- if( req.request_contacts ) {
+ if( bCollision && req.request_contacts ) {
openraveros::Contact rosc;
if( report.contacts.size() > 0 ) {
COLLISIONREPORT::CONTACT& c = report.contacts.front();
@@ -820,11 +885,16 @@
}
res.contacts.push_back(rosc);
}
+ else
+ res.contacts.push_back(openraveros::Contact());
+
- if( req.request_bodies ) {
+ if( bCollision && req.request_bodies ) {
KinBody::Link* plink = report.plink1 != NULL ? report.plink1 : report.plink2;
res.hitbodies.push_back(plink != NULL ? plink->GetParent()->GetNetworkId() : 0);
}
+ else
+ res.hitbodies.push_back(0);
}
GetEnv()->SetCollisionOptions(oldopts);
@@ -1016,8 +1086,8 @@
map<int, boost::shared_ptr<ProblemInstance> >::iterator itprob = _mapproblems.find(req.problemid);
if( itprob == _mapproblems.end() )
return false;
-
- itprob->second->SendCommand(req.cmd.c_str(),res.output);
+
+ AddWorker(new SendProblemWorker(itprob->second,req.cmd,res.output), true);
return true;
}
@@ -1070,7 +1140,7 @@
probot->GetActiveDOFValues(vtemp);
res.values.resize(vtemp.size());
- for(size_t i = 0; i < req.indices.size(); ++i)
+ for(size_t i = 0; i < res.values.size(); ++i)
res.values[i] = vtemp[i];
}
@@ -1378,4 +1448,9 @@
boost::thread _threadviewer;
boost::mutex _mutexViewer;
boost::condition _conditionViewer;
+
+ /// workers
+ boost::mutex _mutexWorker;
+ list<boost::shared_ptr<WorkExecutor> > _listWorkers;
+ boost::condition _conditionWorkers;
};
Modified: pkg/trunk/openrave_planning/openraveros/src/session.h
===================================================================
--- pkg/trunk/openrave_planning/openraveros/src/session.h 2008-12-16 01:45:37 UTC (rev 8141)
+++ pkg/trunk/openrave_planning/openraveros/src/session.h 2008-12-16 02:00:14 UTC (rev 8142)
@@ -48,8 +48,6 @@
{
public:
virtual ~SessionState() {
- if( !!_penv )
- _penv->AttachServer(NULL);
_pserver.reset();
_penv.reset();
}
@@ -298,6 +296,7 @@
_penvViewer->AttachViewer(NULL);
_pviewer.reset();
_penvViewer = NULL;
+ usleep(200000); // give some time for destruction
_conditionViewer.notify_all();
}
}
@@ -361,7 +360,7 @@
}
state._pserver.reset(new ROSServer(new SessionSetViewerFunc(this), state._penv.get(), req.physicsengine, req.collisionchecker, req.viewer));
-
+ state._penv->AttachServer(state._pserver.get());
_mapsessions[id] = state;
res.sessionid = id;
Deleted: pkg/trunk/openrave_planning/openraveros/test/addexamplesdir.m
===================================================================
--- pkg/trunk/openrave_planning/openraveros/test/addexamplesdir.m 2008-12-16 01:45:37 UTC (rev 8141)
+++ pkg/trunk/openrave_planning/openraveros/test/addexamplesdir.m 2008-12-16 02:00:14 UTC (rev 8142)
@@ -1,21 +0,0 @@
-%% addexamplesdir(subdir)
-%%
-%% Adds the openrave examples directory
-function addexamplesdir(subdir)
-
-basepath = [];
-if( isunix() )
- %% try getting from openrave-config
- [status,basepath] = system('openrave-config --prefix');
- basepath = strtrim(basepath);
-end
-
-if( isempty(basepath) )
- basepath = 'C:\Program Files\openrave';
-end
-
-if( ~exist('subdir','var') )
- subdir = [];
-end
-
-addpath(fullfile(basepath,'share','openrave','examples',subdir));
Copied: pkg/trunk/openrave_planning/openraveros/test/getexamplesdir.m (from rev 8092, pkg/trunk/openrave_planning/openraveros/test/addexamplesdir.m)
===================================================================
--- pkg/trunk/openrave_planning/openraveros/test/getexamplesdir.m (rev 0)
+++ pkg/trunk/openrave_planning/openraveros/test/getexamplesdir.m 2008-12-16 02:00:14 UTC (rev 8142)
@@ -0,0 +1,21 @@
+%% exampledir=addexamplesdir(subdir)
+%%
+%% Adds the openrave examples directory
+function exampledir=getexamplesdir(subdir)
+
+basepath = [];
+if( isunix() )
+ %% try getting from openrave-config
+ [status,basepath] = system('openrave-config --prefix');
+ basepath = strtrim(basepath);
+end
+
+if( isempty(basepath) )
+ basepath = 'C:\Program Files\openrave';
+end
+
+if( ~exist('subdir','var') )
+ subdir = [];
+end
+
+exampledir=fullfile(basepath,'share','openrave','examples',subdir);
Added: pkg/trunk/openrave_planning/openraveros/test/testgrasping.m
===================================================================
--- pkg/trunk/openrave_planning/openraveros/test/testgrasping.m (rev 0)
+++ pkg/trunk/openrave_planning/openraveros/test/testgrasping.m 2008-12-16 02:00:14 UTC (rev 8142)
@@ -0,0 +1,5 @@
+%% Runs the openrave hanoi example
+addpath(fullfile(pwd,'..','octave')); % in case launched from test folder
+openraveros_restart();
+cd(getexamplesdir('grasping'));
+MakeBarrettHandTable();
Modified: pkg/trunk/openrave_planning/openraveros/test/testhanoi.m
===================================================================
--- pkg/trunk/openrave_planning/openraveros/test/testhanoi.m 2008-12-16 01:45:37 UTC (rev 8141)
+++ pkg/trunk/openrave_planning/openraveros/test/testhanoi.m 2008-12-16 02:00:14 UTC (rev 8142)
@@ -1,5 +1,5 @@
%% Runs the openrave hanoi example
addpath(fullfile(pwd,'..','octave')); % in case launched from test folder
openraveros_restart();
-addexamplesdir('hanoi_puzzle');
+addpath(getexamplesdir('hanoi_puzzle'));
hanoi
Added: pkg/trunk/openrave_planning/openraveros/test/testrosplotting.m
===================================================================
--- pkg/trunk/openrave_planning/openraveros/test/testrosplotting.m (rev 0)
+++ pkg/trunk/openrave_planning/openraveros/test/testrosplotting.m 2008-12-16 02:00:14 UTC (rev 8142)
@@ -0,0 +1,5 @@
+%% Runs the openrave hanoi example
+addpath(fullfile(pwd,'..','octave')); % in case launched from test folder
+openraveros_restart();
+addpath(getexamplesdir(''));
+testplotting
This was sent by the SourceForge.net collaborative development platform, the world's largest Open Source development site.
|
