def makeConf(self,x,y,th,g=0.07, vertical=False, left=None, right=None,
ignoreStandard=False):
global standardVerticalConf, standardHorizontalConf
if left is None: left = self.useLeft
if right is None: right = self.useRight
dx = dy = dz = 0
dt = 0.0 if vertical else TORSO_Z - 0.3
if vertical and standardVerticalConf and (not ignoreStandard):
c = standardVerticalConf.copy()
c.conf['pr2Base'] = [x, y, th]
if self.useLeft:
c.conf['pr2LeftGripper'] = [g]
if self.useRight:
c.conf['pr2RightGripper'] = [g]
return c
elif (not vertical) and standardHorizontalConf and (not ignoreStandard):
c = standardHorizontalConf.copy()
c.conf['pr2Base'] = [x, y, th]
if self.useLeft:
c.conf['pr2LeftGripper'] = [g]
if self.useRight:
c.conf['pr2RightGripper'] = [g]
return c
else:
c = JointConf(pr2Init.copy(), self)
c = c.set('pr2Base', [x, y, th])
if self.useLeft:
c = c.set('pr2LeftGripper', [g])
if self.useRight:
c = c.set('pr2RightGripper', [g])
cart = c.cartConf()
base = cart['pr2Base']
if vertical:
q = np.array([0.0, 0.7071067811865475, 0.0, 0.7071067811865475])
if left and not right:
h = hu.Transform(p=np.array([[a] for a in [ 0.4+dx, 0.2+dy, 1.1+dt, 1.]]), q=q)
cart = cart.set('pr2LeftArm', base.compose(h))
else:
cart = cart.rem('pr2LeftArm')
if right:
hr = hu.Transform(p=np.array([[a] for a in [ 0.4+dx, -(0.2+dy), 1.1+dt, 1.]]), q=q)
cart = cart.set('pr2RightArm', base.compose(hr))
else:
if left and not right:
h = hu.Pose(0.3+dx,0.3+dy,0.9+dz+dt,-math.pi/2)
cart = cart.set('pr2LeftArm', base.compose(h))
else:
cart = cart.rem('pr2LeftArm')
if right:
hr = hu.Pose(0.3+dx,-(0.3+dy),0.9+dz+dt,math.pi/2)
cart = cart.set('pr2RightArm', base.compose(hr))
c = self.inverseKin(cart, c)
c.conf['pr2Head'] = [0., 0.]
assert all(c.values())
if vertical:
standardVerticalConf = c
else:
standardHorizontalConf = c
return c
def pr2GripperJoints(arm):
o = params['gripperOffset']
palm_dx = params['palmLength']
fing_dx = params['fingerLength']
fing_dy = params['fingerWidth']
fing_dz = params['fingerThick']
return [Rigid(arm+'_palm',
(r_forceSensorOffset if arm == 'r' else l_forceSensorOffset) * \
hu.Transform(transf.translation_matrix([o + palm_dx/2.,0.0,0.0])),
None, None),
Prismatic(arm+'_finger1',
hu.Transform(transf.translation_matrix([palm_dx/2+fing_dx/2.,fing_dy/2.,0.0])),
(0.0, params['gripMax']/2.), (0.,1.,0)),
Prismatic(arm+'_finger2',
hu.Transform(transf.translation_matrix([0.0,-fing_dy,0.0])),
(0.0, 0.081), (0.,-1.,0))]
def interpPose(pose_f, pose_i, minLength, ratio=0.5):
if isinstance(pose_f, (tuple, list)):
return [f*ratio + i*(1-ratio) for (f,i) in zip(pose_f, pose_i)], \
all([abs(f-i)<=minLength for (f,i) in zip(pose_f, pose_i)])
else:
pr = pose_f.point()*ratio + pose_i.point()*(1-ratio)
qr = quaternion_slerp(pose_i.quat().matrix, pose_f.quat().matrix, ratio)
return hu.Transform(None, pr.matrix, qr), \
pose_f.near(pose_i, minLength, minLength)
def pr2ArmLinks(arm):
angle = math.pi/2 if arm=='r' else -math.pi/2
pose = hu.Transform(transf.rotation_matrix(angle, (1,0,0)))
links = [\
Sh(Ba([(-0.10, -0.12, -0.5), (0.24, 0.12, 0.1)], name='shoulder')),
Sh(Ba([(0.12, -0.06, -0.08), (0.47, 0.06, 0.08)], name='upperArm')),
None,
Sh(Ba([(0.07, -0.06, -0.055), (0.18, 0.06, 0.03)], name='foreArm1'),
Ba([(0.18, -0.06, -0.03), (0.36, 0.06, 0.03)], name='foreArm2')).applyTrans(pose),
None,
None,
None]
return links
def fingerSupportFrame(self, hand, width):
# The old way...
# Origin is on the inside surface of the finger (at the far tip).
# The -0.18 is from finger tip to the wrist -- if using wrist frame
# mat = np.dot(transf.euler_matrix(-math.pi/2, math.pi/2, 0.0, 'ryxz'),
# transf.translation_matrix([0.0, -0.18, -width/2]))
# y points along finger approach, z points in closing direction
# offset aligns with the grasp face.
# This is global gripperFaceFrame offset to center object
gripperFaceFrame_dy = hu.Transform(np.array([(0.,1.,0.,0.18),
(0.,0.,1.,-width/2),
(1.,0.,0.,0.),
(0.,0.,0.,1.)]))
if hand == 'right':
gripperFaceFrame_dy = r_forceSensorOffset * gripperFaceFrame_dy
return gripperFaceFrame_dy
def pr2ArmLinksGrown(arm):
angle = math.pi/2 if arm=='r' else -math.pi/2
pose = hu.Transform(transf.rotation_matrix(angle, (1,0,0)))
gr1 = radiusVar + 0.43*reachPct*angleVar
gr2 = radiusVar + 0.76*reachPct*angleVar
gr3 = radiusVar + 1.05*reachPct*angleVar
#print 'gr1', gr1, 'gr2', gr2, 'gr3', gr3
# raw_input('arm growth factors')
links = [\
Sh(Ba([(-0.10-gr1, -0.12-gr1, -0.5), (0.24+gr1, 0.12+gr1, 0.1)], name='shoulder')),
Sh(Ba([(0.12-gr2, -0.06-gr2, -0.08-gr2), (0.47+gr2, 0.06+gr2, 0.08+gr2)], name='upperArm')),
None,
Sh(Ba([(0.07-gr3, -0.06-gr3, -0.055-gr3), (0.18+gr3, 0.06+gr3, 0.03+gr3)], name='foreArm1'),
Ba([(0.18-gr3, -0.06-gr3, -0.03-gr3), (0.36+gr3, 0.06+gr3, 0.03+gr3)], name='foreArm2')).applyTrans(pose),
None,
None,
None]
return links
CENTER_FRAME = hu.Pose(0, 0, 0.025, 0)
def get_grasp_frame_origin(grasp_frame):
objFrame = hu.Pose(0, 0, 0, 0)
faceFrame = grasp_frame.compose(GRASP_FRAME)
centerFrame = faceFrame.compose(CENTER_FRAME)
graspFrame = objFrame.compose(centerFrame)
return graspFrame
Z_OFFSET = 0.02
gMat0 = hu.Transform(
np.array([(0., 1., 0., 0.), (0., 0., 1., -0.025), (1., 0., 0., Z_OFFSET),
(0., 0., 0., 1.)]))
gMat1 = hu.Transform(
np.array([(0., -1., 0., 0.), (0., 0., -1., 0.025), (1., 0., 0., Z_OFFSET),
(0., 0., 0., 1.)]))
gMat4 = hu.Pose(0, 0, 0, math.pi / 2).compose(gMat0)
gMat5 = hu.Pose(0, 0, 0, -math.pi / 2).compose(gMat0)
SIDE_GRASPS = map(get_grasp_frame_origin, [gMat0, gMat1, gMat4, gMat5])
gMat2 = hu.Transform(
np.array([(-1., 0., 0., 0.), (0., 0., -1., 0.025), (0., -1., 0., 0.),
(0., 0., 0., 1.)]))
gMat3 = hu.Transform(
np.array([(1., 0., 0., 0.), (0., 0., 1., -0.025), (0., -1., 0., 0.),
(0., 0., 0., 1.)]))
def flipLtoR(pose):
m = pose.matrix.copy()
m[1, 3] = -m[1, 3]
return hu.Transform(m)
def mapToClass(euler, wrist):
rot = euler_matrix(*euler)
rot[:, 3] = wrist.matrix[:, 3] # set translation
rot[2, 3] = round(2 * rot[2, 3], 1) * 0.5 # round to 0.05
return hu.Transform(rot)
def transFromQPRep(qp):
(q, p) = qp
return hu.Transform(p=np.array([[a] for a in p]), q=np.array(q))
(qpRep(wrist), nangles))]
# Write out transforms as quaternion, position tuples of lists.
def qpRep(tr):
return (quaternion_from_matrix(tr.matrix).tolist(), tr.matrix[:,
3].tolist())
def transFromQPRep(qp):
(q, p) = qp
return hu.Transform(p=np.array([[a] for a in p]), q=np.array(q))
vwrist = hu.Transform(
np.array([[0.0, 0.0, 1.0, 1.10], [0.0, 1.0, 0.0, 0.0],
[-1.0, 0.0, 0.0, 1.0], [0.0, 0.0, 0.0, 1.0]]))
def kinScore(gclass, wrist):
if gclass == 'h':
pose = wrist.pose()
else:
pose = vwrist.compose(wrist.inverse()).pose(zthr=0.1, fail=False)
# TODO: This is pretty random, do better...
return 3 * abs(pose.theta) + abs(pose.y) - abs(pose.x)
def graspClass(wrist):
# is wrist close enough to horizontal or vertical approach?
euler = euler_from_matrix(wrist.matrix)
import math
import itertools
from random import uniform
from collections import deque
import numpy as np
import PyR2.hu as hu
import PyR2.shapes as shapes
from PyR2.transformations import quaternion_slerp
from scipy.optimize import fmin_bfgs, fmin_slsqp
Ident = hu.Transform(np.eye(4, dtype=np.float64)) # identity transform
# fkCount, fkCache, placeCount, placeCache
confCacheStats = [0, 0, 0, 0]
# Controls size of confCache - bigger cache leads to faster motion
# planning, but makes Python bigger, which can lead to swapping.
maxConfCacheSize = 150*10**3
# print '*** maxConfCacheSize', maxConfCacheSize, '***'
# (additions, deletions)
confCacheUpdateStats = [0, 0]
def printStats():
print 'maxConfCacheSize', maxConfCacheSize
print 'confCacheStats = (fkCount, fkCache, placeCount, placeCache)\n', confCacheStats
print 'confCacheUpdateStats = (additions, deletions)\n', confCacheUpdateStats
# These are not specific to a particular robot
class GenericRobot:
def cacheReset(self):
self.confCache = {}
def transProd(lt):
return hu.Transform(reduce(np.dot, lt))
import PyR2.transformations as transf from PyR2.chains import compileChainFramesOS, Chain, MultiChain, getUrdfJoints, GripperChain, Planar, Rigid, Prismatic from PyR2.pr2.pr2IkPoses import ikTrans # base poses from PyR2.pr2.pr2IkMap import ikTransLRz # (z, base pose) from PyR2.pr2.ik2 import armInvKin2, clearInvKinCache2 # Don't reload these files anywhere else! import PyR2.conf import PyR2.genericRobot reload(PyR2.conf) reload(PyR2.genericRobot) from PyR2.conf import RobotJointConf, RobotCartConf from PyR2.genericRobot import GenericRobot Ident = hu.Transform(np.eye(4, dtype=np.float64)) # identity transform import os curDir = os.path.dirname(os.path.abspath(__file__)) urdfPath = curDir + '/pr2.urdf' standardVerticalConf = None standardHorizontalConf = None TORSO_Z = 0.3 BASE_GROWTH_X = 0.1 BASE_GROWTH_Y = 0.1 USE_OLD_IK_POSES = True ################################################################ # Define the robot, ideally would be from a URDF.
