Menu
▾
▴
[Personalrobots-commit] SF.net SVN: personalrobots:[8828] pkg/trunk
|
From: <rdi...@us...> - 2009-01-05 13:39:17
|
Revision: 8828
http://personalrobots.svn.sourceforge.net/personalrobots/?rev=8828&view=rev
Author: rdiankov
Date: 2009-01-05 13:39:12 +0000 (Mon, 05 Jan 2009)
Log Message:
-----------
much more robust openrave grasping and manipulation planning scripts, environment cloning works
Modified Paths:
--------------
pkg/trunk/3rdparty/openrave/Makefile
pkg/trunk/openrave_planning/openraveros/src/openraveros.h
pkg/trunk/openrave_planning/openraveros/src/rosserver.h
pkg/trunk/openrave_planning/openraveros/src/session.h
pkg/trunk/openrave_planning/ormanipulation/octave/GetObjectToManipulate.m
pkg/trunk/openrave_planning/ormanipulation/octave/GraspAndPlaceObject.m
pkg/trunk/openrave_planning/ormanipulation/octave/RobotGoInitial.m
pkg/trunk/openrave_planning/ormanipulation/octave/RunDynamicGrasping.m
pkg/trunk/openrave_planning/ormanipulation/octave/SetupTableScene.m
pkg/trunk/openrave_planning/ormanipulation/octave/StartTrajectory.m
pkg/trunk/openrave_planning/ormanipulation/octave/setclonesession.m
pkg/trunk/openrave_planning/ormanipulation/octave/setrealsession.m
pkg/trunk/openrave_planning/ormanipulation/octave/startup.m
Modified: pkg/trunk/3rdparty/openrave/Makefile
===================================================================
--- pkg/trunk/3rdparty/openrave/Makefile 2009-01-05 05:55:50 UTC (rev 8827)
+++ pkg/trunk/3rdparty/openrave/Makefile 2009-01-05 13:39:12 UTC (rev 8828)
@@ -2,7 +2,7 @@
SVN_DIR = openrave_svn
# Should really specify a revision
-SVN_REVISION = -r 590
+SVN_REVISION = -r 597
SVN_URL = https://openrave.svn.sourceforge.net/svnroot/openrave
include $(shell rospack find mk)/svn_checkout.mk
Modified: pkg/trunk/openrave_planning/openraveros/src/openraveros.h
===================================================================
--- pkg/trunk/openrave_planning/openraveros/src/openraveros.h 2009-01-05 05:55:50 UTC (rev 8827)
+++ pkg/trunk/openrave_planning/openraveros/src/openraveros.h 2009-01-05 13:39:12 UTC (rev 8828)
@@ -148,7 +148,7 @@
class SetViewerFunc
{
public:
- virtual bool SetViewer(EnvironmentBase* penv, const string& viewername) = 0;
+ virtual bool SetViewer(EnvironmentBase* penv, const string& viewername, const string& title) = 0;
};
#endif
Modified: pkg/trunk/openrave_planning/openraveros/src/rosserver.h
===================================================================
--- pkg/trunk/openrave_planning/openraveros/src/rosserver.h 2009-01-05 05:55:50 UTC (rev 8827)
+++ pkg/trunk/openrave_planning/openraveros/src/rosserver.h 2009-01-05 13:39:12 UTC (rev 8828)
@@ -160,8 +160,8 @@
};
public:
- ROSServer(SetViewerFunc* psetviewer, EnvironmentBase* penv, const string& physicsengine, const string& collisionchecker, const string& viewer)
- : RaveServerBase(penv), _nNextFigureId(1), _nNextPlannerId(1), _nNextProblemId(1) {
+ ROSServer(int nSessionId, SetViewerFunc* psetviewer, EnvironmentBase* penv, const string& physicsengine, const string& collisionchecker, const string& viewer)
+ : RaveServerBase(penv), _nSessionId(nSessionId), _nNextFigureId(1), _nNextPlannerId(1), _nNextProblemId(1) {
_psetviewer.reset(psetviewer);
_bThreadDestroyed = false;
_bCloseClient = false;
@@ -295,8 +295,11 @@
if( viewer.size() == 0 )
return true;
- if( !!_psetviewer )
- return _psetviewer->SetViewer(GetEnv(),viewer);
+ if( !!_psetviewer ) {
+ stringstream ss;
+ ss << "OpenRAVE - session " << _nSessionId;
+ return _psetviewer->SetViewer(GetEnv(),viewer,ss.str());
+ }
_threadviewer.join(); // wait for the viewer
@@ -1551,6 +1554,7 @@
map<int, boost::shared_ptr<PlannerBase> > _mapplanners;
map<int, boost::shared_ptr<ProblemInstance> > _mapproblems;
map<int, boost::shared_ptr<FIGURE> > _mapFigureIds;
+ int _nSessionId;
int _nNextFigureId, _nNextPlannerId, _nNextProblemId;
float _fSimulationTimestep;
Vector _vgravity;
Modified: pkg/trunk/openrave_planning/openraveros/src/session.h
===================================================================
--- pkg/trunk/openrave_planning/openraveros/src/session.h 2009-01-05 05:55:50 UTC (rev 8827)
+++ pkg/trunk/openrave_planning/openraveros/src/session.h 2009-01-05 13:39:12 UTC (rev 8828)
@@ -60,8 +60,8 @@
{
public:
SessionSetViewerFunc(SessionServer* pserv) : _pserv(pserv) {}
- virtual bool SetViewer(EnvironmentBase* penv, const string& viewername) {
- return _pserv->SetViewer(penv,viewername);
+ virtual bool SetViewer(EnvironmentBase* penv, const string& viewername, const string& title) {
+ return _pserv->SetViewer(penv,viewername,title);
}
private:
@@ -228,7 +228,7 @@
_penvViewer->AttachViewer(NULL);
}
- bool SetViewer(EnvironmentBase* penv, const string& viewer)
+ bool SetViewer(EnvironmentBase* penv, const string& viewer, const string& title)
{
boost::mutex::scoped_lock lock(_mutexViewer);
@@ -241,6 +241,7 @@
ROS_ASSERT(!_penvViewer);
+ _strviewertitle = title;
_strviewer = viewer;
if( viewer.size() == 0 || !penv )
return false;
@@ -263,7 +264,7 @@
boost::mutex _mutexViewer;
boost::condition _conditionViewer;
EnvironmentBase* _penvViewer;
- string _strviewer;
+ string _strviewer, _strviewertitle;
bool _ok;
@@ -293,6 +294,9 @@
continue;
}
+ if( _strviewertitle.size() > 0 )
+ _pviewer->ViewerSetTitle(_strviewertitle.c_str());
+
_pviewer->main(); // spin until quitfrommainloop is called
boost::mutex::scoped_lock lockcreate(_mutexViewer);
@@ -364,7 +368,7 @@
state._penv.reset(_pParentEnvironment->CloneSelf(0));
}
- state._pserver.reset(new ROSServer(new SessionSetViewerFunc(this), state._penv.get(), req.physicsengine, req.collisionchecker, req.viewer));
+ state._pserver.reset(new ROSServer(id, new SessionSetViewerFunc(this), state._penv.get(), req.physicsengine, req.collisionchecker, req.viewer));
state._penv->AttachServer(state._pserver.get());
_mapsessions[id] = state;
res.sessionid = id;
Modified: pkg/trunk/openrave_planning/ormanipulation/octave/GetObjectToManipulate.m
===================================================================
--- pkg/trunk/openrave_planning/ormanipulation/octave/GetObjectToManipulate.m 2009-01-05 05:55:50 UTC (rev 8827)
+++ pkg/trunk/openrave_planning/ormanipulation/octave/GetObjectToManipulate.m 2009-01-05 13:39:12 UTC (rev 8828)
@@ -94,5 +94,13 @@
curobj.dests(:,i) = [Rnew(:);pos];
end
+%% scroll through the destinations
+% Tbody = orBodyGetTransform(curobj.id);
+% for i = 1:size(curobj.dests,2)
+% orBodySetTransform(curobj.id,curobj.dests(:,i));
+% pause(0.02);
+% end
+% orBodySetTransform(curobj.id,Tbody);
+
%% for now
outmemory.ignorelist(end+1) = curobj.id;
Modified: pkg/trunk/openrave_planning/ormanipulation/octave/GraspAndPlaceObject.m
===================================================================
--- pkg/trunk/openrave_planning/ormanipulation/octave/GraspAndPlaceObject.m 2009-01-05 05:55:50 UTC (rev 8827)
+++ pkg/trunk/openrave_planning/ormanipulation/octave/GraspAndPlaceObject.m 2009-01-05 13:39:12 UTC (rev 8828)
@@ -82,7 +82,8 @@
offset = 0.02;
- cmd = ['testallgrasps combinepreshapetraj execute 0 outputtraj palmdir ' sprintf('%f ', handrobot.palmdir) ...
+ cmd = ['testallgrasps execute 0 outputtraj palmdir ' sprintf('%f ', handrobot.palmdir) ...
+ ' seedik 1 seedgrasps 10 seeddests 8 randomdests 1 randomgrasps 1 ' ...
' target ' curobj.info.name ' robothand ' handrobot.name ...
' robothandjoints ' sprintf('%d ', length(handjoints), handjoints) ...
' handjoints ' sprintf('%d ', length(handrobot.handjoints), handrobot.handjoints) ...
@@ -124,7 +125,7 @@
end
% start the trajectory
- success = StartTrajectory(robotid,probs.manip,trajdata);
+ success = StartTrajectory(robotid,trajdata);
if( ~success )
warning('failed to start initial traj');
return;
@@ -136,7 +137,7 @@
if( ~success )
error('failed to movehandstraight');
end
- success = StartTrajectory(robotid,probs.manip,trajdata);
+ success = StartTrajectory(robotid,trajdata);
if( ~success )
warning('failed to move hand straight');
return;
@@ -178,7 +179,7 @@
if( ~success )
error('failed to movehandstraight');
end
- success = StartTrajectory(robotid,probs.manip,trajdata);
+ success = StartTrajectory(robotid,trajdata);
if( ~success )
warning('failed to close fingers');
return;
@@ -199,7 +200,7 @@
%% not going to work well
%ExecuteOnRealSession(@() orProblemSendCommand(['grabbody name ' curobj.info.name], probs.manip));
- success = StartTrajectory(robotid,probs.manip,trajdata);
+ success = StartTrajectory(robotid,trajdata);
if( ~success )
warning('failed to move hand straight');
return;
@@ -224,9 +225,9 @@
if( squeezesuccess > 0 )
display('planning to destination');
- [trajdata,success] = orProblemSendCommand(['MoveToHandPosition execute 0 outputtraj matrices ' sprintf('%d ', size(goaldests,2)) ' sprintf('%f ', goaldests)], probs.manip);
+ [trajdata,success] = orProblemSendCommand(['MoveToHandPosition maxiter 1000 maxtries 1 seedik 4 execute 0 outputtraj matrices ' sprintf('%d ', size(destgoals,2)) sprintf('%f ', destgoals)], probs.manip);
if( success )
- success = StartTrajectory(robotid,probs.manip,trajdata);
+ success = StartTrajectory(robotid,trajdata);
if( ~success )
warning('failed to start trajectory');
return;
@@ -277,7 +278,7 @@
if( ~success )
error('failed to movehandstraight');
end
- success = StartTrajectory(robotid,probs.manip,trajdata);
+ success = StartTrajectory(robotid,trajdata);
if( ~success )
warning('failed to move hand straight');
return;
@@ -297,7 +298,7 @@
error('failed to release fingers');
end
- success = StartTrajectory(robotid,probs.manip,trajdata,4);
+ success = StartTrajectory(robotid,trajdata,4);
if( ~success )
warning('trajectory failed to release fingers');
return;
Modified: pkg/trunk/openrave_planning/ormanipulation/octave/RobotGoInitial.m
===================================================================
--- pkg/trunk/openrave_planning/ormanipulation/octave/RobotGoInitial.m 2009-01-05 05:55:50 UTC (rev 8827)
+++ pkg/trunk/openrave_planning/ormanipulation/octave/RobotGoInitial.m 2009-01-05 13:39:12 UTC (rev 8828)
@@ -21,7 +21,7 @@
return;
end
-success = StartTrajectory(robotid, probs.manip, trajdata)
+success = StartTrajectory(robotid, trajdata)
if( ~success )
return;
end
Modified: pkg/trunk/openrave_planning/ormanipulation/octave/RunDynamicGrasping.m
===================================================================
--- pkg/trunk/openrave_planning/ormanipulation/octave/RunDynamicGrasping.m 2009-01-05 05:55:50 UTC (rev 8827)
+++ pkg/trunk/openrave_planning/ormanipulation/octave/RunDynamicGrasping.m 2009-01-05 13:39:12 UTC (rev 8828)
@@ -55,8 +55,11 @@
continue;
end
- %% if found, create a clone
- %setclonesession(openraveros_makeclone(openraveros_openrave_session().CloneBodies()));
+ %% if not simulation, create a clone
+ if( ~simulation )
+ setclonesession(openraveros_makeclone(openraveros_openrave_session().CloneBodies()));
+ scenedata.SetupCloneFn();
+ end
%% try to find the new object
curobj.info = orEnvGetBodies(curobj.id);
Modified: pkg/trunk/openrave_planning/ormanipulation/octave/SetupTableScene.m
===================================================================
--- pkg/trunk/openrave_planning/ormanipulation/octave/SetupTableScene.m 2009-01-05 05:55:50 UTC (rev 8827)
+++ pkg/trunk/openrave_planning/ormanipulation/octave/SetupTableScene.m 2009-01-05 13:39:12 UTC (rev 8828)
@@ -94,6 +94,7 @@
scenedata.TargetObjPattern = TargetObjPattern;
scenedata.SwitchModelPatterns = SwitchModelPatterns;
scenedata.GetDestsFn = @() GetDests('^willow_table$');
+scenedata.SetupCloneFn = @() SetupClone(robot.name);
updir = [0;0;1];
@@ -156,8 +157,8 @@
X = [];
Y = [];
for x = 0:(Nx-1)
- X = [X 0.5*ones(1,Ny)/(Nx+1) + (x+0.5)/(Nx+1)];
- Y = [Y 0.5*ones(1,Ny)/(Ny+1) + ([0:(Ny-1)]+0.5)/(Ny+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)];
end
offset = [ab(1,1)-ab(1,2);ab(2,1); ab(3,1)+ab(3,2)];
@@ -177,3 +178,17 @@
orEnvClose();
orEnvPlot(dests(10:12,:)','size',10);
+
+function SetupClone(robotname)
+
+global probs
+
+probs.task = orEnvCreateProblem('TaskManipulation',robotname);
+if( isempty(probs.task) )
+ error('failed to create TaskManipulation problem');
+end
+
+probs.manip = orEnvCreateProblem('BaseManipulation',robotname);
+if( isempty(probs.manip) )
+ error('failed to create BaseManipulation problem');
+end
Modified: pkg/trunk/openrave_planning/ormanipulation/octave/StartTrajectory.m
===================================================================
--- pkg/trunk/openrave_planning/ormanipulation/octave/StartTrajectory.m 2009-01-05 05:55:50 UTC (rev 8827)
+++ pkg/trunk/openrave_planning/ormanipulation/octave/StartTrajectory.m 2009-01-05 13:39:12 UTC (rev 8828)
@@ -1,8 +1,9 @@
-%% success = StartTrajectory(robotid, probid, trajdata)
+%% success = StartTrajectory(robotid, trajdata)
%%
%% Starts a trajectory on the real robot and waits for it, in the end sets the new robot values
%% in the cloned (current) world.
-function success = StartTrajectory(robotid, probid, trajdata,timelimit)
+function success = StartTrajectory(robotid,trajdata,timelimit)
+global probs
if( isempty(trajdata) )
success = 1;
@@ -14,8 +15,15 @@
end
prevsession = openraveros_getglobalsession();
+prevprobs = probs;
setrealsession();
-orProblemSendCommand(['traj stream ' trajdata],probid);
+[out,trajsuc] = orProblemSendCommand(['traj stream ' trajdata],probs.manip);
+if( ~trajsuc )
+ success = 0;
+ setclonesession(prevsession);
+ probs = prevprobs;
+ return;
+end
display('waiting for robot');
success = 1;
@@ -40,4 +48,5 @@
newjointconfig = orBodyGetJointValues(robotid);
setclonesession(prevsession);
+probs = prevprobs;
orBodySetJointValues(robotid,newjointconfig);
Modified: pkg/trunk/openrave_planning/ormanipulation/octave/setclonesession.m
===================================================================
--- pkg/trunk/openrave_planning/ormanipulation/octave/setclonesession.m 2009-01-05 05:55:50 UTC (rev 8827)
+++ pkg/trunk/openrave_planning/ormanipulation/octave/setclonesession.m 2009-01-05 13:39:12 UTC (rev 8828)
@@ -1,6 +1,6 @@
%% setclonesession(session)
function setclonesession(session)
-global realsession
+global realsession realprobs probs
if( isempty(session) )
error('setting an empty clone');
@@ -9,6 +9,8 @@
if( isempty(realsession) )
%% revert back to the real session
realsession = openraveros_getglobalsession();
+ realprobs = probs;
end
openraveros_setglobalsession(session);
+probs = [];
Modified: pkg/trunk/openrave_planning/ormanipulation/octave/setrealsession.m
===================================================================
--- pkg/trunk/openrave_planning/ormanipulation/octave/setrealsession.m 2009-01-05 05:55:50 UTC (rev 8827)
+++ pkg/trunk/openrave_planning/ormanipulation/octave/setrealsession.m 2009-01-05 13:39:12 UTC (rev 8828)
@@ -1,9 +1,14 @@
%% setrealsession()
function setrealsession()
-global realsession
+global realsession realprobs probs
if( ~isempty(realsession) )
%% revert back to the real session
openraveros_setglobalsession(realsession);
+
+ if( ~isempty(realprobs) )
+ probs = realprobs;
+ end
+
realsession = [];
end
Modified: pkg/trunk/openrave_planning/ormanipulation/octave/startup.m
===================================================================
--- pkg/trunk/openrave_planning/ormanipulation/octave/startup.m 2009-01-05 05:55:50 UTC (rev 8827)
+++ pkg/trunk/openrave_planning/ormanipulation/octave/startup.m 2009-01-05 13:39:12 UTC (rev 8828)
@@ -15,4 +15,4 @@
openraveros_restart();
orEnvSetOptions('wdims 800 600');
%orEnvSetOptions('collision bullet');
-orEnvSetOptions('debug debug');
+%orEnvSetOptions('debug debug');
This was sent by the SourceForge.net collaborative development platform, the world's largest Open Source development site.
|
