Analysis of STAR Robot Workspace Based on RCM Position
1. Workspace Analysis of the STAR System based on
Different RCM Positions
I. INTRODUCTION
Robotic manipulators have become a extensively used tool
in Minimally Invasive Surgical(MIS) procedures. Manipulators
of different configuration like kinematic structure, degree of
freedom tend to vary their reachable workspace, probability
of singularity and also their performance. This work analysis
the reachable workspace of the Smart Tissue Anastomosis
Robot(STAR) considering the virtual remote centre of mo-
tion(RCM) assigned to it. Remote centre of motion refers to
the incision made on the patients body during laproscopic
procedures. The RCM is placed in different position with
respect to the robot and the volume of the reachable workspace
is calculated. Then based on the received data, an optimal
position for the STAR robot from the RCM is determined.
The tests are done in rviz[1], a simulation environment in
ROS [3].The reachable points of the end effector are plotted as
point cloud data in rviz and simultaneously a 3D map structure
is created using the open source Octomap[2] library. The map
created is then processed for volume calculation.
II. SYSTEM OVERVIEW
Initially the Universal Robot Description Format(URDF) of
the STAR is imported into rviz. The RCM point is visualized
in a green circular shape. As show in Fig. 1, for a given RCM
point the valid points(shown as green) and invalid point(shown
as red).
The volume calculation for a given RCM position goes
through the following steps:
• Cartesian coordinates generation and plotting point
clouds.
• Creating the Octomap based on the points generated.
• Calculating the volume from the Octomap
A. Cartesian coordinates generation and plotting point clouds
Based on the configuration of the KUKA arm LWR-4, the
maximum workspace reachable would be a hemisphere with
the tool’s radius. Initially the position of the RCM is set with
respect to the robot’s base. The Cartesian coordinates which
falls within the length of the tool is fed to the IK solver(which
is constrained by RCM). Fig. 2 shows a pictorial representation
of the points given to the solver. Fig. 3 shows the final point
cloud data.
B. Creating the Octomap based on the points generated
While plotting the point clouds in rviz, the same points are
constructed as an octomap in the background. The Octomap
methodology follows a tree based representation considering
Fig. 1. STAR plotting the valid points as green and invalid points as red in
rviz. The violet circle is the plane, which has the rcm point, which is green
in color.
Fig. 2. Cartesian coordinates fed to the RCM IK solver
mapped area and resolution. Octree is the data structure
involved in the construction of this map. In Fig. 4 an example
of an octree is shown. Each node of is called as an voxel and its
classified into occupied, empty and unknown voxel. For the
volume calculation, only the occupied voxels are taken into
consideration. The c++ code snippet takes in the Cartesian
coordinates of the point cloud and creates an octree structure.
Fig. 5 shows an octomap.
1 / / Minimum r e s o u l t i o n s e t to the 0.016 and i t i s used
f o r a l l the experiments
2 OcTree t r e e ( 0 . 0 1 6 ) ;
2. Fig. 3. A top view(left) and bottom view(right) of the point cloud data
which shows the reachable workspace of the STAR robot. The green points
are reachable points and the red points are not reachable.
Fig. 4. A basic octree structure. Black cells are occupied voxels and the white
ones are un-occupied voxels [2].
3 void g e t p o i n t s c a l l b a c k ( const geometry msgs : :
P o i n t C o n s t P t r& p )
4 {
5 ROS INFO( ” C o n s t r u c t i n g map . . . ” ) ;
6 / / r e c e i v e the c a r t e s i a n c o o r d i n a t e s and develop
a t r e e s t r u c u t e r e a l time
7 point3d endpoint ( ( f l o a t ) p−>x , ( f l o a t ) p−>y , (
f l o a t ) p−>z ) ;
8 t r e e . updateNode ( endpoint , true ) ;
9 }
10 void p l o t o n o f f ( const std msgs : : BoolConstPtr& s )
11 {
12 s t a t u s =s−>data ;
13 / / I f workspace g e n e r a t i o n i s over , then stop
and w r i t e the t r e e s t r u c t u r e to f i l e .
14 i f ( s t a t u s ==0)
15 {
16
17 t r e e . w r i t e B i n a r y ( ” t r e e a l l . bt ” ) ;
18 e x i t ( 0 ) ;
19
20 }
21 }
Fig. 5. A top(left) and profile(right) view of the octomap created from the
point cloud data
C. Calculating the volume from the Octomap
Basically counting the number of occupied voxels and
integrating the volume of each cube would give us the volume.
In order to validate this approach, cubical and rectangular
structure is plotted in the form of point clouds and an octomap
is constructed from that. Then the volume from the octomap
and the volume of the cube are compared. Fig. 6 shows a
rectangular box made of point cloud data. The table shown in
Fig. 7 shows the volume calculated from the point clouds in
the form of cube. Rows 6 and 7 in the table shows that closer
point clouds are, the lesser is the error in volume calculation.
Since we are generating the points with an interval of 0.01m,
the error we get in the volume calculation will be negligible.
Fig. 6. Rectangular box in the form of point cloud data.
Fig. 7. 7 test cases are taken to calculate the volume of the octomap.
The code snippet below shows the volume calculation routine.
For a given tree file all the voxels are iterated through and
only occupied voxels are taken into consideration. The variable
”v1” calculates the volume of each voxel and integrates it to
get the full volume of the map.
1
2 s t r i n g btFilename =” t r e e . bt ” ;
3 OcTree∗ t r e e = new OcTree ( btFilename ) ;
4 i f ( t r e e )
5 s t d : : cout<<” red t r e e open s u c c e s s f u l l ! ! ” ;
6
7 for ( OcTree : : i t e r a t o r i t = tree −>begin (16) , end= tree
−>end ( ) ; i t != end ; ++ i t )
8 {
9 i f ( tree −>isNodeOccupied (∗ i t ) )
10 {
11 v1=v1+pow ( i t . g e t S i z e ( ) ,3) ;
12 }
13
14
15 }
3. Fig. 8. ROS nodes involved in point cloud generation and volume calculation
Fig. 8 shows th ROS nodes[3] which performs the process
explained in the previous sections. Functionalities of the
nodes:
• rcm plot: This node basically generates the point
clouds(both reachable and unreachable points) in rviz.
Also for a given position and orientation, it gives the joint
angles(RCM constrained IK solver), which is then visu-
alized in rviz through the node robot state publisher
• all tree&tree red: Both of these nodes are responsible
for creating the octree by taking the points from the
node rcm plot during run time. Once the points are done
generating, the tree is saved in a binary tree format.The
node all tree builds the tree based on both reachable
and unreachable points. The node tree red builds the
tree based on the unreachable points. Subtracting these
volumes would give us the volume of the reachable
workspace of the STAR.
• volume calc: This node reads the occupied voxels from
the trees created in the nodes all tree and tree red sepa-
rately. Calculating the volume of each voxel and summing
it up would give the volume of the region.
III. EXPERIMENTS
In the first two entries, the height of the RCM from the
base link(of the STAR) is reduced by 10 cm. That shows an
increase in volume. In third row the RCM is located at a height
of 32.2 cm above the base link, which seems to most suitable
height. Coming to X axis locating the RCM anywhere from
40-46 cm seems, esp when 46 cm from the base. If the RCM
is taken far off like 59 cm from the base(see entry 14), the
volume reduces a lot. Likewise increasing the height like 47.2
cm is not a desirable position. Finally in Y axis a range of
-30 to 30 cm would be preferred. Row number 13 shows one
such desirable position in the mentioned range.
IV. CONCLUSION
The reachable workspace of the STAR is explored through
a constructive method. A total of 14 experiments is conducted
to verify and compare the results. But We have only explored
the reachable workspace of the robot, looking into dexterous
workspace would give us more insight. This will be included
in the future work.
Fig. 9. Octomap volume for different position of the RCM from the robot
REFERENCES
[1] wiki.ros.org/rviz.
[2] Armin Hornung, Kai M. Wurm, Maren Bennewitz, Cyrill Stachniss, and
Wolfram Burgard. OctoMap: An efficient probabilistic 3D mapping
framework based on octrees. Autonomous Robots, 2013. Software
available at http://octomap.github.com.
[3] K.Conley J.Faust T.Foote J.Leibs E.Berger R.Wheeler M.Quigley,
B.Gerkey and A.Ng. ”ros:an open-source robot operating system”. In
Proc. ICRA workshop on Open-Source Software, 2009.