def MoveForwardHandConfig(env, r, n_steps=1):
robot = youbots[r]
solns = []
rn = range(n_steps)
for step in rn:
with env:
with robot:
transform = robot.GetManipulators()[0].GetEndEffectorTransform(
)
vec = np.zeros((3, 1))
vec[2] = BACKUP_AMT * (float(step + 1) / float(n_steps))
diff = np.dot(transform[:-1, :-1], vec)
transform2 = copy.deepcopy(transform)
transform2[0, 3] += diff[0]
transform2[1, 3] += diff[1]
transform2[2, 3] += diff[2]
#blah = orpy.misc.DrawAxes(env,transform2,0.25,2.0)
sol = yik.FindIKSolutions(robot, transform2)
#IPython.embed()
if len(sol) > 0:
print 'Found solution for move forward ', vec[2]
target_arm = GetClosestArm(robot.GetDOFValues()[0:5], sol)
if n_steps == 1:
return target_arm
else:
solns.append(target_arm)
else:
if n_steps == 1:
return None
else:
continue
return solns
def BackupHandConfigInZ(env, r, n_steps=1):
robot = youbots[r]
solns = []
rn = range(n_steps)
rn.reverse()
for step in rn:
with env:
with robot:
transform = robot.GetManipulators()[0].GetEndEffectorTransform(
)
z = BACKUP_AMT * (float(step + 1) / float(n_steps))
transform2 = copy.deepcopy(transform)
transform2[2, 3] += z
sol = yik.FindIKSolutions(robot, transform2)
if len(sol) > 0:
print 'Found solution for back up in z ', z
target_arm = GetClosestArm(robot.GetDOFValues()[0:5], sol)
if n_steps == 1:
return target_arm
else:
solns.append(target_arm)
else:
if n_steps == 1:
return None
else:
continue
return solns
def FindNearbyIKSolution(robot,ee_pose):
sol = yik.FindIKSolutions(robot, ee_pose)
if len(sol) > 0:
current_arm = robot.GetDOFValues()[0:5]
target_arm = GetClosestArm(current_arm, sol)
# Check if the change is too big per joint (and translation) and warn.
for i in range(5):
if abs(current_arm[i]-target_arm[i]) > np.pi/36.0:
print 'FindNearbyIK: Change in joint ',i,' to big: ',abs(current_arm[i]-target_arm[i])
return None
print 'FindNearbyIK: Found nearby ik'
return target_arm
else:
print 'FindNearbyIK: Failed to find any ik solution.'
return None
def CheckConstraint2(self, world_config, grasps, part_responsibilities):
self.ngraspstried += 1 # TODO remove
with WorldConfig(self.env, world_config):
base_configs = {}
arm_configs = {}
for part in part_responsibilities:
robot_name = part_responsibilities[part]
base_configs[robot_name] = self.SampleBaseConfig(
self.env.GetKinBody(part).GetTransform()[:2, 3])
arm_configs[robot_name] = np.zeros(5)
if not self.IsConfigFeasible(base_configs, arm_configs):
print 'Bases collide!'
return None, None
self.nconfigstried += 1 # TODO remove
with RobotsConfig(self.env, base_configs, arm_configs, range(5)):
for each in grasps:
robot_name = part_responsibilities[each]
eepose = np.dot(
self.env.GetKinBody(each).GetTransform(), grasps[each])
#eepose = grasps[each]
stt = time.time() # TODO remove
iksolns = yik.FindIKSolutions(self.youbots[robot_name],
eepose,
checkenvcollision=False)
self.iktimes.append(time.time() - stt) # TODO remove
if len(iksolns) < 1:
print 'No ik soln'
return None, None
else:
arm_configs[robot_name] = iksolns[random.randint(
0,
len(iksolns) - 1)]
if not self.IsConfigFeasible(base_configs, arm_configs,
part_responsibilities):
print 'ik soln collide'
return None, None
return base_configs, arm_configs
def Execute(env, youbots, robot_name, op_name, assembly, config, planners,
robot_base_homes):
##EnableGripperLinks(env,robot_name,False)
#extract_plan(robot_name, op_name, config)
# Move home
# MoveBaseTo(youbots[robot_name],robot_base_homes[robot_name],planners[robot_name])
if op_name == 'regrasp' or op_name == 'regrasp_release':
print 'REGRASP!'
MoveArmTo(youbots[robot_name], np.array([0., 0., 0., 0., 0.]),
planners[robot_name])
new_ee_pose_in_assembly = np.dot(
assembly.GetPartPoseInAssembly(config.part_grasp.part_name),
config.part_grasp.grasp)
regrasped_asm_in_gripper = np.linalg.inv(new_ee_pose_in_assembly)
Regrasp(env, youbots[robot_name], assembly, regrasped_asm_in_gripper)
# Move to config
#Go 5 cm away from target
#IPython.embed()
if not (op_name.startswith('fastener')):
#raw_input('Move to backed up pose')
with env:
with youbots[robot_name]:
#h = []
old_arm = copy.deepcopy(config.arm_config)
dofs = youbots[robot_name].GetDOFValues()
dofs[0:5] = old_arm
youbots[robot_name].SetDOFValues(dofs)
youbots[robot_name].SetTransform(config.base_config)
EnableGrabbedObjects(env, robot_name, False)
EnableGripperLinks(env, robot_name, False)
if op_name == 'transfer' or op_name == 'transfer_release':
backed_up = BackupHandConfigInZ(env,
robot_name,
n_steps=20)
else:
backed_up = BackupHandConfig(env, robot_name, n_steps=20)
EnableGripperLinks(env, robot_name, True)
EnableGrabbedObjects(env, robot_name, True)
if not (backed_up is None) and not (len(backed_up) == 0):
#IPython.embed()
#floor_pose = env.GetKinBody('floor').GetTransform()
#floor_pose[2,3] += 0.015
#env.GetKinBody('floor').SetTransform(floor_pose)
MoveArmTo(youbots[robot_name], backed_up[0], planners[robot_name])
#floor_pose[2,3] -= 0.015
#env.GetKinBody('floor').SetTransform(floor_pose)
#raw_input('Move to actual pose')
#if 'rightside' == config.part_grasp.part_name:
# EnableGripperLinks(env,robot_name,False)
MoveBaseTo(youbots[robot_name], config.base_config,
planners[robot_name])
#time.sleep(0.3)
#MoveBaseTo(youbots[robot_name],config.base_config,planners[robot_name])
#time.sleep(0.3)
#MoveBaseTo(youbots[robot_name],config.base_config,planners[robot_name])
adjust = False
if adjust: # TODO not working yet.
raw_input('Adjust end-effector pose.')
target_pose = np.dot(
env.GetTransform(config.part_grasp.part_name),
config.part_grasp.grasp)
target_backed_up_pose = target_pose.copy()
target_backed_up_pose[:3, 3] = target_backed_up_pose[:3, 3] - (
BACKUP_AMT * target_backed_up_pose[:3, 2])
MoveEndEffectorTo(youbots[robot_name],
youbotenv.youbot_hands[robot_name],
target_backed_up_pose, planners[robot_name])
#if 'rightside' == config.part_grasp.part_name:
# EnableGripperLinks(env,robot_name,True)
#raw_input('jerk forward 5cm')
EnableGripperLinks(env, robot_name, False)
EnableGrabbedObjects(env, robot_name, False)
env.GetKinBody(config.part_grasp.part_name).Enable(False)
if adjust:
forward_configs = MoveForwardHandConfig(env,
robot_name,
n_steps=20)
backed_up = forward_configs
print 'BEFORE for b in backedup'
for b in backed_up[1:]:
print 'IN for b in backedup'
MoveArmTo(youbots[robot_name], b, planners[robot_name])
if not adjust: # FIXME do we need this?
MoveArmTo(youbots[robot_name], config.arm_config,
planners[robot_name])
env.GetKinBody(config.part_grasp.part_name).Enable(True)
EnableGrabbedObjects(env, robot_name, True)
EnableGripperLinks(env, robot_name, True)
#IPython.embed()
else:
#raise Exception('Cannot go to backed up pose.')
raw_input('Move to totally final pose')
MoveArmTo(youbots[robot_name], config.arm_config,
planners[robot_name])
env.GetKinBody(config.part_grasp.part_name).Enable(False)
MoveBaseTo(youbots[robot_name], config.base_config,
planners[robot_name])
env.GetKinBody(config.part_grasp.part_name).Enable(True)
else:
raw_input('Move to totally final pose')
MoveArmTo(youbots[robot_name], config.arm_config, planners[robot_name])
env.GetKinBody(config.part_grasp.part_name).Enable(False)
MoveBaseTo(youbots[robot_name], config.base_config,
planners[robot_name])
env.GetKinBody(config.part_grasp.part_name).Enable(True)
##EnableGripperLinks(env,robot_name,True)
if op_name == 'grasp':
if 'fastener' in config.part_grasp.part_name:
raw_input('Ready to grasp')
time.sleep(3.0)
#raw_input('hit enter')
GrabAssembly(env, robot_name, assembly, True)
if not ('fastener' in config.part_grasp.part_name):
xspace = np.linspace(0., 0.05, 20)
yspace = np.linspace(0., 0.05, 20)
xyspace = list(
np.transpose([
np.tile(xspace, len(yspace)),
np.repeat(yspace, len(xspace))
]))
for xy in xyspace:
lift_amt = 0.02
transform = youbots[robot_name].GetManipulators(
)[0].GetEndEffectorTransform()
transform[2, 3] += lift_amt
transform[0, 3] += xy[0]
transform[1, 3] += xy[1]
sol = yik.FindIKSolutions(youbots[robot_name], transform)
if len(sol) > 0:
target_arm = GetClosestArm(
youbots[robot_name].GetDOFValues()[0:5], sol)
MoveArmTo(youbots[robot_name], target_arm,
planners[robot_name])
break
else:
print 'Lift attempt failed'
def file_to_commands(filename):
idx = 0
with open(filename) as f:
for line in f:
print 'idx', idx
IPython.embed()
idx += 1
if line == '-\n':
#End of spline
idx = 0
continue
point = point_from_string(line)
if idx == 1:
#move base to position
#TODO: Raise arm
MoveArmTo(robot, np.array([0., 0., 0., 0., 0.]), planners[r])
#TODO: who cares about theta?
"""
current = robot.GetTransform()
yaw = np.arctan2(current[1,0],current[0,0])
MoveBaseTo(robot,np.array([point[0], point[1], yaw]),planners[r],skip_waypoints=False)
#lower_arm
"""
MoveArmTo(robot,start_config,planners[r])
#Finally, move the arm to the appropriate height
current_pose = get_relative_pose(getEndEffector())
current_pose[3, 2] += point[2]
sols = yik.FindIKSolutions(robot, get_global_pose(current_pose))
current_arm = robot.GetDOFValues()[0:5]
sol = GetClosestArm(current_arm, sols)
MoveArmTo(robot, sol, planners[r])
else:
#First, rotate to point
point = point_from_string(line)
robot_pos = robot.GetTransform()[0:2, -1]
robot_dir = robot.GetTransform()[0:2, 0]
robot_dir_self = [1., 0.]
xy1 = robot_dir
xy2 = point[0:-1] - prev_point[0:-1]
#angle = np.arccos( np.dot(xy1, xy2) / (np.linalg.norm(xy1, 2) * np.linalg.norm(xy2, 2)) )
print 'tilt!'
#move(0., 0., angle)
yaw = np.arctan2(xy2[1],-xy2[0]) #not sure I understand the geometry but x should be negative here
#this offset is needed because we're rotating around the base now, not the hand.
htb = (getEndEffector() - robot.GetTransform())[:-2, -1]
htb_dist = np.linalg.norm(htb, 2)
#current_angle = np.arctan2(robot_dir[1], robot_dir[0])
#angle_diff = -robot_pos - (-yaw)
offset_x = np.cos(-yaw) * htb_dist
offset_y = np.sin(-yaw) * htb_dist
#IPython.embed()
MoveBaseTo(robot,np.array([prev_point[0] - offset_x, prev_point[1] - offset_y, -yaw]),planners[r],skip_waypoints=False)
#angle2 = np.arccos( np.dot(robot_dir_self, xy2) / (np.linalg.norm(robot_dir_self, 2) * np.linalg.norm(xy2, 2)) )
#now we need to rotate this onto the xz plane
robot_dir = robot.GetTransform()[0:2, 0]
angle2 = np.arctan2(robot_dir[1], robot_dir[0])
#rot_mat = tr.rotation_matrix(angle2, np.array([0., 0., 1.]))[:-1, :-1] #just get the rotation part
rot_mat = tr.rotation_matrix(angle2, np.array([0., 0., 1.]))[:-1, :-1] #just get the rotation part
xz_diff = rot_mat.dot(prev_point - point)
print xz_diff
xz_diff[1] = 0. #clamp away numerical error
xz_diff[2] *= -1
if xz_diff[0] > 0:
xz_diff[0] *= -1 #make suer it's negative
#And now move the arm by this amount
#IPython.embed()
MoveStraight(0.3, xz_diff)
#Back up by the amount we moved and move the arm back into position
dist = np.linalg.norm(point - prev_point, 2)
robot_pos = robot.GetTransform()[0:2, -1]
tar = -dist * robot_dir + robot_pos
#IPython.embed()
MoveBaseTo(robot,np.array([tar[0], tar[1], -yaw]),planners[r],skip_waypoints=False)
#move(-dist, 0., 0.) #finally, back up
#Arm back into correct x position:
current_pose = getEndEffector()
current_pose[0, 0:2] +=dist * robot_dir #keep the height but move it back
sols = yik.FindIKSolutions(robot, current_pose)
current_arm = robot.GetDOFValues()[0:5]
sol = GetClosestArm(start_config, sols)
#IPython.embed()
MoveArmTo(robot, sol, planners[r])
prev_point = point #bookkeeping
def moveArmWrapper():
pose = get_relative_pose(getEndEffector())
#pose[0, 3] += 0.01
arm = yik.FindIKSolutions(robot, get_global_pose(pose))
print arm
MoveArmTo(robot, arm, planners[r])
raw_input('Ready to grasp')
time.sleep(3.0)
GrabAssembly(env,robot_name,assembly,True)
if not ('fastener' in config.part_grasp.part_name):
<<<<<<< .mine
xspace = np.linspace(0.,0.05,20)
yspace = np.linspace(0.,0.05,20)
xyspace = list(np.transpose([np.tile(xspace, len(yspace)), np.repeat(yspace, len(xspace))]))
for xy in xyspace:
lift_amt = 0.02
transform = youbots[robot_name].GetManipulators()[0].GetEndEffectorTransform()
transform[2, 3] += lift_amt
transform[0, 3] += xy[0]
transform[1, 3] += xy[1]
sol = yik.FindIKSolutions(youbots[robot_name], transform)
if len(sol) > 0:
target_arm = GetClosestArm(youbots[robot_name].GetDOFValues()[0:5], sol)
MoveArmTo(youbots[robot_name],target_arm,planners[robot_name])
break
else:
print 'Lift attempt failed'
if use_boots:
boot_name = "boot_" + config.part_grasp.part_name
if 'seat' in boot_name or 'back' in boot_name:
neg_y_boot = 1.0 * env.GetKinBody(boot_name).GetTransform()[:3,0]
else:
neg_y_boot = -1.0 * env.GetKinBody(boot_name).GetTransform()[:3,1]
boot_name = "boot_" + config.part_grasp.part_name