import numpy as np
import tf.transformations as tfm
import capsen.capsen
from ik.roshelper import pubFrame
# bin_1
# 1.1069; -0.018219; 1.3762 -0.54475; 0.075625; 0.20345; 0.81003
#-0.035814156250925766, 0.5470560007467684, -0.8243085337401062, -2.70526034059, 0.8721846604532582, -0.3148573970598978
rospy.init_node('listener', anonymous=True)
#desired_joint_pose = [-0.0087, 0.6423, -0.1222, 1.4731, 1.4643, 1.2] # for bin 7
home_joint_pose = [0, -0.2, 0.2, 0.01, 1, 1.4]
plan = Plan()
plan.q_traj = [home_joint_pose]
plan.visualize()
plan.execute()
br = tf.TransformBroadcaster()
# generate targets for horizontal scan
#bin_cnstr = get_bin_cnstr()
bin_cnstr = get_bin_inner_cnstr()
target_poses = []
refind = 7
for i in range(12):
#target_pose_7_vert = [1.1756, 0, 0.79966, 0.69537, 0.11132, 0.049168, 0.70827] # for bin 7 vertical
#target_pose_7_hori = [1.1756, -0.0080496, 0.79966, 0.96262, -0.039399, -0.25934, 0.06743] # for bin 7 horizontal
target_pose_7_vert = [1.1756, 0, 0.79966] + tfm.quaternion_from_euler(
math.pi, 1.4, math.pi).tolist() # for bin 7 vertical
target_pose_7_hori = [1.1756, 0, 0.79966] + tfm.quaternion_from_euler(
def manual_cal_1(binNum=0, strPos=None):
if strPos == None:
print 'No desired position entered'
return
## initialize listener rospy
listener = tf.TransformListener()
rospy.sleep(0.1)
br = tf.TransformBroadcaster()
rospy.sleep(0.1)
joint_topic = '/joint_states'
# set tcp
l1 = 0.06345
l2 = 0.400
tip_hand_transform = [
l1, 0, l2, 0, 0, 0
] # to be updated when we have a hand design finalized
# broadcast frame attached to tcp
rospy.sleep(0.1)
br.sendTransform(tuple(tip_hand_transform[0:3]),
tfm.quaternion_from_euler(*tip_hand_transform[3:6]),
rospy.Time.now(), 'tip', "link_6")
rospy.sleep(0.1)
# get position of the tcp in world frame
pose_world = coordinateFrameTransform(tip_hand_transform[0:3], 'link_6',
'map', listener)
tcpPos = [
pose_world.pose.position.x, pose_world.pose.position.y,
pose_world.pose.position.z
]
tcpPosHome = tcpPos
orientation = [0.0, 0.7071, 0.0,
0.7071] # set topple orientation (rotate wrist)
rospy.sleep(0.1)
br.sendTransform(tuple(tip_hand_transform[0:3]),
tfm.quaternion_from_euler(*tip_hand_transform[3:6]),
rospy.Time.now(), 'tip', "link_6")
rospy.sleep(0.1)
pose_world = coordinateFrameTransform(tip_hand_transform[0:3], 'link_6',
'map', listener)
tcpPos = [
pose_world.pose.position.x, pose_world.pose.position.y,
pose_world.pose.position.z
]
print 'TCP is at:', tcpPos
distFromShelf = 0.05
mouthBinShelf, binBaseHeightShelf = getBinMouthAndFloor(
distFromShelf, binNum)
mouthBin = coordinateFrameTransform(mouthBinShelf, 'shelf', 'map',
listener)
mouthBin = [
mouthBin.pose.position.x, mouthBin.pose.position.y,
mouthBin.pose.position.z
]
# define modes here:
# go home
if strPos == 'home':
print 'planning to go home'
plan = Plan()
plan.q_traj = [[0, -0.3959, 0.58466, 0.03, 0.1152, -0.1745]]
plan.visualize()
raw_input('execute?')
plan.execute()
print 'done going home'
# go mouth
if strPos == 'mouth':
print 'planning to go to mouth'
Orientation = [0.0, 0.7071, 0.0, 0.7071]
targetPt = mouthBin
planner = IK(q0=None,
target_tip_pos=targetPt,
target_tip_ori=Orientation,
tip_hand_transform=tip_hand_transform,
joint_topic=joint_topic)
plan = planner.plan()
plan.visualize()
s = plan.success()
if s:
raw_input('execute?')
plan.execute()
if not s:
print 'could not find plan'
print 'done going to mouth'
# go mid back bin
if strPos == 'shelf':
print 'planning to go to mid back of the bin'
Orientation = [0.0, 0.7071, 0.0, 0.7071]
targetPt = [mouthBin[0], mouthBin[1], mouthBin[2]]
planner = IK(q0=None,
target_tip_pos=targetPt,
target_tip_ori=Orientation,
tip_hand_transform=tip_hand_transform,
joint_topic=joint_topic)
plan = planner.plan()
plan.visualize()
s = plan.success()
if s:
raw_input('execute?')
plan.execute()
if not s:
print 'could not find plan'
print 'done going to mid back of the bin'
def manual_cal_1(binNum = 0, strPos = None):
if strPos == None:
print 'No desired position entered'
return
## initialize listener rospy
listener = tf.TransformListener()
rospy.sleep(0.1)
br = tf.TransformBroadcaster()
rospy.sleep(0.1)
joint_topic = '/joint_states'
# set tcp
l1 = 0.06345
l2 = 0.400
tip_hand_transform = [l1, 0, l2, 0,0,0] # to be updated when we have a hand design finalized
# broadcast frame attached to tcp
rospy.sleep(0.1)
br.sendTransform(tuple(tip_hand_transform[0:3]), tfm.quaternion_from_euler(*tip_hand_transform[3:6]), rospy.Time.now(), 'tip', "link_6")
rospy.sleep(0.1)
# get position of the tcp in world frame
pose_world = coordinateFrameTransform(tip_hand_transform[0:3], 'link_6', 'map', listener)
tcpPos =[pose_world.pose.position.x, pose_world.pose.position.y, pose_world.pose.position.z]
tcpPosHome = tcpPos
orientation = [0.0, 0.7071, 0.0, 0.7071] # set topple orientation (rotate wrist)
rospy.sleep(0.1)
br.sendTransform(tuple(tip_hand_transform[0:3]), tfm.quaternion_from_euler(*tip_hand_transform[3:6]), rospy.Time.now(), 'tip', "link_6")
rospy.sleep(0.1)
pose_world = coordinateFrameTransform(tip_hand_transform[0:3], 'link_6', 'map', listener)
tcpPos=[pose_world.pose.position.x, pose_world.pose.position.y, pose_world.pose.position.z]
print 'TCP is at:', tcpPos
distFromShelf = 0.05
mouthBinShelf, binBaseHeightShelf = getBinMouthAndFloor(distFromShelf, binNum)
mouthBin = coordinateFrameTransform(mouthBinShelf, 'shelf', 'map', listener)
mouthBin = [mouthBin.pose.position.x, mouthBin.pose.position.y, mouthBin.pose.position.z]
# define modes here:
# go home
if strPos == 'home':
print 'planning to go home'
plan = Plan()
plan.q_traj = [[0, -0.3959, 0.58466, 0.03, 0.1152, -0.1745]]
plan.visualize()
raw_input('execute?')
plan.execute()
print 'done going home'
# go mouth
if strPos == 'mouth':
print 'planning to go to mouth'
Orientation = [0.0, 0.7071, 0.0, 0.7071]
targetPt = mouthBin
planner = IK(q0 = None, target_tip_pos = targetPt,
target_tip_ori = Orientation, tip_hand_transform=tip_hand_transform,
joint_topic=joint_topic)
plan = planner.plan()
plan.visualize()
s = plan.success()
if s:
raw_input('execute?')
plan.execute()
if not s:
print 'could not find plan'
print 'done going to mouth'
# go mid back bin
if strPos == 'shelf':
print 'planning to go to mid back of the bin'
Orientation = [0.0, 0.7071, 0.0, 0.7071]
targetPt = [mouthBin[0], mouthBin[1], mouthBin[2]]
planner = IK(q0 = None, target_tip_pos = targetPt,
target_tip_ori = Orientation, tip_hand_transform=tip_hand_transform,
joint_topic=joint_topic)
plan = planner.plan()
plan.visualize()
s = plan.success()
if s:
raw_input('execute?')
plan.execute()
if not s:
print 'could not find plan'
print 'done going to mid back of the bin'
import tf
import math
import numpy as np
import tf.transformations as tfm
import capsen.capsen
from ik.roshelper import pubFrame
# bin_1
# 1.1069; -0.018219; 1.3762 -0.54475; 0.075625; 0.20345; 0.81003
#-0.035814156250925766, 0.5470560007467684, -0.8243085337401062, -2.70526034059, 0.8721846604532582, -0.3148573970598978
rospy.init_node('listener', anonymous=True)
#desired_joint_pose = [-0.0087, 0.6423, -0.1222, 1.4731, 1.4643, 1.2] # for bin 7
home_joint_pose = [0, -0.2, 0.2, 0.01, 1, 1.4]
plan = Plan(); plan.q_traj = [home_joint_pose]; plan.visualize(); plan.execute()
br = tf.TransformBroadcaster()
# generate targets for horizontal scan
#bin_cnstr = get_bin_cnstr()
bin_cnstr = get_bin_inner_cnstr()
target_poses = []
refind = 7
for i in range(12):
#target_pose_7_vert = [1.1756, 0, 0.79966, 0.69537, 0.11132, 0.049168, 0.70827] # for bin 7 vertical
#target_pose_7_hori = [1.1756, -0.0080496, 0.79966, 0.96262, -0.039399, -0.25934, 0.06743] # for bin 7 horizontal
target_pose_7_vert = [1.1756, 0, 0.79966] + tfm.quaternion_from_euler(math.pi, 1.4, math.pi).tolist() # for bin 7 vertical
target_pose_7_hori = [1.1756, 0, 0.79966] + tfm.quaternion_from_euler(2.2, 0, math.pi/2).tolist()
target_pose_7_invhori = [1.1756, 0, 0.79966] + tfm.quaternion_from_euler(-math.pi/2, 0, -math.pi/2).tolist()
