Skip to content

Commit e92e32a

Browse files
mkovalmkoval
committed
Merge pull request #74 from personalrobotics/feature/vector_field_planner
Planning with vector fields.
2 parents 361d6bc + 9eeb4e1 commit e92e32a

File tree

3 files changed

+307
-4
lines changed

3 files changed

+307
-4
lines changed

‎src/prpy/planning/__init__.py

Lines changed: 1 addition & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -40,3 +40,4 @@
4040
from openrave import BiRRTPlanner
4141
from tsr import TSRPlanner
4242
from workspace import GreedyIKPlanner
43+
from vectorfield import VectorFieldPlanner

‎src/prpy/planning/vectorfield.py

Lines changed: 253 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,253 @@
1+
#!/usr/bin/env python
2+
3+
# Copyright (c) 2015, Carnegie Mellon University
4+
# All rights reserved.
5+
# Authors: Siddhartha Srinivasa <siddh@cs.cmu.edu>
6+
#
7+
# Redistribution and use in source and binary forms, with or without
8+
# modification, are permitted provided that the following conditions are met:
9+
#
10+
# - Redistributions of source code must retain the above copyright notice, this
11+
# list of conditions and the following disclaimer.
12+
# - Redistributions in binary form must reproduce the above copyright notice,
13+
# this list of conditions and the following disclaimer in the documentation
14+
# and/or other materials provided with the distribution.
15+
# - Neither the name of Carnegie Mellon University nor the names of its
16+
# contributors may be used to endorse or promote products derived from this
17+
# software without specific prior written permission.
18+
#
19+
# THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
20+
# AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
21+
# IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
22+
# ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE
23+
# LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
24+
# CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
25+
# SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
26+
# INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
27+
# CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
28+
# ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
29+
# POSSIBILITY OF SUCH DAMAGE.
30+
31+
import logging
32+
import numpy
33+
import openravepy
34+
import time
35+
from base import BasePlanner, PlanningError, PlanningMethod
36+
import prpy.util
37+
from enum import Enum
38+
39+
logger = logging.getLogger('planning')
40+
41+
42+
class Status(Enum):
43+
'''
44+
TERMINATE - stop, exit gracefully, and return current trajectory
45+
CACHE_AND_CONTINUE - save the current trajectory and CONTINUE.
46+
return the saved trajectory if Exception.
47+
CONTINUE - keep going
48+
'''
49+
TERMINATE = -1
50+
CACHE_AND_CONTINUE = 0
51+
CONTINUE = 1
52+
53+
54+
class VectorFieldPlanner(BasePlanner):
55+
def __init__(self):
56+
super(VectorFieldPlanner, self).__init__()
57+
58+
def __str__(self):
59+
return 'VectorFieldPlanner'
60+
61+
@PlanningMethod
62+
def PlanToEndEffectorPose(self, robot, goal_pose, timelimit=5.0,
63+
pose_error_tol=0.01, **kw_args):
64+
"""
65+
Plan to an end effector pose by following a geodesic loss function
66+
in SE(3) via an optimized Jacobian.
67+
68+
@param robot
69+
@param goal_pose desired end-effector pose
70+
@param timelimit time limit before giving up
71+
@param pose_error_tol in meters
72+
@return traj
73+
"""
74+
manip = robot.GetActiveManipulator()
75+
76+
def vf_geodesic():
77+
twist = prpy.util.GeodesicTwist(manip.GetEndEffectorTransform(),
78+
goal_pose)
79+
dqout, tout = prpy.util.ComputeJointVelocityFromTwist(
80+
robot, twist)
81+
# Go as fast as possible
82+
dqout = min(abs(robot.GetDOFVelocityLimits()/dqout))*dqout
83+
84+
return dqout
85+
86+
def CloseEnough():
87+
pose_error = prpy.util.GeodesicDistance(
88+
manip.GetEndEffectorTransform(),
89+
goal_pose)
90+
if pose_error < pose_error_tol:
91+
return Status.TERMINATE
92+
return Status.CONTINUE
93+
94+
return self.FollowVectorField(robot, vf_geodesic,
95+
CloseEnough, timelimit)
96+
97+
@PlanningMethod
98+
def PlanToEndEffectorOffset(self, robot, direction, distance,
99+
max_distance=None, timelimit=5.0,
100+
position_tolerance=0.01,
101+
angular_tolerance=0.15,
102+
**kw_args):
103+
"""
104+
Plan to a desired end-effector offset with move-hand-straight
105+
constraint. movement less than distance will return failure. The motion
106+
will not move further than max_distance.
107+
@param robot
108+
@param direction unit vector in the direction of motion
109+
@param distance minimum distance in meters
110+
@param max_distance maximum distance in meters
111+
@param timelimit timeout in seconds
112+
@param position_tolerance constraint tolerance in meters
113+
@param angular_tolerance constraint tolerance in radians
114+
@return traj
115+
"""
116+
if distance < 0:
117+
raise ValueError('Distance must be non-negative.')
118+
elif numpy.linalg.norm(direction) == 0:
119+
raise ValueError('Direction must be non-zero')
120+
elif max_distance is not None and max_distance < distance:
121+
raise ValueError('Max distance is less than minimum distance.')
122+
elif position_tolerance < 0:
123+
raise ValueError('Position tolerance must be non-negative.')
124+
elif angular_tolerance < 0:
125+
raise ValueError('Angular tolerance must be non-negative.')
126+
127+
# Normalize the direction vector.
128+
direction = numpy.array(direction, dtype='float')
129+
direction /= numpy.linalg.norm(direction)
130+
131+
# Default to moving an exact distance.
132+
if max_distance is None:
133+
max_distance = distance
134+
135+
manip = robot.GetActiveManipulator()
136+
Tstart = manip.GetEndEffectorTransform()
137+
138+
def vf_straightline():
139+
twist = prpy.util.GeodesicTwist(manip.GetEndEffectorTransform(),
140+
Tstart)
141+
twist[0:3] = direction
142+
dqout, tout = prpy.util.ComputeJointVelocityFromTwist(
143+
robot, twist)
144+
145+
# Go as fast as possible
146+
dqout = min(abs(robot.GetDOFVelocityLimits()/dqout))*dqout
147+
return dqout
148+
149+
def TerminateMove():
150+
'''
151+
Fail if deviation larger than position and angular tolerance.
152+
Succeed if distance moved is larger than max_distance.
153+
Cache and continue if distance moved is larger than distance.
154+
'''
155+
Tnow = manip.GetEndEffectorTransform()
156+
error = prpy.util.GeodesicError(Tstart, Tnow)
157+
if numpy.fabs(error[3]) > angular_tolerance:
158+
raise PlanningError('Deviated from orientation constraint.')
159+
distance_moved = numpy.dot(error[0:3], direction)
160+
position_deviation = numpy.linalg.norm(error[0:3] -
161+
distance_moved*direction)
162+
if position_deviation > position_tolerance:
163+
raise PlanningError('Deviated from straight line constraint.')
164+
165+
if distance_moved > max_distance:
166+
return Status.TERMINATE
167+
168+
if distance_moved > distance:
169+
return Status.CACHE_AND_CONTINUE
170+
171+
return Status.CONTINUE
172+
173+
return self.FollowVectorField(robot, vf_straightline,
174+
TerminateMove, timelimit)
175+
176+
@PlanningMethod
177+
def FollowVectorField(self, robot, fn_vectorfield, fn_terminate,
178+
timelimit=5.0, dq_tol=0.0001, **kw_args):
179+
"""
180+
Follow a joint space vectorfield to termination.
181+
182+
@param robot
183+
@param fn_vectorfield a vectorfield of joint velocities
184+
@param fn_terminate custom termination condition
185+
@param timelimit time limit before giving up
186+
@param dq_tol velocity tolerance for termination
187+
@param kw_args keyword arguments to be passed to fn_vectorfield
188+
@return traj
189+
"""
190+
start_time = time.time()
191+
192+
try:
193+
with robot:
194+
manip = robot.GetActiveManipulator()
195+
robot.SetActiveDOFs(manip.GetArmIndices())
196+
# Populate joint positions and joint velocities
197+
cspec = manip.GetArmConfigurationSpecification('quadratic')
198+
cspec.AddDerivativeGroups(1, False)
199+
cspec.AddDeltaTimeGroup()
200+
cspec.ResetGroupOffsets()
201+
qtraj = openravepy.RaveCreateTrajectory(self.env,
202+
'GenericTrajectory')
203+
qtraj.Init(cspec)
204+
cached_traj = None
205+
206+
dqout = robot.GetActiveDOFVelocities()
207+
dt = min(robot.GetDOFResolutions() /
208+
robot.GetDOFVelocityLimits())
209+
while True:
210+
# Check for a timeout.
211+
current_time = time.time()
212+
if (timelimit is not None and
213+
current_time - start_time > timelimit):
214+
raise PlanningError('Reached time limit.')
215+
216+
# Check for collisions.
217+
if self.env.CheckCollision(robot):
218+
raise PlanningError('Encountered collision.')
219+
if robot.CheckSelfCollision():
220+
raise PlanningError('Encountered self-collision.')
221+
222+
# Add to trajectory
223+
waypoint = []
224+
q_curr = robot.GetActiveDOFValues()
225+
waypoint.append(q_curr) # joint position
226+
waypoint.append(dqout) # joint velocity
227+
waypoint.append([dt]) # delta time
228+
waypoint = numpy.concatenate(waypoint)
229+
qtraj.Insert(qtraj.GetNumWaypoints(), waypoint)
230+
dqout = fn_vectorfield()
231+
if (numpy.linalg.norm(dqout) < dq_tol):
232+
raise PlanningError('Local minimum, \
233+
unable to progress')
234+
235+
status = fn_terminate()
236+
237+
if status == Status.CACHE_AND_CONTINUE:
238+
cached_traj = prpy.util.CopyTrajectory(qtraj)
239+
240+
if status == Status.TERMINATE:
241+
break
242+
243+
qnew = q_curr + dqout*dt
244+
robot.SetActiveDOFValues(qnew)
245+
246+
except PlanningError as e:
247+
if cached_traj is not None:
248+
logger.warning('Terminated early: %s', e.message)
249+
return cached_traj
250+
else:
251+
raise
252+
253+
return qtraj

‎src/prpy/util.py

Lines changed: 53 additions & 4 deletions
Original file line numberDiff line numberDiff line change
@@ -3,10 +3,11 @@
33
# Copyright (c) 2013, Carnegie Mellon University
44
# All rights reserved.
55
# Authors: Michael Koval <mkoval@cs.cmu.edu>
6+
# Authors: Siddhartha Srinivasa <siddh@cs.cmu.edu>
67
#
78
# Redistribution and use in source and binary forms, with or without
89
# modification, are permitted provided that the following conditions are met:
9-
#
10+
#
1011
# - Redistributions of source code must retain the above copyright notice, this
1112
# list of conditions and the following disclaimer.
1213
# - Redistributions in binary form must reproduce the above copyright notice,
@@ -15,7 +16,7 @@
1516
# - Neither the name of Carnegie Mellon University nor the names of its
1617
# contributors may be used to endorse or promote products derived from this
1718
# software without specific prior written permission.
18-
#
19+
#
1920
# THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
2021
# AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
2122
# IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
@@ -433,6 +434,8 @@ def quadraticObjective(dq, *args):
433434
@param dq joint velocity
434435
@param args[0]Jacobian
435436
@param args[1] desired twist
437+
@return objective the objective function
438+
@return gradient the analytical gradient of the objective
436439
'''
437440
J = args[0]
438441
dx = args[1]
@@ -456,6 +459,9 @@ def ComputeJointVelocityFromTwist(robot, twist,
456459
defaults to quadraticObjective
457460
@params dq_init optional initial guess for optimal joint velocity
458461
defaults to robot.GetActiveDOFVelocities()
462+
@return dq_opt optimal joint velocity
463+
@return twist_opt actual achieved twist
464+
can be different from desired twist due to constraints
459465
'''
460466
manip = robot.GetActiveManipulator()
461467
robot.SetActiveDOFs(manip.GetArmIndices())
@@ -484,8 +490,51 @@ def ComputeJointVelocityFromTwist(robot, twist,
484490
bounds=dq_bounds, approx_grad=False)
485491

486492
dq_opt = opt[0]
487-
if opt[1] > 0:
488-
print "Unable to produce desired twist."
489493
twist_opt = numpy.dot(jacobian, dq_opt)
490494

491495
return dq_opt, twist_opt
496+
497+
498+
def GeodesicTwist(t1, t2):
499+
'''
500+
Computes the twist in global coordinates that corresponds
501+
to the gradient of the geodesic distance between two transforms.
502+
503+
@param t1 current transform
504+
@param t2 goal transform
505+
@return twist in se(3)
506+
'''
507+
trel = numpy.dot(numpy.linalg.inv(t1), t2)
508+
trans = numpy.dot(t1[0:3, 0:3], trel[0:3, 3])
509+
omega = numpy.dot(t1[0:3, 0:3],
510+
openravepy.axisAngleFromRotationMatrix(
511+
trel[0:3, 0:3]))
512+
return numpy.hstack((trans, omega))
513+
514+
515+
def GeodesicError(t1, t2):
516+
'''
517+
Computes the error in global coordinates between two transforms
518+
519+
@param t1 current transform
520+
@param t2 goal transform
521+
@return a 4-vector of [dx, dy, dz, solid angle]
522+
'''
523+
trel = numpy.dot(numpy.linalg.inv(t1), t2)
524+
trans = numpy.dot(t1[0:3, 0:3], trel[0:3, 3])
525+
omega = openravepy.axisAngleFromRotationMatrix(trel[0:3, 0:3])
526+
angle = numpy.linalg.norm(omega)
527+
return numpy.hstack((trans, angle))
528+
529+
530+
def GeodesicDistance(t1, t2, r=1.0):
531+
'''
532+
Computes the geodesic distance between two transforms
533+
534+
@param t1 current transform
535+
@param t2 goal transform
536+
@param r in units of meters/radians converts radians to meters
537+
'''
538+
error = GeodesicError(t1, t2)
539+
error[3] = r*error[3]
540+
return numpy.linalg.norm(error)

0 commit comments

Comments
 (0)