def execute(self, userdata):
print "State: IdentifyCutPoint"
# rospy.loginfo('Enter to Identity Cut Point')
# raw_input()
currPoseRight = self.davinciArmRight.getGripperPose()
currPoseRight = currPoseRight.as_tf() * tfx.pose(
tfx.tb_angles(180, 0, 0)).as_tf() * tfx.pose(
tfx.tb_angles(0, -75, 0))
currPoseRight.position.y += 0.009
currPoseRight.position.z += -0.03
currPoseRight.position.x += 0.004
currPoseRight = currPoseRight.as_tf() * tfx.pose(
tfx.tb_angles(180, 0, 0))
currPoseRight.stamp = None
cutPointCurr = tfx.convertToFrame(currPoseRight,
'/one_remote_center_link')
self.cut_point_pub.publish(cutPointCurr.msg.PoseStamped())
# rospy.loginfo('Check cut point')
# raw_input()
userdata.cutPoint = cutPointCurr
return 'success'
def deltaPose(currPose, desPose, posFrame=None, rotFrame=None):
"""
Returns pose0 - pose1
"""
currPose, desPose = tfx.pose(currPose), tfx.pose(desPose)
currPoseFrame, desPoseFrame = currPose.frame, desPose.frame
currPos, desPos = currPose.position, desPose.position
currRot, desRot = currPose.orientation, desPose.orientation
if posFrame is not None and currPoseFrame is not None:
tf_currPos_to_posFrame = tfx.lookupTransform(posFrame, currPoseFrame, wait=10)
currPos = tf_currPos_to_posFrame * currPos
tf_desPos_to_posFrame = tfx.lookupTransform(posFrame, desPoseFrame, wait=10)
desPos = tf_desPos_to_posFrame * desPos
if rotFrame is not None and currPoseFrame is not None:
tf_currRot_to_rotFrame = tfx.lookupTransform(rotFrame, currPoseFrame, wait=10)
currRot = tf_currRot_to_rotFrame * currRot
tf_desRot_to_rotFrame = tfx.lookupTransform(rotFrame, desPoseFrame, wait=10)
desRot = tf_desRot_to_rotFrame * desRot
deltaPosition = desPos.array - currPos.array
currQuat, desQuat = tfx.tb_angles(currRot).quaternion, tfx.tb_angles(desRot).quaternion
deltaQuat = tft.quaternion_multiply(tft.quaternion_inverse(currQuat), desQuat)
deltaPose = tfx.pose(deltaPosition, deltaQuat)
return deltaPose
def alignGrippers(self):
rightArmPose = self.rightArm.get_pose()
leftArmPose = self.leftArm.get_pose()
newLeftArmPose = tfx.pose(leftArmPose.position,
tfx.tb_angles(0.0, 0.0,
0.0)) # Create new pose
newRightArmPose = tfx.pose(rightArmPose.position,
tfx.tb_angles(0.0, 0.0, 0.0))
newRightJoints = self.rightArm.ik(
newRightArmPose) # Updates arm pose if valid
newLeftJoints = self.leftArm.ik(newLeftArmPose)
if newRightJoints is not None:
self.rightArm.go_to_joints(newRightJoints)
print('new_pose: {0}'.format(newRightArmPose))
print "Aligning right gripper..."
self.rightArm.open_gripper()
self.valid = True
else:
rospy.loginfo('Invalid Right Arm Pose')
print "RIGHT ALIGNMENT FAILED"
self.valid = False
return self
rospy.sleep(0.5)
if newLeftJoints is not None:
self.leftArm.go_to_joints(newLeftJoints)
print "Aligning left gripper..."
self.leftArm.open_gripper()
else:
rospy.loginfo('Invalid Left Arm Pose')
print "LEFT ALIGNMENT FAILED"
rospy.sleep(0.5)
def get_orientation_from_lines(self, v0, v1):
"""
Takes in two vectors, v0 and v1, (which are KNOWN to be in the same plane) and finds
the normal, and creates a rotation matrix from v0, v1, and the normal; then converts this
rotation matrix to a quaternion
"""
v0, v1 = np.array(v0), np.array(v1)
v0 = v0 / np.linalg.norm(v0)
v1 = v1 / np.linalg.norm(v1)
n = np.cross(v0, v1)
parallel = average_vectors(v0, v1)
parallel = parallel / np.linalg.norm(parallel)
third = np.cross(n, parallel)
third = third / np.linalg.norm(third)
#n = n / np.linalg.norm(n)
#v1 = np.cross(n, v0)
#v1 = v1 / np.linalg.norm(v1)
#rotMat = np.vstack((n, v1, v0)).T
rotMat = np.vstack((parallel, third, n)).T
matrix = rotMat
tbRot = tfx.tb_angles(matrix).matrix
#tbRot = self.rotate(-90, "yaw", tbRot) #FIXME: get correct yaw pitch roll values
#tbRot = self.rotate(60, "pitch", tbRot)
tbRot = self.rotate(180, "roll", tbRot)
quat = tfx.tb_angles(tbRot).quaternion
return list(quat)
def testAngleBetween():
quat0 = tfx.tb_angles(-82,90,98).msg
quat1 = tfx.tb_angles(-3,90,-8).msg
theta = angleBetweenQuaternions(quat0, quat1)
print("theta = {0}".format(theta))
def test_grasp_planner():
sim = simulator.Simulator(view=True)
rarm = arm.Arm('right', sim=sim)
p = planner.Planner('right', sim=sim)
rarm.set_posture('mantis')
curr_pose = rarm.get_pose()
curr_pose.orientation = tfx.tb_angles(0,0,180)
rarm.set_pose(curr_pose)
target_pose = rarm.get_pose() + [0.5, 0.4, -0.3]
target_pose.orientation *= tfx.tb_angles(25,-20,10)
sim.plot_transform(target_pose.matrix)
start_joints = rarm.get_joints()
n_steps = 20
joint_traj = p.get_grasp_joint_trajectory(start_joints, target_pose, n_steps=n_steps, ignore_orientation=True, link_name='r_gripper_center_frame')
poses = [tfx.pose(rarm.fk(joints)) for joints in joint_traj]
for pose in poses:
sim.plot_transform(pose.matrix)
for n in xrange(n_steps-1):
angle = (poses[n].orientation.inverse()*poses[-1].orientation).angle
print('angle[{0}]: {1}'.format(n, angle))
#IPython.embed()
print('Press enter to exit')
raw_input()
def test_grasp_planner():
sim = simulator.Simulator(view=True)
rarm = arm.Arm('right', sim=sim)
p = planner.Planner('right', sim=sim)
rarm.set_posture('mantis')
curr_pose = rarm.get_pose()
curr_pose.orientation = tfx.tb_angles(0, 0, 180)
rarm.set_pose(curr_pose)
target_pose = rarm.get_pose() + [0.5, 0.4, -0.3]
target_pose.orientation *= tfx.tb_angles(25, -20, 10)
sim.plot_transform(target_pose.matrix)
start_joints = rarm.get_joints()
n_steps = 20
joint_traj = p.get_grasp_joint_trajectory(
start_joints,
target_pose,
n_steps=n_steps,
ignore_orientation=True,
link_name='r_gripper_center_frame')
poses = [tfx.pose(rarm.fk(joints)) for joints in joint_traj]
for pose in poses:
sim.plot_transform(pose.matrix)
for n in xrange(n_steps - 1):
angle = (poses[n].orientation.inverse() * poses[-1].orientation).angle
print('angle[{0}]: {1}'.format(n, angle))
#IPython.embed()
print('Press enter to exit')
raw_input()
def poseFromMarkers2(self, markers, target_ids, rotate=False):
if len(markers) >= 3:
points = [np.zeros((3,1)), np.zeros((3,1)), np.zeros((3,1))]
for m in markers:
#print 'Pos',m.id,m.x,m.y,m.z
points[target_ids.index(m.id)] = np.array([m.x, m.y, m.z])
leftPoint = points[0]
centerPoint = points[1]
rightPoint = points[2]
leftVec = leftPoint - centerPoint
rightVec = rightPoint - centerPoint
leftVec = leftVec / np.linalg.norm(leftVec)
rightVec = rightVec / np.linalg.norm(rightVec)
zAxis = np.cross(rightVec, leftVec)
xAxis = (leftVec + rightVec) / 2
xAxis= xAxis / np.linalg.norm(xAxis)
yAxis = np.cross(zAxis, xAxis)
rotMat = np.vstack((xAxis, yAxis, zAxis)).T
tbRot = tfx.tb_angles(rotMat).matrix
quat = tfx.tb_angles(tbRot).quaternion
return 1e-3*centerPoint, quat
return None
def least_squares_plane_normal(self, points_3d):
""" returns a pose perpendicular to the plance of the points.
note: the pose is arbitrary set to have the position as the first point
in points_3d"""
# form lists for x, y, and z values
x_list = [a.point.x for a in points_3d]
y_list = [a.point.y for a in points_3d]
z_list = [a.point.z for a in points_3d]
# convert to numpy arrays
x_data = np.array(x_list)
y_data = np.array(y_list)
z_data = np.array(z_list)
A = np.vstack([x_data, y_data, np.ones(len(x_data))]).T
result = np.linalg.lstsq(A, z_data)[0]
normal = np.array([result[0], result[1], -1]) # vector normal to fitted plane
# get a pose by producing two arbitrary vectors perpendicular to normal
x_axis = normal
y_axis = np.array([0.5/x_axis[0], 0.5/x_axis[1], 1]) # arbitrary perpendicular vector
z_axis = np.cross(x_axis, y_axis)
# construct the pose
rotMat = np.vstack((x_axis, y_axis, z_axis)).T
tbRot = tfx.tb_angles(rotMat).matrix
quat = tfx.tb_angles(tbRot).quaternion
position = [x_list[0], y_list[0], z_list[0]]
pose = tfx.pose(position, quat, frame='left_BC')
if DEBUG:
self.leastsq_plane_pub.publish(pose.msg.PoseStamped())
return pose
def test_create_dict(self):
vals = {
'yaw': self.yaw[2],
'pitch': self.pitch[4],
'roll': self.roll[9]
}
for yaw_key in ['yaw', 'y', None]:
for pitch_key in ['pitch', 'p', None]:
for roll_key in ['roll', 'r', None]:
d = {}
v = defaultdict(float)
for key1, key2 in zip(['yaw', 'pitch', 'roll'],
[yaw_key, pitch_key, roll_key]):
if key2 is None:
continue
d[key2] = vals[key1]
v[key1] = vals[key1]
if not d:
continue
tb = tb_angles(d)
self.compare_deg(tb, v['yaw'], v['pitch'], v['roll'])
tb = tb_angles(**d)
self.compare_deg(tb, v['yaw'], v['pitch'], v['roll'])
def test_return_from_grasp_planner():
sim = simulator.Simulator(view=True)
rarm = arm.Arm('right', sim=sim)
p = planner.Planner('right', sim=sim)
rarm.set_posture('mantis')
curr_pose = rarm.get_pose()
curr_pose.orientation = tfx.tb_angles(0, 0, 180)
rarm.set_pose(curr_pose)
target_pose = rarm.get_pose() + [0.5, 0.4, -0.3]
target_pose.orientation *= tfx.tb_angles(25, -20, 10)
sim.plot_transform(target_pose.matrix)
start_joints = rarm.get_joints()
n_steps = 40
joint_traj = p.get_return_from_grasp_joint_trajectory(start_joints,
target_pose,
n_steps=n_steps)
poses = [tfx.pose(rarm.fk(joints)) for joints in joint_traj]
for pose in poses:
sim.plot_transform(pose.matrix)
print('Press enter to exit')
raw_input()
def deltaPose(currPose, desPose, posFrame=None, rotFrame=None):
"""
Returns pose0 - pose1
"""
currPose, desPose = tfx.pose(currPose), tfx.pose(desPose)
currPoseFrame, desPoseFrame = currPose.frame, desPose.frame
currPos, desPos = currPose.position, desPose.position
currRot, desRot = currPose.orientation, desPose.orientation
if posFrame is not None and currPoseFrame is not None:
tf_currPos_to_posFrame = tfx.lookupTransform(posFrame, currPoseFrame, wait=10)
currPos = tf_currPos_to_posFrame * currPos
tf_desPos_to_posFrame = tfx.lookupTransform(posFrame, desPoseFrame, wait=10)
desPos = tf_desPos_to_posFrame * desPos
if rotFrame is not None and currPoseFrame is not None:
tf_currRot_to_rotFrame = tfx.lookupTransform(rotFrame, currPoseFrame, wait=10)
currRot = tf_currRot_to_rotFrame * currRot
tf_desRot_to_rotFrame = tfx.lookupTransform(rotFrame, desPoseFrame, wait=10)
desRot = tf_desRot_to_rotFrame * desRot
deltaPosition = desPos.array - currPos.array
currQuat, desQuat = tfx.tb_angles(currRot).quaternion, tfx.tb_angles(desRot).quaternion
deltaQuat = tft.quaternion_multiply(tft.quaternion_inverse(currQuat), desQuat)
deltaPose = tfx.pose(deltaPosition, deltaQuat)
return deltaPose
def poseFromMarkers2(self, markers, target_ids, rotate=False):
if len(markers) >= 3:
points = [np.zeros((3, 1)), np.zeros((3, 1)), np.zeros((3, 1))]
for m in markers:
#print 'Pos',m.id,m.x,m.y,m.z
points[target_ids.index(m.id)] = np.array([m.x, m.y, m.z])
leftPoint = points[0]
centerPoint = points[1]
rightPoint = points[2]
leftVec = leftPoint - centerPoint
rightVec = rightPoint - centerPoint
leftVec = leftVec / np.linalg.norm(leftVec)
rightVec = rightVec / np.linalg.norm(rightVec)
zAxis = np.cross(rightVec, leftVec)
xAxis = (leftVec + rightVec) / 2
xAxis = xAxis / np.linalg.norm(xAxis)
yAxis = np.cross(zAxis, xAxis)
rotMat = np.vstack((xAxis, yAxis, zAxis)).T
tbRot = tfx.tb_angles(rotMat).matrix
quat = tfx.tb_angles(tbRot).quaternion
return 1e-3 * centerPoint, quat
return None
def move(arm, pos, rot, speed='Slow'):
""" Handles the different speeds we're using.
Parameters
----------
arm: [dvrk arm]
The current DVK arm.
pos: [list]
The desired position.
rot: [list]
The desired rotation, in list form with yaw, pitch, and roll.
SPEED_CLASS: [String]
Slow, Medium, or Fast, case insensitive.
"""
if pos[2] < -0.17:
raise ValueError("Desired z-coord of {} is not safe! Terminating!".format(pos[2]))
if speed.lower() == 'slow':
arm.move_cartesian_frame_linear_interpolation(
tfx.pose(pos, tfx.tb_angles(rot[0],rot[1],rot[2])), 0.03
)
elif speed.lower() == 'medium':
arm.move_cartesian_frame_linear_interpolation(
tfx.pose(pos, tfx.tb_angles(rot[0],rot[1],rot[2])), 0.06
)
elif speed.lower() == 'fast':
arm.move_cartesian_frame(tfx.pose(pos, tfx.tb_angles(rot[0],rot[1],rot[2])))
else:
raise ValueError()
def alignGrippers(self):
rightArmPose = self.rightArm.get_pose()
leftArmPose = self.leftArm.get_pose()
newLeftArmPose = tfx.pose(leftArmPose.position, tfx.tb_angles(0.0, 0.0, 0.0)) # Create new pose
newRightArmPose = tfx.pose(rightArmPose.position, tfx.tb_angles(0.0, 0.0, 0.0))
newRightJoints = self.rightArm.ik(newRightArmPose) # Updates arm pose if valid
newLeftJoints = self.leftArm.ik(newLeftArmPose)
if newRightJoints is not None:
self.rightArm.go_to_joints(newRightJoints)
print ("new_pose: {0}".format(newRightArmPose))
print "Aligning right gripper..."
self.rightArm.open_gripper()
self.valid = True
else:
rospy.loginfo("Invalid Right Arm Pose")
print "RIGHT ALIGNMENT FAILED"
self.valid = False
return self
rospy.sleep(0.5)
if newLeftJoints is not None:
self.leftArm.go_to_joints(newLeftJoints)
print "Aligning left gripper..."
self.leftArm.open_gripper()
else:
rospy.loginfo("Invalid Left Arm Pose")
print "LEFT ALIGNMENT FAILED"
rospy.sleep(0.5)
def get_orientation_from_lines(self, v0, v1):
"""
Takes in two vectors, v0 and v1, (which are KNOWN to be in the same plane) and finds
the normal, and creates a rotation matrix from v0, v1, and the normal; then converts this
rotation matrix to a quaternion
"""
v0, v1 = np.array(v0), np.array(v1)
v0 = v0 / np.linalg.norm(v0)
v1 = v1 / np.linalg.norm(v1)
n = np.cross(v0, v1)
parallel = average_vectors(v0, v1)
parallel = parallel / np.linalg.norm(parallel)
third = np.cross(n, parallel)
third = third / np.linalg.norm(third)
#n = n / np.linalg.norm(n)
#v1 = np.cross(n, v0)
#v1 = v1 / np.linalg.norm(v1)
#rotMat = np.vstack((n, v1, v0)).T
rotMat = np.vstack((parallel, third, n)).T
matrix = rotMat
tbRot = tfx.tb_angles(matrix).matrix
#tbRot = self.rotate(-90, "yaw", tbRot) #FIXME: get correct yaw pitch roll values
#tbRot = self.rotate(60, "pitch", tbRot)
tbRot = self.rotate(180, "roll", tbRot)
quat = tfx.tb_angles(tbRot).quaternion
return list(quat)
def test_config1():
jl = jointDeg2rad({
0: 20.6,
1: 79.0,
2: -.112,
4: -84.3,
5: 29.5,
6: 32.8,
7: 29.2
})
pl = tfx.pose((0.008, -0.001, -0.131),
tfx.tb_angles(24.7, 62.2, 179.6),
frame='/0_link')
gl = 61.994 * pi / 180.
jr = jointDeg2rad({
0: 6.3,
1: 107.1,
2: -.085,
4: 20.0,
5: -2.8,
6: -41.9,
7: -15.1
})
pr = tfx.pose((-0.109, 0.018, -0.098),
tfx.tb_angles(54.3, 71.0, 165.0),
frame='/0_link')
gr = 57.009 * pi / 180.
return jl, pl, gl, jr, pr, gr
def testAngleBetween():
quat0 = tfx.tb_angles(-82, 90, 98).msg
quat1 = tfx.tb_angles(-3, 90, -8).msg
theta = angleBetweenQuaternions(quat0, quat1)
print("theta = {0}".format(theta))
def left_image_callback(self, msg):
if rospy.is_shutdown():
return
self.left_image = self.bridge.imgmsg_to_cv2(msg, "bgr8")
self.left_points = self.process_image(self.left_image)
poses = []
if self.right_points is not None and self.left_points is not None:
self.left_points.sort(key=lambda x: x[0])
self.right_points.sort(key=lambda x: x[0])
disparities = self.assign_disparities(self.left_points,
self.right_points)
for i in range(len(self.left_points)):
x = self.left_points[i][0]
y = self.left_points[i][1]
disparity = disparities[i]
print x, y, disparity
pt = Util.convertStereo(x, y, disparity, self.info)
pt = tfx.point(pt)
pt = tfx.convertToFrame(pt, '/two_remote_center_link')
pose = tfx.pose(pt)
pose = pose.as_tf() * tfx.pose(tfx.tb_angles(
-90, 0, 180)).as_tf() * tfx.pose(tfx.tb_angles(90, 0, 0))
poses.append(pose)
print poses
pose_array = PoseArray()
pose_array.header = poses[0].msg.PoseStamped().header
for pose in poses:
pose_array.poses.append(pose)
self.grasp_poses_pub.publish(pose_array)
def test_create_axis_angle(self): axis = tft.random_vector(3) angle = random.random() * 2 * pi q = tft.quaternion_about_axis(angle, axis) q2 = tb_angles(axis,angle).quaternion self.assertTrue(np.allclose(q, q2) or np.allclose(-q, q2),msg='%s and %s not close!' % (list(q),list(q2))) q2 = tb_angles(angle,axis).quaternion self.assertTrue(np.allclose(q, q2) or np.allclose(-q, q2),msg='%s and %s not close!' % (list(q),list(q2))) q2 = tb_angles((axis,angle)).quaternion self.assertTrue(np.allclose(q, q2) or np.allclose(-q, q2),msg='%s and %s not close!' % (list(q),list(q2))) q2 = tb_angles((angle,axis)).quaternion self.assertTrue(np.allclose(q, q2) or np.allclose(-q, q2),msg='%s and %s not close!' % (list(q),list(q2))) for _ in xrange(1000): axis = tft.random_vector(3) angle = random.random() * 2 * pi q = tft.quaternion_about_axis(angle, axis) q2 = tb_angles(axis,angle).quaternion self.assertTrue(np.allclose(q, q2) or np.allclose(-q, q2),msg='%s and %s not close! for axis %s and angle %f' % (list(q),list(q2),tuple(axis),angle))
def _handles_loop(self):
"""
For each handle in HandleListMsg,
calculate average pose
"""
while not rospy.is_shutdown():
self.handle_list_msg = None
self.camera_pose = None
while not rospy.is_shutdown() and (self.handle_list_msg is None or self.camera_pose is None):
rospy.sleep(.01)
handle_list_msg = self.handle_list_msg
camera_pose = self.camera_pose
pose_array = gm.PoseArray()
pose_array.header.frame_id = 'base_link'
pose_array.header.stamp = rospy.Time.now()
avg_pose_array = gm.PoseArray()
avg_pose_array.header.frame_id = 'base_link'
avg_pose_array.header.stamp = rospy.Time.now()
if handle_list_msg.header.frame_id.count('base_link') > 0:
cam_to_base = np.eye(4)
else:
cam_to_base = camera_pose.matrix #tfx.lookupTransform('base_link', handle_list_msg.header.frame_id).matrix
switch = np.matrix([[0, 1, 0],
[1, 0, 0],
[0, 0, 1]])
for handle in handle_list_msg.handles:
all_poses = [tfx.pose(cylinder.pose, stamp=rospy.Time.now(), frame=handle_list_msg.header.frame_id) for cylinder in handle.cylinders]
rotated_poses = [tfx.pose(p.position, tfx.tb_angles(p.orientation.matrix*switch)) for p in all_poses]
filtered_poses = list()
for rot_pose in rotated_poses:
r_base = cam_to_base[:3,:3]*rot_pose.orientation.matrix
if r_base[0,0] > 0:
if r_base[2,2] > 0:
rot_pose.orientation = tfx.tb_angles(rot_pose.orientation.matrix*tfx.tb_angles(0,0,180).matrix)
filtered_poses.append(rot_pose)
filtered_poses = [tfx.pose(cam_to_base*pose.matrix, frame='base_link') for pose in filtered_poses]
filtered_poses = filter(lambda pose: min(self.min_positions < pose.position.array) and min(pose.position.array < self.max_positions), filtered_poses)
pose_array.poses += [pose.msg.Pose() for pose in filtered_poses]
if len(filtered_poses) > 0:
avg_position = sum([p.position.array for p in filtered_poses])/float(len(filtered_poses))
avg_quat = sum([p.orientation.quaternion for p in filtered_poses])/float(len(filtered_poses))
avg_pose_array.poses.append(tfx.pose(avg_position, avg_quat).msg.Pose())
self.handles_pose_pub.publish(pose_array)
self.avg_handles_pose_pub.publish(avg_pose_array)
self.logger_pub.publish('handles {0}'.format(len(avg_pose_array.poses)))
def testRotation():
rospy.init_node('ar_image_detection')
imageDetector = ARImageDetector()
listener = tf.TransformListener()
tf_br = tf.TransformBroadcaster()
while not rospy.is_shutdown():
if imageDetector.hasFoundGripper(Constants.Arm.Left):
obj = tfx.pose([0,0,0], imageDetector.normal).msg.PoseStamped()
gripper = imageDetector.getGripperPose(Constants.Arm.Left)
print('gripper')
print(gripper)
# obj_tb = tfx.tb_angles(obj.pose.orientation)
gripper_tb = tfx.tb_angles(gripper.pose.orientation)
print "gripper ori", gripper_tb
obj_tb = tfx.tb_angles(imageDetector.normal)
print "obj ori", obj_tb
pt = gripper.pose.position
ori = imageDetector.normal
tf_br.sendTransform((pt.x, pt.y, pt.z), (ori.x, ori.y, ori.z, ori.w),
gripper.header.stamp, '/des_pose', Constants.AR.Frames.Base)
between = Util.angleBetweenQuaternions(ori, gripper_tb.msg)
print('Angle between')
print(between)
quat = tft.quaternion_multiply(gripper_tb.quaternion, tft.quaternion_inverse(obj_tb.quaternion))
print 'new', tfx.tb_angles(quat)
#rot = gripper_tb.rotation_to(ori)
rot = gripper_tb.rotation_to(obj_tb)
print('Rotation from gripper to obj')
print(rot)
deltaPoseTb = tfx.pose(Util.deltaPose(gripper, obj)).orientation
print('deltaPose')
print(deltaPoseTb)
deltaPose = tfx.pose([0,0,0], deltaPoseTb).msg.PoseStamped()
time = listener.getLatestCommonTime('0_link', 'tool_L')
deltaPose.header.stamp = time
deltaPose.header.frame_id = '0_link'
deltaPose = listener.transformPose('tool_L', deltaPose)
print "transformed", tfx.tb_angles(deltaPose.pose.orientation)
endQuatMat = gripper_tb.matrix * rot.matrix
print 'desOri', tfx.tb_angles(endQuatMat)
rospy.sleep(1)
def test_create_quat(self): for yaw in self.yaw: for pitch in self.pitch: for roll in self.roll: q = tft.quaternion_from_euler(yaw * pi / 180., pitch * pi / 180., roll * pi / 180., 'rzyx') tb = tb_angles(q) self.assertTbEqual(tb, yaw, pitch, roll) q2 = list(q) tb = tb_angles(q2) self.assertTbEqual(tb, yaw, pitch, roll)
def rotate(self, angle, axis, matrix):
"""
Takes a rotation matrix and rotates it a certain angle about a certain axis
"""
if axis == "yaw":
rot = tfx.tb_angles(angle, 0, 0).matrix
elif axis == "pitch":
rot = tfx.tb_angles(0, angle, 0).matrix
elif axis == "roll":
rot = tfx.tb_angles(0, 0, angle).matrix
return matrix*rot
def test_to_tf(self): tb = tb_angles(45,-5,24) self.assertTrue(tb.to_tf().flags.writeable) for yaw in self.yaw: for pitch in self.pitch: for roll in self.roll: tb = tb_angles(yaw,pitch,roll) m = tb.to_tf() m2 = tft.euler_matrix(yaw * pi / 180, pitch * pi / 180, roll * pi / 180, 'rzyx') self.assertTrue(np.allclose(m, m2),msg='%s and %s not close!' % (m,m2))
def rotate(self, angle, axis, matrix):
"""
Takes a rotation matrix and rotates it a certain angle about a certain axis
"""
if axis == "yaw":
rot = tfx.tb_angles(angle, 0, 0).matrix
elif axis == "pitch":
rot = tfx.tb_angles(0, angle, 0).matrix
elif axis == "roll":
rot = tfx.tb_angles(0, 0, angle).matrix
return matrix * rot
def testSwitchPlaces(armNames=[MyConstants.Arm.Left,MyConstants.Arm.Right],fixedPose=False):
rospy.init_node('testSwitchPlaces',anonymous=True)
rp = RavenPlanner(armNames)
rospy.sleep(2)
leftStartJoints = None
rightStartJoints = None
if fixedPose:
rightCurrPose = tfx.pose([-0.068,-0.061,-0.129],tfx.tb_angles(-139.6,88.5,111.8),frame=MyConstants.Frames.Link0)
leftCurrPose = tfx.pose([-.072,-.015,-.153],tfx.tb_angles(43.9,78.6,100.9),frame=MyConstants.Frames.Link0)
leftStartJoints = rp.getJointsFromPose(MyConstants.Arm.Left, leftCurrPose, grasp=0)
rightStartJoints = rp.getJointsFromPose(MyConstants.Arm.Right, rightCurrPose, grasp=0)
else:
leftCurrPose = Util.convertToFrame(tfx.pose([0,0,0],frame=rp.toolFrame[MyConstants.Arm.Left]),MyConstants.Frames.Link0)
rightCurrPose = Util.convertToFrame(tfx.pose([0,0,0],frame=rp.toolFrame[MyConstants.Arm.Right]),MyConstants.Frames.Link0)
#trajoptpy.SetInteractive(True)
if fixedPose:
rp.getTrajectoryFromPose(MyConstants.Arm.Left, rightCurrPose, endGrasp=0, startJoints=leftStartJoints, debug=True)
rp.getTrajectoryFromPose(MyConstants.Arm.Right, leftCurrPose, endGrasp=0, startJoints=rightStartJoints, debug=True)
else:
rp.getTrajectoryFromPose(MyConstants.Arm.Left, rightCurrPose, leftStartJoints, debug=True)
rp.getTrajectoryFromPose(MyConstants.Arm.Right, leftCurrPose, rightStartJoints, debug=True)
"""
for index in range(len(armNames)):
armName = armNames[index]
otherArmName = armNames[(index+1)%len(armNames)]
desPose = Util.convertToFrame(tfx.pose([0,0,0],frame=rp.toolFrame[otherArmName]),MyConstants.Frames.Link0)
rp.getTrajectoryFromPose(armName, desPose, debug=True)
"""
rp.getTrajectoryFromPoseThread(once=True)
# while rp.trajRequest[armNames[0]] and rp.trajRequest[armNames[1]] and not rospy.is_shutdown():
# rospy.sleep(.05)
leftTraj = rp.jointTraj[MyConstants.Arm.Left]
rightTraj = rp.jointTraj[MyConstants.Arm.Right]
rp.env.SetViewer('qtcoin')
for left, right in zip(leftTraj,rightTraj):
rospy.loginfo('Press enter to go to next step')
raw_input()
rp.updateOpenraveJoints('L', left)
rp.updateOpenraveJoints('R', right)
def test_to_tf(self):
tb = tb_angles(45, -5, 24)
self.assertTrue(tb.to_tf().flags.writeable)
for yaw in self.yaw:
for pitch in self.pitch:
for roll in self.roll:
tb = tb_angles(yaw, pitch, roll)
m = tb.to_tf()
m2 = tft.euler_matrix(yaw * pi / 180, pitch * pi / 180,
roll * pi / 180, 'rzyx')
self.assertTrue(np.allclose(m, m2),
msg='%s and %s not close!' % (m, m2))
def test_config2():
jl = {0: .517, 1: 1.623, 2: -.05, 4: .16, 5: .161, 6: .99, 7: 1.023}
pl = tfx.pose((-.015, -.014, -.069),
tfx.tb_angles(-160.6, 75.7, -87.2),
frame='/0_link')
gl = 2.013
jr = {0: .511, 1: 1.607, 2: -.05, 4: .109, 5: .110, 6: -.652, 7: -.634}
pr = tfx.pose((-.136, -.017, -.068),
tfx.tb_angles(-66.3, 68.8, 40.3),
frame='/0_link')
gr = 1.286
return jl, pl, gl, jr, pr, gr
def test_config3():
jl = {0: .517, 1: 1.621, 2: -.04995, 4: .084, 5: .123, 6: .946, 7: .977}
pl = tfx.pose((-.015, -.015, -.069),
tfx.tb_angles(-151.1, 76.0, -73.8),
frame='/0_link')
gl = 1.923
jr = {0: .509, 1: 1.609, 2: -.04984, 4: .111, 5: .117, 6: -.637, 7: -.604}
pr = tfx.pose((-.136, -.017, -.068),
tfx.tb_angles(-67.6, 68.8, 39.1),
frame='/0_link')
gr = 1.240
return jl, pl, gl, jr, pr, gr
def test_create_quat(self):
for yaw in self.yaw:
for pitch in self.pitch:
for roll in self.roll:
q = tft.quaternion_from_euler(yaw * pi / 180.,
pitch * pi / 180.,
roll * pi / 180., 'rzyx')
tb = tb_angles(q)
self.assertTbEqual(tb, yaw, pitch, roll)
q2 = list(q)
tb = tb_angles(q2)
self.assertTbEqual(tb, yaw, pitch, roll)
def calculateDeltaPose(self, prev_frame, curr_frame, grip, tipDistance): #TODO: MEASURE CHANGE IN POSE -- ABSOLUTE?
#ROTATION = tfx.pose([0,0,0],[0,180,90]).matrix
#print "here"
prevPose = self.prevPose
#prevPose = ROTATION*prevPose
prev_x, prev_y, prev_z = prevPose.translation.x, prevPose.translation.y, prevPose.translation.z
prevOrientation = prevPose.orientation
if type(prev_frame) != type(None) and type(curr_frame) != type(None):
# IF we actually have frames
translation, rotation, palm_normal, palm_direction = calculateTransform(prev_frame, curr_frame, 0)
if type(translation) != type(None) and type(rotation) != type(None):
# IF we have a valid translation/rotation
# translation stuff
dx, dy, dz = (translation[0]*self.x_scale, translation[2]*self.y_scale, translation[1]*self.z_scale)
# orientation stuff
if self.FIXED_ORIENTATION:
#print "fixed"
currOrientation = tfx.tb_angles(-90,90,0)
else:
y = palm_direction.yaw
p = palm_direction.pitch
r = palm_normal.roll
y *= 57.2957795 # convert to degrees
p *= 57.2957795
r *= 57.2957795
y += -90 # adjust values for frame transformation
p += 90
r += 0
print "YPR: "+str(y) + ", "+str(p) + ", "+str(r)
currOrientation = tfx.tb_angles(y, p, r)
print "CURR ORIENTATION: "+str(currOrientation)
print "PALM NORMAL: "+str(palm_normal)
if math.fabs(dx) > DX_UPPER_BOUND or math.fabs(dy) > DY_UPPER_BOUND or math.fabs(dz) > DZ_UPPER_BOUND:
# IF the movement goes beyond bounds, don't do it
(dx, dy, dz) = (0,0,0)
currOrientation = prevOrientation
else:
# IF we don't have a valid translation or rotation, don't move
dx, dy, dz = (0,0,0)
currOrientation = prevOrientation
else:
# IF we don't have frames, also don't move
dx, dy, dz = (0,0,0)
currOrientation = prevOrientation
curr_x, curr_y, curr_z = (prev_x+dx, prev_y+dy, prev_z+dz) # ADD IN THE DELTA COMMAND
return (dx, dy, dz), currOrientation
def __init__(self, arm=Constants.Arm.Left):
self.arm = arm
if self.arm == Constants.Arm.Left:
self.toolframe = Constants.Frames.LeftTool
else:
self.toolframe = Constants.Frames.RightTool
self.transFrame = Constants.Frames.Link0
self.rotFrame = self.toolframe
self.gripperControl = GripperControlClass(arm, tf.TransformListener())
self.imageDetector = ARImageDetectionClass()
self.homePose = self.imageDetector.getHomePose()
# ar_frame is the target
self.ar_frame = '/stereo_33'
self.objectPose = tfx.pose([0,0,0], frame=self.ar_frame)
tf_ar_to_link0 = tfx.lookupTransform(Constants.Frames.Link0, self.ar_frame, wait=5)
self.objectPose = tf_ar_to_link0 * self.objectPose
self.objectPose = tfx.pose(self.objectPose.position, tfx.tb_angles(-90,90,0))
self.objectPose.position.y -= .002
rospy.sleep(3)
def withinBounds(ps0,
ps1,
transBound,
rotBound,
transFrame=None,
rotFrame=None):
"""
Returns if ps0 and ps1 (PoseStamped) are within translation and rotation bounds of each other
Note: rotBound is in degrees
"""
dPose = tfx.pose(deltaPose(ps0, ps1, transFrame, rotFrame))
deltaPositions = dPose.position.list
for deltaPos in deltaPositions:
if abs(deltaPos) > transBound:
return False
between = angleBetweenQuaternions(
tfx.tb_angles([0, 0, 0, 1]).msg, dPose.orientation)
if between > rotBound:
return False
return True
def sound(self):
"""
Position the gripper (grasping the rod) parallel to ground and move down
BE SURE TO HAVE LAUNCH roslaunch audio_capture capture.launch
"""
iters = 3 # number of times to hit
home_pose = tfx.pose([0.48, -0.67, 0.85],
tfx.tb_angles(0, 0, 0),
frame='base_link')
home_joints = [-1.49, 0.276231, -1.8, -1.43, 1.33627, -0.254, -25.1481]
delta_pos = [0, 0, -0.10]
speed = 0.25
file = '../data/sound_{0}.bag'.format(self.object_material)
print('Recording to file: {0}'.format(file))
sd = save_data.SaveData(file, save_data.PR2_TOPICS_AND_TYPES)
print('Going to home joints')
self.rarm.go_to_joints(home_joints)
print('Starting recording and moving by {0} for {1} times'.format(
delta_pos, iters))
sd.start()
for _ in xrange(iters):
self.rarm.go_to_pose(home_pose + delta_pos,
speed=speed,
block=True)
self.rarm.go_to_joints(home_joints)
sd.stop()
def teleop(self):
print('{0} arm teleop'.format(self.lrlong))
pos_step = .01
delta_position = {'a' : [0, pos_step, 0],
'd' : [0, -pos_step, 0],
'w' : [pos_step, 0, 0],
'x' : [-pos_step, 0, 0],
'+' : [0, 0, pos_step],
'-' : [0, 0, -pos_step]}
angle_step = 2.0
delta_angle = {'o' : [angle_step, 0, 0],
'p' : [-angle_step, 0, 0],
'k' : [0, angle_step, 0],
'l' : [0, -angle_step, 0],
'n' : [0, 0, angle_step],
'm' : [0, 0, -angle_step]}
char = ''
while char != 'q':
char = utils.Getch.getch()
pose = self.get_pose()
new_pose = tfx.pose(pose)
if delta_position.has_key(char):
new_pose.position = pose.position.array + delta_position[char]
ypr = np.array([pose.tb_angles.yaw_deg, pose.tb_angles.pitch_deg, pose.tb_angles.roll_deg])
if delta_angle.has_key(char):
ypr += np.array(delta_angle[char])
new_pose = tfx.pose(pose.position, tfx.tb_angles(ypr[0], ypr[1], ypr[2]))
self.set_pose(new_pose)
time.sleep(.01)
print('{0} arm end teleop'.format(self.lrlong))
def __init__(self, arm=Constants.Arm.Left):
self.arm = arm
if self.arm == Constants.Arm.Left:
self.toolframe = Constants.Frames.LeftTool
else:
self.toolframe = Constants.Frames.RightTool
self.transFrame = Constants.Frames.Link0
self.rotFrame = self.toolframe
self.gripperControl = GripperControlClass(arm, tf.TransformListener())
self.imageDetector = ARImageDetectionClass()
# manually add by checking print_state
self.startPose = None
# ar_frame is the target
self.ar_frame = '/stereo_32'
self.objectPose = tfx.pose([0,0,0],tfx.tb_angles(-90,90,0),frame=self.ar_frame)
self.listener = tf.TransformListener()
self.tf_br = tf.TransformBroadcaster()
self.delta_pub = rospy.Publisher('delta_pose', PoseStamped)
self.obj_pub = rospy.Publisher('object_pose', PoseStamped)
rospy.sleep(3)
def _handles_loop(self):
"""
For each handle in HandleListMsg,
calculate average pose
"""
rospy.sleep(5)
while not rospy.is_shutdown() and not self.exited:
rospy.sleep(.01)
handle_list_msg = self.handle_list_msg
pose_array = gm.PoseArray()
pose_array.header.frame_id = handle_list_msg.header.frame_id
pose_array.header.stamp = rospy.Time.now()
avg_pose_array = gm.PoseArray()
avg_pose_array.header.frame_id = handle_list_msg.header.frame_id
avg_pose_array.header.stamp = rospy.Time.now()
cam_to_base = tfx.lookupTransform('base_link', handle_list_msg.header.frame_id).matrix[:3,:3]
switch = np.matrix([[0, 1, 0],
[1, 0, 0],
[0, 0, 1]])
for handle in handle_list_msg.handles:
all_poses = [tfx.pose(cylinder.pose, stamp=rospy.Time.now(), frame=handle_list_msg.header.frame_id) for cylinder in handle.cylinders]
rotated_poses = [tfx.pose(p.position, tfx.tb_angles(p.orientation.matrix*switch)) for p in all_poses]
filtered_poses = list()
for rot_pose in rotated_poses:
r_base = cam_to_base*rot_pose.orientation.matrix
if r_base[0,0] > 0:
if r_base[2,2] > 0:
rot_pose.orientation = tfx.tb_angles(rot_pose.orientation.matrix*tfx.tb_angles(0,0,180).matrix)
filtered_poses.append(rot_pose)
pose_array.poses += [pose.msg.Pose() for pose in filtered_poses]
if len(filtered_poses) > 0:
avg_position = sum([p.position.array for p in filtered_poses])/float(len(filtered_poses))
avg_quat = sum([p.orientation.quaternion for p in filtered_poses])/float(len(filtered_poses))
avg_pose_array.poses.append(tfx.pose(avg_position, avg_quat).msg.Pose())
if len(pose_array.poses) > 0:
self.handles_pose_pub.publish(pose_array)
self.avg_handles_pose_pub.publish(avg_pose_array)
def succesorState(self):
''' Updates current state to the next succesor state
by default it just returns current state
'''
rightArmPose = self.rightArm.get_pose()
leftArmPose = self.leftArm.get_pose()
newRightArmPose = tfx.pose(rightArmPose) # Create new pose
newLeftArmPose = tfx.pose(leftArmPose) # Create new pose
''' ------ CHANGE CODE BELOW THIS POINT ------ '''
if self.valid:
x1, y1, z1 = TARGET_GREEN
x2, y2, z2 = GRASP_GREEN
currentX, currentY, currentZ = rightArmPose.position.array
acceptableAbsError = 0.05
if abs(x1 - currentX) <= acceptableAbsError and abs(
y1 - currentY) <= acceptableAbsError and abs(
z1 - currentZ) <= acceptableAbsError:
newRightArmPose.position = GRASP_GREEN
elif abs(x2 - currentX) <= acceptableAbsError and abs(
y2 - currentY) <= acceptableAbsError and abs(
z2 - currentZ) <= acceptableAbsError:
self.rightArm.close_gripper()
print "Grasping..."
rospy.sleep(1)
self.raiseArms()
newRightArmPose.position = DEST_GREEN
else:
newRightArmPose.position = TARGET_GREEN # either update by adding to xyz offset array "+ [0, -pos_step, 0]" or assign new position, defualt keeps position the same
else:
print "The last succesor state was invalid"
print "Enter a custom pose x y z"
coord = raw_input().split()
newRightArmPose.position = [float(x) for x in coord]
newLeftArmPose.position = leftArmPose.position
''' ------ CHANGE CODE ABOVE THIS POINT ------ '''
newRightArmPose = tfx.pose(newRightArmPose.position,
tfx.tb_angles(0.0, 0.0, 0.0))
newRightJoints = self.rightArm.ik(
newRightArmPose) # Updates arm pose if valid
newLeftJoints = self.leftArm.ik(newLeftArmPose)
if newRightJoints is not None:
self.rightArm.go_to_joints(newRightJoints)
print('new_pose: {0}'.format(newRightArmPose))
self.valid = True
else:
rospy.loginfo('Invalid Right Arm Pose')
print newRightArmPose
self.valid = False
return self
rospy.sleep(.01)
if newLeftJoints is not None:
self.leftArm.go_to_joints(newLeftJoints)
else:
rospy.loginfo('Invalid Left Arm Pose')
rospy.sleep(.01)
return self # Modify to generate a valid succesor states, does not update state if pose is invalid and instead reverts to last valid state
def iterate_angles():
roll = np.linspace(120, 160, num=10)
pitch = np.linspace(-50, 50, num=10)
yaw = np.linspace(50, 120, num=10)
time.sleep(2)
initial_angle = (88.0984719856, -12.0248544998, 131.680496265)
quaternion = tfx.tb_angles(initial_angle[0], initial_angle[1], initial_angle[2])
frame = get_frame_rot(quaternion)
psm1.move_cartesian_frame(frame)
psm1.open_gripper(80.0)
for i in range(10):
angle = (yaw[i], initial_angle[1], initial_angle[2])
quaternion = tfx.tb_angles(angle[0], angle[1], angle[2])
frame = get_frame_rot(quaternion)
psm1.move_cartesian_frame(frame)
time.sleep(0.2)
def test_down():
down = tfx.tb_angles(-0.0,90.0,-0.0)
print(down.quaternion)
point = tfx.point(1,2,3)
pose = tfx.pose(point)
pose.orientation = down
print(pose)
code.interact(local=locals())
def test_str(self):
for yaw in self.yaw:
for pitch in self.pitch:
for roll in self.roll:
tb = tb_angles(yaw,pitch,roll)
for brackets in ['[]','()']:
for deg in [None,True,False]:
for rad in [None,True,False]:
if deg is False and rad is False:
continue
for short_names in [None,True,False]:
if short_names:
y_str = 'y'
p_str = 'p'
r_str = 'r'
else:
y_str = 'yaw'
p_str = 'pitch'
r_str = 'roll'
for fixed_width in [None,True,False]:
if fixed_width:
deg_str = r'([ -]\d{3}| [ -]\d{2}| [ -]\d{1})\.\d{1}'
rad_str = r'[ -]\d{1}\.\d{3}'
else:
deg_str = r'-?\d{1,3}\.\d{1}'
rad_str = r'-?\d{1}\.\d{3}'
def add_num(regex):
if deg or (deg is None and rad is None):
regex += deg_str
if deg is True:
regex += ' deg'
if rad:
if deg:
regex += r' \('
regex += rad_str + ' rad'
if deg:
regex += r'\)'
return regex
regex = '^\\' + brackets[0]
regex += y_str + ':'
regex = add_num(regex)
regex += ', '
regex += p_str + ':'
regex = add_num(regex)
regex += ', '
regex += r_str + ':'
regex = add_num(regex)
regex += '\\' + brackets[1] + '$'
if deg is False and rad is False:
with self.assertRaises(ValueError):
s = tb.tostring(brackets=brackets,deg=deg,rad=rad,short_names=short_names,fixed_width=fixed_width)
else:
s = tb.tostring(brackets=brackets,deg=deg,rad=rad,short_names=short_names,fixed_width=fixed_width)
self.assertRegexpMatches(s, regex,msg = '%s' % [brackets,deg,rad,short_names,fixed_width])
def test_create(self):
for yaw in self.yaw:
for pitch in self.pitch:
for roll in self.roll:
tb = tb_angles(yaw, pitch, roll)
self.compare_deg(tb, yaw, pitch, roll)
tb = tb_angles(yaw, pitch, roll, deg=True)
self.compare_deg(tb, yaw, pitch, roll)
tb = tb_angles(yaw, pitch, roll, rad=False)
self.compare_deg(tb, yaw, pitch, roll)
yaw_rad = yaw * pi / 180.
pitch_rad = pitch * pi / 180.
roll_rad = roll * pi / 180.
tb = tb_angles(yaw_rad, pitch_rad, roll_rad, rad=True)
self.compare_rad(tb, yaw_rad, pitch_rad, roll_rad)
tb = tb_angles(yaw_rad, pitch_rad, roll_rad, deg=False)
self.compare_rad(tb, yaw_rad, pitch_rad, roll_rad)
def test_home_pose():
a = arm.Arm('left')
# push home_pose
home_pose = tfx.pose([0.54, 0.2, 0.71], tfx.tb_angles(-90,0,0), frame='base_link')
home_joints = [0.6857, 0.31154, 2.21, -1.062444, -0.33257,-1.212881, -0.81091]
a.go_to_joints(home_joints)
IPython.embed()
def broadcast(self):
if not self.config:
return
config = self.config
self.br.sendTransform((config.x, config.y, config.z),
tfx.tb_angles(config.yaw, config.pitch, config.roll).quaternion,
rospy.Time.now(),
'/left_optical_frame',
'/world')
def test_create(self): for yaw in self.yaw: for pitch in self.pitch: for roll in self.roll: tb = tb_angles(yaw,pitch,roll) self.compare_deg(tb, yaw, pitch, roll) tb = tb_angles(yaw,pitch,roll,deg=True) self.compare_deg(tb, yaw, pitch, roll) tb = tb_angles(yaw,pitch,roll,rad=False) self.compare_deg(tb, yaw, pitch, roll) yaw_rad = yaw * pi / 180. pitch_rad = pitch * pi / 180. roll_rad = roll * pi / 180. tb = tb_angles(yaw_rad,pitch_rad,roll_rad,rad=True) self.compare_rad(tb, yaw_rad, pitch_rad, roll_rad) tb = tb_angles(yaw_rad,pitch_rad,roll_rad,deg=False) self.compare_rad(tb, yaw_rad, pitch_rad, roll_rad)
def test_box():
sim = simulator.Simulator(view=True)
pose = tfx.pose([1,0,0], tfx.tb_angles(0,45,0))
extents = [0.1,0.1,0.1]
sim.add_box(pose.matrix, extents)
print('Press enter to exit')
raw_input()
def test_arm():
a = arm.Arm('right')
p = a.get_pose()
print('Pose : {0}'.format(p))
home_pose = tfx.pose([0.48, -0.67, 0.85], tfx.tb_angles(0,0,0), frame='base_link')
a.go_to_pose(home_pose)
IPython.embed()
def iterate_angles():
roll = np.linspace(120, 160, num=10)
pitch = np.linspace(-50, 50, num=10)
yaw = np.linspace(50, 120, num=10)
time.sleep(2)
initial_angle = (88.0984719856, -12.0248544998, 131.680496265)
quaternion = tfx.tb_angles(initial_angle[0], initial_angle[1],
initial_angle[2])
frame = get_frame_rot(quaternion)
psm1.move_cartesian_frame(frame)
psm1.open_gripper(80.0)
for i in range(10):
angle = (yaw[i], initial_angle[1], initial_angle[2])
quaternion = tfx.tb_angles(angle[0], angle[1], angle[2])
frame = get_frame_rot(quaternion)
psm1.move_cartesian_frame(frame)
time.sleep(0.2)
def test_to_mat(self): tb = tb_angles(45,-5,24) m1 = tb.matrix self.assertFalse(m1.flags.writeable) m1a = tb.matrix self.assertIs(m1, m1a) m2 = tb.to_matrix() self.assertTrue(m2.flags.writeable) self.assertIsNot(m1,m2) for yaw in self.yaw: for pitch in self.pitch: for roll in self.roll: tb = tb_angles(yaw,pitch,roll) m = tb.matrix self.assertAlmostEqual(np.linalg.det(m), 1) m2 = tft.euler_matrix(yaw * pi / 180, pitch * pi / 180, roll * pi / 180, 'rzyx')[0:3,0:3] self.assertTrue(np.allclose(m, m2),msg='%s and %s not close!' % (m,m2))
def test_create_matrix(self): for yaw in self.yaw: for pitch in self.pitch: for roll in self.roll: m = tft.euler_matrix(yaw * pi / 180., pitch * pi / 180., roll * pi / 180., 'rzyx') tb = tb_angles(m) self.assertTbEqual(tb, yaw, pitch, roll) m2 = m[0:3,0:3] tb = tb_angles(m2) self.assertTbEqual(tb, yaw, pitch, roll) m3 = np.mat(m) tb = tb_angles(m3) self.assertTbEqual(tb, yaw, pitch, roll) m4 = m.tolist() tb = tb_angles(m4) self.assertTbEqual(tb, yaw, pitch, roll)
def test_arm():
a = arm.Arm('right')
p = a.get_pose()
print('Pose : {0}'.format(p))
home_pose = tfx.pose([0.48, -0.67, 0.85],
tfx.tb_angles(0, 0, 0),
frame='base_link')
a.go_to_pose(home_pose)
IPython.embed()
def touch(self):
"""
Position the gripper facing downwards and move down
"""
home_pose = tfx.pose(tfx.pose([0.54, 0.2, 0.85], tfx.tb_angles(0,90,0), frame='base_link'))
home_joints = [0.57, 0.1233, 1.288, -1.58564, 1.695, -1.85322, 14.727]
delta_pos = [0, 0, -0.10]
speed = 0.02
file = '../data/touch_{0}.bag'.format(self.object_material)
self.execute_experiment(file, home_joints, home_pose, delta_pos, speed=speed)
def test_create_matrix(self):
for yaw in self.yaw:
for pitch in self.pitch:
for roll in self.roll:
m = tft.euler_matrix(yaw * pi / 180., pitch * pi / 180.,
roll * pi / 180., 'rzyx')
tb = tb_angles(m)
self.assertTbEqual(tb, yaw, pitch, roll)
m2 = m[0:3, 0:3]
tb = tb_angles(m2)
self.assertTbEqual(tb, yaw, pitch, roll)
m3 = np.mat(m)
tb = tb_angles(m3)
self.assertTbEqual(tb, yaw, pitch, roll)
m4 = m.tolist()
tb = tb_angles(m4)
self.assertTbEqual(tb, yaw, pitch, roll)
def alignGrippers(self):
rightArmPose = self.rightArm.get_pose()
leftArmPose = self.leftArm.get_pose()
newLeftArmPose = tfx.pose(leftArmPose.position,
tfx.tb_angles(0.0, 0.0,
0.0)) # Create new pose
# print "yaw(current = " + str(rightArmPose.tb_angles.yaw_deg) + "):"
# coord = raw_input()
# yaw = float(coord)
# print "pitch(current = " + str(rightArmPose.tb_angles.pitch_deg) + "):"
# coord = raw_input()
# pitch = float(coord)
# print "roll(current = " + str(rightArmPose.tb_angles.roll_deg) + "):"
# coord = raw_input()
# roll = float(coord)
newRightArmPose = tfx.pose(rightArmPose.position,
tfx.tb_angles(0.0, 0.0, 0.0))
newRightJoints = self.rightArm.ik(
newRightArmPose) # Updates arm pose if valid
newLeftJoints = self.leftArm.ik(newLeftArmPose)
if newRightJoints is not None:
self.rightArm.go_to_joints(newRightJoints)
print('new_pose: {0}'.format(newRightArmPose))
print "Aligning right gripper..."
self.rightArm.open_gripper()
self.valid = True
else:
rospy.loginfo('Invalid Right Arm Pose')
print "RIGHT ALIGNMENT FAILED"
self.valid = False
return self
rospy.sleep(0.5)
if newLeftJoints is not None:
self.leftArm.go_to_joints(newLeftJoints)
print "Aligning left gripper..."
self.leftArm.open_gripper()
else:
rospy.loginfo('Invalid Left Arm Pose')
print "LEFT ALIGNMENT FAILED"
rospy.sleep(0.5)
return self
def test_to_mat(self):
tb = tb_angles(45, -5, 24)
m1 = tb.matrix
self.assertFalse(m1.flags.writeable)
m1a = tb.matrix
self.assertIs(m1, m1a)
m2 = tb.to_matrix()
self.assertTrue(m2.flags.writeable)
self.assertIsNot(m1, m2)
for yaw in self.yaw:
for pitch in self.pitch:
for roll in self.roll:
tb = tb_angles(yaw, pitch, roll)
m = tb.matrix
self.assertAlmostEqual(np.linalg.det(m), 1)
m2 = tft.euler_matrix(yaw * pi / 180, pitch * pi / 180,
roll * pi / 180, 'rzyx')[0:3, 0:3]
self.assertTrue(np.allclose(m, m2),
msg='%s and %s not close!' % (m, m2))
def step(self, arm, action):
"""it now takes in a list/tuple of [position, rotation], execute the action(move arm to indicated pos, rot)
and returns:"""
"""
observation: object --- an environment-specific object representing your observation of the environment;
in this env new list of pos, rot
reward: float --- amount of reward achieved by the previous action.
The scale varies between environments, and can also be customized using BLANK method.
done: boolean --- whether it's time to reset the environment again.
Most tasks are divided up into well-defined episodes, and done being True indicates the episode has terminated.
info: dict --- value TBD, naive thoughts are to include info for next step
"""
# TODO: make more complicating actions
pos, rot = action
t1, t2, t3 = pos
# observation = action
# TODO: check if out of range
# Take action
obs_pos = arm.get_current_cartesian_position().position
obs_rot = tfx.tb_angles(arm.get_current_cartesian_position().rotation)
for i in range(0, 3):
pos[i] += obs_pos[i]
r1 = obs_rot.yaw_rad
r2 = obs_rot.pitch_rad
r3 = obs_rot.roll_rad
new_rot = tfx.tb_angles(r1 + rot[0],
r2 + rot[1],
r3 + rot[2],
rad=True)
observation = [pos, new_rot]
self.psmR.move_cartesian_frame(tfx.pose(pos, new_rot))
self.time_counter += 1
# Get reward
reward = self.reward(self.state, action)
# Determine done
done = self.time_counter >= self.env_time
# TODO: implement info
info = {}
return [observation, reward, done, info]
def pos_rot_cpos(cpos, nparrays=False):
"""
It's annoying to have to do this every time. I just want two lists.
To be clear, `cpos = arm.get_current_cartesian_position()`.
"""
pos = np.squeeze(np.array(cpos.position[:3])).tolist()
rott = tfx.tb_angles(cpos.rotation)
rot = [rott.yaw_deg, rott.pitch_deg, rott.roll_deg]
assert len(pos) == len(rot) == 3
if nparrays:
pos = np.array(pos)
rot = np.array(rot)
return pos, rot
