Menu
▾
▴
[Personalrobots-commit] SF.net SVN: personalrobots:[9162] pkg/trunk
|
From: <rdi...@us...> - 2009-01-10 02:01:21
|
Revision: 9162
http://personalrobots.svn.sourceforge.net/personalrobots/?rev=9162&view=rev
Author: rdiankov
Date: 2009-01-10 02:01:15 +0000 (Sat, 10 Jan 2009)
Log Message:
-----------
ormanipulation work, robot demo becoming more stable
Modified Paths:
--------------
pkg/trunk/3rdparty/openrave/Makefile
pkg/trunk/controllers/pr2_mechanism_controllers/src/arm_trajectory_controller.cpp
pkg/trunk/openrave_planning/openraveros/octave/openraveros_restart.m
pkg/trunk/openrave_planning/openraveros/octave/openraveros_startup.m
pkg/trunk/openrave_planning/openraveros/src/session.h
pkg/trunk/openrave_planning/openraveros/srv/openrave_session.srv
pkg/trunk/openrave_planning/ormanipulation/data/pr2table.env.xml
pkg/trunk/openrave_planning/ormanipulation/data/pr2table_real.env.xml
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/runperception.m
pkg/trunk/openrave_planning/ormanipulation/octave/startup.m
Added Paths:
-----------
pkg/trunk/openrave_planning/ormanipulation/octave/ParseTrajData.m
Modified: pkg/trunk/3rdparty/openrave/Makefile
===================================================================
--- pkg/trunk/3rdparty/openrave/Makefile 2009-01-10 01:58:21 UTC (rev 9161)
+++ pkg/trunk/3rdparty/openrave/Makefile 2009-01-10 02:01:15 UTC (rev 9162)
@@ -2,7 +2,7 @@
SVN_DIR = openrave_svn
# Should really specify a revision
-SVN_REVISION = -r 600
+SVN_REVISION = -r 601
SVN_URL = https://openrave.svn.sourceforge.net/svnroot/openrave
include $(shell rospack find mk)/svn_checkout.mk
Modified: pkg/trunk/controllers/pr2_mechanism_controllers/src/arm_trajectory_controller.cpp
===================================================================
--- pkg/trunk/controllers/pr2_mechanism_controllers/src/arm_trajectory_controller.cpp 2009-01-10 01:58:21 UTC (rev 9161)
+++ pkg/trunk/controllers/pr2_mechanism_controllers/src/arm_trajectory_controller.cpp 2009-01-10 02:01:15 UTC (rev 9162)
@@ -272,8 +272,8 @@
node_->unadvertise_service(service_prefix_ + "/get_command");
node_->unadvertise_service(service_prefix_ + "/set_target");
*/
- node_->unadvertise_service(service_prefix_ + "/TrajectoryStart");
- node_->unadvertise_service(service_prefix_ + "/TrajectoryQuery");
+ node_->unadvertise_service(service_prefix_ + "TrajectoryStart");
+ node_->unadvertise_service(service_prefix_ + "TrajectoryQuery");
if(topic_name_ptr_ && topic_name_.c_str())
{
Modified: pkg/trunk/openrave_planning/openraveros/octave/openraveros_restart.m
===================================================================
--- pkg/trunk/openrave_planning/openraveros/octave/openraveros_restart.m 2009-01-10 01:58:21 UTC (rev 9161)
+++ pkg/trunk/openrave_planning/openraveros/octave/openraveros_restart.m 2009-01-10 02:01:15 UTC (rev 9162)
@@ -1,6 +1,8 @@
-%% openraveros_restart(sessionserver, viewer)
+%% openraveros_restart(sessionserver, viewer, destroyall)
%%
%% restars an openraveros session if the current one is invalid
+%% if destroyall is 1 and openraveros cannot find a currently existing session,
+%% destroys all other sessions before creating a new one
%% Software License Agreement (BSD License)
%% Copyright (c) 2008, Willow Garage, Inc.
@@ -27,9 +29,13 @@
%% POSSIBILITY OF SUCH DAMAGE.
%%
%% author: Rosen Diankov
-function openraveros_restart(sessionserver,viewer)
+function openraveros_restart(sessionserver,viewer,destroyall)
global openraveros_globalsession
+if( ~exist('destroyall','var') )
+ destroyall = 0;
+end
+
if( ~exist('sessionserver','var') || isempty(sessionserver) )
sessionserver = 'openrave_session';
end
@@ -38,7 +44,7 @@
viewer = 'qtcoin';
end
-openraveros_startup(sessionserver, 1, viewer);
+openraveros_startup(sessionserver, 1, viewer,destroyall);
if( ~isempty(openraveros_globalsession) )
%% send a dummy env_set command
@@ -49,6 +55,15 @@
end
end
+if( destroyall )
+ reqdestroy = openraveros_openrave_session();
+ reqdestroy.sessionid = -1;
+ resdestroy = rosoct_service_call(sessionserver,reqdestroy);
+ if( isempty(resdestroy) )
+ warning('failed to destroy sessions');
+ end
+end
+
openraveros_globalsession = openraveros_createsession(sessionserver);
if( ~isempty(viewer) && ~isempty(openraveros_globalsession) )
Modified: pkg/trunk/openrave_planning/openraveros/octave/openraveros_startup.m
===================================================================
--- pkg/trunk/openrave_planning/openraveros/octave/openraveros_startup.m 2009-01-10 01:58:21 UTC (rev 9161)
+++ pkg/trunk/openrave_planning/openraveros/octave/openraveros_startup.m 2009-01-10 02:01:15 UTC (rev 9162)
@@ -1,5 +1,8 @@
-%% openraveros_startup(sessionserver, createsession, viewer)
+%% openraveros_startup(sessionserver, createsession, viewer, destroyall)
+%%
%% adds all the necessary paths for the openraveros octave client
+%% if destroyall is 1 and openraveros cannot find a currently existing session,
+%% destroys all other sessions before creating a new one
%% Software License Agreement (BSD License)
%% Copyright (c) 2008, Willow Garage, Inc.
@@ -26,13 +29,16 @@
%% POSSIBILITY OF SUCH DAMAGE.
%%
%% author: Rosen Diankov
-function openraveros_startup(sessionserver,createsession, viewer)
+function openraveros_startup(sessionserver,createsession, viewer, destroyall)
global openraveros_globalsession
persistent openraveros_initialized
if( ~exist('createsession','var') )
createsession = 1;
end
+if( ~exist('destroyall','var') )
+ destroyall = 0;
+end
if( isempty(openraveros_initialized))
[status,rosoctpath] = system('rospack find rosoct');
@@ -55,6 +61,15 @@
viewer = 'qtcoin';
end
+
+ if( destroyall )
+ reqdestroy = openraveros_openrave_session();
+ reqdestroy.sessionid = -1;
+ resdestroy = rosoct_service_call(sessionserver,reqdestroy);
+ if( isempty(resdestroy) )
+ warning('failed to destroy sessions');
+ end
+ end
req = openraveros_openrave_session();
req.viewer = viewer; % default viewer
while(1)
Modified: pkg/trunk/openrave_planning/openraveros/src/session.h
===================================================================
--- pkg/trunk/openrave_planning/openraveros/src/session.h 2009-01-10 01:58:21 UTC (rev 9161)
+++ pkg/trunk/openrave_planning/openraveros/src/session.h 2009-01-10 02:01:15 UTC (rev 9162)
@@ -331,15 +331,23 @@
bool session_callback(openrave_session::request& req, openrave_session::response& res)
{
if( req.sessionid != 0 ) {
- // destory the session
boost::mutex::scoped_lock lock(_mutexsession);
- if( _mapsessions.find(req.sessionid) != _mapsessions.end() ) {
+
+ if( req.sessionid == -1 ) {
+ // destroy all sessions
+ RAVELOG_DEBUGA("destroying all sessions\n");
+ _mapsessions.clear();
+ }
+ else {
+ // destory the session
+ if( _mapsessions.find(req.sessionid) == _mapsessions.end() )
+ return false;
+
_mapsessions.erase(req.sessionid);
RAVELOG_INFOA("destroyed openrave session: %d\n", req.sessionid);
- return true;
}
-
- return false;
+
+ return true;
}
int id = rand();
Modified: pkg/trunk/openrave_planning/openraveros/srv/openrave_session.srv
===================================================================
--- pkg/trunk/openrave_planning/openraveros/srv/openrave_session.srv 2009-01-10 01:58:21 UTC (rev 9161)
+++ pkg/trunk/openrave_planning/openraveros/srv/openrave_session.srv 2009-01-10 02:01:15 UTC (rev 9162)
@@ -2,7 +2,7 @@
# that gets loaded.
# if 0, creates an openrave session
-# if -1, destroys all other session instances and creates an openrave session
+# if -1, destroys all other session instances
# otherwise, destroy the sessionid
int32 sessionid
Modified: pkg/trunk/openrave_planning/ormanipulation/data/pr2table.env.xml
===================================================================
--- pkg/trunk/openrave_planning/ormanipulation/data/pr2table.env.xml 2009-01-10 01:58:21 UTC (rev 9161)
+++ pkg/trunk/openrave_planning/ormanipulation/data/pr2table.env.xml 2009-01-10 02:01:15 UTC (rev 9162)
@@ -5,7 +5,7 @@
<Robot file="robots/pr2full.robot.xml">
<translation>-0.64 0.31 0.0102</translation>
- <jointvalues> 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</jointvalues>
+ <jointvalues> 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.500000</jointvalues>
</Robot>
<Kinbody name="floor">
@@ -18,7 +18,9 @@
</Body>
</Kinbody>
- <Kinbody file="willow_table.kinbody.xml"/>
+ <Kinbody file="willow_table.kinbody.xml">
+ <translation>2 0 0</translation>
+ </Kinbody>
<Kinbody name="ricebox0" file="ricebox.kinbody.xml">
<rotationmat>0 0 -1 0 1 0 1 0 0</rotationmat>
Modified: pkg/trunk/openrave_planning/ormanipulation/data/pr2table_real.env.xml
===================================================================
--- pkg/trunk/openrave_planning/ormanipulation/data/pr2table_real.env.xml 2009-01-10 01:58:21 UTC (rev 9161)
+++ pkg/trunk/openrave_planning/ormanipulation/data/pr2table_real.env.xml 2009-01-10 02:01:15 UTC (rev 9162)
@@ -11,7 +11,7 @@
<prerotationaxis>0 0 1 180</prerotationaxis>
</phasespace>
<!-- default values to get left arm out of the way -->
- <jointvalues>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</jointvalues>
+ <jointvalues>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.500000</jointvalues>
</Robot>
<Kinbody file="willow_table.kinbody.xml"/>
Modified: pkg/trunk/openrave_planning/ormanipulation/octave/GraspAndPlaceObject.m
===================================================================
--- pkg/trunk/openrave_planning/ormanipulation/octave/GraspAndPlaceObject.m 2009-01-10 01:58:21 UTC (rev 9161)
+++ pkg/trunk/openrave_planning/ormanipulation/octave/GraspAndPlaceObject.m 2009-01-10 02:01:15 UTC (rev 9162)
@@ -315,9 +315,10 @@
if( squeezesuccess > 0 & putsuccess > 0 )
display('success, putting down');
+ orBodySetJointValues(robotid,open_config,handjoints);
% only break when succeeded
%orProblemSendCommand(['MoveHandStraight stepsize 0.003 minsteps ' sprintf('%f ', 90) ' maxsteps ' sprintf('%f ', 100) ' direction ' sprintf('%f ', updir')]);
- %RobotGoInitial(robot);
+ RobotGoInitial(robot);
if( MySwitchModels(0) )
curobj.id = orEnvGetBody(curobj.info.name);
Added: pkg/trunk/openrave_planning/ormanipulation/octave/ParseTrajData.m
===================================================================
--- pkg/trunk/openrave_planning/ormanipulation/octave/ParseTrajData.m (rev 0)
+++ pkg/trunk/openrave_planning/ormanipulation/octave/ParseTrajData.m 2009-01-10 02:01:15 UTC (rev 9162)
@@ -0,0 +1,12 @@
+%% [trajvals,transvals,timevals] = ParseTrajData(trajdata)
+%%
+%% parses a trajectory file
+function [trajvals,transvals,timevals] = ParseTrajData(trajdata)
+vals = sscanf(trajdata, '%f');
+numpts = vals(1);
+numdof = vals(2);
+
+newvals = reshape(vals(4:(3+numpts*(numdof+8))),[numdof+8 numpts]);
+timevals = newvals(1,:);
+transvals = newvals((2+numdof):end,:);
+trajvals = newvals(2:(1+numdof),:);
Modified: pkg/trunk/openrave_planning/ormanipulation/octave/RobotGoInitial.m
===================================================================
--- pkg/trunk/openrave_planning/ormanipulation/octave/RobotGoInitial.m 2009-01-10 01:58:21 UTC (rev 9161)
+++ pkg/trunk/openrave_planning/ormanipulation/octave/RobotGoInitial.m 2009-01-10 02:01:15 UTC (rev 9162)
@@ -29,11 +29,13 @@
display('moving hand');
prevsession = openraveros_getglobalsession();
+prevprobs = probs;
setrealsession();
-orRobotSetDOFValues(robotid,home(handjoints),handjoints);
+orRobotSetDOFValues(robotid,home(handjoints+1),handjoints);
pause(0.4); % pause a little to give a chance for controller to start
success = WaitForRobot(robotid);
setclonesession(prevsession);
+probs = prevprobs;
%orRobotSetActiveDOFs(robotid,0:(robot.dof-1));
%curvalues = orRobotGetDOFValues(robotid);
Modified: pkg/trunk/openrave_planning/ormanipulation/octave/RunDynamicGrasping.m
===================================================================
--- pkg/trunk/openrave_planning/ormanipulation/octave/RunDynamicGrasping.m 2009-01-10 01:58:21 UTC (rev 9161)
+++ pkg/trunk/openrave_planning/ormanipulation/octave/RunDynamicGrasping.m 2009-01-10 02:01:15 UTC (rev 9162)
@@ -83,5 +83,10 @@
end
% switch back to real
+ sessionclone = openraveros_getglobalsession();
setrealsession();
+ if( sessionclone.id ~= openraveros_getglobalsession().id )
+ %destroy
+ openraveros_destroysession(sessionclone);
+ end
end
Modified: pkg/trunk/openrave_planning/ormanipulation/octave/SetupTableScene.m
===================================================================
--- pkg/trunk/openrave_planning/ormanipulation/octave/SetupTableScene.m 2009-01-10 01:58:21 UTC (rev 9161)
+++ pkg/trunk/openrave_planning/ormanipulation/octave/SetupTableScene.m 2009-01-10 02:01:15 UTC (rev 9162)
@@ -92,9 +92,23 @@
orProblemSendCommand('setactivemanip 1',probs.manip); % right arm
robot.activemanip = 2; % update
+tableaabb = [];
+tablepattern = '^willow_table$';
+bodies = orEnvGetBodies(0,openraveros_BodyInfo().Req_Names());
+for i = 1:length(bodies)
+ if( regexp(bodies{i}.name, tablepattern) )
+ Tidentity = eye(4);
+ Ttable = bodies{i}.T;
+ orBodySetTransform(bodies{i}.id,Tidentity(1:3,1:4));
+ ab = orBodyGetAABB(bodies{i}.id);
+ orBodySetTransform(bodies{i}.id,Ttable);
+ break;
+ end
+end
+
scenedata.TargetObjPattern = TargetObjPattern;
scenedata.SwitchModelPatterns = SwitchModelPatterns;
-scenedata.GetDestsFn = @() GetDests('^willow_table$');
+scenedata.GetDestsFn = @() GetDests(tablepattern, tableaabb);
scenedata.SetupCloneFn = @() SetupClone(robot.name);
updir = [0;0;1];
@@ -120,11 +134,11 @@
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', ['trajectoryservice / joints ' jointnames_str]);
+ orRobotControllerSet(robot.id, 'ROSRobot', ['trajectoryservice /right_arm_trajectory_controller joints ' jointnames_str]);
end
%% dests is a 12xN array where every column is a 3x4 matrix
-function [dests, surfaceplane] = GetDests(tablepattern)
+function [dests, surfaceplane] = GetDests(tablepattern, ab)
dests = [];
surfaceplane = [];
@@ -146,8 +160,12 @@
end
%% table up is assumed to be +z, sample the +y axis of the table
-ab = orBodyGetAABB(id);
if( isempty(ab) )
+ ab = orBodyGetAABB(id);
+ ab(1:3,1) = ab(1:3,1) - Ttable(1:3,4);
+end
+
+if( isempty(ab) )
display('missed table, trying again');
dests = GetDests(tablepattern);
return;
@@ -159,11 +177,11 @@
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) + 2*([0:(Ny-1)]-Ny/2)/(Ny+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)];
-trans = [offset(1)+2*ab(1,2)*X; offset(2)+ab(2,2)*Y; repmat(offset(3),size(X))];
+offset = [ab(1,1)-ab(1,2);ab(2,1)-ab(2,2); ab(3,1)+ab(3,2)];
+trans = [offset(1)+2*ab(1,2)*X; offset(2)+2*ab(2,2)*Y; repmat(offset(3),size(X))];
trans = Ttable*[trans;ones(size(X))];
Rots = [];
Modified: pkg/trunk/openrave_planning/ormanipulation/octave/StartTrajectory.m
===================================================================
--- pkg/trunk/openrave_planning/ormanipulation/octave/StartTrajectory.m 2009-01-10 01:58:21 UTC (rev 9161)
+++ pkg/trunk/openrave_planning/ormanipulation/octave/StartTrajectory.m 2009-01-10 02:01:15 UTC (rev 9162)
@@ -19,9 +19,10 @@
setrealsession();
[out,trajsuc] = orProblemSendCommand(['traj stream ' trajdata],probs.manip);
if( ~trajsuc )
+ display('trajectory failed');
success = 0;
setclonesession(prevsession);
- probs = prevprobs;
+ probs = prevprobs
return;
end
@@ -46,7 +47,8 @@
end
end
+display('wait ended');
newjointconfig = orBodyGetJointValues(robotid);
setclonesession(prevsession);
-probs = prevprobs;
+probs = prevprobs
orBodySetJointValues(robotid,newjointconfig);
Modified: pkg/trunk/openrave_planning/ormanipulation/octave/runperception.m
===================================================================
--- pkg/trunk/openrave_planning/ormanipulation/octave/runperception.m 2009-01-10 01:58:21 UTC (rev 9161)
+++ pkg/trunk/openrave_planning/ormanipulation/octave/runperception.m 2009-01-10 02:01:15 UTC (rev 9162)
@@ -1,8 +1,8 @@
#!/usr/bin/env octave
+cd(fullfile(rosoct_findpackage('ormanipulation'),'octave'));
+
global updir probs
-cd(fullfile(rosoct_findpackage('ormanipulation'),'octave'));
-
startup;
[robot, scenedata] = SetupTableScene('data/pr2table_real.env.xml',1);
Modified: pkg/trunk/openrave_planning/ormanipulation/octave/startup.m
===================================================================
--- pkg/trunk/openrave_planning/ormanipulation/octave/startup.m 2009-01-10 01:58:21 UTC (rev 9161)
+++ pkg/trunk/openrave_planning/ormanipulation/octave/startup.m 2009-01-10 02:01:15 UTC (rev 9162)
@@ -12,7 +12,7 @@
end
setrealsession();
-openraveros_restart();
+openraveros_restart('openrave_session','qtcoin',1);
orEnvSetOptions('wdims 800 600');
orEnvSetOptions('simulation timestep 0.001');
%orEnvSetOptions('collision bullet');
This was sent by the SourceForge.net collaborative development platform, the world's largest Open Source development site.
|
