def getPFull(W, armKin):
# return (normal, rPos) where normal is the normal of the plane and
# rPos is a point on the plane ( position of the end effector )
# Get camera extrinsic parameters from robot arm
T = armKin.forward_position_kinematics()[0:3]
rOrientation = armKin.forward_position_kinematics()[3:7]
#print "lPos", T
#print "lOrientation", rOrientation
myQuat = Quat(rOrientation)
R = myQuat._quat2transform()
M = np.append(R,T.reshape(3,1),1)
P = np.dot(W,M)
return P
def getPlaneParams(armKin):
# return (normal, rPos) where normal is the normal of the plane and
# rPos is a point on the plane ( position of the end effector )
# Get laser plane parameters from a connected and running Baxter
rPos = armKin.forward_position_kinematics()[0:3]
rOrientation = armKin.forward_position_kinematics()[3:7]
#print "rPos", rPos
#print "rOrientation", rOrientation
myQuat = Quat(rOrientation)
R_0R = myQuat._quat2transform()
# we know that the x direction in the manipulator frame is the direction
# perpendicular to the laser plane through inspection of the baxter model
# and the transform tree
ex = np.array([[1],[0],[0]])
normal = np.dot(R_0R.T, ex)
print normal
return (normal, rPos)
