max/robotics
1
0
Fork 0
Code Issues Pull Requests Actions Packages Projects Releases Wiki Activity

Update

Browse Source
This commit is contained in:
Max Hunt 2020-02-13 00:01:25 +00:00
parent 8907a9ba94
commit 47b908f9ac
2 changed files with 4 additions and 32 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

30
main.py
View File

@ -265,41 +265,11 @@ if __name__ == "__main__":
left_pick = otc.tf_lookup('a1') left_pick = otc.tf_lookup('a1')
left_pick_angles = euler_from_quaternion([left_pick.orientation.x , left_pick.orientation.y , left_pick.orientation.z, left_pick.orientation.w])
xangle = left_pick_angles[0] + radians(-90)
xangle1 = left_pick_angles[1] + radians(-90)
xangle2 = left_pick_angles[2] + radians(-180)
left_pick_angles = (xangle, left_pick_angles[1], left_pick_angles[2])
target_quat = quaternion_from_euler(left_pick_angles[0], left_pick_angles[1], left_pick_angles[2])
left_pick_proc = Pose()
left_pick_proc.position.x = left_pick.position.x - 0.05
left_pick_proc.position.y = left_pick.position.y
left_pick_proc.position.z = left_pick.position.z
left_pick_proc.orientation.x = target_quat[0]
left_pick_proc.orientation.y = target_quat[1]
left_pick_proc.orientation.z = target_quat[2]
left_pick_proc.orientation.w = target_quat[3]
left_pnp.pick(left_pick_proc) left_pnp.pick(left_pick_proc)
left_place = otc.tf_lookup('f2') left_place = otc.tf_lookup('f2')
left_place_angles = euler_from_quaternion([left_place.orientation.x , left_place.orientation.y , left_place.orientation.z, left_place.orientation.w])
xangle = left_place_angles[0]+ radians(180)
xangle1 = left_place_angles[1]
xangle2 = left_place_angles[2]
left_place_angles = (xangle, left_place_angles[1], left_place_angles[2])
target_quat = quaternion_from_euler(left_place_angles[0], left_place_angles[1], left_pick_angles[2])
left_place_pos = Pose()
left_place_pos.position.x = left_place.position.x
left_place_pos.position.y = left_place.position.y
left_place_pos.position.z = left_place.position.z
left_place_pos.orientation.x = target_quat[0]
left_place_pos.orientation.y = target_quat[1]
left_place_pos.orientation.z = target_quat[2]
left_place_pos.orientation.w = target_quat[3]
left_pnp.place(left_place_pos) left_pnp.place(left_place_pos)

6
object_tf_client.py
View File

@ -19,8 +19,10 @@ def tf_lookup(object_name):
continue continue
object_angles = tf.transformations.euler_from_quaternion([rot[0] , rot[1] , rot[2], rot[3]]) object_angles = tf.transformations.euler_from_quaternion([rot[0] , rot[1] , rot[2], rot[3]])
xangle = object_angles[0] + radians(90) xangle = object_angles[0]
object_angles = (xangle, object_angles[1], object_angles[2]) xangle1 = object_angles[1]
xangle2 = object_angles[2]
object_angles = (xangle, xangle1, xangle2)
target_quat = tf.transformations.quaternion_from_euler(object_angles[0], object_angles[1], object_angles[2]) target_quat = tf.transformations.quaternion_from_euler(object_angles[0], object_angles[1], object_angles[2])
target_pose = Pose() target_pose = Pose()
target_pose.position.x = trans[0] target_pose.position.x = trans[0]