Menu
▾
▴
[Personalrobots-commit] SF.net SVN: personalrobots:[9128] pkg/trunk
|
From: <rdi...@us...> - 2009-01-09 13:44:45
|
Revision: 9128
http://personalrobots.svn.sourceforge.net/personalrobots/?rev=9128&view=rev
Author: rdiankov
Date: 2009-01-09 13:44:38 +0000 (Fri, 09 Jan 2009)
Log Message:
-----------
several openrave manipulation changes and bug fixes, openrave can now control the real robot and go through a part of the manipulation script with it
Modified Paths:
--------------
pkg/trunk/3rdparty/openrave/Makefile
pkg/trunk/openrave_planning/openraveros/octave/orBodyGetTransform.m
pkg/trunk/openrave_planning/openraveros/src/rosserver.h
pkg/trunk/openrave_planning/ormanipulation/data/pr2table_real.env.xml
pkg/trunk/openrave_planning/ormanipulation/octave/GetObjectToManipulate.m
pkg/trunk/openrave_planning/ormanipulation/octave/GraspAndPlaceObject.m
pkg/trunk/openrave_planning/ormanipulation/octave/MakePR2GraspTables.m
pkg/trunk/openrave_planning/ormanipulation/octave/RunDynamicGrasping.m
pkg/trunk/openrave_planning/ormanipulation/octave/SetupTableScene.m
pkg/trunk/openrave_planning/ormanipulation/octave/grasp_pr2_ricebox.mat
pkg/trunk/openrave_planning/ormanipulation/octave/rungrasptables.m
pkg/trunk/openrave_planning/ormanipulation/octave/runperception.m
pkg/trunk/openrave_planning/ormanipulation/octave/startup.m
pkg/trunk/openrave_planning/ormanipulation/octave/testtrajectories.m
pkg/trunk/openrave_planning/orrosplanning/src/objecttransformsystem.h
pkg/trunk/openrave_planning/orrosplanning/src/rosrobotcontroller.h
Modified: pkg/trunk/3rdparty/openrave/Makefile
===================================================================
--- pkg/trunk/3rdparty/openrave/Makefile 2009-01-09 13:24:54 UTC (rev 9127)
+++ pkg/trunk/3rdparty/openrave/Makefile 2009-01-09 13:44:38 UTC (rev 9128)
@@ -2,7 +2,7 @@
SVN_DIR = openrave_svn
# Should really specify a revision
-SVN_REVISION = -r 599
+SVN_REVISION = -r 600
SVN_URL = https://openrave.svn.sourceforge.net/svnroot/openrave
include $(shell rospack find mk)/svn_checkout.mk
Modified: pkg/trunk/openrave_planning/openraveros/octave/orBodyGetTransform.m
===================================================================
--- pkg/trunk/openrave_planning/openraveros/octave/orBodyGetTransform.m 2009-01-09 13:24:54 UTC (rev 9127)
+++ pkg/trunk/openrave_planning/openraveros/octave/orBodyGetTransform.m 2009-01-09 13:44:38 UTC (rev 9128)
@@ -7,4 +7,8 @@
function values = orBodyGetTransform(bodyid)
links = orBodyGetLinks(bodyid);
-values = links(:,1);
\ No newline at end of file
+if( ~isempty(links) )
+ values = links(:,1);
+else
+ values = [];
+end
Modified: pkg/trunk/openrave_planning/openraveros/src/rosserver.h
===================================================================
--- pkg/trunk/openrave_planning/openraveros/src/rosserver.h 2009-01-09 13:24:54 UTC (rev 9127)
+++ pkg/trunk/openrave_planning/openraveros/src/rosserver.h 2009-01-09 13:44:38 UTC (rev 9128)
@@ -201,9 +201,12 @@
// destroy environment specific state: problems, planners, figures
_mapFigureIds.clear();
- FOREACH(itprob, _mapproblems)
- itprob->second->GetEnv()->RemoveProblem(itprob->second.get());
- _mapproblems.clear();
+ {
+ boost::mutex::scoped_lock lock(_mutexProblems);
+ FOREACH(itprob, _mapproblems)
+ itprob->second->GetEnv()->RemoveProblem(itprob->second.get());
+ _mapproblems.clear();
+ }
_mapplanners.clear();
}
@@ -616,6 +619,8 @@
if( !pproblem )
return false;
+ boost::mutex::scoped_lock lock(_mutexProblems);
+
if( req.destroyduplicates ) {
map<int, boost::shared_ptr<ProblemInstance> >::iterator itprob = _mapproblems.begin();
while(itprob != _mapproblems.end()) {
@@ -666,6 +671,7 @@
bool env_destroyproblem_srv(env_destroyproblem::request& req, env_destroyproblem::response& res)
{
+ boost::mutex::scoped_lock lock(_mutexProblems);
map<int, boost::shared_ptr<ProblemInstance> >::iterator itprob = _mapproblems.find(req.problemid);
if( itprob == _mapproblems.end() )
return false;
@@ -1012,6 +1018,7 @@
bool env_set_srv(env_set::request& req, env_set::response& res)
{
if( req.setmask & env_set::request::Set_Simulation ) {
+ LockEnvironment envlock(this);
switch(req.sim_action) {
case env_set::request::SimAction_Start:
if( req.sim_timestep > 0 )
@@ -1196,6 +1203,7 @@
bool problem_sendcommand_srv(problem_sendcommand::request& req, problem_sendcommand::response& res)
{
+ boost::mutex::scoped_lock lock(_mutexProblems);
map<int, boost::shared_ptr<ProblemInstance> >::iterator itprob = _mapproblems.find(req.problemid);
if( itprob == _mapproblems.end() )
return false;
@@ -1563,7 +1571,7 @@
/// viewer control variables
boost::shared_ptr<RaveViewerBase> _pviewer;
boost::thread _threadviewer;
- boost::mutex _mutexViewer;
+ boost::mutex _mutexViewer, _mutexProblems;
boost::condition _conditionViewer;
/// workers
Modified: pkg/trunk/openrave_planning/ormanipulation/data/pr2table_real.env.xml
===================================================================
--- pkg/trunk/openrave_planning/ormanipulation/data/pr2table_real.env.xml 2009-01-09 13:24:54 UTC (rev 9127)
+++ pkg/trunk/openrave_planning/ormanipulation/data/pr2table_real.env.xml 2009-01-09 13:44:38 UTC (rev 9128)
@@ -18,6 +18,7 @@
<Kinbody name="floor">
<Body>
+ <translation>0 0 -0.03</translation>
<Geom type="box">
<extents>4 4 0.01</extents>
<translation>0 0 -0.01</translation>
Modified: pkg/trunk/openrave_planning/ormanipulation/octave/GetObjectToManipulate.m
===================================================================
--- pkg/trunk/openrave_planning/ormanipulation/octave/GetObjectToManipulate.m 2009-01-09 13:24:54 UTC (rev 9127)
+++ pkg/trunk/openrave_planning/ormanipulation/octave/GetObjectToManipulate.m 2009-01-09 13:44:38 UTC (rev 9128)
@@ -83,7 +83,7 @@
[dests, surfaceplane] = scenedata.GetDestsFn(); % compute a set of destinations
-offsetfromtable = 0.02; %% set destination a little above the table
+offsetfromtable = 0.04; %% set destination a little above the table
distup = surfaceplane(1:3)*(transpose(surfaceplane(1:3))*curobj.info.T(1:3,4) + surfaceplane(4) + offsetfromtable);
curobj.dests = zeros(size(dests));
Modified: pkg/trunk/openrave_planning/ormanipulation/octave/GraspAndPlaceObject.m
===================================================================
--- pkg/trunk/openrave_planning/ormanipulation/octave/GraspAndPlaceObject.m 2009-01-09 13:24:54 UTC (rev 9127)
+++ pkg/trunk/openrave_planning/ormanipulation/octave/GraspAndPlaceObject.m 2009-01-09 13:44:38 UTC (rev 9128)
@@ -76,13 +76,11 @@
robotid = robot.id;
while(curgrasp < size(grasps,1))
-
- %% C++ grasp testing (fast)
g = transpose(grasps(curgrasp:end,:));
offset = 0.02;
basetime = toc;
- cmd = ['testallgrasps execute 0 outputtraj palmdir ' sprintf('%f ', handrobot.palmdir) ...
+ cmd = ['testallgrasps maxiter 1000 execute 0 outputtraj palmdir ' sprintf('%f ', handrobot.palmdir) ...
' seedik 1 seedgrasps 5 seeddests 8 randomdests 1 randomgrasps 1 ' ...
' target ' curobj.info.name ' robothand ' handrobot.name ...
' robothandjoints ' sprintf('%d ', length(handjoints), handjoints) ...
@@ -175,7 +173,8 @@
% orRobotControllerSend(robotid, 'ignoreproxy 1');
% else
display('closing fingers');
- [trajdata, success] = orProblemSendCommand(['CloseFingers execute 0 outputtraj offset ' sprintf('%f ', 0.04*ones(size(handjoints)))] , probs.manip);
+ closeoffset = 0.12;
+ [trajdata, success] = orProblemSendCommand(['CloseFingers execute 0 outputtraj offset ' sprintf('%f ', closeoffset*ones(size(handjoints)))] , probs.manip);
if( ~success )
error('failed to movehandstraight');
end
@@ -295,7 +294,13 @@
%% cannot wait forever since hand might get stuck
if( ~success )
- error('failed to release fingers');
+ warning('problems releasing, releasing target first');
+ orProblemSendCommand('releaseall', probs.manip);
+ [trajdata, success] = orProblemSendCommand(['ReleaseFingers execute 0 outputtraj target ' curobj.info.name ...
+ ' movingdir ' sprintf('%f ', handrobot.releasedir)], probs.manip);
+ if( ~success )
+ error('failed to release fingers');
+ end
end
success = StartTrajectory(robotid,trajdata,4);
Modified: pkg/trunk/openrave_planning/ormanipulation/octave/MakePR2GraspTables.m
===================================================================
--- pkg/trunk/openrave_planning/ormanipulation/octave/MakePR2GraspTables.m 2009-01-09 13:24:54 UTC (rev 9127)
+++ pkg/trunk/openrave_planning/ormanipulation/octave/MakePR2GraspTables.m 2009-01-09 13:44:38 UTC (rev 9128)
@@ -64,7 +64,7 @@
orBodySetTransform(Target.id, [0 0 0], [1 0 0 0]); % identity
-standoffs = [0.01 0.025];
+standoffs = [0.01];
rolls = [0 pi/2]; % hand is symmetric
% start simulating grasps
Modified: pkg/trunk/openrave_planning/ormanipulation/octave/RunDynamicGrasping.m
===================================================================
--- pkg/trunk/openrave_planning/ormanipulation/octave/RunDynamicGrasping.m 2009-01-09 13:24:54 UTC (rev 9127)
+++ pkg/trunk/openrave_planning/ormanipulation/octave/RunDynamicGrasping.m 2009-01-09 13:44:38 UTC (rev 9128)
@@ -30,9 +30,9 @@
global updir probs realsession
setrealsession();
-if( ~simulation )
- DeleteObjects(scenedata.TargetObjPattern);
-end
+% if( ~simulation )
+% DeleteObjects(scenedata.TargetObjPattern);
+% end
MySwitchModels = @(x) SwitchModels(scenedata.SwitchModelPatterns, x);
%MySwitchModels = @(x) 0;
@@ -66,7 +66,7 @@
curobj.info = orEnvGetBodies(curobj.id);
if( isempty(curobj.info) )
- display(sprintf('failed to get info for obj %d (might have been deleted)', curobj.info.name));
+ display(sprintf('failed to get info for obj %d (might have been deleted)', curobj.id));
continue;
end
Modified: pkg/trunk/openrave_planning/ormanipulation/octave/SetupTableScene.m
===================================================================
--- pkg/trunk/openrave_planning/ormanipulation/octave/SetupTableScene.m 2009-01-09 13:24:54 UTC (rev 9127)
+++ pkg/trunk/openrave_planning/ormanipulation/octave/SetupTableScene.m 2009-01-09 13:44:38 UTC (rev 9128)
@@ -28,6 +28,7 @@
function [robot, scenedata] = SetupTableScene(scene,realrobot,randomize)
global updir probs robothome
+setrealsession();
if( ~exist('realrobot','var') )
realrobot = 0;
@@ -119,7 +120,7 @@
enabledjoints([robot.manips{1}.armjoints; robot.manips{1}.handjoints]) = [];
jointnames_cell = transpose(robot.jointnames(enabledjoints+1));
jointnames_str = cell2mat (cellfun(@(x) [x ' '], jointnames_cell,'uniformoutput',false));
- orRobotControllerSet(robot.id, 'ROSRobot', ['joints ' jointnames_str]);
+ orRobotControllerSet(robot.id, 'ROSRobot', ['trajectoryservice / joints ' jointnames_str]);
end
%% dests is a 12xN array where every column is a 3x4 matrix
@@ -153,12 +154,12 @@
end
Nx = 4;
-Ny = 5;
+Ny = 10;
X = [];
Y = [];
for x = 0:(Nx-1)
X = [X 0.5*rand(1,Ny)/(Nx+1) + (x+1)/(Nx+1)];
- Y = [Y 0.5*rand(1,Ny)/(Ny+1) + ([0:(Ny-1)]+0.5)/(Ny+1)];
+ Y = [Y 0.5*rand(1,Ny)/(Ny+1) + 2*([0:(Ny-1)]-Ny/2)/(Ny+1)];
end
offset = [ab(1,1)-ab(1,2);ab(2,1); ab(3,1)+ab(3,2)];
Modified: pkg/trunk/openrave_planning/ormanipulation/octave/grasp_pr2_ricebox.mat
===================================================================
(Binary files differ)
Modified: pkg/trunk/openrave_planning/ormanipulation/octave/rungrasptables.m
===================================================================
--- pkg/trunk/openrave_planning/ormanipulation/octave/rungrasptables.m 2009-01-09 13:24:54 UTC (rev 9127)
+++ pkg/trunk/openrave_planning/ormanipulation/octave/rungrasptables.m 2009-01-09 13:44:38 UTC (rev 9128)
@@ -10,4 +10,5 @@
%% loop forever
while(1)
RunGrasps('grasp_pr2_ricebox.mat');
+ pause(0.1);
end
Modified: pkg/trunk/openrave_planning/ormanipulation/octave/runperception.m
===================================================================
--- pkg/trunk/openrave_planning/ormanipulation/octave/runperception.m 2009-01-09 13:24:54 UTC (rev 9127)
+++ pkg/trunk/openrave_planning/ormanipulation/octave/runperception.m 2009-01-09 13:44:38 UTC (rev 9128)
@@ -4,11 +4,14 @@
cd(fullfile(rosoct_findpackage('ormanipulation'),'octave'));
startup;
-robot = SetupTableScene('data/pr2table_real.env.xml',1);
+[robot, scenedata] = SetupTableScene('data/pr2table_real.env.xml',1);
-Tcamera = [0 0 1 0.05;
- -1 0 0 0.05;
- 0 -1 0 0.095];
+Tlaser = [1.00000 0.00062 0.00232 0.01577
+ -0.00066 0.99988 0.01554 -0.00208
+ -0.00231 -0.01555 0.99988 0.03934];
+Tcamera = [0.01611 0.01631 0.99974 0.02890
+ -0.99966 -0.02020 0.01644 0.03236
+ 0.02047 -0.99966 0.01598 0.06732];
out = orProblemSendCommand(['createsystem ObjectTransform topic /checkerdetector/ObjectDetection thresh 0.1 robot ' sprintf('%d ', robot.id) ' matrixoffset ' sprintf('%f ', Tcamera(1:3,1:4))],probs.task);
if( isempty(out) )
error('failed to create checkerboard detector');
Modified: pkg/trunk/openrave_planning/ormanipulation/octave/startup.m
===================================================================
--- pkg/trunk/openrave_planning/ormanipulation/octave/startup.m 2009-01-09 13:24:54 UTC (rev 9127)
+++ pkg/trunk/openrave_planning/ormanipulation/octave/startup.m 2009-01-09 13:44:38 UTC (rev 9128)
@@ -14,5 +14,6 @@
setrealsession();
openraveros_restart();
orEnvSetOptions('wdims 800 600');
+orEnvSetOptions('simulation timestep 0.001');
%orEnvSetOptions('collision bullet');
%orEnvSetOptions('debug debug');
Modified: pkg/trunk/openrave_planning/ormanipulation/octave/testtrajectories.m
===================================================================
--- pkg/trunk/openrave_planning/ormanipulation/octave/testtrajectories.m 2009-01-09 13:24:54 UTC (rev 9127)
+++ pkg/trunk/openrave_planning/ormanipulation/octave/testtrajectories.m 2009-01-09 13:44:38 UTC (rev 9128)
@@ -9,7 +9,7 @@
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 /']);
+orRobotControllerSet(robot.id, 'ROSRobot', ['trajectoryservice /']);
probs.manip = orEnvCreateProblem('BaseManipulation',robot.name);
imanipulator = 2;
@@ -34,14 +34,17 @@
trajectory = [trajectory sscanf(iksol, '%f ')];
pause(0.1);
end
+trajectory = [trajectory trajectory(:,end:-1:1)];
%% make a service call
rosoct_add_srvs('pr2_mechanism_controllers');
+resquery = rosoct_service_call('/TrajectoryQuery',pr2_mechanism_controllers_TrajectoryQuery());
+
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);
+ req.traj.points{i}.positions = [trajectory(:,i);0.4];
end
res = rosoct_service_call('/TrajectoryStart',req);
Modified: pkg/trunk/openrave_planning/orrosplanning/src/objecttransformsystem.h
===================================================================
--- pkg/trunk/openrave_planning/orrosplanning/src/objecttransformsystem.h 2009-01-09 13:24:54 UTC (rev 9127)
+++ pkg/trunk/openrave_planning/orrosplanning/src/objecttransformsystem.h 2009-01-09 13:44:38 UTC (rev 9128)
@@ -95,6 +95,7 @@
ROSSensorSystem<checkerboard_detector::ObjectDetection, ObjectTransformXMLID>::startsubscriptions();
if( _bSubscribed ) {
+ boost::mutex::scoped_lock lock(_mutex);
ros::node* pnode = check_roscpp_nocreate();
if( pnode != NULL ) {
double tf_cache_time_secs;
@@ -155,7 +156,7 @@
Transform tnew;
// if on robot, have to find the global transformation
- if( bHasRobotTransform ) {
+ if( bHasRobotTransform && !!_tf ) {
posestamped.pose = GetPose(_toffset * GetTransform(itobj->pose));
posestamped.header = _topicmsg.header;
@@ -164,8 +165,17 @@
tnew = trobot * GetTransform(poseout.pose);
}
catch(tf::TransformException& ex) {
- RAVELOG_WARNA("failed to get tf frames %s (body link:%s) for object %s\n",posestamped.header.frame_id.c_str(), strrobotbaselink.c_str(), itobj->type.c_str());
- tnew = GetTransform(itobj->pose);
+
+ try {
+ // try getting the latest value by passing a 0 timestamp
+ posestamped.header.stamp = ros::Time();
+ _tf->transformPose(strrobotbaselink, posestamped, poseout);
+ tnew = trobot * GetTransform(poseout.pose);
+ }
+ catch(tf::TransformException& ex) {
+ RAVELOG_WARNA("failed to get tf frames %s (body link:%s) for object %s\n",posestamped.header.frame_id.c_str(), strrobotbaselink.c_str(), itobj->type.c_str());
+ tnew = GetTransform(itobj->pose);
+ }
}
}
else
@@ -222,12 +232,16 @@
continue;
}
- if( AddKinBody(pbody, NULL) == NULL ) {
+ BODY* b = AddKinBody(pbody, NULL);
+ if( b == NULL ) {
delete pbody;
continue;
}
- pbody->SetTransform(itobj->second);
+ b->tnew = itobj->second;
+
+ // put somewhere at infinity until UpdateBodies thread gets to it
+ pbody->SetTransform(Transform(Vector(1,0,0,0), Vector(10000,10000,10000)));
}
GetEnv()->LockPhysics(false);
}
Modified: pkg/trunk/openrave_planning/orrosplanning/src/rosrobotcontroller.h
===================================================================
--- pkg/trunk/openrave_planning/orrosplanning/src/rosrobotcontroller.h 2009-01-09 13:24:54 UTC (rev 9127)
+++ pkg/trunk/openrave_planning/orrosplanning/src/rosrobotcontroller.h 2009-01-09 13:44:38 UTC (rev 9128)
@@ -51,17 +51,17 @@
{
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);
+
+ _srvTrajectoryStart = ros::service::createHandle<pr2_mechanism_controllers::TrajectoryStart::request, pr2_mechanism_controllers::TrajectoryStart::response>(_strTrajectoryServiceDir+"TrajectoryStart", false);
+ _srvTrajectoryCancel = ros::service::createHandle<pr2_mechanism_controllers::TrajectoryCancel::request, pr2_mechanism_controllers::TrajectoryCancel::response>(_strTrajectoryServiceDir+"TrajectoryCancel", false);
+ _srvTrajectoryWait = ros::service::createHandle<pr2_mechanism_controllers::TrajectoryWait::request, pr2_mechanism_controllers::TrajectoryWait::response>(_strTrajectoryServiceDir+"TrajectoryWait", false);
+ _srvTrajectoryQuery = ros::service::createHandle<pr2_mechanism_controllers::TrajectoryQuery::request, pr2_mechanism_controllers::TrajectoryQuery::response>(_strTrajectoryServiceDir+"TrajectoryQuery", false);
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) ) {
@@ -230,15 +230,32 @@
pr2_mechanism_controllers::TrajectoryStart::request req;
pr2_mechanism_controllers::TrajectoryStart::response res;
- vector<dReal> vcurvalues, vnewvalues;
- _probot->GetJointValues(vcurvalues);
+ vector<dReal> vnewvalues;
{
RobotBase::RobotStateSaver saver(_probot);
_probot->SetJointValues(NULL, NULL, pValues,true);
- _probot->GetJointValues(vcurvalues);
+ _probot->GetJointValues(vnewvalues);
}
+ // check if values are sufficiently different
+ if( _vcurvalues.size() == vnewvalues.size() ) {
+ dReal fthresh = 0.01;
+ bool bSendTraj = false;
+ for(size_t i = 0; i < _vcurvalues.size(); ++i) {
+ if( RaveFabs(_vcurvalues[i] - vnewvalues[i]) > fthresh ) {
+ bSendTraj = true;
+ }
+ }
+
+ if( !bSendTraj ) {
+ RAVELOG_VERBOSEA("setdesired sent same joint value, ignoring\n");
+ return false;
+ }
+ }
+ else
+ RAVELOG_WARNA("number of current values (%d) != desird values (%d)\n", _vcurvalues.size(), vnewvalues.size());
+
FOREACH(ittrajcontroller, _listControllers) {
if( !(*ittrajcontroller)->_srvTrajectoryStart ) {
RAVELOG_ERRORA("no start trajectory service\n");
@@ -249,7 +266,7 @@
//req.requesttiming = 1; // request back the timestamps
req.hastiming = 0;
req.traj.points.resize(2);
- (*ittrajcontroller)->GetTrajPoint(vcurvalues, req.traj.points[0]);
+ (*ittrajcontroller)->GetTrajPoint(_vcurvalues, req.traj.points[0]);
(*ittrajcontroller)->GetTrajPoint(vnewvalues, req.traj.points[1]);
if( (*ittrajcontroller)->_srvTrajectoryStart->call(req,res) )
@@ -269,6 +286,9 @@
virtual bool SetPath(const Trajectory* ptraj)
{
+ if( ptraj == NULL )
+ return false;
+
boost::mutex::scoped_lock lock(_mutexTrajectories);
bool bSuccess = true;
@@ -286,12 +306,8 @@
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;
- }
+ FOREACHC(itpoint, ptraj->GetPoints())
+ (*ittrajcontroller)->GetTrajPoint(itpoint->q, *ittraj++);
if( (*ittrajcontroller)->_srvTrajectoryStart->call(req,res) )
(*ittrajcontroller)->_listTrajectories.push_back(res.trajectoryid);
@@ -329,7 +345,7 @@
FOREACHC(itj, _mapjoints) {
values[itj->second] = _mstate.joint_states[itj->first].position;
while(values[itj->second] > vupper[itj->second] ) {
- if( values[itj->second]-vupper[itj->second] > PI )
+ if( values[itj->second]-2*PI >= vlower[itj->second] )
values[itj->second] -= 2*PI;
else {
values[itj->second] = vupper[itj->second];
@@ -337,7 +353,7 @@
}
}
while(values[itj->second] < vlower[itj->second] ) {
- if( values[itj->second]-vlower[itj->second] < -PI )
+ if( values[itj->second]+2*PI <= vupper[itj->second] )
values[itj->second] += 2*PI;
else {
values[itj->second] = vlower[itj->second];
@@ -347,6 +363,7 @@
}
ROS_ASSERT( (int)values.size() == _probot->GetDOF() );
+ _vcurvalues = values;
_probot->SetJointValues(NULL, NULL, &values[0], true);
}
}
@@ -444,47 +461,48 @@
virtual void mechanismstatecb()
{
- if( !_bCalibrated ) {
- // check the robot joint/link names
- do {
- _mapjoints.clear();
- FOREACH(itj, _setEnabledJoints) {
- bool bAdded = false;
- for(size_t j = 0; j < _mstate_cb.get_joint_states_size(); ++j) {
- if( itj->first == _mstate_cb.joint_states[j].name ) {
- _mapjoints[j] = itj->second;
- bAdded = true;
+ {
+ boost::mutex::scoped_lock lock(_mutexstate);
+
+ if( !_bCalibrated ) {
+ // check the robot joint/link names
+ do {
+ _mapjoints.clear();
+ FOREACH(itj, _setEnabledJoints) {
+ bool bAdded = false;
+ for(size_t j = 0; j < _mstate_cb.get_joint_states_size(); ++j) {
+ if( itj->first == _mstate_cb.joint_states[j].name ) {
+ _mapjoints[j] = itj->second;
+ bAdded = true;
+ break;
+ }
+ }
+
+ if( !bAdded ) {
+ RAVELOG_WARNA("could not find robot joint %s in mechanism state\n", itj->first.c_str());
break;
}
}
-
- if( !bAdded ) {
- RAVELOG_WARNA("could not find robot joint %s in mechanism state\n", itj->first.c_str());
+
+ if( _mapjoints.size() != _setEnabledJoints.size() ) {
+ _mapjoints.clear();
break;
}
- }
- if( _mapjoints.size() != _setEnabledJoints.size() ) {
- _mapjoints.clear();
- break;
- }
+ _bCalibrated = true;
+ } while(0);
+ }
+ else {
+ if( _probot->GetDOF() != (int)_mstate_cb.get_joint_states_size() )
+ _bCalibrated = false;
+ }
- _bCalibrated = true;
- } while(0);
- }
- else {
- if( _probot->GetDOF() != (int)_mstate_cb.get_joint_states_size() )
- _bCalibrated = false;
- }
+ if( !_bCalibrated )
+ return;
- if( !_bCalibrated )
- return;
-
- {
- boost::mutex::scoped_lock lock(_mutexstate);
_mstate = _mstate_cb;
}
-
+
// do some monitoring of the joint state (try to look for stalls)
}
@@ -497,7 +515,6 @@
// check if the first trajectory is done
boost::mutex::scoped_lock lock(_mutexTrajectories);
- uint32_t trajectoryid;
bool bDone = true;
if( _listControllers.size() > 0 ) {
@@ -523,9 +540,9 @@
}
if( bPopTrajectory ) {
- RAVELOG_DEBUGA("robot trajectory finished, left: %d\n", _listControllers.front()->_listTrajectories.size());
FOREACH(ittraj, _listControllers)
(*ittraj)->_listTrajectories.pop_front();
+ RAVELOG_DEBUGA("robot trajectory finished, left: %d\n", _listControllers.front()->_listTrajectories.size());
}
bDone = _listControllers.front()->_listTrajectories.size()==0;
@@ -544,6 +561,7 @@
vector<dReal> _vecdesired;
set< pair<string, int> > _setEnabledJoints; // set of enabled joints and their indices
mutable boost::mutex _mutexstate;
+ vector<dReal> _vcurvalues; ///< current robot values
ofstream flog;
int logid;
@@ -561,7 +579,7 @@
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
+ bool _bDestroyThread; ///< if true, destroy the thread
};
#endif
This was sent by the SourceForge.net collaborative development platform, the world's largest Open Source development site.
|
