Menu
▾
▴
[Personalrobots-commit] SF.net SVN: personalrobots:[9026] pkg/trunk
|
From: <rdi...@us...> - 2009-01-08 01:14:46
|
Revision: 9026
http://personalrobots.svn.sourceforge.net/personalrobots/?rev=9026&view=rev
Author: rdiankov
Date: 2009-01-08 01:14:36 +0000 (Thu, 08 Jan 2009)
Log Message:
-----------
fixed several bugs in the laser/camera calibration
Modified Paths:
--------------
pkg/trunk/calibration/laser_camera_calibration/gatherdata.ros.xml
pkg/trunk/calibration/laser_camera_calibration/octave/QuatFromRotationMatrix.m
pkg/trunk/calibration/laser_camera_calibration/octave/SegmentLines.m
pkg/trunk/calibration/laser_camera_calibration/octave/calibratevalues.m
pkg/trunk/calibration/laser_camera_calibration/octave/runcalibration.m
pkg/trunk/calibration/laser_camera_calibration/octave/startgathering.m
pkg/trunk/openrave_planning/openraveros/octave/openraveros_restart.m
pkg/trunk/openrave_planning/openraveros/octave/openraveros_startup.m
pkg/trunk/openrave_planning/ormanipulation/octave/runperception.m
pkg/trunk/openrave_planning/orrosplanning/manifest.xml
pkg/trunk/openrave_planning/orrosplanning/src/rosrobotcontroller.h
Modified: pkg/trunk/calibration/laser_camera_calibration/gatherdata.ros.xml
===================================================================
--- pkg/trunk/calibration/laser_camera_calibration/gatherdata.ros.xml 2009-01-08 01:09:40 UTC (rev 9025)
+++ pkg/trunk/calibration/laser_camera_calibration/gatherdata.ros.xml 2009-01-08 01:14:36 UTC (rev 9026)
@@ -1,12 +1,6 @@
<launch>
<machine name="localhost_cameras" address="localhost" default="false"/>
-<!-- <node machine="cameras" name="dcam" pkg="dcam" type="dcam" respawn="false">
- <param name="video_mode" type="str" value="640x480_videre_rectified"/>
- <param name="do_rectify" type="bool" value="False"/>
- <param name="do_stereo" type="bool" value="False"/>
- <param name="do_calc_points" type="bool" value="False"/>
- </node> -->
<group ns="checkerdetector" clear_params="true">
<param name="display" type="int" value="1"/>
<param name="frame_id" type="string" value="head_tilt_link"/>
@@ -18,20 +12,31 @@
<node machine="localhost_cameras" pkg="checkerboard_detector" type="checkerboard_detector" respawn="true" output="screen">
<remap from="CamInfo" to="/stereo/left/cam_info"/>
<remap from="Image" to="/stereo/left/image_rect"/>
-<!-- <env name="DISPLAY" value=":0.0"/> -->
+ <!-- <env name="DISPLAY" value=":0.0"/> -->
</node>
</group>
-
+
+ <!-- start nodding laser -->
+ <node pkg="mechanism_control" type="spawner.py" args="$(find wg_robot_description)/laser_tilt/laser_tilt_controller.xml" />
+ <node pkg="pr2_mechanism_controllers" type="control_laser.py" args="laser_tilt_controller sine 10 .75 .25" />
+
<!-- start the filtering node -->
<node machine="localhost_cameras" pkg="laser_camera_calibration" type="gatherdata.py" output="screen">
<remap from="ObjectDetection" to="/checkerdetector/ObjectDetection"/>
</node>
- <!-- start nodding laser -->
- <node pkg="mechanism_control" type="spawner.py" args="$(find wg_robot_description)/laser_tilt/laser_tilt_controller.xml" />
- <node pkg="pr2_mechanism_controllers" type="control_laser.py" args="laser_tilt_controller sine 10 .75 .25" />
-
<!-- start recording -->
<node pkg="rosrecord" type="rosrecord" args="-f $(find laser_camera_calibration)/lasercamcalib /new_ObjectDetection /new_tilt_scan /new_mechanism_state"/>
+
+ <!-- DCAM - this should be brough up with the robot, but in case they are not, comment this out -->
+<!-- <group ns="stereo">
+ <param name="videre_mode" type="str" value="rectified"/>
+ <param name="do_colorize" type="bool" value="False"/>
+ <param name="do_rectify" type="bool" value="False"/>
+ <param name="do_stereo" type="bool" value="False"/>
+ <param name="do_calc_points" type="bool" value="False"/>
+ </group>
+
+ <node machine="camera-machine" pkg="dcam" type="stereodcam" respawn="false"/>
+ <node machine="camera-machine" pkg="stereo_image_proc" type="stereoproc" respawn="false"/> -->
</launch>
-
Modified: pkg/trunk/calibration/laser_camera_calibration/octave/QuatFromRotationMatrix.m
===================================================================
--- pkg/trunk/calibration/laser_camera_calibration/octave/QuatFromRotationMatrix.m 2009-01-08 01:09:40 UTC (rev 9025)
+++ pkg/trunk/calibration/laser_camera_calibration/octave/QuatFromRotationMatrix.m 2009-01-08 01:14:36 UTC (rev 9026)
@@ -41,22 +41,22 @@
switch(convcase)
case 1
s = sqrt((R(1,1) - (R(2,2) + R(3,3))) + 1);
- quat(2) = (dReal)(0.5) * s;
- s = (dReal)(0.5) / s;
+ quat(2) = 0.5 * s;
+ s = 0.5 / s;
quat(3) = (R(1,2) + R(2,1)) * s;
quat(4) = (R(3,1) + R(1,3)) * s;
quat(1) = (R(3,2) - R(2,3)) * s;
case 2
- s = RaveSqrt((R(2,2) - (R(3,3) + R(1,1))) + 1);
- quat(3) = (dReal)(0.5) * s;
- s = (dReal)(0.5) / s;
+ s = sqrt((R(2,2) - (R(3,3) + R(1,1))) + 1);
+ quat(3) = 0.5 * s;
+ s = 0.5 / s;
quat(4) = (R(2,3) + R(3,2)) * s;
quat(2) = (R(1,2) + R(2,1)) * s;
quat(1) = (R(1,3) - R(3,1)) * s;
case 3
- s = RaveSqrt((R(3,3) - (R(1,1) + R(2,2))) + 1);
- quat(4) = (dReal)(0.5) * s;
- s = (dReal)(0.5) / s;
+ s = sqrt((R(3,3) - (R(1,1) + R(2,2))) + 1);
+ quat(4) = 0.5 * s;
+ s = 0.5 / s;
quat(2) = (R(3,1) + R(1,3)) * s;
quat(3) = (R(2,3) + R(3,2)) * s;
quat(1) = (R(2,1) - R(1,2)) * s;
Modified: pkg/trunk/calibration/laser_camera_calibration/octave/SegmentLines.m
===================================================================
--- pkg/trunk/calibration/laser_camera_calibration/octave/SegmentLines.m 2009-01-08 01:09:40 UTC (rev 9025)
+++ pkg/trunk/calibration/laser_camera_calibration/octave/SegmentLines.m 2009-01-08 01:14:36 UTC (rev 9026)
@@ -38,7 +38,7 @@
pointthresh = 1;
end
-thresh = 0.015;
+thresh = 0.025;
linedata = [];
% dists = polar_points(1,:);
Modified: pkg/trunk/calibration/laser_camera_calibration/octave/calibratevalues.m
===================================================================
--- pkg/trunk/calibration/laser_camera_calibration/octave/calibratevalues.m 2009-01-08 01:09:40 UTC (rev 9025)
+++ pkg/trunk/calibration/laser_camera_calibration/octave/calibratevalues.m 2009-01-08 01:14:36 UTC (rev 9026)
@@ -1,7 +1,9 @@
-%% [Tcamera, Tlaser, jointoffsets] = calibratevalues(calibdata, robot)
+%% [Tcamera, Tlaser, jointoffsets] = calibratevalues(calibdata, robot, Tcamerainit, Tlaserinit)
%%
-%% Needs openrave to perform the kinematics
-function [Tcamera, Tlaser, jointoffsets] = calibratevalues(calibdata, robot)
+%% The main optimization loop to calibrate the values, needs openraveros to perform the kinematics
+%% Tcamerainit (optional) - 3x4 matrix of the initial camera frame
+%% Tlaserinit (optional) - 3x4 matrix of the initial laser frame
+function [Tcamera, Tlaser, jointoffsets] = calibratevalues(calibdata, robot, Tcamerainit, Tlaserinit)
tic;
links = orBodyGetLinks(robot.id);
@@ -16,7 +18,7 @@
end
% joint offsets to calibrate
-jointnames = {};%'head_pan_joint'};
+jointnames = {'head_pan_joint'};
joints = [];
for i = 1:length(jointnames)
joints(i) = find(cellfun(@(x) strcmp(x,jointnames{i}), robot.jointnames),1,'first');
@@ -32,16 +34,22 @@
end
%% get initial values
-Tlaser = inv([reshape(links(:,laser_tilt_mount_link),[3 4]); 0 0 0 1]) * [reshape(links(:,laser_tilt_link),[3 4]); 0 0 0 1];
-Tinvcamera = inv([reshape(links(:,stereo_link),[3 4]); 0 0 0 1]) * [reshape(links(:,head_tilt_link),[3 4]); 0 0 0 1];
-Xinit = [AxisAngleFromRotation(Tinvcamera(1:3,1:3));Tinvcamera(1:3,4);...
- AxisAngleFromRotation(Tlaser(1:3,1:3));Tlaser(1:3,4);zeros(size(joints))];
+if( ~exist('Tcamerainit','var') || isempty(Tcamerainit) )
+ Tcamerainit = inv([reshape(links(:,head_tilt_link),[3 4]); 0 0 0 1]) * [reshape(links(:,stereo_link),[3 4]); 0 0 0 1];
+end
+if( ~exist('Tlaserinit','var') || isempty(Tlaserinit) )
+ Tlaserinit = inv([reshape(links(:,laser_tilt_mount_link),[3 4]); 0 0 0 1]) * [reshape(links(:,laser_tilt_link),[3 4]); 0 0 0 1];
+end
+
+Tcamerainitinv = inv([Tcamerainit(1:3,1:4); 0 0 0 1]);
+Xinit = [AxisAngleFromRotation(Tcamerainitinv(1:3,1:3));Tcamerainitinv(1:3,4);...
+ AxisAngleFromRotation(Tlaserinit(1:3,1:3));Tlaserinit(1:3,4);zeros(size(joints))];
+
x = zeros(length(calibdata),1);
y = x;
display('starting to optimize');
-[f,X, kvg, iter] = leasqr(x,y, Xinit, @(data,X) calibdist(X,robot,calibdata,joints,laser_tilt_mount_link,head_tilt_link), 0.0001, 100);
-f
+[f,X, kvg, iter] = leasqr(x,y, Xinit, @(data,X) calibdist(X,robot,calibdata,joints,laser_tilt_mount_link,head_tilt_link), 0.0001, 400);
Tcamera = inv([RotationMatrixFromAxisAngle(X(1:3)) X(4:6); 0 0 0 1]);
Tlaser = [RotationMatrixFromAxisAngle(X(7:9)) X(10:12); 0 0 0 1];
jointoffsets = zeros(robot.dof,1);
@@ -71,7 +79,9 @@
end
Nlaserplane = transpose(Tinvcamera * inv(Thead_tilt_link) * Tlaser_tilt_mount_link * Tlaser) * calibdata{i}.Ncamera;
- Nlaserplane = Nlaserplane / norm(Nlaserplane(1:3));
+ if( abs(norm(Nlaserplane(1:3))-1) > 0.01 )
+ error('wtf');
+ end
%% find the 'area' under the line using the plane as the 0
%% first transform the line in plane space so that y axis is plane
@@ -99,9 +109,15 @@
dist = s1(2) * (s1(1)-s2(1));
end
- F(i) = min(dist,1000);
+% dist
+% clf();
+% plot([s1(1) s2(1)],[0 0],'r'); hold on;
+% plot([s1(1) s2(1)],[s1(2) s2(2)],'b','linewidth',3);
+% drawnow;
+ F(i) = max(min(dist,1000),-1000);
else
F(i) = 1000;
end
end
-sum(F.^2)
+%F'
+sqrt(sum(F.^2))
Modified: pkg/trunk/calibration/laser_camera_calibration/octave/runcalibration.m
===================================================================
--- pkg/trunk/calibration/laser_camera_calibration/octave/runcalibration.m 2009-01-08 01:09:40 UTC (rev 9025)
+++ pkg/trunk/calibration/laser_camera_calibration/octave/runcalibration.m 2009-01-08 01:14:36 UTC (rev 9026)
@@ -15,9 +15,12 @@
robot = orEnvGetRobots(robotid);
-calibdata = startgathering(robot);
+calibdata = startgathering(robot);
-[Tcamera, Tlaser, jointoffsets] = calibratevalues(calibdata, robot); % compute the calibration values
+Tcamerainit = [0 0 1 0.05;
+ -1 0 0 0.05;
+ 0 -1 0 0.095];
+[Tcamera, Tlaser, jointoffsets] = calibratevalues(calibdata, robot, Tcamerainit); % compute the calibration values
Tcamera
Tlaser
jointoffsets
Modified: pkg/trunk/calibration/laser_camera_calibration/octave/startgathering.m
===================================================================
--- pkg/trunk/calibration/laser_camera_calibration/octave/startgathering.m 2009-01-08 01:09:40 UTC (rev 9025)
+++ pkg/trunk/calibration/laser_camera_calibration/octave/startgathering.m 2009-01-08 01:14:36 UTC (rev 9026)
@@ -16,7 +16,7 @@
rosoct_add_msgs('checkerboard_detector');
rosoct_add_msgs('robot_msgs');
-queuesize = 1000;
+queuesize = 1000; % need big buffer
__calibdata = {};
lastobjdet = {};
__iterationcount = 0;
@@ -28,7 +28,7 @@
lastlaserscan = {};
__rosoct_msg_unsubscribe("new_tile_scan");
-success = rosoct_subscribe("new_tile_scan", @std_msgs_LaserScan, @(msg) laserscancb(msg,robot),queuesize);
+success = rosoct_subscribe("new_tilt_scan", @std_msgs_LaserScan, @(msg) laserscancb(msg,robot),queuesize);
if( ~success )
error('subscribe failed');
end
@@ -55,7 +55,7 @@
__rosoct_msg_unsubscribe("new_tile_scan");
__rosoct_msg_unsubscribe("new_mechanism_state");
-display('gathering calibration done');
+display(sprintf('gathering calibration done, total extracted: %d, total parsed: %d', length(__calibdata), __iterationcount));
calibdata = __calibdata;
__calibdata = {};
@@ -125,10 +125,11 @@
end
laserpoints = laserpoints(:,validpoints);
-linedata = SegmentLines(laserpoints, laserscanmsg.range_max, 0);
+linedata = SegmentLines(laserpoints, laserscanmsg.range_max, 1);
%% take the line intersecting with x-axis (middle of laser range)
-laserline = linedata(:,find( sign(linedata(2,:)) ~= sign(linedata(4,:)), 1, 'first'));
+ilaserline = find( sign(linedata(2,:)) ~= sign(linedata(4,:)), 1, 'first');
+laserline = linedata(:,ilaserline);
if( isempty(laserline) )
display('could not find laser line');
return;
@@ -136,17 +137,61 @@
%% threshold length of line
laserdir = laserline(1:2)-laserline(3:4);
-if( norm(laserdir) < 0.8 )
- display(sprintf('line not long enough %f', norm(laserdir)));
+if( norm(laserdir) < 0.6 || norm(laserdir) > 1.5 )
+ display(sprintf('line not right size %f', norm(laserdir)));
return;
end
-if( min(norm(laserline(1:2)),norm(laserline(3:4))) > 1.5 )
+if( min(norm(laserline(1:2)),norm(laserline(3:4))) > 2 )
display('line is too far');
return;
end
+%% ground can still be detected as checkerboard, so look for discontinuities
+Nlaser = [laserdir(2);-laserdir(1);0]/norm(laserdir);
+Nlaser(3) = -dot(Nlaser(1:2),laserline(1:2));
+
+testpoints = [];
+iprevline = ilaserline-1;
+while(iprevline > 0)
+ if( norm(linedata(1:2,iprevline)-linedata(3:4,iprevline)) > 0.2 )
+ break;
+ end
+ iprevline = iprevline-1;
+end
+
+if( iprevline > 0 )
+ testpoints = [testpoints [reshape(linedata(:,iprevline), [2 2]); 1 1]];
+end
+
+inextline = ilaserline+1;
+while(inextline < size(linedata,2))
+ if( norm(linedata(1:2,inextline)-linedata(3:4,inextline)) > 0.2 )
+ break;
+ end
+ inextline = inextline+1;
+end
+
+if( inextline < size(linedata,2) )
+ testpoints = [testpoints [reshape(linedata(:,inextline), [2 2]); 1 1]];
+end
+
+if( isempty(testpoints) )
+ display('no points to compare displacement to');
+ return;
+end
+
+maxdisplacement = max(abs(transpose(Nlaser)*testpoints));
+if( maxdisplacement < 0.4 )
+ maxdisplacement
+ plot(laserline([1 3]),laserline([2 4]),'g','linewidth',5); drawnow;
+ display('displacement is off');
+ return;
+end
+
+plot(laserline([1 3]),laserline([2 4]),'r','linewidth',5); drawnow;
data.laserline = laserline;
__calibdata{end+1} = data;
display(sprintf('adding data %d', length(__calibdata)));
+pause(4);
Modified: pkg/trunk/openrave_planning/openraveros/octave/openraveros_restart.m
===================================================================
--- pkg/trunk/openrave_planning/openraveros/octave/openraveros_restart.m 2009-01-08 01:09:40 UTC (rev 9025)
+++ pkg/trunk/openrave_planning/openraveros/octave/openraveros_restart.m 2009-01-08 01:14:36 UTC (rev 9026)
@@ -30,8 +30,16 @@
function openraveros_restart(sessionserver,viewer)
global openraveros_globalsession
-openraveros_startup();
+if( ~exist('sessionserver','var') || isempty(sessionserver) )
+ sessionserver = 'openrave_session';
+end
+if( ~exist('viewer','var') )
+ viewer = 'qtcoin';
+end
+
+openraveros_startup(sessionserver, 1, viewer);
+
if( ~isempty(openraveros_globalsession) )
%% send a dummy env_set command
res = rosoct_session_call(openraveros_globalsession.id,'env_set',openraveros_env_set());
@@ -41,19 +49,14 @@
end
end
-if( ~exist('sessionserver','var') || isempty(sessionserver) )
- sessionserver = 'openrave_session';
-end
openraveros_globalsession = openraveros_createsession(sessionserver);
-if( ~exist('viewer','var') )
- viewer = 'qtcoin';
-end
-
if( ~isempty(viewer) && ~isempty(openraveros_globalsession) )
%% set the viewer
reqset = openraveros_env_set();
- reqset.setmask = reqset.Set_Viewer();
- reqset.viewer = viewer;
+ if( ~isempty(viewer) )
+ reqset.setmask = reqset.Set_Viewer();
+ reqset.viewer = viewer;
+ end
resset = rosoct_session_call(openraveros_globalsession.id,'env_set',reqset);
end
Modified: pkg/trunk/openrave_planning/openraveros/octave/openraveros_startup.m
===================================================================
--- pkg/trunk/openrave_planning/openraveros/octave/openraveros_startup.m 2009-01-08 01:09:40 UTC (rev 9025)
+++ pkg/trunk/openrave_planning/openraveros/octave/openraveros_startup.m 2009-01-08 01:14:36 UTC (rev 9026)
@@ -26,7 +26,7 @@
%% POSSIBILITY OF SUCH DAMAGE.
%%
%% author: Rosen Diankov
-function openraveros_startup(sessionserver,createsession, veiwer)
+function openraveros_startup(sessionserver,createsession, viewer)
global openraveros_globalsession
persistent openraveros_initialized
@@ -46,15 +46,15 @@
openraveros_initialized = 1;
end
-if( ~exist('sessionserver','var') )
- sessionserver = 'openrave_session';
-end
+if( createsession && isempty(openraveros_globalsession) )
+ if( ~exist('sessionserver','var') )
+ sessionserver = 'openrave_session';
+ end
+
+ if( ~exist('viewer','var') )
+ viewer = 'qtcoin';
+ end
-if( ~exist('viewer','var') )
- viewer = 'qtcoin';
-end
-
-if( createsession && isempty(openraveros_globalsession) )
req = openraveros_openrave_session();
req.viewer = viewer; % default viewer
while(1)
Modified: pkg/trunk/openrave_planning/ormanipulation/octave/runperception.m
===================================================================
--- pkg/trunk/openrave_planning/ormanipulation/octave/runperception.m 2009-01-08 01:09:40 UTC (rev 9025)
+++ pkg/trunk/openrave_planning/ormanipulation/octave/runperception.m 2009-01-08 01:14:36 UTC (rev 9026)
@@ -8,9 +8,9 @@
robot = SetupTableScene('data/pr2table_real.env.xml',1);
-Tcamera = [0 0 1 -0.05;
- -1 0 0 -0.05;
- 0 -1 0 -0.095];
+Tcamera = [0 0 1 0.05;
+ -1 0 0 0.05;
+ 0 -1 0 0.095];
out = orProblemSendCommand(['createsystem ObjectTransform topic /checkerdetector/ObjectDetection thresh 0.1 robot ' sprintf('%d ', robot.id) ' matrixoffset ' sprintf('%f ', Tcamera(1:3,1:4))],probs.task);
if( isempty(out) )
error('failed to create checkerboard detector');
Modified: pkg/trunk/openrave_planning/orrosplanning/manifest.xml
===================================================================
--- pkg/trunk/openrave_planning/orrosplanning/manifest.xml 2009-01-08 01:09:40 UTC (rev 9025)
+++ pkg/trunk/openrave_planning/orrosplanning/manifest.xml 2009-01-08 01:14:36 UTC (rev 9026)
@@ -12,5 +12,6 @@
<depend package="boost"/>
<depend package="checkerboard_detector"/>
<depend package="robot_msgs"/>
+ <depend package="pr2_mechanism_controllers"/>
<depend package="tf"/>
</package>
Modified: pkg/trunk/openrave_planning/orrosplanning/src/rosrobotcontroller.h
===================================================================
--- pkg/trunk/openrave_planning/orrosplanning/src/rosrobotcontroller.h 2009-01-08 01:09:40 UTC (rev 9025)
+++ pkg/trunk/openrave_planning/orrosplanning/src/rosrobotcontroller.h 2009-01-08 01:14:36 UTC (rev 9026)
@@ -1,5 +1,5 @@
// Software License Agreement (BSD License)
-// Copyright (c) 2008, Willow Garage, Inc.
+// Copyright (c) 2008, Rosen Diankov
// Redistribution and use in source and binary forms, with or without
// modification, are permitted provided that the following conditions are met:
// * Redistributions of source code must retain the above copyright notice,
@@ -21,14 +21,15 @@
// 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
#ifndef RAVE_ROS_ROBOT_CONTROLLER
#define RAVE_ROS_ROBOT_CONTROLLER
#include <robot_msgs/MechanismState.h>
+#include <pr2_mechanism_controllers/StartTrajectory.h>
+#include <pr2_mechanism_controllers/CancelTrajectory.h>
+#include <pr2_mechanism_controllers/WaitTrajectory.h>
+#include <pr2_mechanism_controllers/QueryTrajectory.h>
-// controller for the SSC-32 board
class ROSRobotController : public ControllerBase
{
enum ControllerState{
@@ -127,6 +128,7 @@
virtual bool SetDesired(const dReal* pValues)
{
+ // set a path between the current and desired positions
return true;
}
@@ -307,6 +309,11 @@
// do some monitoring of the joint state (try to look for stalls)
}
+ void ControllerThread()
+ {
+
+ }
+
RobotBase* _probot; ///< robot owning this controller
string _topic;
@@ -323,6 +330,8 @@
double _fTimeCommandStarted;
const Trajectory* _ptraj;
+ // trajectory services
+ service::ServiceHandlePtr srvStartTrajectory, srvCancelTrajectory, srvWaitTrajectory, srvQueryTrajectory;
bool _bIsDone;
bool _bSendTimestamps; ///< if true, will send timestamps along with traj
bool _bSubscribed; ///< if true, succesfully subscribed to the mechanism state msgs
This was sent by the SourceForge.net collaborative development platform, the world's largest Open Source development site.
|
