Menu
▾
▴
[Personalrobots-commit] SF.net SVN: personalrobots:[8072] pkg/trunk
|
From: <rdi...@us...> - 2008-12-14 00:26:14
|
Revision: 8072
http://personalrobots.svn.sourceforge.net/personalrobots/?rev=8072&view=rev
Author: rdiankov
Date: 2008-12-14 00:26:09 +0000 (Sun, 14 Dec 2008)
Log Message:
-----------
openraveros octave client-side added
Modified Paths:
--------------
pkg/trunk/3rdparty/openrave/Makefile
pkg/trunk/openrave_planning/openraveros/CMakeLists.txt
pkg/trunk/openrave_planning/openraveros/manifest.xml
pkg/trunk/openrave_planning/openraveros/src/openraveros.cpp
pkg/trunk/openrave_planning/openraveros/src/rosserver.h
pkg/trunk/openrave_planning/openraveros/src/session.h
pkg/trunk/openrave_planning/openraveros/srv/body_destroy.srv
pkg/trunk/openrave_planning/openraveros/srv/body_enable.srv
pkg/trunk/openrave_planning/openraveros/srv/body_getaabb.srv
pkg/trunk/openrave_planning/openraveros/srv/body_getaabbs.srv
pkg/trunk/openrave_planning/openraveros/srv/body_getdof.srv
pkg/trunk/openrave_planning/openraveros/srv/body_getjointvalues.srv
pkg/trunk/openrave_planning/openraveros/srv/body_getlinks.srv
pkg/trunk/openrave_planning/openraveros/srv/body_setjointvalues.srv
pkg/trunk/openrave_planning/openraveros/srv/body_settransform.srv
pkg/trunk/openrave_planning/openraveros/srv/env_checkcollision.srv
pkg/trunk/openrave_planning/openraveros/srv/env_closefigures.srv
pkg/trunk/openrave_planning/openraveros/srv/env_createbody.srv
pkg/trunk/openrave_planning/openraveros/srv/env_createplanner.srv
pkg/trunk/openrave_planning/openraveros/srv/env_createproblem.srv
pkg/trunk/openrave_planning/openraveros/srv/env_createrobot.srv
pkg/trunk/openrave_planning/openraveros/srv/env_destroyproblem.srv
pkg/trunk/openrave_planning/openraveros/srv/env_getbodies.srv
pkg/trunk/openrave_planning/openraveros/srv/env_getbody.srv
pkg/trunk/openrave_planning/openraveros/srv/env_getrobots.srv
pkg/trunk/openrave_planning/openraveros/srv/env_loadplugin.srv
pkg/trunk/openrave_planning/openraveros/srv/env_loadscene.srv
pkg/trunk/openrave_planning/openraveros/srv/env_plot.srv
pkg/trunk/openrave_planning/openraveros/srv/env_raycollision.srv
pkg/trunk/openrave_planning/openraveros/srv/env_set.srv
pkg/trunk/openrave_planning/openraveros/srv/env_triangulate.srv
pkg/trunk/openrave_planning/openraveros/srv/env_wait.srv
pkg/trunk/openrave_planning/openraveros/srv/openrave_session.srv
pkg/trunk/openrave_planning/openraveros/srv/planner_init.srv
pkg/trunk/openrave_planning/openraveros/srv/planner_plan.srv
pkg/trunk/openrave_planning/openraveros/srv/problem_sendcommand.srv
pkg/trunk/openrave_planning/openraveros/srv/robot_controllersend.srv
pkg/trunk/openrave_planning/openraveros/srv/robot_controllerset.srv
pkg/trunk/openrave_planning/openraveros/srv/robot_getactivevalues.srv
pkg/trunk/openrave_planning/openraveros/srv/robot_sensorgetdata.srv
pkg/trunk/openrave_planning/openraveros/srv/robot_sensorsend.srv
pkg/trunk/openrave_planning/openraveros/srv/robot_setactivedofs.srv
pkg/trunk/openrave_planning/openraveros/srv/robot_setactivevalues.srv
pkg/trunk/openrave_planning/openraveros/srv/robot_starttrajectory.srv
Added Paths:
-----------
pkg/trunk/openrave_planning/openraveros/octave/
pkg/trunk/openrave_planning/openraveros/octave/openraveros_createsession.m
pkg/trunk/openrave_planning/openraveros/octave/openraveros_destroysession.m
pkg/trunk/openrave_planning/openraveros/octave/openraveros_getbodyinfo.m
pkg/trunk/openrave_planning/openraveros/octave/openraveros_getglobalsession.m
pkg/trunk/openrave_planning/openraveros/octave/openraveros_getrobotinfo.m
pkg/trunk/openrave_planning/openraveros/octave/openraveros_restart.m
pkg/trunk/openrave_planning/openraveros/octave/openraveros_rotfromquat.m
pkg/trunk/openrave_planning/openraveros/octave/openraveros_setglobalsession.m
pkg/trunk/openrave_planning/openraveros/octave/openraveros_startup.m
pkg/trunk/openrave_planning/openraveros/octave/orBodyDestroy.m
pkg/trunk/openrave_planning/openraveros/octave/orBodyEnable.m
pkg/trunk/openrave_planning/openraveros/octave/orBodyGetAABB.m
pkg/trunk/openrave_planning/openraveros/octave/orBodyGetAABBs.m
pkg/trunk/openrave_planning/openraveros/octave/orBodyGetDOF.m
pkg/trunk/openrave_planning/openraveros/octave/orBodyGetJointValues.m
pkg/trunk/openrave_planning/openraveros/octave/orBodyGetLinks.m
pkg/trunk/openrave_planning/openraveros/octave/orBodyGetTransform.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/orEnvCreateKinBody.m
pkg/trunk/openrave_planning/openraveros/octave/orEnvCreatePlanner.m
pkg/trunk/openrave_planning/openraveros/octave/orEnvCreateProblem.m
pkg/trunk/openrave_planning/openraveros/octave/orEnvCreateRobot.m
pkg/trunk/openrave_planning/openraveros/octave/orEnvDestroyProblem.m
pkg/trunk/openrave_planning/openraveros/octave/orEnvGetBodies.m
pkg/trunk/openrave_planning/openraveros/octave/orEnvGetBody.m
pkg/trunk/openrave_planning/openraveros/octave/orEnvGetRobots.m
pkg/trunk/openrave_planning/openraveros/octave/orEnvLoadPlugin.m
pkg/trunk/openrave_planning/openraveros/octave/orEnvLoadScene.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/orEnvWait.m
pkg/trunk/openrave_planning/openraveros/octave/orPlannerInit.m
pkg/trunk/openrave_planning/openraveros/octave/orPlannerPlan.m
pkg/trunk/openrave_planning/openraveros/octave/orProblemSendCommand.m
pkg/trunk/openrave_planning/openraveros/octave/orRobotControllerSend.m
pkg/trunk/openrave_planning/openraveros/octave/orRobotControllerSet.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/orRobotSensorGetData.m
pkg/trunk/openrave_planning/openraveros/octave/orRobotSensorSend.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/test/
pkg/trunk/openrave_planning/openraveros/test/testsessioncreation.m
Modified: pkg/trunk/3rdparty/openrave/Makefile
===================================================================
--- pkg/trunk/3rdparty/openrave/Makefile 2008-12-13 21:49:47 UTC (rev 8071)
+++ pkg/trunk/3rdparty/openrave/Makefile 2008-12-14 00:26:09 UTC (rev 8072)
@@ -2,7 +2,7 @@
SVN_DIR = openrave_svn
# Should really specify a revision
-SVN_REVISION = -r 543
+SVN_REVISION = -r 549
SVN_URL = https://openrave.svn.sourceforge.net/svnroot/openrave
include $(shell rospack find mk)/svn_checkout.mk
@@ -11,7 +11,7 @@
build: SVN_UP openrave
openrave: SVN_UP
- @cd $(SVN_DIR) && export PATH="$(shell rospack find soqt)/bin:$(shell rospack find opende)/opende/bin:$(PATH)" && export CPATH="$(shell rospack find bullet)/include" && export LD_LIBRARY_PATH=$(shell rospack find bullet)/lib:$(LD_LIBRARY_PATH) && export BOOST_ROOT=$(shell rospack find boost)/boost && svn up && make prefix=$(PWD) && make install
+ @cd $(SVN_DIR) && export PATH="$(shell rospack find soqt)/bin:$(shell rospack find opende)/opende/bin:$(PATH)" && export CPATH="$(shell rospack find bullet)/include" && export LD_LIBRARY_PATH=$(shell rospack find bullet)/lib:$(LD_LIBRARY_PATH) && export BOOST_ROOT=$(shell rospack find boost)/boost && svn up && mkdir -p build && cd build && $(shell rospack find cmake)/cmake/bin/cmake -DCMAKE_INSTALL_PREFIX=$(PWD) -DCMAKE_BUILD_TYPE=Release .. && make install
clean:
make -C $(SVN_DIR) clean
Modified: pkg/trunk/openrave_planning/openraveros/CMakeLists.txt
===================================================================
--- pkg/trunk/openrave_planning/openraveros/CMakeLists.txt 2008-12-13 21:49:47 UTC (rev 8071)
+++ pkg/trunk/openrave_planning/openraveros/CMakeLists.txt 2008-12-14 00:26:09 UTC (rev 8072)
@@ -6,3 +6,7 @@
gensrv()
rospack_add_executable(openraveros src/openraveros.cpp)
target_link_libraries (openraveros openrave-core boost_thread)
+
+# add sessions dependency
+find_ros_package(roscpp_sessions)
+set_source_files_properties(src/openraveros.cpp PROPERTIES OBJECT_DEPENDS ${roscpp_sessions_PACKAGE_PATH}/include/ros/session.h)
Modified: pkg/trunk/openrave_planning/openraveros/manifest.xml
===================================================================
--- pkg/trunk/openrave_planning/openraveros/manifest.xml 2008-12-13 21:49:47 UTC (rev 8071)
+++ pkg/trunk/openrave_planning/openraveros/manifest.xml 2008-12-14 00:26:09 UTC (rev 8072)
@@ -4,8 +4,10 @@
</description>
<author>Rosen Diankov (rdi...@cs...)</author>
<license>BSD</license>
+ <depend package="boost"/>
<depend package="roscpp"/>
+ <depend package="roscpp_sessions"/>
<depend package="openrave"/>
<depend package="std_msgs"/>
- <depend package="boost"/>
+ <depend package="rosoct"/>
</package>
Added: pkg/trunk/openrave_planning/openraveros/octave/openraveros_createsession.m
===================================================================
--- pkg/trunk/openrave_planning/openraveros/octave/openraveros_createsession.m (rev 0)
+++ pkg/trunk/openrave_planning/openraveros/octave/openraveros_createsession.m 2008-12-14 00:26:09 UTC (rev 8072)
@@ -0,0 +1,61 @@
+%% session = openraveros_createsession(sessionserver, cloneuuid)
+%
+%% creates a session and returns its id.
+%% clonesession (optional) - if passed in, will clone the new session with this session
+%% the environment id that uniquely determines a session is passed in the uuid
+%% Output:
+%% session.id - local identifier used to make session calls or session terminations
+%% session.uuid - unique identifier for the session server across the entire ROS network
+%% session.server - name of the session server
+
+%% Software License Agreement (BSD License)
+%% Copyright (c) 2008, Willow Garage, Inc.
+%% 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.
+%% * The name of the author may not 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.
+%%
+%% author: Rosen Diankov
+function session = openraveros_createsession(sessionserver, cloneuuid, cloneoptions)
+
+if( ~exist('sessionserver','var') )
+ sessionserver = 'openrave_session';
+end
+openraveros_startup(sessionserver);
+
+req = openraveros_openrave_session();
+if( exist('cloneuuid','var') )
+ req.clone_sessionid = cloneuuid;
+ if( exist('cloneoptions','var') )
+ req.clone_options = cloneoptions;
+ end
+end
+
+session = [];
+[localid,res] = rosoct_create_session(sessionserver,req);
+
+if( ~isempty(localid) && ~isempty(res) )
+ if( res.sessionid==0 )
+ error('bad session id');
+ end
+ session.id = localid;
+ session.server = sessionserver;
+ session.uuid = res.sessionid;
+end
Added: pkg/trunk/openrave_planning/openraveros/octave/openraveros_destroysession.m
===================================================================
--- pkg/trunk/openrave_planning/openraveros/octave/openraveros_destroysession.m (rev 0)
+++ pkg/trunk/openrave_planning/openraveros/octave/openraveros_destroysession.m 2008-12-14 00:26:09 UTC (rev 8072)
@@ -0,0 +1,50 @@
+%% openraveros_destroysession(session)
+%%
+%% destroys a session part of a session server
+%% session.id - local session id
+%% if session does not exist will delete all sessions
+
+%% Software License Agreement (BSD License)
+%% Copyright (c) 2008, Willow Garage, Inc.
+%% 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.
+%% * The name of the author may not 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.
+%%
+%% author: Rosen Diankov
+function openraveros_destroysession(session)
+
+openraveros_startup([],0);
+
+gsession = openraveros_getglobalsession();
+if( ~exist('session','var') )
+ if( isempty(gsession) )
+ return;
+ end
+ session = gsession;
+end
+
+%% destroys all the sessions of a session server
+__rosoct_terminate_session(session.id);
+
+%% if this is the global session, make sure to propagate the changes
+if( ~isempty(gsession) && session.id == gsession.id )
+ openraveros_setglobalsession([]);
+end
Added: pkg/trunk/openrave_planning/openraveros/octave/openraveros_getbodyinfo.m
===================================================================
--- pkg/trunk/openrave_planning/openraveros/octave/openraveros_getbodyinfo.m (rev 0)
+++ pkg/trunk/openrave_planning/openraveros/octave/openraveros_getbodyinfo.m 2008-12-14 00:26:09 UTC (rev 8072)
@@ -0,0 +1,24 @@
+%% body = openraveros_getbodyinfo(bodyinfo)
+%%
+%% parses the BodyInfo.msg into a simpler form for octave consumption
+function body = openraveros_getbodyinfo(bodyinfo)
+
+body.id = bodyinfo.bodyid;
+body.name = bodyinfo.name;
+body.type = bodyinfo.type;
+body.filename = bodyinfo.filename;
+
+%% extra
+body.enabled = bodyinfo.enabled;
+body.dof = bodyinfo.dof;
+body.T = reshape(cell2mat(bodyinfo.transform.m),[3 4]);
+
+body.jointvalues = cell2mat(bodyinfo.jointvalues);
+body.links = zeros(1,length(bodyinfo.links));
+for i = 1:length(bodyinfo.links)
+ body.links(:,i) = cell2mat(bodyinfo.links{i}.m);
+end
+
+body.linknames = bodyinfo.linknames;
+body.lowerlimit = cell2mat(bodyinfo.lowerlimit);
+body.upperlimit = cell2mat(bodyinfo.upperlimit);
Added: pkg/trunk/openrave_planning/openraveros/octave/openraveros_getglobalsession.m
===================================================================
--- pkg/trunk/openrave_planning/openraveros/octave/openraveros_getglobalsession.m (rev 0)
+++ pkg/trunk/openrave_planning/openraveros/octave/openraveros_getglobalsession.m 2008-12-14 00:26:09 UTC (rev 8072)
@@ -0,0 +1,34 @@
+%% session = openraveros_getglobalsession()
+%%
+%% gets the current session of openrave, if session is not active, tries to create
+%% a new session. If that also fails, returns an error
+
+%% Software License Agreement (BSD License)
+%% Copyright (c) 2008, Willow Garage, Inc.
+%% 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.
+%% * The name of the author may not 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.
+%%
+%% author: Rosen Diankov
+function session = openraveros_getglobalsession()
+global openraveros_globalsession
+openraveros_startup();
+session = openraveros_globalsession;
Added: pkg/trunk/openrave_planning/openraveros/octave/openraveros_getrobotinfo.m
===================================================================
--- pkg/trunk/openrave_planning/openraveros/octave/openraveros_getrobotinfo.m (rev 0)
+++ pkg/trunk/openrave_planning/openraveros/octave/openraveros_getrobotinfo.m 2008-12-14 00:26:09 UTC (rev 8072)
@@ -0,0 +1,32 @@
+%% robot = openraveros_getrobotinfo(robotinfo)
+%%
+%% parses the RobotInfo.msg into a simpler form for octave consumption
+function robot = openraveros_getrobotinfo(robotinfo)
+
+robot = openraveros_getbodyinfo(robotinfo.bodyinfo);
+
+robot.manips = cell(length(robotinfo.manips),1);
+for i = 1:length(robot.manips)
+ mi = robotinfo.manips{i};
+ robot.manips{i} = struct('baselink',mi.baselink+1,...
+ 'eelink',eelink+1,...
+ 'Tgrasp',reshape(cell2mat(mi.tgrasp.m),[3 4]),...
+ 'handjoints',cell2mat(mi.handjoints),...
+ 'armjoints',cell2mat(mi.armjoints),...
+ 'iksolvername',mi.iksolvername);
+end
+
+robot.sensors = cell(length(robotinfo.sensors),1);
+for i = 1:length(robot.sensors)
+ si = robotinfo.sensors{i};
+ robot.sensors{i} = struct('name',si.name,...
+ 'attachedlink',si.attachedlink+1,...
+ 'Trelative',reshape(cell2mat(si.trelative.m),[3 4]),...
+ 'Tglobal',reshape(cell2mat(si.tglobal.m),[3 4]),...
+ 'type',si.type);
+end
+
+robot.activedof = robotinfo.activedof;
+robot.affinedof = robotinfo.active.affine;
+robot.activejoints = cell2mat(robotinfo.active.joints);
+robot.rotationaxis = cell2mat(robotinfo.active.rotationaxis);
Added: pkg/trunk/openrave_planning/openraveros/octave/openraveros_restart.m
===================================================================
--- pkg/trunk/openrave_planning/openraveros/octave/openraveros_restart.m (rev 0)
+++ pkg/trunk/openrave_planning/openraveros/octave/openraveros_restart.m 2008-12-14 00:26:09 UTC (rev 8072)
@@ -0,0 +1,47 @@
+%% openraveros_restart(sessionserver)
+%%
+%% restars an openraveros session if the current one is invalid
+
+%% Software License Agreement (BSD License)
+%% Copyright (c) 2008, Willow Garage, Inc.
+%% 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.
+%% * The name of the author may not 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.
+%%
+%% author: Rosen Diankov
+function openraveros_restart(sessionserver)
+global openraveros_globalsession
+
+openraveros_startup();
+
+if( ~isempty(openraveros_globalsession) )
+ %% send a dummy env_set command
+ res = rosoct_session_call(openraveros_globalsession.id,'env_set',openraveros_env_set());
+ if( ~isempty(res) )
+ %% success
+ return;
+ end
+end
+
+if( ~exist('sessionserver','var') )
+ sessionserver = 'openrave_session';
+end
+openraveros_globalsession = openraveros_createsession(sessionserver);
Added: pkg/trunk/openrave_planning/openraveros/octave/openraveros_rotfromquat.m
===================================================================
--- pkg/trunk/openrave_planning/openraveros/octave/openraveros_rotfromquat.m (rev 0)
+++ pkg/trunk/openrave_planning/openraveros/octave/openraveros_rotfromquat.m 2008-12-14 00:26:09 UTC (rev 8072)
@@ -0,0 +1,16 @@
+% R = openraveros_rotfromquat(quat)
+function R = openraveros_rotfromquat(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;
Added: pkg/trunk/openrave_planning/openraveros/octave/openraveros_setglobalsession.m
===================================================================
--- pkg/trunk/openrave_planning/openraveros/octave/openraveros_setglobalsession.m (rev 0)
+++ pkg/trunk/openrave_planning/openraveros/octave/openraveros_setglobalsession.m 2008-12-14 00:26:09 UTC (rev 8072)
@@ -0,0 +1,36 @@
+%% openraveros_setglobalsession(session)
+%%
+
+%% Software License Agreement (BSD License)
+%% Copyright (c) 2008, Willow Garage, Inc.
+%% 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.
+%% * The name of the author may not 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.
+%%
+%% author: Rosen Diankov
+function openraveros_setglobalsession(session)
+global openraveros_globalsession
+
+if( ~isempty(session) && (~isfield(session,'id') || ~isfield(session,'uuid') || ~isfield(session,'server')) )
+ error('session in wrong format');
+end
+
+openraveros_globalsession = session;
\ No newline at end of file
Added: pkg/trunk/openrave_planning/openraveros/octave/openraveros_startup.m
===================================================================
--- pkg/trunk/openrave_planning/openraveros/octave/openraveros_startup.m (rev 0)
+++ pkg/trunk/openrave_planning/openraveros/octave/openraveros_startup.m 2008-12-14 00:26:09 UTC (rev 8072)
@@ -0,0 +1,66 @@
+%% openraveros_startup(sessionserver)
+%% adds all the necessary paths for the openraveros octave client
+
+%% Software License Agreement (BSD License)
+%% Copyright (c) 2008, Willow Garage, Inc.
+%% 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.
+%% * The name of the author may not 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.
+%%
+%% author: Rosen Diankov
+function openraveros_startup(sessionserver,createsession)
+global openraveros_globalsession
+persistent openraveros_initialized
+
+if( ~exist('createsession','var') )
+ createsession = 1;
+end
+
+if( isempty(openraveros_initialized))
+ openraveros_initialized = 1;
+ [status,rosoctpath] = system('rospack find rosoct');
+ rosoctpath = strtrim(rosoctpath);
+ addpath(fullfile(rosoctpath,'octave'));
+
+ rosoct_add_msgs('openraveros');
+ rosoct_add_srvs('openraveros');
+
+ rosoct('shutdown'); % restart the client
+end
+
+if( ~exist('sessionserver','var') )
+ sessionserver = 'openrave_session';
+end
+
+if( createsession && isempty(openraveros_globalsession) )
+ req = openraveros_openrave_session();
+ [localid,res] = rosoct_create_session(sessionserver,req);
+
+ if( ~isempty(localid) && ~isempty(res) )
+ if( res.sessionid==0 )
+ error('bad session id');
+ end
+ openraveros_globalsession.id = localid;
+ openraveros_globalsession.server = sessionserver;
+ openraveros_globalsession.uuid = res.sessionid;
+ display(sprintf('created openraveros session uuid %d',res.sessionid));
+ end
+end
Added: pkg/trunk/openrave_planning/openraveros/octave/orBodyDestroy.m
===================================================================
--- pkg/trunk/openrave_planning/openraveros/octave/orBodyDestroy.m (rev 0)
+++ pkg/trunk/openrave_planning/openraveros/octave/orBodyDestroy.m 2008-12-14 00:26:09 UTC (rev 8072)
@@ -0,0 +1,10 @@
+% success=orBodyDestroy(bodyid)
+%
+% Destroys a body of id bodyid. bodyid can also be a robot.
+
+function success = orBodyDestroy(bodyid)
+session = openraveros_getglobalsession();
+req = openraveros_body_destroy();
+req.bodyid = bodyid;
+res = rosoct_session_call(session.id,'body_destroy',req);
+success = ~isempty(res);
Added: pkg/trunk/openrave_planning/openraveros/octave/orBodyEnable.m
===================================================================
--- pkg/trunk/openrave_planning/openraveros/octave/orBodyEnable.m (rev 0)
+++ pkg/trunk/openrave_planning/openraveros/octave/orBodyEnable.m 2008-12-14 00:26:09 UTC (rev 8072)
@@ -0,0 +1,12 @@
+% success = orBodyEnable(bodyid, enable)
+%
+% Enables or disables the body. If a body is disabled, collision detection
+% and physics will will be turned off for it.
+
+function success = orBodyEnable(bodyid,enable)
+session = openraveros_getglobalsession();
+req = openraveros_body_enable();
+req.bodyid = bodyid;
+req.enable = enable;
+res = rosoct_session_call(session.id,'body_enable',req);
+success = ~isempty(res);
Added: pkg/trunk/openrave_planning/openraveros/octave/orBodyGetAABB.m
===================================================================
--- pkg/trunk/openrave_planning/openraveros/octave/orBodyGetAABB.m (rev 0)
+++ pkg/trunk/openrave_planning/openraveros/octave/orBodyGetAABB.m 2008-12-14 00:26:09 UTC (rev 8072)
@@ -0,0 +1,18 @@
+% aabb = orBodyGetAABB(bodyid)
+%
+% returns an axis-aligned boudning box of the body in world coordinates
+% aabb is a 3x2 vector where the first column is the position of the box
+% and the second is the extents
+
+function aabb = orBodyGetAABB(bodyid)
+session = openraveros_getglobalsession();
+req = openraveros_body_getaabb();
+req.bodyid = bodyid;
+res = rosoct_session_call(session.id,'body_getaabb',req);
+
+if(~isempty(res))
+ aabb = [res.center res.extents];
+else
+ aabb = [];
+end
+
Added: pkg/trunk/openrave_planning/openraveros/octave/orBodyGetAABBs.m
===================================================================
--- pkg/trunk/openrave_planning/openraveros/octave/orBodyGetAABBs.m (rev 0)
+++ pkg/trunk/openrave_planning/openraveros/octave/orBodyGetAABBs.m 2008-12-14 00:26:09 UTC (rev 8072)
@@ -0,0 +1,21 @@
+% aabbs = orBodyGetAABBs(bodyid)
+%
+% returns the axis-aligned boudning boxes of all the links of the body in world coordinates
+% aabbs is a 6xn vector where each column describes the box for all n links.
+% The first 3 values in each column describe the position of the aabb, and the next
+% 3 values describe the extents (half width/length/height) on each of the axes.
+
+function aabbs = orBodyGetAABBs(bodyid)
+session = openraveros_getglobalsession();
+req = openraveros_body_aabbs();
+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];
+ end
+else
+ aabbs = [];
+end
Added: pkg/trunk/openrave_planning/openraveros/octave/orBodyGetDOF.m
===================================================================
--- pkg/trunk/openrave_planning/openraveros/octave/orBodyGetDOF.m (rev 0)
+++ pkg/trunk/openrave_planning/openraveros/octave/orBodyGetDOF.m 2008-12-14 00:26:09 UTC (rev 8072)
@@ -0,0 +1,16 @@
+% dof = orBodyGetDOF(robotid)
+%
+% returns the number of active joints of the body
+
+function dof = orBodyGetDOF(bodyid)
+session = openraveros_getglobalsession();
+req = openraveros_body_getdof();
+req.bodyid = bodyid;
+res = rosoct_session_call(session.id,'body_getdof',req);
+
+if(~isempty(res))
+ dof = res.dof;
+else
+ dof = [];
+end
+
Added: pkg/trunk/openrave_planning/openraveros/octave/orBodyGetJointValues.m
===================================================================
--- pkg/trunk/openrave_planning/openraveros/octave/orBodyGetJointValues.m (rev 0)
+++ pkg/trunk/openrave_planning/openraveros/octave/orBodyGetJointValues.m 2008-12-14 00:26:09 UTC (rev 8072)
@@ -0,0 +1,20 @@
+% values = orBodyGetJointValues(bodyid, indices)
+%
+% Gets the body's dof values in a Nx1 vector where N is the DOF
+% bodyid - unique id of the robot
+% indices [optional]- The indices of the joints whose values should be returned.
+% If not specified, all joints are returned
+function values = orBodyGetJointValues(bodyid, indices)
+session = openraveros_getglobalsession();
+req = openraveros_body_getjointvalues();
+req.bodyid = bodyid;
+if( exist('indices','var') && length(indices)>0 )
+ req.indices = mat2cell(indices,1,ones(1,length(indices)));
+end
+res = rosoct_session_call(session.id,'body_getjointvalues',req);
+
+if(~isempty(res))
+ values = cell2mat(res.values);
+else
+ values = [];
+end
Added: pkg/trunk/openrave_planning/openraveros/octave/orBodyGetLinks.m
===================================================================
--- pkg/trunk/openrave_planning/openraveros/octave/orBodyGetLinks.m (rev 0)
+++ pkg/trunk/openrave_planning/openraveros/octave/orBodyGetLinks.m 2008-12-14 00:26:09 UTC (rev 8072)
@@ -0,0 +1,22 @@
+% values = orBodyGetLinks(bodyid)
+%
+% Returns the transformations of all the body's links in a 12 x L matrix. Where L
+% is the number of links and each column is a 3x4 transformation
+% (use T=reshape(., [3 4]) to recover).
+% T * [X;1] = Xnew
+
+function values = orBodyGetLinks(bodyid)
+session = openraveros_getglobalsession();
+req = openraveros_body_getlinks();
+req.bodyid = bodyid;
+res = rosoct_session_call(session.id,'body_getlinks',req);
+
+if(~isempty(res))
+ values = zeros(1,length(res.links));
+ for i = 1:length(res.links)
+ values(:,i) = cell2mat(res.links{i}.m);
+ end
+else
+ values = [];
+end
+
Added: pkg/trunk/openrave_planning/openraveros/octave/orBodyGetTransform.m
===================================================================
--- pkg/trunk/openrave_planning/openraveros/octave/orBodyGetTransform.m (rev 0)
+++ pkg/trunk/openrave_planning/openraveros/octave/orBodyGetTransform.m 2008-12-14 00:26:09 UTC (rev 8072)
@@ -0,0 +1,10 @@
+% values = orBodyGetTransform(bodyid)
+%
+% Returns the transformations of all the body's first link as a 12 x 1 matrix.
+% (use T=reshape(., [3 4]) to recover).
+% T * [X;1] = Xnew
+
+function values = orBodyGetTransform(bodyid)
+
+links = orBodyGetLinks(bodyid);
+values = links(:,1);
\ No newline at end of file
Added: pkg/trunk/openrave_planning/openraveros/octave/orBodySetJointValues.m
===================================================================
--- pkg/trunk/openrave_planning/openraveros/octave/orBodySetJointValues.m (rev 0)
+++ pkg/trunk/openrave_planning/openraveros/octave/orBodySetJointValues.m 2008-12-14 00:26:09 UTC (rev 8072)
@@ -0,0 +1,18 @@
+% success=orBodySetJointValues(bodyid, values, indices)
+%
+% Set the raw joint values of a body. If bodyid is a robot, sets the robot's
+% joints ignoring its current active degrees of freedom. If a controller on
+% the robot is running, this function might not have any effect. Instead
+% use orRobotSetDOFValues
+% indices [optional] - array specifying the indices to control
+
+function success = orBodySetJointValues(bodyid, values, indices)
+session = openraveros_getglobalsession();
+req = openraveros_body_setjointvalues();
+req.bodyid = bodyid;
+req.jointvalues = mat2cell(values,1,ones(1,length(values)));
+if( exist('indices','var') )
+ req.indices = mat2cell(indices,1,ones(1,length(indices)));
+end
+res = rosoct_session_call(session.id,'body_setjointvalues',req);
+success = ~isempty(res);
Added: pkg/trunk/openrave_planning/openraveros/octave/orBodySetTransform.m
===================================================================
--- pkg/trunk/openrave_planning/openraveros/octave/orBodySetTransform.m (rev 0)
+++ pkg/trunk/openrave_planning/openraveros/octave/orBodySetTransform.m 2008-12-14 00:26:09 UTC (rev 8072)
@@ -0,0 +1,25 @@
+% success=orBodySetTransform(bodyid, translation, quaternion)
+% success=orBodySetTransform(bodyid, [quaternion translation])
+% success=orBodySetTransform(bodyid, transform matrix) (12x1, 1x12, or 3x4)
+%
+% Set the affine transformation of the body. The transformation actually describes the first
+% link of the body. The rest of the links are derived by the joint angles.
+% a quaternion is related to axis and angle via: [cos(theta/2);sin(theta/2)*axis]
+function success = orBodySetTransform(varargin)
+session = openraveros_getglobalsession();
+req = openraveros_body_settransform();
+req.bodyid = varargin{1};
+
+if(nargin >= 3)
+ R = openraveros_rotfromquat(varargin{3});
+ req.transform.m(1:9) = mat2cell(R(:));
+ req.transform.m(10:12) = mat2cell(varargin{2});
+elseif(nargin == 2)
+ t = varargin{2};
+ req.transform.m(1:12) = mat2cell(t(:));
+else
+ error('orBodySetTransform not enough arguments');
+end
+
+res = rosoct_session_call(session.id,'body_settransform',req);
+success = ~isempty(res);
Added: pkg/trunk/openrave_planning/openraveros/octave/orEnvCheckCollision.m
===================================================================
--- pkg/trunk/openrave_planning/openraveros/octave/orEnvCheckCollision.m (rev 0)
+++ pkg/trunk/openrave_planning/openraveros/octave/orEnvCheckCollision.m 2008-12-14 00:26:09 UTC (rev 8072)
@@ -0,0 +1,48 @@
+% [collision, colbodyid,contacts, mindist] = orEnvCheckCollision(bodyid,excludeid,req_contacts)
+%
+% 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)
+
+session = openraveros_getglobalsession();
+req = openraveros_env_checkcollision();
+
+req.bodyid = bodyid(1);
+if( length(bodyid)>1)
+ req.linkid = bodyid(2);
+else
+ req.linkid = -1; % all links of the body
+end
+
+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
+
+res = rosoct_session_call(session.id,'env_checkcollision',req);
+
+if(~isempty(res))
+ collision = res.collision;
+ colbodyid = res.collidingbodyid;
+
+ if( ~isempty(res.contacts) )
+ contacts = zeros(6,numrays);
+ for i = 1:length(res.contacts)
+ colinfo(1:3,i) = cell2mat(res.contacts{i}.position);
+ colinfo(4:6,i) = cell2mat(res.contacts{i}.normal);
+ end
+ else
+ contacts = [];
+ end
+
+ hitbodies = cell2mat(res.hitbodies);
+else
+ collision = [];
+ colbodyid = [];
+ contacts = [];
+ mindist = [];
+end
Added: pkg/trunk/openrave_planning/openraveros/octave/orEnvClose.m
===================================================================
--- pkg/trunk/openrave_planning/openraveros/octave/orEnvClose.m (rev 0)
+++ pkg/trunk/openrave_planning/openraveros/octave/orEnvClose.m 2008-12-14 00:26:09 UTC (rev 8072)
@@ -0,0 +1,13 @@
+% success=orEnvClose(figureids)
+%
+% closes the figures and plots
+% figureids - array of ids returned from orEnvPlot or other plotting functions
+function success=orEnvClose(figureids)
+session = openraveros_getglobalsession();
+req = openraveros_env_closefigures();
+if( exist('figureids','var') && ~isempty(figureids) )
+ req.figureids = mat2cell(figureids,1,ones(1,length(figureids)));
+end
+
+res = rosoct_session_call(session.id,'env_closefigures',req);
+success = ~isempty(res);
Added: pkg/trunk/openrave_planning/openraveros/octave/orEnvCreateKinBody.m
===================================================================
--- pkg/trunk/openrave_planning/openraveros/octave/orEnvCreateKinBody.m (rev 0)
+++ pkg/trunk/openrave_planning/openraveros/octave/orEnvCreateKinBody.m 2008-12-14 00:26:09 UTC (rev 8072)
@@ -0,0 +1,15 @@
+% bodyid = orEnvCreateKinBody(name, xmlfile)
+
+function bodyid = orEnvCreateKinBody(name, xmlfile)
+session = openraveros_getglobalsession();
+req = openraveros_env_createbody();
+req.name = name;
+req.file = xmlfile;
+res = rosoct_session_call(session.id,'env_createbody',req);
+
+if(~isempty(res))
+ bodyid = res.bodyid;
+else
+ bodyid = [];
+end
+
Added: pkg/trunk/openrave_planning/openraveros/octave/orEnvCreatePlanner.m
===================================================================
--- pkg/trunk/openrave_planning/openraveros/octave/orEnvCreatePlanner.m (rev 0)
+++ pkg/trunk/openrave_planning/openraveros/octave/orEnvCreatePlanner.m 2008-12-14 00:26:09 UTC (rev 8072)
@@ -0,0 +1,13 @@
+% plannerid = orEnvCreatePlanner(plannertype)
+
+function plannerid = orEnvCreatePlanner(plannertype)
+session = openraveros_getglobalsession();
+req = openraveros_env_createplanner();
+req.plannertype = plannertype;
+res = rosoct_session_call(session.id,'env_createplanner',req);
+
+if(~isempty(res))
+ plannerid = res.plannerid;
+else
+ plannerid = [];
+end
Added: pkg/trunk/openrave_planning/openraveros/octave/orEnvCreateProblem.m
===================================================================
--- pkg/trunk/openrave_planning/openraveros/octave/orEnvCreateProblem.m (rev 0)
+++ pkg/trunk/openrave_planning/openraveros/octave/orEnvCreateProblem.m 2008-12-14 00:26:09 UTC (rev 8072)
@@ -0,0 +1,28 @@
+% problemid = orEnvCreateProblem(problemtype, args, destroyduplicates)
+%
+% Creates an instance of a problem and returns its id for future communicate with it
+% problemtype - the problem type
+% args - a string of arguments to send to the problem's main function
+% destroyduplicates [optional] - if 1, will destroy any previous problems with the same problem name.
+% If 0, will not destroy anything.
+% The default value is 1.
+function problemid = orEnvCreateProblem(problemtype, args, destroyduplicates)
+
+session = openraveros_getglobalsession();
+req = openraveros_env_createproblem();
+req.problemtype = problemtype;
+if( exist('args','var') )
+ req.args = args;
+end
+if( exist('destroyduplicates', 'var') )
+ req.destroyduplicates = destroyduplicates;
+else
+ req.destroyduplicates = 1;
+end
+res = rosoct_session_call(session.id,'env_createproblem',req);
+
+if(~isempty(res))
+ problemid = res.problemid;
+else
+ problemid = [];
+end
Added: pkg/trunk/openrave_planning/openraveros/octave/orEnvCreateRobot.m
===================================================================
--- pkg/trunk/openrave_planning/openraveros/octave/orEnvCreateRobot.m (rev 0)
+++ pkg/trunk/openrave_planning/openraveros/octave/orEnvCreateRobot.m 2008-12-14 00:26:09 UTC (rev 8072)
@@ -0,0 +1,22 @@
+% robotid = orEnvCreateRobot(robotname, xmlfile, type)
+%
+% Creates a robot of the given type. If type is not specified, creates a generic robot
+
+function bodyid = orEnvCreateRobot(robotname, xmlfile, type)
+
+session = openraveros_getglobalsession();
+req = openraveros_env_createrobot();
+req.robotname = robotname;
+req.file = xmlfile;
+if( exist('type','var') )
+ req.type = type;
+else
+ req.type = 'GenericRobot';
+end
+res = rosoct_session_call(session.id,'env_createrobot',req);
+
+if(~isempty(res))
+ bodyid = res.bodyid;
+else
+ bodyid = [];
+end
Added: pkg/trunk/openrave_planning/openraveros/octave/orEnvDestroyProblem.m
===================================================================
--- pkg/trunk/openrave_planning/openraveros/octave/orEnvDestroyProblem.m (rev 0)
+++ pkg/trunk/openrave_planning/openraveros/octave/orEnvDestroyProblem.m 2008-12-14 00:26:09 UTC (rev 8072)
@@ -0,0 +1,10 @@
+% success=orEnvDestroyProblem(problemid)
+%
+% Destroys problem instance whose id is problemid.
+
+function success = orEnvDestroyProblem(problemid)
+session = openraveros_getglobalsession();
+req = openraveros_env_destroyproblem();
+req.problemid = problemid;
+res = rosoct_session_call(session.id,'env_destroyproblem',req);
+success = ~isempty(res);
Added: pkg/trunk/openrave_planning/openraveros/octave/orEnvGetBodies.m
===================================================================
--- pkg/trunk/openrave_planning/openraveros/octave/orEnvGetBodies.m (rev 0)
+++ pkg/trunk/openrave_planning/openraveros/octave/orEnvGetBodies.m 2008-12-14 00:26:09 UTC (rev 8072)
@@ -0,0 +1,34 @@
+% bodies = orEnvGetBodies(bodyid,options)
+%
+% Input:
+% bodyid (optional) - specific body to get info for, if not specified returns all bodies
+% options - mask of BodyInfo.Req_X options (if not specified gets all information)
+% Output:
+% bodies is a cell array of all body objects in the scene
+% every cell contains a struct with the following parameters
+% id - bodyid
+% filename - filename used to initialize the body with
+% name - human robot name
+% type - xml type of body
+
+function bodies = orEnvGetBodies(bodyid,options)
+session = openraveros_getglobalsession();
+req = openraveros_env_getbodies();
+if( exist('bodyid','var') )
+ req.bodyid = bodyid;
+end
+if( exist('options','var') )
+ req.options = options;
+else
+ req.options = 65535; % get everything
+end
+res = rosoct_session_call(session.id,'env_getbodies',req);
+
+if(~isempty(res))
+ bodies = cell(length(res.bodies),1);
+ for i = 1:length(res.bodies)
+ bodies{i} = openraveros_getbodyinfo(res.bodies{i});
+ end
+else
+ bodies = [];
+end
\ No newline at end of file
Added: pkg/trunk/openrave_planning/openraveros/octave/orEnvGetBody.m
===================================================================
--- pkg/trunk/openrave_planning/openraveros/octave/orEnvGetBody.m (rev 0)
+++ pkg/trunk/openrave_planning/openraveros/octave/orEnvGetBody.m 2008-12-14 00:26:09 UTC (rev 8072)
@@ -0,0 +1,15 @@
+% id = orEnvGetBody(bodyname)
+%
+% returns the id of the body that corresponds to bodyname
+
+function id = orEnvGetBody(bodyname)
+session = openraveros_getglobalsession();
+req = openraveros_env_getbody();
+req.name = bodyname;
+res = rosoct_session_call(session.id,'env_getbody',req);
+
+if(~isempty(res))
+ id = res.bodyid;
+else
+ id = [];
+end
Added: pkg/trunk/openrave_planning/openraveros/octave/orEnvGetRobots.m
===================================================================
--- pkg/trunk/openrave_planning/openraveros/octave/orEnvGetRobots.m (rev 0)
+++ pkg/trunk/openrave_planning/openraveros/octave/orEnvGetRobots.m 2008-12-14 00:26:09 UTC (rev 8072)
@@ -0,0 +1,35 @@
+% robots = orEnvGetRobots(robotid,options)
+%
+%% Input:
+%% robotid - uid of robot
+%% options - set of options that controls what is sent back (BodyInfo.Req_X and RobotInfo.Req_X).
+%% If none specified gets everything.
+%% Output:
+% robots is a cell array of robots
+% every cell contains a struct with the following parameters
+% id - robotid
+% filename - filename used to initialize the body with
+% name - human robot name
+% type - type of robot
+
+function robots = orEnvGetRobots(robotid, options)
+session = openraveros_getglobalsession();
+req = openraveros_env_getrobots();
+if( exist('robotid','var') )
+ req.bodyid = robotid;
+end
+if( exist('options','var') )
+ req.options = options;
+else
+ req.options = 65535; % get everything
+end
+res = rosoct_session_call(session.id,'env_getrobots',req);
+
+if(~isempty(res))
+ robots = cell(length(res.robots),1);
+ for i = 1:length(res.robots)
+ robots{i} = openraveros_getrobotinfo(res.robots{i});
+ end
+else
+ robots = {};
+end
Added: pkg/trunk/openrave_planning/openraveros/octave/orEnvLoadPlugin.m
===================================================================
--- pkg/trunk/openrave_planning/openraveros/octave/orEnvLoadPlugin.m (rev 0)
+++ pkg/trunk/openrave_planning/openraveros/octave/orEnvLoadPlugin.m 2008-12-14 00:26:09 UTC (rev 8072)
@@ -0,0 +1,11 @@
+% success = orEnvLoadPlugin(filename)
+%
+% Loads a plugin.
+% filename - the relative path of the plugin to load. (*.so for linux, *.dll for windows)
+
+function success = orEnvLoadPlugin(filename)
+session = openraveros_getglobalsession();
+req = openraveros_env_loadplugin();
+req.filename = filename;
+res = rosoct_session_call(session.id,'env_loadplugin',req);
+success = ~isempty(res);
Added: pkg/trunk/openrave_planning/openraveros/octave/orEnvLoadScene.m
===================================================================
--- pkg/trunk/openrave_planning/openraveros/octave/orEnvLoadScene.m (rev 0)
+++ pkg/trunk/openrave_planning/openraveros/octave/orEnvLoadScene.m 2008-12-14 00:26:09 UTC (rev 8072)
@@ -0,0 +1,18 @@
+% success = orEnvLoadScene(filename, [ClearScene])
+%
+% Loads a new environment.
+% filename - The filename of the scene to load. If a relative file
+% is specified, note that it is relative to the current direction
+% of the OpenRAVE executable.
+% ClearScene - If 1, then clears the scene before loading. Else leaves the
+% scene alone and loads in addition to it.
+
+function success = orEnvLoadScene(filename, ClearScene)
+session = openraveros_getglobalsession();
+req = openraveros_env_loadscene();
+req.filename = filename;
+if( exist('ClearScene','var') )
+ req.resetscene = ClearScene;
+end
+res = rosoct_session_call(session.id,'env_loadscene',req);
+success = ~isempty(res);
\ No newline at end of file
Added: pkg/trunk/openrave_planning/openraveros/octave/orEnvPlot.m
===================================================================
--- pkg/trunk/openrave_planning/openraveros/octave/orEnvPlot.m (rev 0)
+++ pkg/trunk/openrave_planning/openraveros/octave/orEnvPlot.m 2008-12-14 00:26:09 UTC (rev 8072)
@@ -0,0 +1,58 @@
+% figureid = orEnvPlot(points,...)
+%
+% plots points or lines in the openrave viewer
+% points - Nx3 vector of xyz positions
+% optional arguments include 'size', 'color', and 'line'
+% color - Nx3 vector of RGB values between 0 and 1
+% size - Nx1 vector of the sizes in pixels of each point/line
+% line (or linestrip) - if specified, then openrave renders a line strip
+% linelist - if specified, openrave renders a line for every two points
+% trilist - if specified, openrave renders a triangle for every three
+% vertices should be specified in counter-clockwise order
+% sphere - if specified, openrave renders each point as a sphere
+% transparency - [0,1], set transparency of plotted objects (0 is opaque)
+function figureid = orEnvPlot(varargin)
+
+session = openraveros_getglobalsession();
+req = openraveros_env_plot();
+
+points = varargin{1}';
+numpoints = size(points,2);
+req.points = mat2cell(points(:),1,ones(numel(points),1));
+
+req.size = 0.5;
+req.drawtype = req.Draw_Point();
+req.transparency = 0;
+
+i = 2;
+while(i <= nargin)
+ if( strcmp(varargin{i},'size') )
+ i = i + 1;
+ colors = varargin{i};
+ req.size = mat2cell(colors(:),1,ones(numel(colors;),1));
+ elseif( strcmp(varargin{i},'color') )
+ i = i + 1;
+ req.colors = varargin{i};
+ elseif( strcmp(varargin{i},'line') | strcmp(varargin{i},'linestrip') )
+ req.drawtype = req.Draw_LineStrip();
+ elseif( strcmp(varargin{i},'linelist') )
+ req.drawtype = req.Draw_LineList();
+ elseif( strcmp(varargin{i},'sphere') )
+ req.drawtype = req.Draw_Sphere();
+ elseif( strcmp(varargin{i},'trilist') )
+ req.drawtype = req.Draw_TriList();
+ elseif( strcmp(varargin{i},'transparency') )
+ i = i + 1;
+ req.transparency = varargin{i};
+ end
+
+ i = i + 1;
+end
+
+res = rosoct_session_call(session.id,'env_plot',req);
+
+if(~isempty(res))
+ figureid = req.figureid;
+else
+ figureid = [];
+end
Added: pkg/trunk/openrave_planning/openraveros/octave/orEnvRayCollision.m
===================================================================
--- pkg/trunk/openrave_planning/openraveros/octave/orEnvRayCollision.m (rev 0)
+++ pkg/trunk/openrave_planning/openraveros/octave/orEnvRayCollision.m 2008-12-14 00:26:09 UTC (rev 8072)
@@ -0,0 +1,59 @@
+% [collision, colinfo, hitbodies] = orEnvRayCollision(rays,[bodyid],req_contacts,req_bodyinfo)
+%
+% performs ray collision checks and returns the position and normals
+% where all the rays collide
+% bodyid (optional) - if non zero, will only collide with that ray
+% rays - a 6xN matrix where the first 3
+% rows are the ray position and last 3 are the ray direction
+% collision - N dim vector that is 1 for colliding rays and 0
+% for non-colliding rays colinfo is a 6xN vector that describes
+% where the ray hit and the normal to the surface of the hit point
+% where the first 3 columns are position and last 3 are normals
+% if bodyid is specified, only checks collisions with that body
+
+function [collision, colinfo, hitbodies] = orEnvRayCollision(rays,bodyid,req_contacts,req_bodyinfo)
+
+session = openraveros_getglobalsession();
+req = openraveros_env_raycollision();
+
+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)};
+end
+
+if( exist('bodyid','var') )
+ req.bodyid = bodyid;
+end
+if( exist('req_contacts','var') && req_contacts )
+ req.request_contacts = 1;
+end
+if( exist('req_bodyinfo','var') && req_bodyinfo )
+ req.request_bodies = req_bodyinfo;
+end
+
+res = rosoct_session_call(session.id,'env_raycollision',req);
+
+if(~isempty(res))
+ collision = cell2mat(res.collision);
+
+ if( req.request_contacts )
+ if( length(res.contacts) ~= numrays )
+ error('not equal');
+ end
+ colinfo = zeros(6,numrays);
+ for i = 1:numrays
+ colinfo(1:3,i) = cell2mat(res.contacts{i}.position);
+ colinfo(4:6,i) = cell2mat(res.contacts{i}.normal);
+ end
+ else
+ colinfo = [];
+ end
+
+ hitbodies = cell2mat(res.hitbodies);
+else
+ collision = [];
+ colinfo = [];
+ hitbodies = [];
+end
Added: pkg/trunk/openrave_planning/openraveros/octave/orEnvSetOptions.m
===================================================================
--- pkg/trunk/openrave_planning/openraveros/octave/orEnvSetOptions.m (rev 0)
+++ pkg/trunk/openrave_planning/openraveros/octave/orEnvSetOptions.m 2008-12-14 00:26:09 UTC (rev 8072)
@@ -0,0 +1,70 @@
+% success = orEnvSetOptions(options)
+%
+% A string of various environment options. Example usage:
+% orEnvSetOptions('publishanytime 1');
+%
+% Current options:
+% - simulation [start/stop] [time_step] - toggles the internal simulation loop, ie all the calls to SimulationStep. If time_step is specified, will set the simulation time step for all objects. Note that this is not tied to real time at all, how fast the simulation goes in reality depends on complexity of the scene and the physics engine being used.
+% - physics engine_name - switches the physics engine to another one with id 'engine_name'
+% - collision checker_name - switches the collision checker to a new one with id 'checker_name'
+% - gravity [x y z] - changes to gravity vector
+% - publishanytime [1/0] - switch between publishing the body transformations
+% to the GUI anytime or only between stepsimulation and server messsages.
+% When publishing anytime, the GUI will reflect the body movements after every
+% move. This is useful when visualizing internal C++ states. When off, the GUI
+% will only reflect the state of robots after all calls to stepsimulation and
+% server send messages have been done. The default is off.
+% - debug [debug level] - toggles debugging messages by RAVELOG.
+% 0 - only RAVEPRINT statements show
+% 1+ - RAVELOG statements with various debug levels show
+% - quit - closes the openrave instance
+function success = orEnvSetOptions(options)
+
+session = openraveros_getglobalsession();
+req = openraveros_env_set();
+
+[cmd, rem] = strtok(options, ' ');
+cmd = strtrim(cmd);
+switch(cmd)
+ case 'simulation'
+ req.setmask = req.Set_Simulation();
+ [cmd,rem] = strtok(cmd);
+ cmd = strtrim(cmd);
+
+ if( strcmp(cmd,'start') )
+ req.sim_action = req.SimAction_Start();
+ elseif( strcmp(cmd,'stop') )
+ req.sim_action = req.SimAction_Stop();
+ elseif( strcmp(cmd,'timestep') )
+ req.sim_action = req.SimAction_Timestep();
+ end
+
+ n = str2num(rem);
+ if( ~isempty(n) )
+ req.sim_timestep = n;
+ end
+
+ case 'physics'
+ req.setmask = req.Set_PhysicsEngine();
+ req.physicsengine = strtrim(rem);
+ case 'collision'
+ req.setmask = req.Set_CollisionChecker();
+ req.collisionchecker = strtrim(rem);
+ case 'viewer'
+ req.setmask = req.Set_Viewer();
+ req.viewer = strtrim(rem);
+ case 'gravity'
+ req.setmask = req.Set_Gravity();
+ req.gravity(1:3) = mat2cell(str2num(rem),1,[1 1 1]);
+ case 'publishanytime'
+ req.setmask = req.Set_PublishAnytime();
+ req.publishanytime = str2num(rem);
+ case 'debug'
+ req.setmask = req.Set_DebugLevel();
+ req.debuglevel = str2num(rem);
+ otherwise
+ display('unknown command');
+end
+
+res = rosoct_session_call(session.id,'env_set',req);
+success = ~isempty(res);
Added: pkg/trunk/openrave_planning/openraveros/octave/orEnvTriangulate.m
===================================================================
--- pkg/trunk/openrave_planning/openraveros/octave/orEnvTriangulate.m (rev 0)
+...
[truncated message content] |
