def UniformSamples(self, env, translational_resolution,
rotational_resolution):
samples = []
for Tw_e, Bw in izip(self.Tw_e_list, self.Bw_list):
Xs = self.SampleUniformGrid(Bw[0], Bw[1], translational_resolution)
y = 0.
z = 0.
# FIXME for now just sample from x and pitch. The others do not make sense/needed.
#roll = random.uniform(Bw[6], Bw[7])*np.array([1.,0.,0.])
Rolls = self.SampleUniformGrid(Bw[6], Bw[7], rotational_resolution)
Pitchs = self.SampleUniformGrid(Bw[8], Bw[9],
rotational_resolution)
#yaw = random.uniform(Bw[10], Bw[11])*np.array([0.,0.,1.])
Yaws = self.SampleUniformGrid(Bw[10], Bw[11],
rotational_resolution)
for x, pitch, yaw, roll in product(Xs, Pitchs, Yaws, Rolls):
s = np.eye(4)
pitch = pitch * np.array([0., 1., 0.])
yaw = yaw * np.array([0., 0., 1.])
roll = roll * np.array([1., 0., 0.])
s[0, 3] = x
s[1, 3] = y
s[2, 3] = z
s[:3, :3] = np.dot(
np.dot(orpy.rotationMatrixFromAxisAngle(roll),
orpy.rotationMatrixFromAxisAngle(pitch)),
orpy.rotationMatrixFromAxisAngle(yaw))
s = np.dot(Tw_e, s)
samples.append(s)
return samples
def PoseFromXYAndYaw(youbot, pt2d, yaw):
pose = np.eye(4)
pose[:2, 3] = pt2d
pose[2, 3] = youbot.GetTransform()[2, 3] # use current height of robot.
pose[:3, :3] = orpy.rotationMatrixFromAxisAngle(
np.array([0., 0., 1.]) * yaw)
return pose
def XYThetaToMatrix(x, y, theta):
pose = np.eye(4)
pose[:3, :3] = orpy.rotationMatrixFromAxisAngle(theta *
np.array([0., 0., 1.]))
pose[0, 3] = x
pose[1, 3] = y
return pose
def put_left_arm_over_tray(robot, tray):
"""Move the robot's left arm so that it's about to grasp the right edge
of the tray. Raises a generate_reaching_poses.GraspingPoseError if no
IK solution can be found.
Parameters:
robot: a Robot instance
tray: a KinBody instance
"""
manip = robot.GetManipulator('leftarm')
ikmodel = openravepy.databases.inversekinematics.InverseKinematicsModel(
robot,iktype=openravepy.IkParameterization.Type.Transform6D)
if not ikmodel.load():
ikmodel.autogenerate()
min_x, max_x, min_y, max_y, z = utils.get_object_limits(tray)
z -= 0.06
x = min_x + (max_x - min_x)/2
y = max_y - 0.02
gripper_angle = (np.pi, 0., 0) #pointing downward
rot_mat = openravepy.rotationMatrixFromAxisAngle(gripper_angle)
T = np.eye(4)
T[:3,:3] = rot_mat
T[:3,3] = [x,y,z]
sol = generate_reaching_poses.check_reachable(env, tray, manip, [T], False)
if sol is not None:
robot.SetDOFValues(sol, manip.GetArmIndices())
#opening gripper
robot.SetDOFValues([0.2], manip.GetGripperJoints())
else:
raise generate_reaching_poses.GraspingPoseError("No IK solution for tray right side")
def drop(self, obj, table):
pos1 = [0.4, -0.7, 1.1]
rot_z = [0.7071, 0, 0, -0.7071]
rot_x = [0, 1, 0, 0]
rot = openravepy.quatMult(rot_z, rot_x).tolist()
traj1, _ = self.traj_generator.traj_from_pose(pos1, rot)
with self.env:
# saving values
orig_values = self.robot.GetDOFValues(
self.robot.GetManipulator('rightarm').GetArmIndices())
self.robot.SetDOFValues(traj1[-1],
self.robot.GetManipulator('rightarm').GetArmIndices())
pos2 = [0.0, -0.7, 1.0]
traj2, _ = self.traj_generator.traj_from_pose(pos2, rot)
# reset
self.robot.SetDOFValues(orig_values,
self.robot.GetManipulator('rightarm').GetArmIndices())
self._execute_traj(traj1.tolist() + traj2.tolist())
# open gripper
self.robot.Release(obj)
if self.use_ros:
self.pr2.rgrip.open()
# transforming the object
T = obj.GetTransform()
rot_angle = (np.pi / 2., 0., 0) #got this from the model
rot_mat = openravepy.rotationMatrixFromAxisAngle(rot_angle)
T[:3, :3] = rot_mat
_, _, _, _, z = utils.get_object_limits(table)
T[2, 3] = z
obj.SetTransform(T)
def placeontray(self, unused1, obj_name, unused2, tray_name, unused4):
"""Put an item on the tray.
"""
tray = self.env.GetKinBody(tray_name)
if tray is None:
raise ValueError("Object %s does not exist" % tray_name)
obj = self.env.GetKinBody(obj_name)
if obj is None:
raise ValueError("Object %s does not exist" % obj_name)
if (not len(self.tray_stack) == 0 and
not tray_world.can_stack(self.tray_stack[-1], obj)):
e = ExecutingException("Incompatible objects")
e.robot = self.robot
e.object_to_grasp = obj
e.stacktop = self.tray_stack[-1]
raise e
T = tray_world.tray_putdown_pose(tray, self.tray_stack)
try:
(pose,
sol,
torso_angle) = generate_reaching_poses.get_collision_free_ik_pose(
self.robot,
T,
)
except generate_reaching_poses.GraspingPoseError:
raise ExecutingException("Putting down on tray has problems!")
self.pause("Going to the tray")
self.robot.SetTransform(pose)
self.pause("Arm/Torso in position")
self.robot.SetDOFValues([torso_angle],
[self.robot.GetJointIndex('torso_lift_joint')])
self.robot.SetDOFValues(sol,
self.robot.GetActiveManipulator().GetArmIndices())
print "Releasing object"
self.robot.Release(obj)
self.tray_stack.append(obj)
#putting the object straight
rot_angle = (np.pi / 2., 0., 0) #got this from the model
rot_mat = openravepy.rotationMatrixFromAxisAngle(rot_angle)
T[:3, :3] = rot_mat
T[2,3] -= 0.05
obj.SetTransform(T)
try:
del self.grasping_locations_cache[obj_name]
except KeyError:
print "funny, object ", obj_name, " was not in cache.. something wrong here!"
print "cache is: ", self.grasping_locations_cache
raw_input("Press return to continue")
print "Back to rest"
utils.pr2_tuck_arm(self.robot)
print "The tray now has: ", self.tray_stack
def update_robot(self, name, new):
with env:
joint = self.robot.GetJoint(name)
if joint:
self.robot.SetDOFValues([new],[joint.GetDOFIndex()])
elif name.startswith("tf"):
T = np.eye(4)
T[:3,:3] = rave.rotationMatrixFromAxisAngle([self.tfrx, self.tfry, self.tfrz])
T[:3,3] = [self.tftx, self.tfty, self.tftz]
self.robot.SetTransform(T)
print "joints:", self.robot.GetDOFValues()
print "tf:", self.robot.GetTransform()
print "active dof", self.robot.GetActiveDOFValues()
def update_robot(self, name, new):
with env:
joint = self.robot.GetJoint(name)
if joint:
self.robot.SetDOFValues([new],[joint.GetDOFIndex()])
elif name.startswith("tf"):
T = np.eye(4)
T[:3,:3] = rave.rotationMatrixFromAxisAngle([self.tfrx, self.tfry, self.tfrz])
T[:3,3] = [self.tftx, self.tfty, self.tftz]
self.robot.SetTransform(T)
print "joints:", self.robot.GetDOFValues()
print "tf:", self.robot.GetTransform()
print "active dof", self.robot.GetActiveDOFValues()
def pitcher_grasp(robot, pitcher, push_distance=0.1, manip=None, **kw_args):
'''
@param robot The robot performing the grasp
@param pitcher The pitcher to grasp
@param manip The manipulator to perform the grasp, if None
the active manipulator on the robot is used
'''
if manip is None:
manip_idx = robot.GetActiveManipulatorIndex()
else:
manip.SetActive()
manip_idx = manip.GetRobot().GetActiveManipulatorIndex()
T0_w = pitcher.GetTransform()
spout_in_pitcher = numpy.array([[-0.7956, 0.6057, 0., -0.0662],
[-0.6057, -0.7956, 0., -0.0504],
[0., 0., 1., 0.2376], [0., 0., 0., 1.]])
# we want a hand pose orthogonal to the direction of the spout
spout_direction = numpy.arctan2(spout_in_pitcher[0, 1],
spout_in_pitcher[0, 0])
palm_direction = spout_direction - 0.5 * numpy.pi
ee_in_pitcher = numpy.eye(4)
ee_in_pitcher[:3, :3] = numpy.array([[0., 0., 1.], [-1., 0., 0.],
[0., -1., 0.]])
ee_in_pitcher[:3, :3] = numpy.dot(
ee_in_pitcher[:3, :3],
openravepy.rotationMatrixFromAxisAngle([0, palm_direction, 0]))
pitcher_extents = [0.07877473, 0.06568845, 0.11882638]
offset = pitcher_extents[
0] + 0.18 + push_distance # pitcher radius + ee_offset
ee_in_pitcher[:2, 3] = -offset * ee_in_pitcher[:2, 2]
ee_in_pitcher[2, 3] = 0.45 * pitcher_extents[2]
Bw = numpy.zeros((6, 2))
Bw[2, :] = [-0.01, 0.01] # Allow a little vertical movement
grasp_tsr = prpy.tsr.TSR(T0_w=T0_w,
Tw_e=ee_in_pitcher,
Bw=Bw,
manip=manip_idx)
grasp_chain = prpy.tsr.TSRChain(sample_start=False,
sample_goal=True,
constrain=False,
TSR=grasp_tsr)
return [grasp_chain]
def UniformSamples(self,translational_resolution,rotational_resolution):
samples = []
for Tw_e,Bw in izip(self.Tw_e_list,self.Bw_list):
Xs = self.SampleUniformGrid(Bw[0],Bw[1],translational_resolution)
y = 0.
z = 0.
#roll = random.uniform(Bw[6], Bw[7])*np.array([1.,0.,0.])
Rolls = self.SampleUniformGrid(Bw[6],Bw[7],rotational_resolution)
Pitchs = self.SampleUniformGrid(Bw[8],Bw[9],rotational_resolution)
#yaw = random.uniform(Bw[10], Bw[11])*np.array([0.,0.,1.])
Yaws = self.SampleUniformGrid(Bw[10],Bw[11],rotational_resolution)
for x,pitch,yaw,roll in product(Xs,Pitchs,Yaws,Rolls):
s = np.eye(4)
pitch = pitch*np.array([0.,1.,0.])
yaw = yaw*np.array([0.,0.,1.])
roll = roll*np.array([1.,0.,0.])
s[0,3] = x
s[1,3] = y
s[2,3] = z
s[:3,:3] = np.dot(np.dot(orpy.rotationMatrixFromAxisAngle(roll),orpy.rotationMatrixFromAxisAngle(pitch)),orpy.rotationMatrixFromAxisAngle(yaw))
s = np.dot(Tw_e,s)
samples.append(s)
return samples
def SampleObjectRelative(self, env):
# FIXME The sampling is not exactly uniform right now. Different line
# of grasps can have different lengths.
n_grasp_lines = len(self.Tw_e_list)
random_line = random.randint(0, n_grasp_lines - 1)
Tw_e = self.Tw_e_list[random_line]
Bw = self.Bw_list[random_line]
randoffset = np.eye(4)
xoffset = random.uniform(Bw[0], Bw[1])
yoffset = random.uniform(Bw[2], Bw[3])
zoffset = random.uniform(Bw[4], Bw[5])
rolloffset = random.uniform(Bw[6], Bw[7]) * np.array([1., 0., 0.])
pitchoffset = random.uniform(Bw[8], Bw[9]) * np.array([0., 1., 0.])
yawoffset = random.uniform(Bw[10], Bw[11]) * np.array([0., 0., 1.])
randoffset[0, 3] = xoffset
randoffset[1, 3] = yoffset
randoffset[2, 3] = zoffset
randoffset[:3, :3] = np.dot(
np.dot(orpy.rotationMatrixFromAxisAngle(rolloffset),
orpy.rotationMatrixFromAxisAngle(pitchoffset)),
orpy.rotationMatrixFromAxisAngle(yawoffset))
sample = np.dot(Tw_e, randoffset)
return sample
def visualizeTSR(env,params,yname,nsamples=20):
# This iterates only for drc1
for i in range(nsamples):
sample = GetGoal(env,params,yname)
if sample is None:
print 'No valid sample.'
return
env.GetRobot(yname).GetController().Reset(0)
pose = np.eye(4)
pose[:3,:3] = orpy.rotationMatrixFromAxisAngle(sample[7]*np.array([0.,0.,1.]))
pose[:2,3] = sample[5:7]
env.GetRobot(yname).SetTransform(pose)
env.GetRobot(yname).SetDOFValues(sample[:5], [0,1,2,3,4])
#IPython.embed()
time.sleep(0.01)
def GetFeasibleGoal(self, env, params, youbots, grabdict, timeout=100.0):
starttime = time.time()
while (time.time() - starttime) < timeout:
with env:
basesamples = {}
armsamples = {}
feasible = True
for y in youbots:
basesample, armsample = self.GetGoal(
env, params, y, timeout)
if basesample is None:
feasible = False
print 'no goal for %s' % (y)
break
pose = np.eye(4)
pose[:3, :3] = orpy.rotationMatrixFromAxisAngle(
basesample[2] * np.array([0., 0., 1.]))
pose[:2, 3] = basesample[:2]
youbots[y].SetTransform(pose)
youbots[y].SetDOFValues(armsample[:5], [0, 1, 2, 3, 4])
basesamples[y] = basesample
armsamples[y] = armsample
env.UpdatePublishedBodies() # TODO remove for timing
if not feasible:
break
try:
for y in grabdict:
grabobjs = grabdict[y]
for grabobj in grabobjs:
youbots[y].Grab(grabobj)
for y in youbots:
collision = env.CheckCollision(youbots[y])
if collision:
feasible = False
#time.sleep(0.1)
break
finally:
for y in grabdict:
grabobjs = grabdict[y]
for grabobj in grabobjs:
youbots[y].Release(grabobj)
if feasible:
return basesamples, armsamples
return None, None
def tray_putdown_pose(tray, stack_of_items = None):
"""Returns a position right above the tray with the gripper pointing down.
"""
T = tray.GetTransform()
gripper_angle = (np.pi, 0., 0) #pointing downward
rot_mat = openravepy.rotationMatrixFromAxisAngle(gripper_angle)
T[:3,:3] = rot_mat
#fixing the height
correction = 0.1
if stack_of_items is not None:
correction += get_stack_height(stack_of_items)
T[2,3] += correction
return T
def visualizeTSR(env, params, yname, nsamples=20):
# This iterates only for drc1
for i in range(nsamples):
sample = GetGoal(env, params, yname)
if sample is None:
print 'No valid sample.'
return
env.GetRobot(yname).GetController().Reset(0)
pose = np.eye(4)
pose[:3, :3] = orpy.rotationMatrixFromAxisAngle(sample[7] *
np.array([0., 0., 1.]))
pose[:2, 3] = sample[5:7]
env.GetRobot(yname).SetTransform(pose)
env.GetRobot(yname).SetDOFValues(sample[:5], [0, 1, 2, 3, 4])
#IPython.embed()
time.sleep(0.01)
def putdown(self, obj_name, table_name, _unused1):
print "Putting down object %s on %s" % (obj_name, table_name)
obj = self.env.GetKinBody(obj_name)
if obj is None:
raise ValueError("Object %s does not exist" % obj_name)
if table_name.startswith("dest_"):
#this is a fixed location!!!
T = getattr(tray_world, table_name, None)
if T is None:
raise ValueError(
"The location %s is unknown! check spelling?" % table_name)
T = T.copy()
#put the gripper facing down
gripper_angle = (np.pi, 0., 0) #just got this from trial and test
rot_mat = openravepy.rotationMatrixFromAxisAngle(gripper_angle)
T[:3, :3] = rot_mat
T[2, 3] += 0.03
try:
(pose, sol, torso_angle
) = generate_reaching_poses.get_collision_free_ik_pose(
self.getGoodBodies(),
self.env,
obj,
self.robot,
T,
only_reachable=False,
max_trials=1000)
except generate_reaching_poses.GraspingPoseError:
raise ExecutingException(
"Putting down on location has problems!")
else:
table = self.env.GetKinBody(table_name)
if table is None:
raise ValueError("Object %s does not exist" % table_name)
try:
pose, sol, torso_angle = generate_reaching_poses.get_collision_free_surface_pose(
self.getGoodBodies(),
self.robot,
table,
)
except generate_reaching_poses.GraspingPoseError, e:
raise e
def putdown(self, obj_name, table_name, _unused1):
print "Putting down object %s on %s" %(obj_name, table_name)
obj = self.env.GetKinBody(obj_name)
if obj is None:
raise ValueError("Object %s does not exist" % obj_name)
if table_name.startswith("dest_"):
#this is a fixed location!!!
T = getattr(tray_world, table_name, None)
if T is None:
raise ValueError("The location %s is unknown! check spelling?" % table_name)
T = T.copy()
#put the gripper facing down
gripper_angle = (np.pi, 0., 0) #just got this from trial and test
rot_mat = openravepy.rotationMatrixFromAxisAngle(gripper_angle)
T[:3,:3] = rot_mat
T[2, 3] += 0.03
try:
(pose,
sol,
torso_angle) = generate_reaching_poses.get_collision_free_ik_pose(
self.getGoodBodies(),
self.env,
obj,
self.robot,
T,
only_reachable=False,
max_trials =1000
)
except generate_reaching_poses.GraspingPoseError:
raise ExecutingException("Putting down on location has problems!")
else:
table = self.env.GetKinBody(table_name)
if table is None:
raise ValueError("Object %s does not exist" % table_name)
try:
pose, sol, torso_angle = generate_reaching_poses.get_collision_free_surface_pose(self.getGoodBodies(),
self.robot,
table,
)
except generate_reaching_poses.GraspingPoseError, e:
raise e
def GetFeasibleGoal(self,env,params,youbots,grabdict,timeout=100.0):
starttime = time.time()
while (time.time() - starttime) < timeout:
with env:
basesamples = {}
armsamples = {}
feasible = True
for y in youbots:
basesample,armsample = self.GetGoal(env,params,y,timeout)
if basesample is None:
feasible = False
print 'no goal for %s'%(y)
break
pose = np.eye(4)
pose[:3,:3] = orpy.rotationMatrixFromAxisAngle(basesample[2]*np.array([0.,0.,1.]))
pose[:2,3] = basesample[:2]
youbots[y].SetTransform(pose)
youbots[y].SetDOFValues(armsample[:5],[0,1,2,3,4])
basesamples[y] = basesample
armsamples[y] = armsample
env.UpdatePublishedBodies() # TODO remove for timing
if not feasible:
break
try:
for y in grabdict:
grabobjs = grabdict[y]
for grabobj in grabobjs:
youbots[y].Grab(grabobj)
for y in youbots:
collision = env.CheckCollision(youbots[y])
if collision:
feasible = False
#time.sleep(0.1)
break
finally:
for y in grabdict:
grabobjs = grabdict[y]
for grabobj in grabobjs:
youbots[y].Release(grabobj)
if feasible:
return basesamples,armsamples
return None,None
def pitcher_grasp(robot, pitcher, push_distance=0.1, manip=None, **kw_args):
'''
@param robot The robot performing the grasp
@param pitcher The pitcher to grasp
@param manip The manipulator to perform the grasp, if None
the active manipulator on the robot is used
'''
if manip is None:
manip_idx = robot.GetActiveManipulatorIndex()
else:
manip.SetActive()
manip_idx = manip.GetRobot().GetActiveManipulatorIndex()
T0_w = pitcher.GetTransform()
spout_in_pitcher = numpy.array([[-0.7956, 0.6057, 0., -0.0662],
[-0.6057, -0.7956, 0., -0.0504],
[0., 0., 1., 0.2376],
[0., 0., 0., 1.]])
# we want a hand pose orthogonal to the direction of the spout
spout_direction = numpy.arctan2(spout_in_pitcher[0,1], spout_in_pitcher[0,0])
palm_direction = spout_direction - 0.5*numpy.pi
ee_in_pitcher = numpy.eye(4)
ee_in_pitcher[:3,:3] = numpy.array([[0., 0., 1.],
[-1., 0., 0.],
[0., -1., 0.]])
ee_in_pitcher[:3,:3] = numpy.dot(ee_in_pitcher[:3,:3],
openravepy.rotationMatrixFromAxisAngle([0, palm_direction, 0]))
pitcher_extents = [0.07877473,0.06568845,0.11882638]
offset = pitcher_extents[0] + 0.18 + push_distance # pitcher radius + ee_offset
ee_in_pitcher[:2,3] = -offset*ee_in_pitcher[:2,2]
ee_in_pitcher[2,3] = 0.45*pitcher_extents[2]
Bw = numpy.zeros((6,2))
Bw[2,:] = [-0.01, 0.01] # Allow a little vertical movement
grasp_tsr = prpy.tsr.TSR(T0_w = T0_w, Tw_e = ee_in_pitcher, Bw = Bw, manip = manip_idx)
grasp_chain = prpy.tsr.TSRChain(sample_start=False, sample_goal = True,
constrain=False, TSR = grasp_tsr)
return [grasp_chain]
def test_position(manip, env, gripper_angle = None):
if gripper_angle is None:
gripper_angle = (np.deg2rad(180), np.deg2rad(0), np.deg2rad(0))
rot_mat = openravepy.rotationMatrixFromAxisAngle(gripper_angle)
T = manip.GetEndEffectorTransform()
T[:3,:3] = rot_mat
p2 = np.array([0.1, 0, 0, 1]);
arrow = env.drawarrow(np.dot(T,p1), np.dot(T, p2), 0.01, np.array([1.0,0,0]))
sol = manip.FindIKSolution(T,
openravepy.IkFilterOptions.IgnoreEndEffectorCollisions)
if sol is not None:
robot.SetDOFValues(sol, robot.GetActiveManipulator().GetArmIndices())
else:
print "NO SOLUTION"
return arrow
def SampleObjectRelative(self):
# FIXME The sampling is not exactly uniform right now. Different line
# of grasps can have different lengths.
n_grasp_lines = len(self.Tw_e_list)
random_line = random.randint(0,n_grasp_lines-1)
Tw_e = self.Tw_e_list[random_line]
Bw = self.Bw_list[random_line]
randoffset = np.eye(4)
xoffset = random.uniform(Bw[0], Bw[1])
yoffset = random.uniform(Bw[2], Bw[3])
zoffset = random.uniform(Bw[4], Bw[5])
rolloffset = random.uniform(Bw[6], Bw[7])*np.array([1.,0.,0.])
pitchoffset = random.uniform(Bw[8], Bw[9])*np.array([0.,1.,0.])
yawoffset = random.uniform(Bw[10], Bw[11])*np.array([0.,0.,1.])
randoffset[0,3] = xoffset
randoffset[1,3] = yoffset
randoffset[2,3] = zoffset
randoffset[:3,:3] = np.dot(np.dot(orpy.rotationMatrixFromAxisAngle(rolloffset),orpy.rotationMatrixFromAxisAngle(pitchoffset)),orpy.rotationMatrixFromAxisAngle(yawoffset))
sample = np.dot(Tw_e,randoffset)
return sample
def generate_manip_above_surface(obj, num_poses = 20):
gripper_angle = (np.pi, 0., 0) #just got this from trial and test
rot_mat = openravepy.rotationMatrixFromAxisAngle(gripper_angle)
min_x, max_x, min_y, max_y, z = utils.get_object_limits(obj)
gripper_height = 0.18
z += gripper_height + 0.03
poses = []
for _ in range(num_poses):
x = np.random.uniform(min_x, max_x)
y = np.random.uniform(min_y, max_y)
T = np.eye(4)
T[:3,:3] = rot_mat
T[:3,3] = [x,y,z]
poses.append(T)
return poses
def openrave_sim(self):
# rospy.init_node('baxter_openrave')
env = self.env
env.StopSimulation()
#add this robot
# env.Add(baxter_robot)
cameraRot = orpy.rotationMatrixFromAxisAngle([-0.472792, -0.713688, 0.516833], 2.5656)
Tstart = np.concatenate([cameraRot, [np.zeros(3)]], axis=0)
Tstart = np.concatenate([Tstart, [[0.94666], [0.350879], [1.65288], [1]]], axis=1)
# Tstart=np.array([ [ 0.9983323 , 0.03651881, 0.04471023, -1.427967],
# [-0.02196539, -0.47593777, 0.87920462, 2.333576],
# [ 0.0533868 , -0.87872044, -0.47434189, 2.153447],
# [ 0. , 0. , 0. , 1. ]])
env.SetViewer('qtcoin')
env.GetViewer().SetCamera(Tstart,
#focalDistance=0.826495
)
#set initial posture
self.set_manipulator_jnt_vals(self.writing_start_jnt_)
self.pentip_handle = self.openrave_draw_pen_barrel(self.pentip_handle)
return
goals = {}
wingskin_goal = np.dot(ladder.GetTransform(), params['wingskin_in_ladder'])
wingskin.SetTransform(wingskin_goal)
starttime = time.time()
for i in range(1000):
with env:
samples = {}
feasible = True
for y in youbots:
sample = GetGoal(env,params,y)
if sample is None:
feasible = False
print 'no goal for %s'%(y)
break
pose = np.eye(4)
pose[:3,:3] = orpy.rotationMatrixFromAxisAngle(sample[7]*np.array([0.,0.,1.]))
pose[:2,3] = sample[5:7]
youbots[y].SetTransform(pose)
youbots[y].SetDOFValues(sample[:5],[0,1,2,3,4])
samples[y] = sample
env.UpdatePublishedBodies()
if not feasible:
break
try:
drc1.Grab(wingskin)
for y in youbots:
collision = env.CheckCollision(youbots[y])
if collision:
def placeontray(self, unused1, obj_name, unused2, tray_name, unused4):
"""Put an item on the tray.
"""
tray = self.env.GetKinBody(tray_name)
if tray is None:
raise ValueError("Object %s does not exist" % tray_name)
obj = self.env.GetKinBody(obj_name)
if obj is None:
raise ValueError("Object %s does not exist" % obj_name)
if (not len(self.tray_stack) == 0
and not tray_world.can_stack(self.tray_stack[-1], obj)):
e = ExecutingException("Incompatible objects")
e.robot = self.robot
e.object_to_grasp = obj
e.stacktop = self.tray_stack[-1]
raise e
T = tray_world.tray_putdown_pose(tray, self.tray_stack)
try:
(pose, sol,
torso_angle) = generate_reaching_poses.get_collision_free_ik_pose(
self.getGoodBodies(),
self.env,
obj,
self.robot,
T,
)
except generate_reaching_poses.GraspingPoseError:
raise ExecutingException("Putting down on tray has problems!")
self.pause("Going to the tray")
self.robot.SetTransform(pose)
self.pause("Arm/Torso in position")
self.robot.SetDOFValues([torso_angle],
[self.robot.GetJointIndex('torso_lift_joint')])
self.robot.SetDOFValues(
sol,
self.robot.GetActiveManipulator().GetArmIndices())
print "Releasing object"
self.robot.Release(obj)
self.tray_stack.append(obj)
#putting the object straight
rot_angle = (np.pi / 2., 0., 0) #got this from the model
rot_mat = openravepy.rotationMatrixFromAxisAngle(rot_angle)
T[:3, :3] = rot_mat
T[2, 3] -= 0.05
obj.SetTransform(T)
try:
del self.grasping_locations_cache[obj_name]
except KeyError:
print "funny, object ", obj_name, " was not in cache.. something wrong here!"
print "cache is: ", self.grasping_locations_cache
raw_input("Press return to continue")
print "Back to rest"
utils.pr2_tuck_arm(self.robot)
print "The tray now has: ", self.tray_stack
class Executor(object):
def __init__(self, robot, viewer=False):
self.robot = robot
self.env = robot.GetEnv()
self.grasping_locations_cache = {}
self.viewMode = viewer
self.tray_stack = []
self.unMovableObjects = ['table']
self.objSequenceInPlan = []
self.handled_objs = set()
v = self.robot.GetActiveDOFValues()
v[self.robot.GetJoint('torso_lift_joint').GetDOFIndex()] = 1.0
self.robot.SetDOFValues(v)
if use_ros:
self.pr2robot = PR2Robot(self.env)
self.pr2robot.robot = self.robot
self.arm_mover = PR2MoveArm()
#loading the IK models
utils.pr2_tuck_arm(robot)
robot.SetActiveManipulator('leftarm')
ikmodel = openravepy.databases.inversekinematics.InverseKinematicsModel(
robot, iktype=openravepy.IkParameterization.Type.Transform6D)
if not ikmodel.load():
ikmodel.autogenerate()
robot.SetActiveManipulator('rightarm')
ikmodel = openravepy.databases.inversekinematics.InverseKinematicsModel(
robot, iktype=openravepy.IkParameterization.Type.Transform6D)
if not ikmodel.load():
ikmodel.autogenerate()
def getGoodBodies(self):
if not doJointInterpretation:
body_filter = lambda b: b.GetName().startswith("random") or\
b.GetName().startswith('object')
else:
futureObjects = set(self.objSequenceInPlan) - self.handled_objs
body_filter = lambda b: (b.GetName() not in futureObjects) \
and (b.GetName() not in self.unMovableObjects)
return filter(body_filter, self.env.GetBodies())
def setObjSequenceInPlan(self, objList):
self.objSequenceInPlan = objList
print
print "Will try to pick objs in the order " + repr(objList)
def clear_gp_cache(self):
self.grasping_locations_cache = {}
self.objSequenceInPlan = []
def pause(self, msg=None):
if self.viewMode:
if msg is None:
raw_input("Press return to continue")
else:
print msg
#time.sleep(0.5)
raw_input(msg + "... [press return]")
def moveto(self, _unused1, pose):
if type(pose) is str:
print "Pose ", pose, " ignored"
return
else:
print "Moving to pose "
self.robot.SetTransform(pose)
def movetowithinr1(self, _unused1, unused2):
print "Ignore me!"
def movetowithinr2(self, _unused1, unused2):
print "ignore me !"
# def movetoacrossrooms(self, _unused1, unused2, unused):
# print "ignore me !"
def movetoacrossrooms(self, _unused1, unused2):
print "ignore me !"
def grasp(self, obj_name, _unused1, _unused2):
# update openrave
if use_ros:
self.pr2robot.update_rave()
print "Grasping object ", obj_name
obj = self.env.GetKinBody(obj_name)
if obj is None:
raise ValueError("Object %s does not exist" % obj_name)
cached_value = self.grasping_locations_cache.get(obj_name, None)
if cached_value is None:
print "Object %s is not cached, looking for a value" % obj_name
try:
pose, sol, torso_angle = generate_reaching_poses.get_collision_free_grasping_pose(
self.getGoodBodies(),
self.robot,
obj,
max_trials=collision_free_grasping_samples)
self.grasping_locations_cache[
obj_name] = pose, sol, torso_angle
except generate_reaching_poses.GraspingPoseError:
e = ExecutingException("Object in collision")
e.robot = self.robot
e.object_to_grasp = obj
raise e
else:
print "Object %s already cached" % obj_name
pose, sol, torso_angle = cached_value
if OpenRavePlanning:
test_grasp_move.move_arm(robot, pose, 'r')
if use_ros:
# # object matching
detector, _ = detector_and_cluster_map
# index = cluster_map[obj_name]
# # generating grasps
# box_msg = detector.detect_bounding_box(detector.last_detection_msg.detection.clusters[index])
# desired_grasps = grasp_generator.generate_grasps(box_msg, 8)
# grasp_generator.draw_grasps(desired_grasps)
# object matching
detector.detect()
box_msgs = []
for cluster in detector.last_detection_msg.detection.clusters:
box_msgs.append(detector.detect_bounding_box(cluster))
box_msg, index = utils.find_nearest_box(obj, box_msgs)
# generating grasps
desired_grasps = grasp_generator.generate_grasps(box_msg, 16)
grasp_generator.draw_grasps(desired_grasps)
# pickup
grabber = object_pickup.Grabber()
processing_reply = detector.call_collision_map_processing(
detector.last_detection_msg)
# # reset planning scene
self.arm_mover.reset_collision_map()
self.arm_mover.update_planning_scene()
res = grabber.pickup_object(
processing_reply.graspable_objects[index],
processing_reply.collision_object_names[index],
processing_reply.collision_support_surface_name,
'right_arm',
desired_grasps=desired_grasps,
lift_desired_distance=0.3)
if res is None:
e = ExecutingException("ROS pickup failed")
e.robot = self.robot
e.object_to_grasp = obj
raise e
# update openrave
if use_ros:
self.pr2robot.update_rave()
else:
self.pause("Moving to location")
self.robot.SetTransform(pose)
self.pause("Moving arm")
self.robot.SetDOFValues(
[torso_angle], [self.robot.GetJointIndex('torso_lift_joint')])
self.robot.SetDOFValues(
sol,
self.robot.GetActiveManipulator().GetArmIndices())
self.robot.Grab(obj)
def putdown(self, obj_name, table_name, _unused1):
print "Putting down object %s on %s" % (obj_name, table_name)
obj = self.env.GetKinBody(obj_name)
if obj is None:
raise ValueError("Object %s does not exist" % obj_name)
if table_name.startswith("dest_"):
#this is a fixed location!!!
T = getattr(tray_world, table_name, None)
if T is None:
raise ValueError(
"The location %s is unknown! check spelling?" % table_name)
T = T.copy()
#put the gripper facing down
gripper_angle = (np.pi, 0., 0) #just got this from trial and test
rot_mat = openravepy.rotationMatrixFromAxisAngle(gripper_angle)
T[:3, :3] = rot_mat
T[2, 3] += 0.03
try:
(pose, sol, torso_angle
) = generate_reaching_poses.get_collision_free_ik_pose(
self.getGoodBodies(),
self.env,
obj,
self.robot,
T,
only_reachable=False,
max_trials=1000)
except generate_reaching_poses.GraspingPoseError:
raise ExecutingException(
"Putting down on location has problems!")
else:
table = self.env.GetKinBody(table_name)
if table is None:
raise ValueError("Object %s does not exist" % table_name)
try:
pose, sol, torso_angle = generate_reaching_poses.get_collision_free_surface_pose(
self.getGoodBodies(),
self.robot,
table,
)
except generate_reaching_poses.GraspingPoseError, e:
raise e
if use_ros:
# Move arm to drop location
p = (0.3, -0.5, 0.3)
q = transformations.quaternion_from_euler(0, 0, -numpy.pi / 2)
res = self.arm_mover.move_right_arm(p, q, '/torso_lift_link', 30)
if not res:
e = ExecutingException("ROS putdown step 1 failed")
e.robot = self.robot
e.object_to_grasp = obj
raise e
p = (0.1, -0.8, -0.2)
q = transformations.quaternion_from_euler(0, 0, -numpy.pi / 2)
res = self.arm_mover.move_right_arm(p, q, '/torso_lift_link', 30)
if not res:
e = ExecutingException("ROS putdown step 2 failed")
e.robot = self.robot
e.object_to_grasp = obj
raise e
# Drop object
joint_mover = PR2JointMover()
joint_mover.open_right_gripper(True)
# update openrave
if use_ros:
self.pr2robot.update_rave()
else:
self.pause("Moving to location")
self.robot.SetTransform(pose)
self.pause("Moving arm")
self.robot.SetDOFValues(
[torso_angle], [self.robot.GetJointIndex('torso_lift_joint')])
self.robot.SetDOFValues(
sol,
self.robot.GetActiveManipulator().GetArmIndices())
self.robot.Release(obj)
#putting the object straight
if table_name.startswith("dest_"):
print "Putting object in the right location"
T = getattr(tray_world, table_name, None)
else:
T = obj.GetTransform()
rot_angle = (np.pi / 2., 0., 0) #got this from the model
rot_mat = openravepy.rotationMatrixFromAxisAngle(rot_angle)
T[:3, :3] = rot_mat
_, _, _, _, z = utils.get_object_limits(table)
T[2, 3] = z
obj.SetTransform(T)
try:
del self.grasping_locations_cache[obj_name]
except KeyError:
print "funny, object ", obj_name, " was not in cache.. something wrong here!"
raw_input("Press return to continue")
goals = {}
wingskin_goal = np.dot(ladder.GetTransform(), params['wingskin_in_ladder'])
wingskin.SetTransform(wingskin_goal)
starttime = time.time()
for i in range(1000):
with env:
samples = {}
feasible = True
for y in youbots:
sample = GetGoal(env, params, y)
if sample is None:
feasible = False
print 'no goal for %s' % (y)
break
pose = np.eye(4)
pose[:3, :3] = orpy.rotationMatrixFromAxisAngle(
sample[7] * np.array([0., 0., 1.]))
pose[:2, 3] = sample[5:7]
youbots[y].SetTransform(pose)
youbots[y].SetDOFValues(sample[:5], [0, 1, 2, 3, 4])
samples[y] = sample
env.UpdatePublishedBodies()
if not feasible:
break
try:
drc1.Grab(wingskin)
for y in youbots:
collision = env.CheckCollision(youbots[y])
if collision:
IPython.lib.inputhook.set_inputhook(viewer.Step)
plot_handles = []
door = env.GetKinBody("door")
handle = door.GetLink("handle")
draw_ax(handle.GetGlobalMassFrame(), .1, env, plot_handles)
viewer.Idle()
door.SetDOFValues([np.pi / 3], [1])
angles = np.linspace(0, -np.pi * .4, 10)
T_world_handle = np.eye(4)
T_world_handle[:3, 3] += [0.06, -.39120 + .02, .952 - .04]
T_world_handle[:3, :3] = rave.rotationMatrixFromAxisAngle([1, 0, 0], np.pi / 3)
T_world_hinge = np.eye(4)
T_world_hinge[:3, 3] += door.GetJoint("doorhinge").GetAnchor()
T_hinge_handle = np.linalg.inv(T_world_hinge).dot(T_world_handle)
hmats = [
T_world_hinge.dot(rave.matrixFromAxisAngle([0, 0, 1],
a)).dot(T_hinge_handle)
for a in angles
]
request = pose_traj_request(robot, hmats)
s = json.dumps(request)
print "REQUEST:", s
trajoptpy.SetInteractive(True)
prob = trajoptpy.ConstructProblem(s, env)
def set_base_conf(body, base_values):
trans = body.GetTransform()
trans[:3, :3] = rotationMatrixFromAxisAngle(
np.array([0, 0, base_values[-1]])) # matrixFromAxisAngle
trans[:2, 3] = base_values[:2]
body.SetTransform(trans)
env, robot = herbpy.initialize(sim=True, attach_viewer=args.viewer)
push_arm = robot.right_arm
grasp_arm = robot.left_arm
# Put the hand in a preshape for pushing
push_arm.hand.MoveHand(spread=0, f1=0.75, f2=0.75, f3=0.75)
# Table
import os
table_path = os.path.join('objects', 'table.kinbody.xml')
table = env.ReadKinBodyXMLFile(table_path)
import numpy, openravepy
table_transform = numpy.eye(4)
table_transform[:3, :3] = openravepy.rotationMatrixFromAxisAngle([1.20919958, 1.20919958, 1.20919958])
table_transform[:3, 3] = [0.65, 0.0, 0.0]
table.SetTransform(table_transform)
env.Add(table)
# Glass
glass_path = os.path.join('objects', 'plastic_glass.kinbody.xml')
glass = env.ReadKinBodyXMLFile(glass_path)
env.Add(glass)
glass_transform = numpy.eye(4)
glass_transform[:3, 3] = [0.6239455840637041, -0.4013916328109689, 0.75]
glass.SetTransform(glass_transform)
# Goal pose for the object
import numpy
goal_in_table = [0.15, 0., -0.03, 1.]
IPython.lib.inputhook.set_inputhook(viewer.Step)
plot_handles = []
door = env.GetKinBody("door")
handle = door.GetLink("handle")
draw_ax(handle.GetGlobalMassFrame(), .1, env, plot_handles)
viewer.Idle()
door.SetDOFValues([np.pi/3], [1])
angles = np.linspace(0, -np.pi*.4, 10)
T_world_handle = np.eye(4);
T_world_handle[:3,3] += [0.06, -.39120+.02, .952-.04];
T_world_handle[:3,:3] = rave.rotationMatrixFromAxisAngle([1,0,0], np.pi/3)
T_world_hinge = np.eye(4);
T_world_hinge[:3,3] += door.GetJoint("doorhinge").GetAnchor()
T_hinge_handle = np.linalg.inv(T_world_hinge).dot(T_world_handle)
hmats = [T_world_hinge.dot(rave.matrixFromAxisAngle([0,0,1],a)).dot(T_hinge_handle) for a in angles]
request = pose_traj_request(robot, hmats)
s = json.dumps(request)
print "REQUEST:",s
trajoptpy.SetInteractive(True);
prob = trajoptpy.ConstructProblem(s, env)
result = trajoptpy.OptimizeProblem(prob)
traj = result.GetTraj()
traj_samples = range(10)
def pose_to_tf(pose): tf = np.eye(4) tf[:3,:3] = rave.rotationMatrixFromAxisAngle(np.array(pose[3:])) tf[:3,3] = pose[:3] return tf
def PoseFromXYAndYaw(youbot,pt2d,yaw):
pose = np.eye(4)
pose[:2,3] = pt2d
pose[2,3] = youbot.GetTransform()[2,3] # use current height of robot.
pose[:3,:3] = orpy.rotationMatrixFromAxisAngle(np.array([0.,0.,1.])*yaw)
return pose
env, robot = herbpy.initialize(sim=True, attach_viewer=args.viewer)
push_arm = robot.right_arm
grasp_arm = robot.left_arm
# Put the hand in a preshape for pushing
push_arm.hand.MoveHand(spread=0, f1=0.75, f2=0.75, f3=0.75)
# Table
import os
table_path = os.path.join('objects', 'table.kinbody.xml')
table = env.ReadKinBodyXMLFile(table_path)
import numpy, openravepy
table_transform = numpy.eye(4)
table_transform[:3,:3] = openravepy.rotationMatrixFromAxisAngle([1.20919958, 1.20919958, 1.20919958])
table_transform[:3,3] = [0.65, 0.0, 0.0]
table.SetTransform(table_transform)
env.Add(table)
# Glass
glass_path = os.path.join('objects', 'plastic_glass.kinbody.xml')
glass = env.ReadKinBodyXMLFile(glass_path)
env.Add(glass)
glass_transform = numpy.eye(4)
glass_transform[:3,3] = [0.6239455840637041, -0.4013916328109689, 0.75]
glass.SetTransform(glass_transform)
# Goal pose for the object
import numpy
goal_in_table = [0.15, 0., -0.03, 1.]
