def move_to_grasp_object(self,object):
"""Sets the robot's configuration so the gripper grasps object at
one of its potential grasp locations. Might change self.active_limb
to the appropriate limb.
If successful, changes self.config and returns True.
Otherwise, does not change self.config and returns False.
"""
possibleGrasps = self.knowledge.grasp_xforms(object)
oldConfig = self.robot.getConfig()
grasp = 0
for grasp in possibleGrasps: # loop until we find a working grasp
# try left arm first
self.robot.setConfig(oldConfig)
goal = ik.objective(self.left_gripper_link, R=grasp[0], t=grasp[1])
if ik.solve(goal):
worked = True
self.active_limb = "Left"
self.config = self.robot.getConfig()
return True
else:
# left arm failed, try right arm
self.robot.setConfig(oldConfig)
goal = ik.objective(self.right_gripper_link, R=grasp[0], t=grasp[1])
if ik.solve(goal):
worked = True
self.active_limb = "Right"
self.config = self.robot.getConfig()
return True
self.robot.setConfig(oldConfig)
print "IK solver failed to find a grasp."
return False
def move_camera_to_bin(self,bin_name):
"""Starts a motion so the camera has a viewpoint that
observes bin_name. Will also change self.active_limb to the
appropriate limb.
If successful, sends the motion to the low-level controller and
returns True.
Otherwise, does not modify the low-level controller and returns False.
"""
world_offset = self.knowledge.bin_vantage_point(bin_name)
#place +z in the +x axis, y in the +z axis, and x in the -y axis
left_goal = ik.objective(self.left_camera_link,R=[0,0,-1,1,0,0,0,1,0],t=world_offset)
right_goal = ik.objective(self.right_camera_link,R=[0,0,-1,1,0,0,0,1,0],t=world_offset)
qcmd = self.controller.getCommandedConfig()
old_config = [c for c in qcmd]
for i in range(100):
if random.random() < 0.5:
if i == 0:
self.robot.setConfig(qcmd)
else:
self.randomize_limb_position('left')
result = ik.solve(left_goal)
if result:
# TODO: plan a path, DONE
# change print_detail to True if you want to know what parts are colliding
if self.planner.is_in_self_collision(print_detail=False):
#print "collided"
continue
#try:
# self.planner.plan(old_config, self.robot.getConfig())
#except:
# continue
#print "Left planning done"
self.controller.setMilestone(self.robot.getConfig())
self.active_limb = 'left'
return True
else:
if i == 0:
self.robot.setConfig(qcmd)
else:
self.randomize_limb_position('right')
result = ik.solve(right_goal)
if result:
# TODO: plan a path, DONE
# change print_detail to True if you want to know what parts are colliding
if self.planner.is_in_self_collision(print_detail=False):
#print "collided"
continue
#try:
# self.planner.plan(old_config, self.robot.getConfig())
#except:
# print "Right Failed"
# continue
#print "Right planning done"
self.controller.setMilestone(self.robot.getConfig())
self.active_limb = 'right'
return True
return False
def move_to_order_bin(self,object):
"""Sets the robot's configuration so the gripper is over the order bin
If successful, changes self.config and returns True.
Otherwise, does not change self.config and returns False.
"""
#TODO: remove me
left_target = se3.apply(order_bin_xform,[0.0,0.2,order_bin_bounds[1][2]+0.1])
right_target = se3.apply(order_bin_xform,[0.0,-0.2,order_bin_bounds[1][2]+0.1])
qmin,qmax = self.robot.getJointLimits()
for i in range(100):
if self.active_limb == 'left':
q = baxter_rest_config[:]
for j in self.left_arm_indices:
q[j] = random.uniform(qmin[j],qmax[j])
#goal = ik.objective(self.left_gripper_link,local=[vectorops.sub(left_gripper_center_xform[1],[0,0,0.1]),left_gripper_center_xform[1]],world=[vectorops.add(target,[0,0,0.1]),target])
goal = ik.objective(self.left_gripper_link,local=left_gripper_center_xform[1],world=left_target)
if ik.solve(goal,tol=0.1):
self.config = self.robot.getConfig()
return True
self.config = self.robot.getConfig()
else:
q = baxter_rest_config[:]
for j in self.right_arm_indices:
q[j] = random.uniform(qmin[j],qmax[j])
#goal = ik.objective(self.right_gripper_link,local=[vectorops.sub(right_gripper_center_xform[1],[0,0,0.1]),right_gripper_center_xform[1]],world=[vectorops.add(target,[0,0,0.1]),target])
goal = ik.objective(self.right_gripper_link,local=right_gripper_center_xform[1],world=right_target)
if ik.solve(goal,tol=0.1):
self.config = self.robot.getConfig()
return True
self.config = self.robot.getConfig()
#TODO: put my code here
return False
def move_to_grasp_object(self,object):
# return True
"""Sets the robot's configuration so the gripper grasps object at
one of its potential grasp locations. Might change self.active_limb
to the appropriate limb.
If successful, changes self.config and returns True.
Otherwise, does not change self.config and returns False.
"""
#TODO: put my code here
if self.active_limb=="LEFT":
first_gripper = self.left_gripper_link
second_gripper = self.right_gripper_link
first_offset = left_gripper_center_xform
second_offset = right_gripper_center_xform
first_limb = "LEFT"
second_limb = "RIGHT"
elif self.active_limb=="RIGHT":
first_gripper = self.right_gripper_link
second_gripper = self.left_gripper_link
first_offset = right_gripper_center_xform
second_offset = left_gripper_center_xform
first_limb = "RIGHT"
second_limb = "LEFT"
else:
print "Grasping... Unrecognized active limb"
return False
item_to_world = object.xform
for grasp in object.info.grasps:
gripper_to_item = grasp.grasp_xform
for _ in xrange(100):
self.robot.setConfig(self.random_init())
(R_net, t_net) = se3.mul(item_to_world, se3.mul(gripper_to_item, se3.inv(first_offset)))
goal = ik.objective(first_gripper, R=R_net, t=t_net)
res = ik.solve(goal)
if res==True:
self.config = self.robot.getConfig()
#print "Grasp succeeded"
self.active_limb=first_limb
return True
else:
pass
#self.config = self.robot.getConfig()
self.robot.setConfig(self.random_init())
(R_net, t_net) = se3.mul(item_to_world, se3.mul(gripper_to_item, se3.inv(second_offset)))
goal = ik.objective(second_gripper, R=R_net, t=t_net)
res = ik.solve(goal)
if res==True:
self.config = self.robot.getConfig()
#print "Grasp succeeded"
self.active_limb=second_limb
return True
else:
#self.config = self.robot.getConfig()
pass
'''
def attempt_random_config(self):
"""Attempts to configure arms in a random configuration"""
goal = ik.objective(self.left_gripper_link, local=[0, 0, 0], world=[random.random(), random.random(), random.random()])
ik.solve(goal)
goal = ik.objective(self.right_gripper_link, local=[0, 0, 0], world=[random.random(), random.random(), random.random()])
ik.solve(goal)
self.config = self.robot.getConfig()
def move_camera_to_bin(self,bin_name):
"""Starts a motion so the camera has a viewpoint that
observes bin_name. Will also change self.active_limb to the
appropriate limb.
If successful, sends the motion to the low-level controller and
returns True.
Otherwise, does not modify the low-level controller and returns False.
"""
world_offset = self.knowledge.bin_vantage_point(bin_name)
#place +z in the +x axis, y in the +z axis, and x in the -y axis
left_goal = ik.objective(self.left_camera_link,R=[0,0,-1,1,0,0,0,1,0],t=world_offset)
right_goal = ik.objective(self.right_camera_link,R=[0,0,-1,1,0,0,0,1,0],t=world_offset)
qcmd = self.controller.getCommandedConfig()
s_temp = bin_name
ss = s_temp.split('_')
if ord(ss[1]) <= 70:
if ord(ss[1]) % 3 == 2:
ss[0] = 'left'
else:
ss[0] = 'right'
else:
if ord(ss[1]) % 3 == 1:
ss[0] = 'right'
else:
ss[0] = 'left'
for i in range(100):
#if random.random() < 0.5:
if i == 0:
self.robot.setConfig(qcmd)
else:
self.randomize_limb_position(ss[0])
if ss[0] == 'left':
if ik.solve(left_goal):
#TODO: plan a path
no_collision = LimbPlanner.check_collision_free(self.planner, ss[0])
if no_collision:
self.controller.setMilestone(self.robot.getConfig())
self.active_limb = ss[0]
return True
else:
if ik.solve(right_goal):
#TODO: plan a path
no_collision = LimbPlanner.check_collision_free(self.planner, ss[0])
if no_collision:
self.controller.setMilestone(self.robot.getConfig())
self.active_limb = ss[0]
return True
return False
#else:
"""
def move_to_order_bin(self,object):
"""Sets the robot's configuration so the gripper is over the order bin
If successful, changes self.config and returns True.
Otherwise, does not change self.config and returns False.
"""
#TODO: put my code here
if self.active_limb=="LEFT":
# left arm IK
(_,t) = order_bin_xform
R = [1,0,0,0,-1,0,0,0,-1]
t = [t[0], t[1], 0.7]
goal = ik.objective(self.left_gripper_link, R=R, t=t)
res = ik.solve(goal)
if res:
self.config = self.robot.getConfig()
print "Placed to order bin successfully"
return True
else:
for _ in xrange(100):
self.robot.setConfig(self.random_init())
goal = ik.objective(self.left_gripper_link, R=R, t=t)
res = ik.solve(goal)
if res:
self.config = self.robot.getConfig()
print "Placed to order bin successfully"
return True
print "Placing to order bin failed"
return False
elif self.active_limb=="RIGHT":
# right arm IK
(_,t) = order_bin_xform
R = [1,0,0,0,-1,0,0,0,-1]
t = [t[0], t[1], 0.7]
goal = ik.objective(self.right_gripper_link, R=R, t=t)
res = ik.solve(goal)
if res:
self.config = self.robot.getConfig()
print "Placed to order bin successfully"
return True
else:
for _ in xrange(100):
self.robot.setConfig(self.random_init())
goal = ik.objective(self.right_gripper_link, R=R, t=t)
res = ik.solve(goal)
if res:
self.config = self.robot.getConfig()
print "Placed to order bin successfully"
return True
print "Placing to order bin failed"
return False
else:
print "Active limb does not find"
return False
def move_to_grasp_object(self,object):
"""Sets the robot's configuration so the gripper grasps object at
one of its potential grasp locations. Might change self.active_limb
to the appropriate limb. Must change self.active_grasp to the
selected grasp.
If successful, sends the motion to the low-level controller and
returns True.
Otherwise, does not modify the low-level controller and returns False.
"""
grasps = self.knowledge.grasp_xforms(object)
qmin,qmax = self.robot.getJointLimits()
qcmd = self.controller.getCommandedConfig()
#phase 1: init IK from the commanded config, search among grasps
for (grasp,gxform) in grasps:
if self.active_limb == 'left':
Tg = se3.mul(gxform,se3.inv(left_gripper_center_xform))
goal = ik.objective(self.left_gripper_link,R=Tg[0],t=Tg[1])
else:
Tg = se3.mul(gxform,se3.inv(right_gripper_center_xform))
goal = ik.objective(self.right_gripper_link,R=Tg[0],t=Tg[1])
self.robot.setConfig(qcmd)
if ik.solve(goal):
#self.controller.setMilestone(self.robot.getConfig())
#self.active_grasp = grasp
no_collision = LimbPlanner.check_collision_free(self.planner, self.active_limb) ###
if no_collision:
self.active_grasp = grasp
self.controller.setMilestone(self.robot.getConfig())
return True
#Phase 2: that didn't work, now try random sampling
for i in range(100):
#pick a config at random
self.randomize_limb_position(self.active_limb)
#pick a grasp at random
(grasp,gxform) = random.choice(grasps)
if self.active_limb == 'left':
Tg = se3.mul(gxform,se3.inv(left_gripper_center_xform))
goal = ik.objective(self.left_gripper_link,R=Tg[0],t=Tg[1])
else:
Tg = se3.mul(gxform,se3.inv(right_gripper_center_xform))
goal = ik.objective(self.right_gripper_link,R=Tg[0],t=Tg[1])
if ik.solve(goal):
# self.active_grasp = grasp
#TODO: plan a path
no_collision = LimbPlanner.check_collision_free(self.planner, self.active_limb)
if no_collision:
self.active_grasp = grasp
self.controller.setMilestone(self.robot.getConfig())
return True
return False
def move_to_order_bin(self,object):
"""Sets the robot's configuration so the gripper is over the order bin
If successful, changes self.config and returns True.
Otherwise, does not change self.config and returns False.
"""
# find a point a little (0.5 meters) above the order bin)
aboveOrderBinLocation = se3.apply(order_bin_xform, [0, 0, 0])
aboveOrderBinLocation[2] = aboveOrderBinLocation[2] + 0.5
oldConfig = self.robot.getConfig()
# figure out what gripper is holding the object
activeGripper = self.left_gripper_link
if self.active_limb == "Right":
activeGripper = self.right_gripper_link
goal = ik.objective(activeGripper, local=[0, 0, 0], world=aboveOrderBinLocation)
if ik.solve(goal):
self.config = self.robot.getConfig();
return True
#self.robot.setConfig(oldConfig)
print "IK solver failed to find a a point above the order bin."
return False
def move_to_order_bin(self,object):
"""Sets the robot's configuration so the gripper is over the order bin
If successful, changes self.config and returns True.
Otherwise, does not change self.config and returns False.
"""
arms = { 'right': (self.right_gripper_link, right_gripper_center_xform[1]),
'left': (self.left_gripper_link, left_gripper_center_xform[1])
}
# use the active limb
try:
(link, gripper_center) = arms[self.active_limb]
except KeyError:
return False
# move the gripper to order bin top and point opposite the world's z axis
goal = ik.objective(link, R=so3.rotation([1,0,0], math.pi), t=self.knowledge.order_bin_top())
# try with 50 attempts
# randomize after each failed attempt
for attempts in range(50):
# solve the inverse kinematics with 5 cm accuracy (the bin is large)
if ik.solve(goal, tol=0.05):
self.config = goal.robot.getConfig()
return True
else:
# randomize this limb
randomize_links(goal.robot, arm_link_names[self.active_limb])
return False
def move_camera_to_bin(self,bin_name):
"""Sets the robot's configuration to a viewpoint that
observes bin_name. Will also change self.active_limb to the
appropriate limb.
If successful, changes self.config and returns True.
Otherwise, does not change self.config and returns False.
"""
# get the bin position information
front_center = self.knowledge.bin_front_center(bin_name)
vantage_point = self.knowledge.bin_vantage_point(bin_name)
for link, side in [ (self.left_camera_link, 'left'), (self.right_camera_link, 'right') ]:
# move the camera to vantage_point such that it views opposite the shelf's z axis
#goal = ik.objective(link, local=[ [0,0,0], [0,0,vantage_point_offset] ], world=[ vantage_point, front_center ])
#goal = ik.objective(link, R=so3.identity(), t=vantage_point)
goal = ik.objective(link, R=so3.mul(so3.rotation([0,0,1],math.pi), self.knowledge.shelf_xform[0]), t=vantage_point)
# reset the robot to the starting config
goal.robot.setConfig(self.config)
# try with 50 attempts
# randomize after each failed attempt
for attempts in range(50):
# solve the inverse kinematics with 1 cm accuracy
if ik.solve(goal, tol=0.01):
self.active_limb = side
self.config = goal.robot.getConfig()
return True
else:
# randomize this limb
randomize_links(goal.robot, arm_link_names[side])
return False
def move_to_order_bin(self,object):
"""Sets the robot's configuration so the gripper is over the order bin
If successful, changes self.config and returns True.
Otherwise, does not change self.config and returns False.
"""
#TODO: put my code here
#gohere
'''
bin_name = object.bin_name
pointInFrontOfBin = self.knowledge.bin_front_center(bin_name)
goal = ik.objective(self.active_limb,R=[0,1,0,0,0,1,1,0,0],t=pointInFrontOfBin)
self.config = self.robot.getConfig()
return ik.solve(goal)
'''
self.robot.setConfig(self.originalConfig)
self.config = self.originalConfig
orderBinLocalPoint = [(order_bin_bounds[0][i] + order_bin_bounds[1][i])/2 for i in range(3)]
orderBinLocalPoint[2] = .7
gripper = self.active_limb
orderBinWorldPoint = se3.apply(order_bin_xform,orderBinLocalPoint)
goal = ik.objective(gripper,R=[0,1,0,1,0,0,0,0,-1],t=orderBinWorldPoint)
iterations = 0
while iterations!=1000:
iterations+=1
if ik.solve(goal,tol=.1):
self.config = self.robot.getConfig()
return True
print "having trouble putting in order bin"
pointAroundBinWorldPoint = [orderBinWorldPoint[i] + random.random()*.1 for i in range(3)]
goal = ik.objective(gripper,R=[0,1,0,1,0,0,0,0,-1],t=pointAroundBinWorldPoint)
self.robot.setConfig(self.originalConfig)
self.config = self.originalConfig
def move_to_grasp_object(self,object):
"""Sets the robot's configuration so the gripper grasps object at
one of its potential grasp locations. Might change self.active_limb
to the appropriate limb.
If successful, changes self.config and returns True.
Otherwise, does not change self.config and returns False.
"""
#TODO: put my code here
graspToWorldTransforms = self.knowledge.grasp_xforms(object)
grasps_in_world_coords = [se3.apply(transform,[0,0,0]) for transform in graspToWorldTransforms]
if self.active_limb == self.left_camera_link:
gripper = self.left_gripper_link
print "left gripper"
if self.active_limb == self.right_camera_link:
gripper = self.left_gripper_link
print "right gripper"
goals = [ik.objective(gripper,R=graspToWorldTransforms[i][0],t=grasps_in_world_coords[i]) for i in range(len(graspToWorldTransforms))]
iterations = 0
while iterations!= 1000:
iterations += 1
for goal in goals:
if ik.solve(goal,tol=.01):
self.config = self.robot.getConfig()
return True
print "having trouble grasping"
if iterations % 500 == 0:
a = 5
goals = [ik.objective(gripper,R=graspToWorldTransforms[i][0],t=grasps_in_world_coords[i]) for i in range(len(graspToWorldTransforms))]
return False
def move_to_order_bin(self,object):
"""Sets the robot's configuration so the gripper is over the order bin
If successful, changes self.config and returns True.
Otherwise, does not change self.config and returns False.
"""
print
print 'Trying to move over order bin'
world_goal_xyz = se3.apply(order_bin_xform,[0,0,0.75])
cnt = 0;
while(cnt < 100):
#pick a random starting config for the left link and try to solve the ik problem
config = self.robot.getConfig()
for ind in self.left_arm_indices:
config[ind] = random.uniform(0,3);
self.robot.setConfig(config);
goal = ik.objective(self.left_gripper_link,local=([0,0,0]),world=(world_goal_xyz))
if ik.solve(goal,tol = 0.05): #lower tol because the bin is huge
self.active_limb = 'Left'
self.config = self.robot.getConfig()
return True
cnt = cnt + 1
return False
def move_to_grasp_object(self,object):
#Select appropriate limb and link
self.active_limb = self.limb_selector("gripper")
#Extract the contents of the bin
binContents = self.knowledge.bin_contents[self.current_bin]
#Extract the list of possible grasp points
graspList = binContents[0].info.grasps
#Loop through grasp list and attempt to solve inverse kinematics
for graspIndex in range(0, len(graspList)):
#Compute transform from grasp point to bin to world
graspTransform = graspList[graspIndex].grasp_xform
binCoordinate = se3.apply(graspTransform,[0,0,0])
worldCoordinate = se3.apply(binContents[0].xform, binCoordinate)
#Compute objective
objective = ik.objective(self.active_limb, local=[0,0,0], world=worldCoordinate)
#Attempt to solve inverse kinematics
if ik.solve(objective):
self.config = self.robot.getConfig()
return True
else:
return False
def move_to_order_bin(self,object):
"""Sets the robot's configuration so the gripper is over the order bin
If successful, sends the motion to the low-level controller and
returns True.
Otherwise, does not modify the low-level controller and returns False.
"""
left_target = se3.apply(order_bin_xform,[0.0,0.2,order_bin_bounds[1][2]+0.1])
right_target = se3.apply(order_bin_xform,[0.0,-0.2,order_bin_bounds[1][2]+0.1])
qcmd = self.controller.getCommandedConfig()
for i in range(100):
if self.active_limb == 'left':
goal = ik.objective(self.left_gripper_link,local=left_gripper_center_xform[1],world=left_target)
else:
goal = ik.objective(self.right_gripper_link,local=right_gripper_center_xform[1],world=right_target)
#set IK solver initial configuration
if i==0:
self.robot.setConfig(qcmd)
else:
self.randomize_limb_position(self.active_limb)
#solve
if ik.solve(goal,tol=0.1):
if self.planner.check_collision_free('left'):
self.controller.setMilestone(self.robot.getConfig())
self.active_limb = 'left'
return True
return False
def right_arm_ik_near_seed(self, right_target, ignore_elbow_up_constraint=True):
"""Solves IK to move the right arm to the specified
right_target ([x, y, z] in world space)
"""
self.load_real_robot_state()
self.set_model_right_arm(eval("Q_IK_SEED_" + self.current_bin))
oldRSquared = -1
q_ik = None
qmin, qmax = self.robotModel.getJointLimits()
for i in range(1000):
point2_local = vectorops.add(VACUUM_POINT_XFORM[1], [0.5, 0, 0])
point2_world = vectorops.add(right_target, [0, 0, -0.5])
goal1 = ik.objective(self.robotModel.link("right_wrist"), local=VACUUM_POINT_XFORM[1], world=right_target)
goal2 = ik.objective(self.robotModel.link("right_wrist"), local=point2_local, world=point2_world)
if ik.solve([goal1, goal2], tol=0.0001) and (self.elbow_up() or ignore_elbow_up_constraint):
q_vals = [self.robotModel.getConfig()[v] for v in self.right_arm_indices]
rSquared = vectorops.norm(vectorops.sub(q_vals, eval("Q_IK_SEED_" + self.current_bin)))
if oldRSquared < 0 or oldRSquared > rSquared:
oldRSquared = rSquared
q_ik = q_vals
print FAIL_COLOR + "right_arm_ik failed for " + str(right_target) + END_COLOR
if not (self.elbow_up() or ignore_elbow_up_constraint):
print FAIL_COLOR + str([self.robotModel.getConfig()[v] for v in self.right_arm_indices]) + END_COLOR
print FAIL_COLOR + "IK found but elbow wasn't up" + END_COLOR
if oldRSquared >= 0:
self.set_model_right_arm(q_ik)
return True
return False
def move_camera_to_bin(self,bin):
# Camera rotation and translation relative to world frame
R_camera = [0,0,-1, 1,0,0, 0,1,0]
t_camera = knowledge.bin_vantage_point(bin)
# Setup ik objectives for both arms
left_goal = ik.objective(self.left_camera_link, R=R_camera, t=t_camera)
right_goal = ik.objective(self.right_camera_link, R=R_camera, t=t_camera)
# Joint Limits
qmin,qmax = self.robot.getJointLimits()
for i in range(100):
# initialize to the resting pose
# NOTE: that this (without [:]) doesn't work because the list
# must be copied by value, not reference.
# See python shallow copy vs. deep copy for more details
# q = baxter_rest_config
q = baxter_rest_config[:]
# use left hand
if random.random() < 0.5:
# randomly initialize each joint in the arm within joint limits
for jointIndex in self.left_arm_indices:
q[jointIndex] = random.uniform(qmin[jointIndex], qmax[jointIndex])
self.robot.setConfig(q)
# attempt to solve ik objective
if ik.solve(left_goal):
self.config = self.robot.getConfig()
self.active_limb = 'left'
return True
# use right hand
else:
# randomly initialize each joint in the arm within joint limits
for jointIndex in self.right_arm_indices:
q[jointIndex] = random.uniform(qmin[jointIndex], qmax[jointIndex])
self.robot.setConfig(q)
# attempt to solve ik objective
if ik.solve(right_goal):
self.config = self.robot.getConfig()
self.active_limb = 'right'
return True
return False
def move_to_order_bin(self,object):
# apply the order_bin_xform to the given point
left_target_point = se3.apply(order_bin_xform, [0.0, 0.2, order_bin_bounds[1][2]+0.1])
right_target_point = se3.apply(order_bin_xform, [0.0, -0.2, order_bin_bounds[1][2]+0.1])
# Setup ik objectives for both arms
left_goal = ik.objective(self.left_gripper_link, local=left_gripper_center_xform[1], world=left_target_point)
right_goal = ik.objective(self.right_gripper_link, local=right_gripper_center_xform[1], world=right_target_point)
qmin, qmax = self.robot.getJointLimits()
for i in range(100):
# initialize to the resting pose
# NOTE: that this (without [:]) doesn't work because the list
# must be copied by value, not reference.
# See python shallow copy vs. deep copy for more details
# ( q = baxter_rest_config ) doesn't work!
q = baxter_rest_config[:]
# use left hand
if self.active_limb == 'left':
# randomly initialize each joint in the arm within joint limits
for jointIndex in self.left_arm_indices:
q[jointIndex] = random.uniform(qmin[jointIndex], qmax[jointIndex])
self.robot.setConfig(q)
# attempt to solve ik objective
if ik.solve(left_goal):
self.config = self.robot.getConfig()
return True
# use right hand
else:
# randomly initialize each joint in the arm within joint limits
for jointIndex in self.right_arm_indices:
q[jointIndex] = random.uniform(qmin[jointIndex], qmax[jointIndex])
self.robot.setConfig(q)
# attempt to solve ik objective
if ik.solve(right_goal):
self.config = self.robot.getConfig()
return True
self.config = self.robot.getConfig()
return False
def move_camera_to_bin(self,bin_name):
"""Sets the robot's configuration to a viewpoint that
observes bin_name. Will also change self.active_limb to the
appropriate limb.
If successful, changes self.config and returns True.
Otherwise, does not change self.config and returns False.
"""
# Store current config in case we need to reset
originalConfig = self.robot.getConfig()
randomAttempts = 0
while randomAttempts < self.maxRandomAttempts:
# Try goal with left camera first
leftGoal = ik.objective(self.left_camera_link, local=[[0, 0, 0], [0, 0, -1]], world=[self.knowledge.bin_vantage_point(bin_name), [self.knowledge.bin_vantage_point(bin_name)[0] - 1, self.knowledge.bin_vantage_point(bin_name)[1], self.knowledge.bin_vantage_point(bin_name)[2]]])
if ik.solve(leftGoal):
self.active_limb = "left"
self.config = self.robot.getConfig()
return True
else:
# Restore previous config
self.robot.setConfig(originalConfig)
# Try goal with right camera
rightGoal = ik.objective(self.right_camera_link, local=[[0, 0, 0], [0, 0, -1]], world=[self.knowledge.bin_vantage_point(bin_name), [self.knowledge.bin_vantage_point(bin_name)[0] - 1, self.knowledge.bin_vantage_point(bin_name)[1], self.knowledge.bin_vantage_point(bin_name)[2]]])
if ik.solve(rightGoal):
self.active_limb = "right"
self.config = self.robot.getConfig()
return True
# Reset from a random position
self.attempt_random_config()
randomAttempts += 1
# Restore previous config
self.robot.setConfig(originalConfig)
return False
def move_to_grasp_object(self,object):
"""Sets the robot's configuration so the gripper grasps object at
one of its potential grasp locations. Might change self.active_limb
to the appropriate limb.
If successful, changes self.config and returns True.
Otherwise, does not change self.config and returns False.
"""
# Store current config in case we need to reset
originalConfig = self.robot.getConfig()
randomAttempts = 0
while randomAttempts < self.maxRandomAttempts:
# Loop through all potential grasps trying to solve with the active limb
for grasp in self.knowledge.grasp_xforms(object):
objectGraspLocation = se3.apply(object.xform, grasp[1])
# Construct world points for axes alignment
objectGraspXPoint = se3.apply(object.xform, se3.apply(grasp, [1, 0, 0]))
objectGraspYPoint = se3.apply(object.xform, se3.apply(grasp, [0, 1, 0]))
objectGraspZPoint = se3.apply(object.xform, se3.apply(grasp, [0, 0, 1]))
if self.active_limb == "left":
# Construct local points for axes alignment
localXPoint = [left_gripper_center_xform[1][0] + 1, left_gripper_center_xform[1][1], left_gripper_center_xform[1][2]]
localYPoint = [left_gripper_center_xform[1][0], left_gripper_center_xform[1][1] + 1, left_gripper_center_xform[1][2]]
localZPoint = [left_gripper_center_xform[1][0], left_gripper_center_xform[1][1], left_gripper_center_xform[1][2] + 1]
goal = ik.objective(self.left_gripper_link, local=[left_gripper_center_xform[1], localXPoint, localYPoint, localZPoint], world=[objectGraspLocation, objectGraspXPoint, objectGraspYPoint, objectGraspZPoint])
else:
# Construct local points for axes alignment
localXPoint = [right_gripper_center_xform[1][0] + 1, right_gripper_center_xform[1][1], right_gripper_center_xform[1][2]]
localYPoint = [right_gripper_center_xform[1][0], right_gripper_center_xform[1][1] + 1, right_gripper_center_xform[1][2]]
localZPoint = [right_gripper_center_xform[1][0], right_gripper_center_xform[1][1], right_gripper_center_xform[1][2] + 1]
goal = ik.objective(self.right_gripper_link, local=[right_gripper_center_xform[1], localXPoint, localYPoint, localZPoint], world=[objectGraspLocation, objectGraspXPoint, objectGraspYPoint, objectGraspZPoint])
if ik.solve(goal):
self.config = self.robot.getConfig()
return True
else:
# Restore previous config
self.robot.setConfig(originalConfig)
# Reset from a random position
self.attempt_random_config()
randomAttempts += 1
# Restore previous config
self.robot.setConfig(originalConfig)
return False
def prob(self):
self.active_limb = self.left_camera_link
for goal in range(1,150):
rand_pos = [random.randrange(0,2),random.randrange(0,2),random.randrange(0,2)]
objective = ik.objective(self.active_limb, local=[0,0,0], world=rand_pos)
if ik.solve(objective):
self.config = self.robot.getConfig()
break
if goal == 150:
return True
return False
def move_camera_to_bin(self,bin_name):
"""Starts a motion so the camera has a viewpoint that
observes bin_name. Will also change self.active_limb to the
appropriate limb.
If successful, sends the motion to the low-level controller and
returns True.
Otherwise, does not modify the low-level controller and returns False.
"""
world_offset = self.knowledge.bin_vantage_point(bin_name)
start_config = self.robot.getConfig()
#place +z in the +x axis, y in the +z axis, and x in the -y axis
left_goal = ik.objective(self.left_camera_link,R=[0,0,-1,1,0,0,0,1,0],t=world_offset)
right_goal = ik.objective(self.right_camera_link,R=[0,0,-1,1,0,0,0,1,0],t=world_offset)
qcmd = self.controller.getCommandedConfig()
for i in range(100):
if random.random() < 0.5:
if i == 0:
self.robot.setConfig(qcmd)
else:
self.randomize_limb_position('left')
if ik.solve(left_goal):
if self.planner.check_collision_free('left'):
self.controller.setMilestone(self.robot.getConfig())
#self.controller.setMilestone(self.planner.plan(start_config, self.robot.getConfig()))
self.active_limb = 'left'
return True
else:
if i == 0:
self.robot.setConfig(qcmd)
else:
self.randomize_limb_position('right')
if ik.solve(right_goal):
if self.planner.check_collision_free('right'):
self.controller.setMilestone(self.robot.getConfig())
#self.controller.setMilestone(self.planner.plan(start_config, self.robot.getConfig()))
self.active_limb = 'right'
return True
return False
def iksolve(self, config, kEE, pEE, mq):
goal = ik.objective(kEE,local=self.hand_position, world=config)
q = self.setKlamptPos()
pEE.positionCommand(pEE.configFromKlampt(q))
if ik.solve(goal):
print "success!"
q = self.kRobot.getConfig()
mq.setRamp(pEE.configFromKlampt(q))
while(mq.moving()):
time.sleep(0.1)
else:
print "failed. Residual:", ik.solver(goal).getResidual()
def move_camera_to_bin(self,bin_name):
"""Sets the robot's configuration to a viewpoint that
observes bin_name. Will also change self.active_limb to the
appropriate limb.
If successful, changes self.config and returns True.
Otherwise, does not change self.config and returns False.
"""
world_offset = self.knowledge.bin_vantage_point(bin_name)
#place +z in the +x axis, y in the +z axis, and x in the -y axis
left_goal = ik.objective(self.left_camera_link,R=[0,0,-1,1,0,0,0,1,0],t=world_offset)
right_goal = ik.objective(self.right_camera_link,R=[0,0,-1,1,0,0,0,1,0],t=world_offset)
qmin,qmax = self.robot.getJointLimits()
for i in range(100):
if random.random() < 0.5:
q = baxter_rest_config[:]
for j in self.left_arm_indices:
q[j] = random.uniform(qmin[j],qmax[j])
self.robot.setConfig(q)
if ik.solve(left_goal):
self.config = self.robot.getConfig()
self.active_limb = 'left'
return True
else:
q = baxter_rest_config[:]
for j in self.right_arm_indices:
q[j] = random.uniform(qmin[j],qmax[j])
self.robot.setConfig(q)
if ik.solve(right_goal):
self.config = self.robot.getConfig()
self.active_limb = 'right'
return True
return False
#TODO: put my code here
#Hint:
#goal = ik.objective(self.left_camera_link,R=[local-to-world rotation],t=[local-to-world translation])
#if ik.solve(goal):
# the robot model's configuration is changed... but you still have to update the state of the controller.
return False
def move_to_order_bin(self,object):
"""Sets the robot's configuration so the gripper is over the order bin
If successful, changes self.config and returns True.
Otherwise, does not change self.config and returns False.
"""
self.robot.setConfig(baxter_rest_config)
target = se3.apply(order_bin_xform, [0,0,.7])
if(ik.solve(ik.objective(self.active_limb,R=[0,1,0,1,0,0,0,0,-1],t=target))):
self.config = self.robot.getConfig()
return True
self.config = self.robot.getConfig()
return False
def move_to_grasp_object(self,object):
"""Sets the robot's configuration so the gripper grasps object at
one of its potential grasp locations. Might change self.active_limb
to the appropriate limb.
If successful, changes self.config and returns True.
Otherwise, does not change self.config and returns False.
"""
#TODO: remove me
grasps = self.knowledge.grasp_xforms(object)
qmin,qmax = self.robot.getJointLimits()
for i in range(100):
if self.active_limb == 'left':
q = baxter_rest_config[:]
for j in self.left_arm_indices:
q[j] = random.uniform(qmin[j],qmax[j])
g = random.choice(grasps)
#Tg = se3.mul(g,se3.inv(left_gripper_center_xform))
#goal = ik.objective(self.left_gripper_link,R=Tg[0],t=Tg[1])
goal = ik.objective(self.left_gripper_link,local=left_gripper_center_xform[1],world=g[1])
if ik.solve(goal):
self.config = self.robot.getConfig()
return True
else:
q = baxter_rest_config[:]
for j in self.right_arm_indices:
q[j] = random.uniform(qmin[j],qmax[j])
g = random.choice(grasps)
#Tg = se3.mul(g,se3.inv(right_gripper_center_xform))
#goal = ik.objective(self.right_gripper_link,R=Tg[0],t=Tg[1])
goal = ik.objective(self.right_gripper_link,local=right_gripper_center_xform[1],world=g[1])
if ik.solve(goal):
self.config = self.robot.getConfig()
return True
#TODO: put my code here
return False
def move_to_order_bin(self,object):
"""Sets the robot's configuration so the gripper is over the order bin
If successful, changes self.config and returns True.
Otherwise, does not change self.config and returns False.
"""
self.active_limb = self.left_camera_link
order_vantage = self.knowledge.order_vantage()
objective = ik.objective(self.active_limb, local=[0,0,0], world=order_vantage)
if ik.solve(objective):
self.config = self.robot.getConfig()
return True
else:
return False
def move_to_grasp_object(self,object):
"""Sets the robot's configuration so the gripper grasps object at
one of its potential grasp locations. Might change self.active_limb
to the appropriate limb.
If successful, changes self.config and returns True.
Otherwise, does not change self.config and returns False.
"""
# get list of possible grasps
poss = self.knowledge.grasp_xforms(object=object)
target = [se3.apply(transform,[0,0,0]) for transform in poss]
for i in range(len(target)):
self.robot.setConfig(baxter_rest_config)
if(ik.solve(ik.objective(self.active_limb,R=poss[i][0],t=target[i]))):
self.config=self.robot.getConfig()
return True
return False
def move_camera_to_bin(self,bin_name):
#Select appropriate limb and link
self.active_limb = self.limb_selector("camera")
#Calculate camera vantage point
vantagePoint = self.knowledge.bin_vantage_point(bin_name)
#Generate objective
objective = ik.objective(self.active_limb, R=[0, -1, 0, 0, 0, -1, 1, 0, 0], t=vantagePoint)
#Attempt to solve the inverse kinematics
if ik.solve(objective):
self.config = self.robot.getConfig()
return True
else:
return False
