Menu
▾
▴
[Personalrobots-commit] SF.net SVN: personalrobots:[9041] pkg/trunk
|
From: <rdi...@us...> - 2009-01-08 08:11:47
|
Revision: 9041
http://personalrobots.svn.sourceforge.net/personalrobots/?rev=9041&view=rev
Author: rdiankov
Date: 2009-01-08 08:11:42 +0000 (Thu, 08 Jan 2009)
Log Message:
-----------
openrave robot controller now uses the pr2_mechanism controllers services to send trajectories, added a testtrajectories script for testing the services directly with rosoct (bypassing the openrave controller)
Modified Paths:
--------------
pkg/trunk/controllers/pr2_mechanism_controllers/srv/TrajectoryStart.srv
pkg/trunk/openrave_planning/ormanipulation/octave/runperception.m
pkg/trunk/openrave_planning/orrosplanning/src/rosrobotcontroller.h
pkg/trunk/openrave_planning/orrosplanning/src/rossensorsystem.h
Added Paths:
-----------
pkg/trunk/openrave_planning/ormanipulation/octave/GetInterpolatedHandTrajectory.m
pkg/trunk/openrave_planning/ormanipulation/octave/QuatFromRotationMatrix.m
pkg/trunk/openrave_planning/ormanipulation/octave/QuatSlerp.m
pkg/trunk/openrave_planning/ormanipulation/octave/RotationMatrixFromQuat.m
pkg/trunk/openrave_planning/ormanipulation/octave/testtrajectories.m
Modified: pkg/trunk/controllers/pr2_mechanism_controllers/srv/TrajectoryStart.srv
===================================================================
--- pkg/trunk/controllers/pr2_mechanism_controllers/srv/TrajectoryStart.srv 2009-01-08 07:46:06 UTC (rev 9040)
+++ pkg/trunk/controllers/pr2_mechanism_controllers/srv/TrajectoryStart.srv 2009-01-08 08:11:42 UTC (rev 9041)
@@ -1,5 +1,6 @@
## Starts a trajectory
JointTraj traj
+uint8 hastiming # if 1, use timestamps of trajectory points, otherwise do internal retiming
uint8 requesttiming # if 1, send back the timestamps for every trajectory point (first point starts at 0)
---
uint32 trajectoryid # unique trajectory id to be used for later referencing
Added: pkg/trunk/openrave_planning/ormanipulation/octave/GetInterpolatedHandTrajectory.m
===================================================================
--- pkg/trunk/openrave_planning/ormanipulation/octave/GetInterpolatedHandTrajectory.m (rev 0)
+++ pkg/trunk/openrave_planning/ormanipulation/octave/GetInterpolatedHandTrajectory.m 2009-01-08 08:11:42 UTC (rev 9041)
@@ -0,0 +1,31 @@
+%% Ttrajectory = GetInterpolatedHandTrajectory(T0,T1,deltat, deltaq)
+%%
+%% Ttrajerctory is a 12xN array of 3x4 matrices
+function Ttrajectory = GetInterpolatedHandTrajectory(T0,T1,deltat, deltaq)
+
+if( ~exist('deltat','var') )
+ deltat = 0.01;
+end
+
+if( ~exist('deltaq','var') )
+ deltaq = 0.05;
+end
+
+q0 = QuatFromRotationMatrix(T0(1:3,1:3));
+q1 = QuatFromRotationMatrix(T1(1:3,1:3));
+t0 = T0(1:3,4);
+t1 = T1(1:3,4);
+
+if( norm(q0+q1) < norm(q0-q1) )
+ q1 = -q1;
+end
+
+numsegments = 1+max(norm(q0-q1)/deltaq, norm(t0-t1)/deltat);
+times = 0:(1/numsegments):1;
+
+Ttrajectory = zeros(12,length(times));
+for i = 1:length(times)
+ curtime = times(i);
+ Ttrajectory(1:9,i) = reshape(RotationMatrixFromQuat(QuatSlerp(q0,q1,curtime,1e-6)),[9 1]);
+ Ttrajectory(10:12,i) = t0*(1-curtime)+t1*curtime;
+end
Added: pkg/trunk/openrave_planning/ormanipulation/octave/QuatFromRotationMatrix.m
===================================================================
--- pkg/trunk/openrave_planning/ormanipulation/octave/QuatFromRotationMatrix.m (rev 0)
+++ pkg/trunk/openrave_planning/ormanipulation/octave/QuatFromRotationMatrix.m 2009-01-08 08:11:42 UTC (rev 9041)
@@ -0,0 +1,64 @@
+% quat = QuatFromRotationMatrix(R)
+%
+% R - 3x3 orthogonal rotation matrix
+% quat - the format is [cos(angle/2) axis*sin(angle/2)]
+
+% 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.
+% * 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.
+function quat = QuatFromRotationMatrix(R)
+
+quat = zeros(4,1);
+tr = R(1,1) + R(2,2) + R(3,3);
+if (tr >= 0)
+ s = sqrt(tr + 1);
+ quat(1) = 0.5*s;
+ quat(2:4) = [R(3,2)-R(2,3); R(1,3)-R(3,1); R(2,1) - R(1,2)]*0.5/s;
+else
+ %% find the largest diagonal element and jump to the appropriate case
+ [rmax, convcase] = max([R(1,1) R(2,2) R(3,3)]);
+ switch(convcase)
+ case 1
+ s = sqrt((R(1,1) - (R(2,2) + R(3,3))) + 1);
+ quat(2) = 0.5 * s;
+ s = 0.5 / s;
+ quat(3) = (R(1,2) + R(2,1)) * s;
+ quat(4) = (R(3,1) + R(1,3)) * s;
+ quat(1) = (R(3,2) - R(2,3)) * s;
+ case 2
+ s = sqrt((R(2,2) - (R(3,3) + R(1,1))) + 1);
+ quat(3) = 0.5 * s;
+ s = 0.5 / s;
+ quat(4) = (R(2,3) + R(3,2)) * s;
+ quat(2) = (R(1,2) + R(2,1)) * s;
+ quat(1) = (R(1,3) - R(3,1)) * s;
+ case 3
+ s = sqrt((R(3,3) - (R(1,1) + R(2,2))) + 1);
+ quat(4) = 0.5 * s;
+ s = 0.5 / s;
+ quat(2) = (R(3,1) + R(1,3)) * s;
+ quat(3) = (R(2,3) + R(3,2)) * s;
+ quat(1) = (R(2,1) - R(1,2)) * s;
+ end
+end
Added: pkg/trunk/openrave_planning/ormanipulation/octave/QuatSlerp.m
===================================================================
--- pkg/trunk/openrave_planning/ormanipulation/octave/QuatSlerp.m (rev 0)
+++ pkg/trunk/openrave_planning/ormanipulation/octave/QuatSlerp.m 2009-01-08 08:11:42 UTC (rev 9041)
@@ -0,0 +1,74 @@
+% [qm] = QuatSlerp(qi, qn, t, eps)
+%
+% where qi=[w1 x1 y1 z1] - start unit quaternions
+% qn=[w2 x2 y2 z2] - end unit quaternions
+% t=[0 to 1]
+% eps=threshold value
+
+
+% Sagi Dalyot %
+
+% This routine aims for calculating a unit quaternion, describing a rotation matrix,
+% which lies between two known unit quaternions - q1 and q2,
+% using a spherical linear interpolation - Slerp.
+% Slerp follow the shortest great arc on a unit sphere,
+% hence, the shortest possible interpolation path.
+% Consequently, Slerp has constant angular velocity,
+% so it is the optimal interpolation curve between two rotations.
+% (first published by Sheomake K., 1985 - Animating Rotation with Quaternion Curves)
+
+% end of file -> explnation of rotation matrix and quaternions
+
+% in general:
+% slerp(q1, q2, t) = q1*(sin(1-t)*teta)/sin(t) + q2*(sin(t*teta))/sin(teta)
+% where teta is the angle between the two unit quaternions,
+% and t is between [0,1]
+
+% two border cases will be delt:
+% 1: where q1 = q2 (or close by eps)
+% 2: where q1 = -q2 (angle between unit quaternions is 180 degrees).
+% in general, if q1=q2 then Slerp(q; q; t) == q
+
+function [qm] = QuatSlerp(qi, qn, t, eps)
+
+if t==0 % saving calculation time -> where qm=qi
+ qm=qi;
+
+elseif t==1 % saving calculation time -> where qm=qn
+ qm=qn;
+
+else
+
+ C=dot(qi,qn); % Calculating the angle beteen the unit quaternions by dot product
+
+ teta=acos(C);
+
+ if (1 - C) <= eps % if angle teta is close by epsilon to 0 degrees -> calculate by linear interpolation
+ qm=qi*(1-t)+qn*t; % avoiding divisions by number close to 0
+
+ elseif (1 + C) <= eps % when teta is close by epsilon to 180 degrees the result is undefined -> no shortest direction to rotate
+ q2(1) = qi(4); q2(2) = -qi(3); q2(3)= qi(2); q2(4) = -qi(1); % rotating one of the unit quaternions by 90 degrees -> q2
+ qm=qi*(sin((1-t)*(pi/2)))+q2*sin(t*(pi/2));
+
+ else
+ qm=qi*(sin((1-t)*teta))/sin(teta)+qn*sin(t*teta)/sin(teta);
+ end
+end
+
+
+% eof
+% q = [w, (x, y, z)] quaternion definition
+%
+% where
+% R = [1-2*y^2-2*z^2 2*x*y-2*w*z 2*x*z+2*w*y R is function of 3 euler rotation angles
+% 2*x*y+2*w*z 1-2*x^2-2*z^2 2*y*z-2*w*x
+% 2*x*z-2*w*y 2*y*z+2*w*x 1-2*x^2-2*y^2]
+% => R = [M00 M01 M02
+% M10 M11 M12
+% M20 M21 M22]
+% => trace(R) = 4 - 4*(x^2+y^2+z^2), and x^2+y^2+z^2+w^2==1
+% => w=(trace)^.5
+% => x=(M21-M12)/4*w
+% => y=(M02-M21)/4*w
+% => x=(M10-M01)/4*w
+% => q = [w, (x, y, z)]
Added: pkg/trunk/openrave_planning/ormanipulation/octave/RotationMatrixFromQuat.m
===================================================================
--- pkg/trunk/openrave_planning/ormanipulation/octave/RotationMatrixFromQuat.m (rev 0)
+++ pkg/trunk/openrave_planning/ormanipulation/octave/RotationMatrixFromQuat.m 2009-01-08 08:11:42 UTC (rev 9041)
@@ -0,0 +1,43 @@
+% R = RotationMatrixFromQuat(quat)
+%
+% quat - the format is [cos(angle/2) axis*sin(angle/2)]
+
+% 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.
+% * 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.
+function R = RotationMatrixFromQuat(quat)
+
+R = zeros(3,3);
+qq1 = 2*quat(2)*quat(2);
+qq2 = 2*quat(3)*quat(3);
+qq3 = 2*quat(4)*quat(4);
+R(1,1) = 1 - qq2 - qq3;
+R(1,2) = 2*(quat(2)*quat(3) - quat(1)*quat(4));
+R(1,3) = 2*(quat(2)*quat(4) + quat(1)*quat(3));
+R(2,1) = 2*(quat(2)*quat(3) + quat(1)*quat(4));
+R(2,2) = 1 - qq1 - qq3;
+R(2,3) = 2*(quat(3)*quat(4) - quat(1)*quat(2));
+R(3,1) = 2*(quat(2)*quat(4) - quat(1)*quat(3));
+R(3,2) = 2*(quat(3)*quat(4) + quat(1)*quat(2));
+R(3,3) = 1 - qq1 - qq2;
Modified: pkg/trunk/openrave_planning/ormanipulation/octave/runperception.m
===================================================================
--- pkg/trunk/openrave_planning/ormanipulation/octave/runperception.m 2009-01-08 07:46:06 UTC (rev 9040)
+++ pkg/trunk/openrave_planning/ormanipulation/octave/runperception.m 2009-01-08 08:11:42 UTC (rev 9041)
@@ -4,8 +4,6 @@
cd(fullfile(rosoct_findpackage('ormanipulation'),'octave'));
startup;
-%orEnvSetOptions('debug debug');
-
robot = SetupTableScene('data/pr2table_real.env.xml',1);
Tcamera = [0 0 1 0.05;
Added: pkg/trunk/openrave_planning/ormanipulation/octave/testtrajectories.m
===================================================================
--- pkg/trunk/openrave_planning/ormanipulation/octave/testtrajectories.m (rev 0)
+++ pkg/trunk/openrave_planning/ormanipulation/octave/testtrajectories.m 2009-01-08 08:11:42 UTC (rev 9041)
@@ -0,0 +1,58 @@
+#!/usr/bin/env octave
+global updir probs
+
+cd(fullfile(rosoct_findpackage('ormanipulation'),'octave'));
+
+startup;
+orEnvLoadScene('',1);
+robotid = orEnvCreateRobot('pr2','robots/pr2full.robot.xml');
+robot = orEnvGetRobots(robotid);
+orBodySetJointValues(robot.id,[ 0.000000 0.000000 0.000000 0.000000 0.000000 0.000000 0.000000 0.000000 0.000000 0.000000 0.000000 0.000000 0.000000 0.05 0.000000 0.000000 0.000000 0.000000 0.000000 1.582704 1.081165 0.000000 -2.299995 -0.000000 0.000000 0.000000 -0.000000 0.000000 0.000000 0.000000 0.000000 0.000000 -0.979114 -0.400000 0.000000 -1.535151 0.000000 0.000000 0.000000 0.000000 0.000000]);
+
+%orRobotControllerSet(robot.id, 'ROSRobot', ['trajectoryservice /']);
+
+probs.manip = orEnvCreateProblem('BaseManipulation',robot.name);
+imanipulator = 2;
+orProblemSendCommand(sprintf('setactivemanip %d', imanipulator-1), probs.manip);
+
+Tgripperstart = [1.00000 0.00000 0.00000 0.5
+ 0.00000 1.00000 0.00000 -0.04616
+ 0.00000 0.00000 1.00000 0.7];
+Tgripperend = [1.00000 0.00000 0.00000 0.5
+ 0.00000 1.00000 0.00000 -0.04616
+ 0.00000 0.00000 1.00000 1.2];
+
+Tgrippertraj = GetInterpolatedHandTrajectory(Tgripperstart, Tgripperend);
+trajectory = [];
+for i = 1:size(Tgrippertraj,2)
+ iksol = orProblemSendCommand(['iktest matrix ' sprintf('%f ', Tgrippertraj(:,i))],probs.manip)
+ if( isempty(iksol) )
+ display(sprintf('failed to find ik %d/%d', i, size(Tgrippertraj,2)));
+ break;
+ end
+
+ trajectory = [trajectory sscanf(iksol, '%f ')];
+ pause(0.1);
+end
+
+%% make a service call
+rosoct_add_srvs('pr2_mechanism_controllers');
+req = pr2_mechanism_controllers_TrajectoryStart();
+req.traj.points = {};
+for i = 1:size(trajectory,2)
+ req.traj.points{i} = pr2_mechanism_controllers_JointTrajPoint();
+ req.traj.points{i}.positions = trajectory(:,i);
+end
+res = rosoct_service_call('/TrajectoryStart',req);
+
+%% play it out in simulation
+for i = 1:size(trajectory,2)
+ orBodySetJointValues(robot.id,trajectory(:,i),robot.manips{imanipulator}.armjoints);
+ pause(0.1);
+end
+
+robothand = RobotCreatePR2Hand('pr2gripper');
+for i = 1:size(Tgrippertraj,2)
+ orBodySetTransform(robothand.id,Tgrippertraj(:,i));
+ pause(0.1);
+end
Modified: pkg/trunk/openrave_planning/orrosplanning/src/rosrobotcontroller.h
===================================================================
--- pkg/trunk/openrave_planning/orrosplanning/src/rosrobotcontroller.h 2009-01-08 07:46:06 UTC (rev 9040)
+++ pkg/trunk/openrave_planning/orrosplanning/src/rosrobotcontroller.h 2009-01-08 08:11:42 UTC (rev 9041)
@@ -32,20 +32,104 @@
class ROSRobotController : public ControllerBase
{
- enum ControllerState{
+ enum ControllerState {
None = 0,
Servo, // done when servoed to position and the position is held
Traj, // done when reaches last point
Velocity // done when joints stop moving
};
+ class TrajectoryController
+ {
+ public:
+ TrajectoryController(RobotBase* probot, const string& strTrajectoryServiceDir) : _probot(probot), _strTrajectoryServiceDir(strTrajectoryServiceDir) {}
+ virtual ~TrajectoryController() {
+ Destroy();
+ }
+
+ bool Init(ros::node* pnode)
+ {
+ assert(pnode != NULL);
+ Destroy();
+
+ _srvTrajectoryStart = ros::service::createHandle<pr2_mechanism_controllers::TrajectoryStart::request, pr2_mechanism_controllers::TrajectoryStart::response>(_strTrajectoryServiceDir+"TrajectoryStart", true);
+ _srvTrajectoryCancel = ros::service::createHandle<pr2_mechanism_controllers::TrajectoryCancel::request, pr2_mechanism_controllers::TrajectoryCancel::response>(_strTrajectoryServiceDir+"TrajectoryCancel", true);
+ _srvTrajectoryWait = ros::service::createHandle<pr2_mechanism_controllers::TrajectoryWait::request, pr2_mechanism_controllers::TrajectoryWait::response>(_strTrajectoryServiceDir+"TrajectoryWait", true);
+ _srvTrajectoryQuery = ros::service::createHandle<pr2_mechanism_controllers::TrajectoryQuery::request, pr2_mechanism_controllers::TrajectoryQuery::response>(_strTrajectoryServiceDir+"TrajectoryQuery", true);
+
+ if( !_srvTrajectoryQuery ) {
+ RAVELOG_ERRORA("failed to find %s service\n", (_strTrajectoryServiceDir+"TrajectoryQuery").c_str());
+ return false;
+ }
+
+ pr2_mechanism_controllers::TrajectoryQuery::request req;
+ pr2_mechanism_controllers::TrajectoryQuery::response res;
+ if( !_srvTrajectoryQuery->call(req,res) ) {
+ RAVELOG_ERRORA("failed to query trajectory service %s", _strTrajectoryServiceDir.c_str());
+ return false;
+ }
+
+ if( res.jointnames.size() == 0 ) {
+ RAVELOG_ERRORA("no joint names for trajectory service %s", _strTrajectoryServiceDir.c_str());
+ return false;
+ }
+
+ vector<string> vrobotjoints(_probot->GetJoints().size());
+ for(size_t i = 0; i < _probot->GetJoints().size(); ++i)
+ vrobotjoints[i] = _stdwcstombs(_probot->GetJoints()[i]->GetName());
+
+ _vjointmap.reserve(res.jointnames.size());
+ FOREACH(itname, res.jointnames) {
+ vector<string>::iterator itindex = find(vrobotjoints.begin(), vrobotjoints.end(), *itname);
+ if( itindex == vrobotjoints.end() ) {
+ RAVELOG_ERRORA("failed to find joint %s\n", itname->c_str());
+ Destroy();
+ return false;
+ }
+ _vjointmap.push_back((int)(itindex-vrobotjoints.begin()));
+ }
+
+ return true;
+ }
+
+ bool Destroy()
+ {
+ _vjointmap.clear();
+ _listTrajectories.clear();
+ _srvTrajectoryStart.reset();
+ _srvTrajectoryCancel.reset();
+ _srvTrajectoryWait.reset();
+ _srvTrajectoryQuery.reset();
+ return true;
+ }
+
+ void GetTrajPoint(const vector<dReal>& vrobotvalues, pr2_mechanism_controllers::JointTrajPoint& pt)
+ {
+ pt.positions.resize(_vjointmap.size());
+ typeof(pt.positions.begin()) itcontrollerpos = pt.positions.begin();
+ FOREACH(itindex, _vjointmap)
+ *itcontrollerpos++ = vrobotvalues[*itindex];
+ }
+
+ // trajectory services
+ RobotBase* _probot;
+ string _strTrajectoryServiceDir;
+ service::ServiceHandlePtr _srvTrajectoryStart, _srvTrajectoryCancel, _srvTrajectoryWait, _srvTrajectoryQuery;
+ vector<int> _vjointmap;
+ list<uint32_t> _listTrajectories; ///< trajectories currently pending for completion
+ };
+
public:
ROSRobotController(EnvironmentBase* penv) : ControllerBase(penv), _topic("mechanism_state"), _fTimeCommandStarted(0),
- _ptraj(NULL), _bIsDone(false), _bSendTimestamps(false), _bSubscribed(false), _bCalibrated(false) {
+ _ptraj(NULL), _bIsDone(false), _bSendTimestamps(false), _bSubscribed(false), _bCalibrated(false), _bDestroyThread(false) {
+ _threadTrajectories = boost::thread(boost::bind(&ROSRobotController::_TrajectoryThread,this));
}
virtual ~ROSRobotController() {
Destroy();
+
+ _bDestroyThread = true;
+ _threadTrajectories.join();
}
/// args format: host port [proxytype index]
@@ -92,6 +176,12 @@
break;
}
+ else if( stricmp(cmd.c_str(), "trajectoryservice") == 0 ) {
+ string servicedir;
+ ss >> servicedir;
+ boost::mutex::scoped_lock lock(_mutexTrajectories);
+ _listControllers.push_back(boost::shared_ptr<TrajectoryController>(new TrajectoryController(_probot, servicedir)));
+ }
else break;
if( !ss ) {
@@ -120,6 +210,11 @@
_probot = NULL;
_bIsDone = false;
_setEnabledJoints.clear();
+
+ {
+ boost::mutex::scoped_lock lock(_mutexTrajectories);
+ _listControllers.clear();
+ }
}
virtual void Reset(int options)
@@ -128,13 +223,89 @@
virtual bool SetDesired(const dReal* pValues)
{
+ boost::mutex::scoped_lock lock(_mutexTrajectories);
+
// set a path between the current and desired positions
+ bool bSuccess = true;
+ pr2_mechanism_controllers::TrajectoryStart::request req;
+ pr2_mechanism_controllers::TrajectoryStart::response res;
+
+ vector<dReal> vcurvalues, vnewvalues;
+ _probot->GetJointValues(vcurvalues);
+
+ {
+ RobotBase::RobotStateSaver saver(_probot);
+ _probot->SetJointValues(NULL, NULL, pValues,true);
+ _probot->GetJointValues(vcurvalues);
+ }
+
+ FOREACH(ittrajcontroller, _listControllers) {
+ if( !(*ittrajcontroller)->_srvTrajectoryStart ) {
+ RAVELOG_ERRORA("no start trajectory service\n");
+ bSuccess = false;
+ continue;
+ }
+
+ //req.requesttiming = 1; // request back the timestamps
+ req.hastiming = 0;
+ req.traj.points.resize(2);
+ (*ittrajcontroller)->GetTrajPoint(vcurvalues, req.traj.points[0]);
+ (*ittrajcontroller)->GetTrajPoint(vnewvalues, req.traj.points[1]);
+
+ if( (*ittrajcontroller)->_srvTrajectoryStart->call(req,res) )
+ (*ittrajcontroller)->_listTrajectories.push_back(res.trajectoryid);
+ else {
+ RAVELOG_ERRORA("failed to start trajectory\n");
+ bSuccess = false;
+ continue;
+ }
+
+ RAVELOG_DEBUGA("started trajectory %d\n", res.trajectoryid);
+ //res.timestamps // final timestamps
+ }
+
return true;
}
virtual bool SetPath(const Trajectory* ptraj)
{
- return true;
+ boost::mutex::scoped_lock lock(_mutexTrajectories);
+
+ bool bSuccess = true;
+ pr2_mechanism_controllers::TrajectoryStart::request req;
+ pr2_mechanism_controllers::TrajectoryStart::response res;
+
+ FOREACH(ittrajcontroller, _listControllers) {
+ if( !(*ittrajcontroller)->_srvTrajectoryStart ) {
+ RAVELOG_ERRORA("no start trajectory service\n");
+ bSuccess = false;
+ continue;
+ }
+
+ //req.requesttiming = 1; // request back the timestamps
+ req.hastiming = 0;
+ req.traj.points.resize(ptraj->GetPoints().size());
+ typeof(req.traj.points.begin()) ittraj = req.traj.points.begin();
+ FOREACHC(itpoint, ptraj->GetPoints()) {
+ ittraj->positions.reserve(itpoint->q.size()); ittraj->positions.resize(0);
+ FOREACHC(itq, itpoint->q)
+ ittraj->positions.push_back(*itq);
+ ++ittraj;
+ }
+
+ if( (*ittrajcontroller)->_srvTrajectoryStart->call(req,res) )
+ (*ittrajcontroller)->_listTrajectories.push_back(res.trajectoryid);
+ else {
+ RAVELOG_ERRORA("failed to start trajectory\n");
+ bSuccess = false;
+ continue;
+ }
+
+ RAVELOG_DEBUGA("started trajectory %d\n", res.trajectoryid);
+ //res.timestamps // final timestamps
+ }
+
+ return bSuccess;
}
virtual bool SetPath(const Trajectory* ptraj, int nTrajectoryId, float fDivergenceTime)
@@ -143,8 +314,6 @@
return false;
}
- virtual int GetDOF() { return _probot != NULL ? _probot->GetDOF() : 0; }
-
virtual bool SimulationStep(float fTimeElapsed)
{
if( !_bSubscribed )
@@ -248,6 +417,12 @@
RAVELOG_DEBUGA("subscribed to %s\n", _topic.c_str());
else
RAVELOG_ERRORA("failed to subscribe to %s\n", _topic.c_str());
+
+ if( _bSubscribed ) {
+ boost::mutex::scoped_lock lock(_mutexTrajectories);
+ FOREACH(it, _listControllers)
+ (*it)->Init(pnode);
+ }
}
}
@@ -258,6 +433,10 @@
if( pnode != NULL ) {
pnode->unsubscribe(_topic.c_str());
RAVELOG_DEBUGA("unsubscribe from %s\n", _topic.c_str());
+
+ boost::mutex::scoped_lock lock(_mutexTrajectories);
+ FOREACH(it, _listControllers)
+ (*it)->Destroy();
}
_bSubscribed = false;
}
@@ -309,9 +488,58 @@
// do some monitoring of the joint state (try to look for stalls)
}
+ void _TrajectoryThread()
+ {
+ while(!_bDestroyThread) {
+
+ pr2_mechanism_controllers::TrajectoryQuery::request req;
+ pr2_mechanism_controllers::TrajectoryQuery::response res;
+
+ // check if the first trajectory is done
+ boost::mutex::scoped_lock lock(_mutexTrajectories);
+ uint32_t trajectoryid;
+ bool bDone = true;
+
+ if( _listControllers.size() > 0 ) {
+ bool bPopTrajectory = true;
+
+ // check if done
+ FOREACH(ittraj, _listControllers) {
+ assert( (*ittraj)->_listTrajectories.size() == _listControllers.front()->_listTrajectories.size());
+ if( (*ittraj)->_listTrajectories.size() == 0 ) {
+ bPopTrajectory = false;
+ break;
+ }
+ req.trajectoryid = (*ittraj)->_listTrajectories.front();
+ if( !(*ittraj)->_srvTrajectoryQuery->call(req,res) ) {
+ RAVELOG_ERRORA("trajectory query failed\n");
+ bPopTrajectory = false;
+ }
+
+ if( !res.done ) {
+ bPopTrajectory = false;
+ break;
+ }
+ }
+
+ if( bPopTrajectory ) {
+ RAVELOG_DEBUGA("robot trajectory finished, left: %d\n", _listControllers.front()->_listTrajectories.size());
+ FOREACH(ittraj, _listControllers)
+ (*ittraj)->_listTrajectories.pop_front();
+ }
+
+ bDone = _listControllers.front()->_listTrajectories.size()==0;
+ }
+
+ _bIsDone = bDone;
+ usleep(10000); // query every 10ms
+ }
+ }
+
RobotBase* _probot; ///< robot owning this controller
string _topic;
+
robot_msgs::MechanismState _mstate_cb, _mstate;
vector<dReal> _vecdesired;
set< pair<string, int> > _setEnabledJoints; // set of enabled joints and their indices
@@ -321,16 +549,19 @@
int logid;
map<int, int> _mapjoints; ///< maps mechanism state joints to robot joints
+ list<boost::shared_ptr<TrajectoryController> > _listControllers;
double _fTimeCommandStarted;
const Trajectory* _ptraj;
- // trajectory services
- service::ServiceHandlePtr srvStartTrajectory, srvCancelTrajectory, srvWaitTrajectory, srvQueryTrajectory;
+ boost::mutex _mutexTrajectories;
+ boost::thread _threadTrajectories;
+
bool _bIsDone;
bool _bSendTimestamps; ///< if true, will send timestamps along with traj
bool _bSubscribed; ///< if true, succesfully subscribed to the mechanism state msgs
bool _bCalibrated; ///< if true, mechanism state matches robot
+ bool _bDestroyThread; /// if true, destroy the thread
};
#endif
Modified: pkg/trunk/openrave_planning/orrosplanning/src/rossensorsystem.h
===================================================================
--- pkg/trunk/openrave_planning/orrosplanning/src/rossensorsystem.h 2009-01-08 07:46:06 UTC (rev 9040)
+++ pkg/trunk/openrave_planning/orrosplanning/src/rossensorsystem.h 2009-01-08 08:11:42 UTC (rev 9041)
@@ -35,7 +35,7 @@
public:
ROSSensorSystem(EnvironmentBase* penv) : SimpleSensorSystem<XMLID>(penv), _bSubscribed(false)
{
-
+
}
virtual ~ROSSensorSystem() {
Destroy();
This was sent by the SourceForge.net collaborative development platform, the world's largest Open Source development site.
|
