class Primitives:
block_size = 1e-3 * 13.0 # mm
W,H = 10.0, 20.0 # tetris grid units
z_above = 0.07
z_down = 0.03
sqr2 = np.sqrt(2.0)/2.0
sqr7 = np.sqrt(7.0)/4.0
vert = [-.5, .5, .5, .5]
horz = [0, sqr2, 0, sqr2]
#rot_r = [-sqr7, -.25, sqr7, -.25]
rot_r = [-.25, sqr7, 0.25, sqr7]
rot_l = [sqr7, -.25, -sqr7, -.25]
init_pose = {
#'r': [-0.73, -0.17, -1.60, -1.46, -32.69, -1.31, -16.94],
#'l': [ 0.71, -0.04, 1.56, -1.46, -4.72, -1.36, 3.86]}
'l': [ 0.95, 0.0, np.pi/2., -np.pi/2., -np.pi*1.5, -np.pi/2.0, np.pi],
'r': [ -0.7, 0.0, -np.pi/2., -np.pi/2., -20.424894658897493, -np.pi/2.0, np.pi]}
min_time = 1.0
max_time = 5.0
frame = 'tetris'
def __init__(self, whicharm='l'):
self.arm = ArmController()
self.objects = None
self.obj_sub = rospy.Subscriber("/table_publisher/object_poses", PoseArray, self.objCb)
self.arm.command_torso(0.26)
self.whicharm = whicharm
self.move_arm_to_side()
def move_arm_to_side(self):
self.arm.joint_movearm(self.whicharm, self.init_pose[self.whicharm],
3, True)
def lift_arm(self, rating=1.0):
rospy.sleep(0.1)
curr_pos = self.arm.get_cartesian_pose(self.frame)[self.whicharm]
curr_pos[2] = self.z_above *rating
goal =[ self.stiff_pose(curr_pos, 0.1) + [0.25*self.min_time*rating],
self.stiff_pose(curr_pos, 0.5) + [0.5*self.min_time*rating],
self.stiff_pose(curr_pos, 1.0) + [self.min_time*rating] ]
self.arm.cart_movearm(self.whicharm, goal , self.frame, True)
#raw_input("done lift")
def stiff_pose(self, pose, rating = 0.2):
pose_force = int(100*rating)
torque_force = int(50*rating)
return list(pose) + [pose_force]*3 + [torque_force]*3 + [False]*6
def distance_between_two_pts(self, p1,p2):
pts = zip(p1,p2)
dx = [(x1 - x2)**2 for (x1,x2) in pts]
return sum(dx)
def clip_time(self, t):
if t < self.min_time:
return self.min_time
elif t > self.max_time:
return self.max_time
else:
return t
def approach_pose(self, pose):
curr_pose = self.arm.get_cartesian_pose(self.frame)[self.whicharm]
time = self.clip_time(15*self.distance_between_two_pts(curr_pose, pose))
curr_pose[2] = self.z_above
middle_pose = list(pose)
middle_pose[2] = self.z_above
traj = [
self.stiff_pose(curr_pose, 0.1) + [time*.1],
self.stiff_pose(curr_pose, 1.0) + [time*.2],
self.stiff_pose(middle_pose, 1.5) + [time*.8],
self.free_push(pose, 1.5) + [time]
]
self.arm.cart_movearm(self.whicharm, traj, self.frame, True)
rospy.sleep(0.1)
#final_pose = self.arm.get_cartesian_pose(self.frame)[self.whicharm]
#dist = self.distance_between_two_pts(pose, final_pose)
def do_force_control(self, cmd, timeout=4.0):
self.approach_pose(cmd[:7])
rospy.sleep(0.1)
curr_pose = self.arm.get_cartesian_pose(self.frame)[self.whicharm]
ros_timeout = rospy.Time.now() + rospy.Duration(timeout)
self.arm.cart_movearm(self.whicharm, [cmd + [timeout]], self.frame, False)
rospy.sleep(0.2)
while rospy.Time.now() < ros_timeout and not rospy.is_shutdown():
rospy.sleep(0.1)
if not self.arm.is_moving(self.whicharm):
self.arm.cancel_goal(self.whicharm)
break
"""
final_pose = self.arm.get_cartesian_pose(self.frame)[self.whicharm]
if self.distance_between_two_pts(curr_pose, final_pose) < self.block_size:
ros_timeout = rospy.Time.now() + rospy.Duration(timeout)
self.arm.cart_movearm(self.whicharm, [cmd + [timeout]], self.frame, False)
rospy.sleep(1.0)
while rospy.Time.now() < ros_timeout:
rospy.sleep(0.1)
if not self.arm.is_moving(self.whicharm):
self.arm.cancel_goal(self.whicharm)
break
"""
self.lift_arm()
def free_space_push_down(self, pos1, pos2):
pose1 = list(pos1) + self.horz
pose2 = list(pos2) + self.horz
time = self.clip_time(15*self.distance_between_two_pts(pose1,pose2))
self.approach_pose(pose1)
traj = [
self.free_push(pose1) + [time*.1],
self.free_push(pose2) + [time]
]
self.arm.cart_movearm(self.whicharm, traj, self.frame, True)
self.lift_arm(1.)
def free_space_push_right(self, pos1, pos2):
pose1 = list(pos1) + self.vert
pose2 = list(pos2) + self.vert
time = self.clip_time(15*self.distance_between_two_pts(pose1,pose2))
self.approach_pose(pose1)
traj = [
self.free_push(pose1) + [time*.1],
self.free_push(pose2) + [time]
]
self.arm.cart_movearm(self.whicharm, traj, self.frame, True)
self.lift_arm(1.)
def free_push(self, pose, rating=1.0): # position controlled
fx,fy,fz = [300*rating]*3 # max stiffness
ox,oy,oz = [10*rating]*3
fz = -5*rating # force in downard direction
states = [False]*6
states[2] = True # use force control for Z
return list(pose) + [fx,fy,fz,ox,oy,oz] + states
def right_contact_slide_down(self, pos, rating=1.0):
pose = list(pos) + self.rot_r
#use force control
fx = -7*rating
fy = -3 * rating
fz = -2* rating
#keep gripper rotated
ox,oy,oz = [30]*3
states =[True, True, True] + [False]*3
cmd = list(pose) + [fx,fy,fz,ox,oy,oz] + states
self.do_force_control(cmd)
def left_contact_slide_down(self, pos, rating=1.0):
pose = list(pos) + self.rot_l
#use force control
fx = -7*rating
fy = 2*rating
fz = -2*rating
#keep gripper rotated
ox,oy,oz = [30]*3
states =[True, True, True] + [False]*3
cmd = list(pose) + [fx,fy,fz,ox,oy,oz] + states
self.do_force_control(cmd)
def right_contact_slide_right(self, pos, rating=1.0):
pose = list(pos) + self.rot_r
#use force control
fx = -2*rating
fy = -5*rating
fz = -2*rating
#keep gripper rotated
ox,oy,oz = [30]*3
states =[True, True, True] + [False]*3
cmd = list(pose) + [fx,fy,fz,ox,oy,oz] + states
self.do_force_control(cmd)
def push_down(self, pos, rating=1.0):
pose = list(pos) + self.horz
#use force control
fx = -7*rating
fy = 300*rating
fz = -3*rating
#keep gripper rotated
ox,oy,oz = [30]*3
states =[True, False, True] + [False]*3
cmd = list(pose) + [fx,fy,fz,ox,oy,oz] + states
self.do_force_control(cmd)
def push_right(self, pos, rating=1.0):
pose = list(pos) + self.vert
#use force control
fx = 400*rating
fy = -5 *rating
fz = -3*rating
#keep gripper rotated
ox,oy,oz = [30]*3
states =[False, True, True] + [False]*3
cmd = list(pose) + [fx,fy,fz,ox,oy,oz] + states
self.do_force_control(cmd)
def push_left(self, pos, rating=1.0):
pose = list(pos) + self.vert
#use force control
fx = 300*rating
fy = 5 *rating
fz = -3*rating
#keep gripper rotated
ox,oy,oz = [30]*3
states =[False, True, True] + [False]*3
cmd = list(pose) + [fx,fy,fz,ox,oy,oz] + states
self.do_force_control(cmd)
def convert_xy(self, (x,y) ):
return (x*self.block_size, y*self.block_size, self.z_down)
