def continuousSolve(self,
itrs=1000,
auto=False,
extra=[],
show=True,
quitonsolve=False):
""" Continously solve in realtime, allowing a user to reposition
the robot, to quickly get an intuitive idea of what works
and doesn't. Note that TSR's based on object poses are not recalculated
if an object is moved."""
report = _rave.CollisionReport()
env = self.robot.GetEnv()
pose = Pose(self.robot)
for k in xrange(itrs):
pose.send()
with self.robot:
self.run(auto, extra)
colcheck = env.CheckCollision(
self.robot,
report=report) and self.robot.CheckSelfCollision(
) if self.bcollisions else False
if show:
self.goto()
_time.sleep(.1)
if self.solved and not colcheck and quitonsolve:
break
return self.solved()
def do_traj_ik_graph_search(pr2, lr, gripper_poses):
manip = get_manipulator(pr2, lr)
hmats = [juc.pose_to_hmat(pose) for pose in gripper_poses]
def ikfunc(hmat):
return traj_ik_graph_search.ik_for_link(hmat, manip, "%s_gripper_tool_frame"%lr, return_all_solns=True)
pr2.update_rave()
start_joints = pr2.robot.GetDOFValues(manip.GetArmJoints())
robot = manip.GetRobot()
env = robot.GetEnv()
report = rave.CollisionReport()
link_info = []
def nodecost(joints):
robot.SetDOFValues(joints, manip.GetArmJoints())
return 100*env.CheckCollision(robot)
paths, costs, timesteps = traj_ik_graph_search.traj_cart2joint(hmats, ikfunc, start_joints=start_joints, nodecost=nodecost)
i_best = np.argmin(costs)
print "lowest cost of initial trajs:", costs[i_best]
best_path = paths[i_best]
return best_path
def move_out_of_collision(env, body, max_displacement=0.005):
"""
Moves an OpenRAVE body out of collision in the opposite direction to the penetration direction.
@type env: orpy.Environment
@param env: The OpenRAVE environment.
@type body: orpy.KinBody
@param body: The OpenRAVE body.
@type max_displacement: float
@param max_displacement: The maximum displacement we can apply to the body.
"""
if not env.CheckCollision(body):
# Not in collision
return True
# Need to use pqp collision checker
previous_cc = env.GetCollisionChecker()
checker = orpy.RaveCreateCollisionChecker(env, 'pqp')
checker.SetCollisionOptions(orpy.CollisionOptions.Distance
| orpy.CollisionOptions.Contacts)
env.SetCollisionChecker(checker)
# Collision report
report = orpy.CollisionReport()
env.CheckCollision(body, report)
# Restore previous collision checker
env.SetCollisionChecker(previous_cc)
# Get the direction we should push the object
positions = []
normals = []
occurrences = []
for c in report.contacts:
positions.append(c.pos)
if len(normals) == 0:
normals.append(c.norm)
occurrences.append(1)
continue
found = False
for i, normal in enumerate(normals):
if np.allclose(c.norm, normal):
occurrences[i] += 1
found = True
break
if not found:
normals.append(c.norm)
occurrences.append(1)
push_direction = tr.unit_vector(normals[np.argmax(occurrences)])
# Get the distance we should push the object
Tbody = body.GetTransform()
Tnew = np.array(Tbody)
push_distance = 0
while env.CheckCollision(body):
push_distance += 0.001
Tnew[:3, 3] = Tbody[:3, 3] + push_distance * push_direction
body.SetTransform(Tnew)
if push_distance > max_displacement:
print 'push_distance: {0}'.format(push_distance)
body.SetTransform(Tbody)
return False
return True
def PlanToEndEffectorPose(self, robot, goal_pose, **kw_args):
"""
Attempt to plan a straight line trajectory from the robot's
current configuration to a desired end-effector pose. This
happens by finding the closest IK solution to the robot's
current configuration and attempts to snap there
(using PlanToConfiguration) if possible.
In the case of a redundant manipulator, no attempt is
made to check other IK solutions.
@param robot
@param goal_pose desired end-effector pose
@return traj
"""
from prpy.planning.exceptions import CollisionPlanningError
from prpy.planning.exceptions import SelfCollisionPlanningError
ikp = openravepy.IkParameterizationType
ikfo = openravepy.IkFilterOptions
# Find an IK solution. OpenRAVE tries to return a solution that is
# close to the configuration of the arm, so we don't need to do any
# custom IK ranking.
manipulator = robot.GetActiveManipulator()
ik_param = openravepy.IkParameterization(goal_pose, ikp.Transform6D)
ik_solution = manipulator.FindIKSolution(
ik_param, ikfo.CheckEnvCollisions,
ikreturn=False, releasegil=True
)
if ik_solution is None:
# FindIKSolutions is slower than FindIKSolution,
# so call this only to identify and raise error when
# there is no solution
ik_solutions = manipulator.FindIKSolutions(
ik_param, ikfo.IgnoreSelfCollisions,
ikreturn=False, releasegil=True)
for q in ik_solutions:
robot.SetActiveDOFValues(q)
report = openravepy.CollisionReport()
if self.env.CheckCollision(robot, report=report):
raise CollisionPlanningError.FromReport(report)
elif robot.CheckSelfCollision(report=report):
raise SelfCollisionPlanningError.FromReport(report)
raise PlanningError('There is no IK solution at the goal pose.')
return self._Snap(robot, ik_solution, **kw_args)
def Lift(robot, obj, distance=0.05, manip=None, render=True, **kw_args):
"""
@param robot The robot performing the push grasp
@param obj The object to lift
@param distance The distance to lift the cup
@param manip The manipulator to perform the grasp with
(if None active manipulator is used)
@param render Render tsr samples and push direction vectors during planning
"""
if manip is None:
with robot.GetEnv():
manip = robot.GetActiveManipulator()
# Check for collision and disable anything in collision
creport = openravepy.CollisionReport()
disabled_objects = []
# Resolve inconsistencies in grabbed objects
if robot.CheckSelfCollision():
grabbed_objs = robot.GetGrabbed()
for obj in grabbed_objs:
robot.Release(obj)
for obj in grabbed_objs:
robot.Grab(obj)
# Create list of any current collisions so those can be disabled
while robot.GetEnv().CheckCollision(robot, creport):
collision_obj = creport.plink2.GetParent()
disabled_objects.append(collision_obj)
collision_obj.Enable(False)
for obj in disabled_objects:
obj.Enable(True)
# Perform the lift
with prpy.rave.AllDisabled(robot.GetEnv(), disabled_objects):
lift_direction = [0., 0., 1.]
lift_distance = distance
ee_in_world = manip.GetEndEffectorTransform()
with prpy.viz.RenderVector(ee_in_world[:3, 3],
lift_direction,
distance,
robot.GetEnv(),
render=render):
manip.PlanToEndEffectorOffset(direction=lift_direction,
distance=lift_distance,
execute=True,
**kw_args)
def run(self, auto=False, extra=[]):
if auto:
self.activate(extra)
response = self.problem.SendCommand(self.Serialize())
if len(response) > 0:
collisions = _rave.CollisionReport()
if self.robot.CheckSelfCollision(collisions):
print "Self-collision between links {} and {}!".format(
collisions.plink1, collisions.plink2)
return False
if self.robot.GetEnv().CheckCollision(self.robot, collisions):
print "Environment collision between links {} and {}!".format(
collisions.plink1, collisions.plink2)
return False
self.soln = [float(x) for x in response[:-1].split(' ')]
return True
def get_collision_free_surface_pose(good_bodies, robot, obj,
max_trials = 500,
use_general_grasps = True):
env = robot.GetEnv()
robot_pose = robot.GetTransform()
torso_angle = robot.GetJoint("torso_lift_joint").GetValues()[0]
manip = robot.GetActiveManipulator()
ikmodel = openravepy.databases.inversekinematics.InverseKinematicsModel(
robot,iktype=openravepy.IkParameterization.Type.Transform6D)
if not ikmodel.load():
ikmodel.autogenerate()
grasping_poses = generate_manip_above_surface(obj)
env.GetCollisionChecker().SetCollisionOptions(openravepy.CollisionOptions.Contacts)
collision = env.CheckCollision(robot)
sol, _ = check_reachable(good_bodies, env, obj, manip, grasping_poses)
isreachable = sol is not None
min_torso, max_torso = utils.get_pr2_torso_limit(robot)
num_trial = 0
with robot:
while ((collision) or (not isreachable)) and (num_trial < max_trials):
num_trial +=1
torso_angle = move_random_torso(robot, min_torso, max_torso)
robot_pose = generate_random_pos(robot, obj)
robot.SetTransform(robot_pose)
report = openravepy.CollisionReport()
collision = env.CheckCollision(robot, report=report)
if not collision:
grasping_poses = generate_manip_above_surface(obj)
sol, _ = check_reachable(good_bodies, env, obj, manip, grasping_poses)
isreachable = sol is not None
else:
continue
if (sol is None) or collision:
raise GraspingPoseError("No collision free grasping pose found within %d steps" % max_trials)
else:
return (robot_pose, sol, torso_angle)
def get_real_contacts(self):
collision_report = orpy.CollisionReport()
real_contacts = []
# iterate over all fingertip links and determine the contacts
for eel in self._robot.get_fingertip_links():
link = self._robot.GetLink(eel)
self._orEnv.CheckCollision(self._obj, link, report=collision_report)
# self._orEnv.CheckCollision(link, self._obj, report=collision_report)
if len(collision_report.contacts) == 0:
raise ValueError('[HFTSSampler::get_real_contacts] No contacts found')
# TODO the normals reported by the collision check are wrong, so instead we use a nearest
# TODO neighbor lookup. Should see what's wrong with OpenRAVE here...
position = collision_report.contacts[0].pos
normal = self._object_points[self._object_kd_tree.query(position), 3:][1]
# normal = collision_report.contacts[0].norm
real_contacts.append(np.concatenate((position, normal)))
real_contacts = np.asarray(real_contacts)
return real_contacts
def checkCollisionIgnoreHand(self, config):
'''TODO'''
self.robot.SetDOFValues(config, self.manip.GetArmIndices())
self.env.UpdatePublishedBodies()
report = openravepy.CollisionReport()
inCollision = self.env.CheckCollision(self.robot, report)
if not inCollision: return False
inCollision = False
for linkPair in report.vLinkColliding:
if not (linkPair[0].GetName() in self.handLinkNames
or linkPair[1].GetName() in self.handLinkNames):
inCollision = True
break
return inCollision
def plot_contacts(robot, scale=.1):
env = robot.GetEnv()
with env:
# setup the collision checker to return contacts
checker = env.GetCollisionChecker()
checker.SetCollisionOptions(_rave.CollisionOptions.Contacts)
# get first collision
report = _rave.CollisionReport()
env.CheckCollision(robot, report=report)
#_rave.raveLogInfo('{} contacts'.format(len(report.contacts)))
positions = _np.array([c.pos for c in report.contacts])
normals = _np.array([c.norm for c in report.contacts])
if len(positions):
p_handles = env.plot3(positions, 5, [.7, .3, .3])
plist = _np.hstack((positions, positions + normals * scale))
n_handles = env.drawlinelist(_np.reshape(plist, (1, -1)), .5)
handles = (p_handles, n_handles)
else:
handles = None
return handles
def main():
"""Loads an environment and generates random positions until the robot can
reach an oject from a collision-free pose.
"""
env = openravepy.Environment()
env.Load('data/pr2test1.env.xml')
robot = env.GetRobots()[0]
mug = env.GetKinBody('mug1')
manip = robot.SetActiveManipulator('leftarm_torso')
ikmodel = openravepy.databases.inversekinematics.InverseKinematicsModel(
robot, iktype=openravepy.IkParameterization.Type.Transform6D)
if not ikmodel.load():
ikmodel.autogenerate()
num_contacts = 1 #just cheating
isreachable = False
while (num_contacts > 0) or (not isreachable):
T = generate_random_pos(robot, mug)
robot.SetTransform(T)
env.GetCollisionChecker().SetCollisionOptions(
openravepy.CollisionOptions.Contacts)
# get first collision
report = openravepy.CollisionReport()
collision = env.CheckCollision(robot, report=report)
num_contacts = len(report.contacts)
openravepy.raveLogInfo("Number of contacts: " + str(num_contacts))
sol = check_reachable(robot, manip, mug)
isreachable = sol is not None
#time.sleep(1)
robot.SetDOFValues(sol, manip.GetArmIndices())
openravepy.raveLogInfo("Done!!")
env.SetViewer('qtcoin')
def get_torso_grasping_pose(good_bodies, robot,
object_to_grasp,
max_trials = 100,
use_general_grasps = True,
):
"""Returns the torso height from where the robot can grasp an object.
Parameters:
robot: an OpenRave robot instance
object_to_grasp: a KinBody that the robot should grasp
max_trials: how many attempts before giving up
use_general_grasps: f True, don't calculate actual grasp points, but use
a pre-generated list. It is much faster if a grasping model has not been
generated.
Returns:
(sol, torso_angle): ,
the active manipulator angles and the torso joint angle from where the robot
can grasp an object.
Raises GraspingPoseError if no valid solution is found.
"""
env = robot.GetEnv()
robot_pose = robot.GetTransform()
torso_angle = robot.GetJoint("torso_lift_joint").GetValues()[0]
manip = robot.GetActiveManipulator()
ikmodel = openravepy.databases.inversekinematics.InverseKinematicsModel(
robot,iktype=openravepy.IkParameterization.Type.Transform6D)
if not ikmodel.load():
ikmodel.autogenerate()
grasping_poses = generate_grasping_pose(robot, object_to_grasp,
use_general_grasps)
openravepy.raveLogInfo("I've got %d grasping poses" % len(grasping_poses))
env.GetCollisionChecker().SetCollisionOptions(openravepy.CollisionOptions.Contacts)
collision = env.CheckCollision(robot)
sol, _ = check_reachable(good_bodies, env, object_to_grasp, manip, grasping_poses)
isreachable = sol is not None
min_torso, max_torso = utils.get_pr2_torso_limit(robot)
num_trial = 0
with robot:
while ((collision) or (not isreachable)) and (num_trial < max_trials):
num_trial +=1
torso_angle = move_random_torso(robot, min_torso, max_torso)
report = openravepy.CollisionReport()
collision = env.CheckCollision(robot, report=report)
if not collision:
grasping_poses = generate_grasping_pose(robot,
object_to_grasp,
use_general_grasps)
sol, _ = check_reachable(good_bodies, env, object_to_grasp, manip, grasping_poses)
isreachable = sol is not None
else:
continue
if (sol is None) or collision:
e = GraspingPoseError("No collision free grasping pose found within %d steps" % max_trials)
raise e
else:
return (sol, torso_angle)
def get_collision_free_ik_pose(good_bodies, robot, object_to_grasp,
ik_pose,
max_trials = 100,
only_reachable=False
):
"""Returns the position from where the robot can reach a position (in
cartesian coordinates). The active manipulator is used.
Parameters:
robot: an OpenRave robot instance
ik_pose: a 4x4 matrix with the desired 6D pose
max_trials: how many attempts before giving up
use_general_grasps: f True, don't calculate actual grasp points, but use
a pre-generated list. It is much faster if a grasping model has not been
generated.
Returns:
(robot_pose, sol, torso_angle): the robot position (as a transformation matrix),
the active manipulator angles and the torso joint angle from where the robot
can grasp an object.
Raises GraspingPoseError if no valid solution is found.
"""
env = robot.GetEnv()
robot_pose = robot.GetTransform()
torso_angle = robot.GetJoint("torso_lift_joint").GetValues()[0]
manip = robot.GetActiveManipulator()
ikmodel = openravepy.databases.inversekinematics.InverseKinematicsModel(
robot,iktype=openravepy.IkParameterization.Type.Transform6D)
if not ikmodel.load():
ikmodel.autogenerate()
env.GetCollisionChecker().SetCollisionOptions(openravepy.CollisionOptions.Contacts)
collision = env.CheckCollision(robot)
sol, _ = check_reachable(good_bodies, env, object_to_grasp, manip, [ik_pose], only_reachable)
isreachable = sol is not None
min_torso, max_torso = utils.get_pr2_torso_limit(robot)
num_trial = 0
xyz = ik_pose[:3, 3]
with robot:
while ((collision) or (not isreachable)) and (num_trial < max_trials):
num_trial +=1
torso_angle = move_random_torso(robot, min_torso, max_torso)
robot_pose = generate_random_pos(robot, xyz)
robot.SetTransform(robot_pose)
report = openravepy.CollisionReport()
collision = env.CheckCollision(robot, report=report)
if not collision:
sol, _ = check_reachable(good_bodies, env, object_to_grasp, manip, [ik_pose], only_reachable)
isreachable = sol is not None
else:
continue
if (sol is None) or collision:
e = GraspingPoseError("No collision free IK pose found within %d steps" % max_trials)
raise e
else:
return (robot_pose, sol, torso_angle)
def get_occluding_objects(good_bodies, robot,
object_to_grasp,
max_trials = 0,
just_one_attempt = False,
return_pose = False,
body_filter = None
):
"""Generates a list of all the objects that prevent the robot from reaching
a target object. Several (up to max_trials) attempts are performed to grasp
Parameters:
robot: a openravepy.Robot instance
object_to_grasp: a openravepy.KinBody instance representing the object to grasp
Returns:
a list of sets of objects
"""
if just_one_attempt != return_pose:
raise ValueError("If asking for a return poes then set just_one_attempt to True")
env = robot.GetEnv()
robot_pose = robot.GetTransform()
manip = robot.GetActiveManipulator()
ikmodel = openravepy.databases.inversekinematics.InverseKinematicsModel(
robot,iktype=openravepy.IkParameterization.Type.Transform6D)
if not ikmodel.load():
ikmodel.autogenerate()
min_torso, max_torso = utils.get_pr2_torso_limit(robot)
num_trial = 0
collisions_list = []
with robot:
while num_trial < max_trials:
num_trial +=1
# sample a base pose
torso_angle = generate_reaching_poses.move_random_torso(robot,
min_torso,
max_torso)
robot_pose = generate_reaching_poses.generate_random_pos(robot,
object_to_grasp)
robot.SetTransform(robot_pose)
report = openravepy.CollisionReport()
collision = env.CheckCollision(robot, report=report)
if not collision:
openravepy.raveLogInfo("Got a position not in collision")
#sample a gripper pose
grasping_poses = generate_reaching_poses.generate_grasping_pose(robot,
object_to_grasp,
use_general_grasps = True,
checkik=False)
openravepy.raveLogInfo("Got %d grasping poses" % len(grasping_poses))
#check if gripper pose is reachable from base pose
#use robot's base pose to transform precomputed
#gripper poses into the robot's frame of reference
sol, collisions = generate_reaching_poses.check_reachable(good_bodies,
env, object_to_grasp, manip, grasping_poses, only_reachable = True)
if sol is None:
print "Finding collisions: trial {0} No sol from base pose to gripper pose".\
format(num_trial)
goodCollisions = False
if body_filter is not None:
goodCollisions = filter(body_filter, collisions)
print "good collisions: " + repr(goodCollisions)
print "all collisions: " + repr(collisions)
if sol is not None and goodCollisions:
print "Sol from base pose to gripper pose found in trial {0}".\
format(num_trial)
openravepy.raveLogInfo("Getting the list of collisions")
with robot:
#robot.SetDOFValues(sol, robot.GetActiveManipulator().GetArmIndices());
#collisions_list.append(utils.get_all_collisions(robot, env))
collisions_list.append(collisions)
if just_one_attempt:
if return_pose:
return (robot_pose, sol, torso_angle, collisions_list)
else:
return collisions_list
if num_trial == max_trials:
print "Getting obj names: No gripper pose reachable from collision free base pose found",
print "after {0} trials".format(num_trial)
if return_pose:
return (robot_pose, None, torso_angle, collisions_list)
else:
return collisions_list
def showPossibleBasePoses(self, Tgrasp, N=1, Timeout=5):
"""visualizes possible base poses for a grasp specified by Tgrasp and gripper_angle
:param Tgrasp: 4x4 numpy.array, row major matrix, the grasp transform in global frame. equals manip.GetTransform() in the goal state
:param gripper_angle: float, the gripper angle
:param N: int, the number of sample poses we want to get
"""
# find the robot base distribution for the grasp specified by Tgrasp
# Input for computeBaseDistribution():
# Tgrasp: 4x4 numpy.array, row major matrix, the grasp transform in global frame
# equals manip.GetTransform() in the goal state
# Output for computeBaseDistribution():
# densityfn: gaussian kernel density function taking poses of openrave quaternion type, returns probabilities
# samplerfn: gaussian kernel sampler function taking number of sample and weight, returns robot base poses and joint states
# bounds: 2x3 array, bounds of samples, [[min rotation, min x, min y],[max rotation, max x, max y]]
self.irmodel = openravepy.databases.inversereachability.InverseReachabilityModel(
robot=self.robot)
print 'loading irmodel'
if not self.irmodel.load():
self.irmodel.autogenerate()
self.irmodel.load()
densityfn, samplerfn, bounds = self.irmodel.computeBaseDistribution(
Tgrasp)
if densityfn == None:
print 'the specified grasp is not reachable!'
return
# Code fragmenmanip = self.robot.SetActiveManipulator('left_wam')t from `examples.mobilemanipulation`
# initialize sampling parameters
goals = []
numfailures = 0
starttime = time.time()
timeout = Timeout
self.manip = self.robot.SetActiveManipulator('left_wam')
# self.manip = self.robot.GetActiveManipulator()
self.env = self.robot.GetEnv()
with self.robot:
while len(goals) < N:
if time.time() - starttime > timeout:
break
poses, jointstate = samplerfn(N - len(goals))
for pose in poses:
self.robot.SetTransform(pose)
self.robot.SetDOFValues(*jointstate)
# Check if base is in collision
if not self.manip.CheckIndependentCollision(
openravepy.CollisionReport()):
q = self.manip.FindIKSolution(
Tgrasp,
filteroptions=openravepy.IkFilterOptions.
CheckEnvCollisions)
if q is not None:
pose = self.robot.GetTransform()
xy_pose = [pose[0][3], pose[1][3]]
goals.append((Tgrasp, xy_pose, q, pose))
elif self.manip.FindIKSolution(Tgrasp, 0) is None:
numfailures += 1
print "base pose is in collision"
return goals
def GetBasePoseForObjectGrasp(self, obj):
# Load grasp database
self.gmodel = openravepy.databases.grasping.GraspingModel(
self.robot, obj)
if not self.gmodel.load():
self.gmodel.autogenerate()
base_pose = None
grasp_config = None
###################################################################
# TODO: Here you will fill in the function to compute
# a base pose and associated grasp config for the
# grasping the bottle
###################################################################
#get the ordered valid grasp from homework1
print "robot start transformation -----------------"
print self.robot.GetTransform()
self.graspindices = self.gmodel.graspindices
self.grasps = self.gmodel.grasps
self.order_grasps()
# get the grasp transform
Tgrasp = self.gmodel.getGlobalGraspTransform(self.grasps_ordered[10],
collisionfree=True)
# load inverserechability database
irmodel = openravepy.databases.inversereachability.InverseReachabilityModel(
robot=self.robot)
starttime = time.time()
print 'loading irmodel'
if not irmodel.load():
irmodel.autogenerate()
loaded = irmodel.load()
print "irmodel loaded? {}".format(loaded)
densityfn, samplerfn, bounds = irmodel.computeBaseDistribution(Tgrasp)
#find the valid pose and joint states
# initialize sampling parameters
goals = []
numfailures = 0
N = 3
with self.robot.GetEnv():
while len(goals) < N:
poses, jointstate = samplerfn(N - len(goals))
for pose in poses:
self.robot.SetTransform(pose)
self.robot.SetDOFValues(*jointstate)
# validate that base is not in collision
if not self.manip.CheckIndependentCollision(
openravepy.CollisionReport()):
q = self.manip.FindIKSolution(
Tgrasp,
filteroptions=IkFilterOptions.CheckEnvCollisions)
if q is not None:
values = self.robot.GetDOFValues()
values[self.manip.GetArmIndices()] = q
goals.append((Tgrasp, pose, values))
elif self.manip.FindIKSolution(Tgrasp, 0) is None:
numfailures += 1
# To do still
#base_pose = goals[0][1]
#grasp_config = goals[0][2]
for i, goal in enumerate(goals):
grasp_with_pose, pose, values = goal
self.robot.SetTransform(pose)
self.robot.SetJointValues(values)
trans_pose = self.robot.GetTransform()
angle_pose = openravepy.axisAngleFromRotationMatrix(trans_pose)
pose = [trans_pose[0, 3], trans_pose[1, 3], angle_pose[2]]
base_pose = numpy.array(pose)
grasp_config = q
#import IPython
#IPython.embed()
print "grasping result"
print base_pose
print grasp_config
return base_pose, grasp_config
def handle_select_base(self, req): #, task):
#choose_task(req.task)
print 'I have received inputs!'
#print 'My given inputs were: \n'
#print 'goal is: \n',req.goal
#print 'head is: \n', req.head
# The head location is received as a posestamped message and is converted and used as the head location.
pos_temp = [
req.head.pose.position.x, req.head.pose.position.y,
req.head.pose.position.z
]
ori_temp = [
req.head.pose.orientation.x, req.head.pose.orientation.y,
req.head.pose.orientation.z, req.head.pose.orientation.w
]
head = createBMatrix(pos_temp, ori_temp)
#print 'head from input: \n', head
# This lets the service use TF to get the head location instead of requiring it as an input.
#(trans,rot) = self.listener.lookupTransform('/base_link', '/head_frame', rospy.Time(0))
#pos_temp = trans
#ori_temp = rot
#head = createBMatrix(pos_temp,ori_temp)
#print 'head from tf: \n',head
# The goal location is received as a posestamped message and is converted and used as the goal location.
pos_temp = [
req.goal.pose.position.x, req.goal.pose.position.y,
req.goal.pose.position.z
]
ori_temp = [
req.goal.pose.orientation.x, req.goal.pose.orientation.y,
req.goal.pose.orientation.z, req.goal.pose.orientation.w
]
goal = createBMatrix(pos_temp, ori_temp)
#print 'goal: \n',goal
print 'I will move to be able to reach the goal.'
# Sets the wheelchair location based on the location of the head using a few homogeneous transforms.
pr2_B_wc = np.matrix([[head[0, 0], head[0, 1], 0., head[0, 3]],
[head[1, 0], head[1, 1], 0., head[1, 3]],
[0., 0., 1., 0.], [0., 0., 0., 1]])
# Transform from the coordinate frame of the wc model in the back right bottom corner, to the head location
corner_B_head = np.matrix([
[m.cos(0.), -m.sin(0.), 0., .442603],
[m.sin(0.), m.cos(0.), 0., .384275], #0.34
[0., 0., 1., 0.],
[0., 0., 0., 1.]
])
wheelchair_location = pr2_B_wc * corner_B_head.I
self.wheelchair.SetTransform(np.array(wheelchair_location))
# Published the wheelchair location to create a marker in rviz for visualization to compare where the service believes the wheelchair is to
# where the person is (seen via kinect).
pos_goal = wheelchair_location[0:3, 3]
ori_goal = tr.matrix_to_quaternion(wheelchair_location[0:3, 0:3])
self.publish_wc_marker(pos_goal, ori_goal)
#Setup for inside loop
angle = m.pi
# Transforms from end of wrist of PR2 to the end of its gripper. Openrave has a weird coordinate system for the gripper so I use a transform to correct.
goal_B_gripper = np.matrix([[0, 0, 1., .1], [0, 1, 0., 0.],
[-1., 0., 0., 0.], [0., 0., 0., 1.]])
#pr2_B_goal = head * head_B_goal
pr2_B_goal = goal * goal_B_gripper
angle_base = m.pi / 2
#print 'The goal gripper pose is: \n' , np.array(pr2_B_goal)
# This is to publish WC position w.r.t. PR2 after the PR2 reaches goal location.
# Only necessary for testing in simulation to set the wheelchair in reach of PR2.
#goalpr2_B_wc = wc_B_goalpr2.I
#print 'pr2_B_wc is: \n',goalpr2_B_wc
#pos_goal = goalpr2_B_wc[:3,3]
#ori_goal = tr.matrix_to_quaternion(goalpr2_B_wc[0:3,0:3])
#psm_wc = PoseStamped()
#psm_wc.header.frame_id = '/odom_combined'
#psm_wc.pose.position.x=pos_goal[0]
#psm_wc.pose.position.y=pos_goal[1]
#psm_wc.pose.position.z=pos_goal[2]
#psm_wc.pose.orientation.x=ori_goal[0]
#psm_wc.pose.orientation.y=ori_goal[1]
#psm_wc.pose.orientation.z=ori_goal[2]
#psm_wc.pose.orientation.w=ori_goal[3]
#self.wc_position.publish(psm_wc)
# Find a base location by testing various base locations online for IK solution
for i in [
0., .05, .1, .15, .2, .25, .3, .35, .4, .45, .5, -.05, -.1,
-.15, -.2, -.25, -.3
]: #[.1]:#[0.,.1,.3,.5,.8,1,-.1,-.2,-.3]:
for j in [
0., .03, .05, .08, -.03, -.05, -.08, -.1, -.12, -.2, -.3
]: #[.2]:#[0.,.1,.3,.5,.8,-.1,-.2,-.3]:
for k in [
0., m.pi / 4, m.pi / 2, -m.pi / 4, -m.pi / 2, m.pi,
3 * m.pi / 2
]:
#goal_pose = req.goal
# transform from head frame in wheelchair to desired base goal
wc_B_goalpr2 = np.matrix([[
m.cos(angle_base + k), -m.sin(angle_base + k), 0.,
.4 + i
],
[
m.sin(angle_base + k),
m.cos(angle_base + k), 0.,
-.9 + j
], [0., 0., 1, 0.],
[0., 0., 0., 1]])
base_position = pr2_B_wc * wc_B_goalpr2
#print 'base position: \n',base_position
self.robot.SetTransform(np.array(base_position))
#res = self.manipprob.MoveToHandPosition(matrices=[np.array(pr2_B_goal)],seedik=10) # call motion planner with goal joint angles
#self.robot.WaitForController(0) # wait
#print 'res: \n',res
v = self.robot.GetActiveDOFValues()
for name in self.joint_names:
v[self.robot.GetJoint(name).GetDOFIndex(
)] = self.joint_angles[self.joint_names.index(name)]
self.robot.SetActiveDOFValues(v)
with self.env:
#print 'checking goal base location: \n' , np.array(base_position)
if not self.manip.CheckIndependentCollision(
op.CollisionReport()):
sol = self.manip.FindIKSolution(
np.array(pr2_B_goal),
op.IkFilterOptions.CheckEnvCollisions)
if sol is not None:
now = rospy.Time.now() + rospy.Duration(1.0)
self.listener.waitForTransform(
'/odom_combined', '/base_link', now,
rospy.Duration(10))
(trans, rot) = self.listener.lookupTransform(
'/odom_combined', '/base_link', now)
odom_goal = createBMatrix(trans,
rot) * base_position
pos_goal = odom_goal[:3, 3]
ori_goal = tr.matrix_to_quaternion(
odom_goal[0:3, 0:3])
#print 'Got an iksolution! \n', sol
psm = PoseStamped()
psm.header.frame_id = '/odom_combined'
psm.pose.position.x = pos_goal[0]
psm.pose.position.y = pos_goal[1]
psm.pose.position.z = pos_goal[2]
psm.pose.orientation.x = ori_goal[0]
psm.pose.orientation.y = ori_goal[1]
psm.pose.orientation.z = ori_goal[2]
psm.pose.orientation.w = ori_goal[3]
# This is to publish WC position w.r.t. PR2 after the PR2 reaches goal location.
# Only necessary for testing in simulation to set the wheelchair in reach of PR2.
#goalpr2_B_wc = wc_B_goalpr2.I
#print 'pr2_B_wc is: \n',goalpr2_B_wc
#pos_goal = goalpr2_B_wc[:3,3]
#ori_goal = tr.matrix_to_quaternion(goalpr2_B_wc[0:3,0:3])
#psm_wc = PoseStamped()
#psm_wc.header.frame_id = '/odom_combined'
#psm_wc.pose.position.x=pos_goal[0]
#psm_wc.pose.position.y=pos_goal[1]
#psm_wc.pose.position.z=pos_goal[2]
#psm_wc.pose.orientation.x=ori_goal[0]
#psm_wc.pose.orientation.y=ori_goal[1]
#psm_wc.pose.orientation.z=ori_goal[2]
#psm_wc.pose.orientation.w=ori_goal[3]
#self.wc_position.publish(psm_wc)
print 'I found a goal location! It is at B transform: \n', base_position
print 'The quaternion to the goal location is: \n', psm
return psm
#self.robot.SetDOFValues(sol,self.manip.GetArmIndices()) # set the current solution
#Tee = self.manip.GetEndEffectorTransform()
#self.env.UpdatePublishedBodies() # allow viewer to update new robot
# traj = None
# try:
# #res = self.manipprob.MoveToHandPosition(matrices=[np.array(pr2_B_goal)],seedik=10) # call motion planner with goal joint angles
# traj = self.manipprob.MoveManipulator(goal=sol, outputtrajobj=True)
# print 'Got a trajectory! \n'#,traj
# except:
# #print 'traj = \n',traj
# traj = None
# print 'traj failed \n'
# pass
# #traj =1 #This gets rid of traj
# if traj is not None:
else:
print 'I found a bad goal location. Trying again!'
#rospy.sleep(.1)
print 'I found nothing! My given inputs were: \n', req.goal, req.head
print 'I found a goal location! It is at B transform: \n', base_location
print 'The quaternion to the goal location is: \n', psm
return psm
def GetBasePoseForObjectGrasp(self, obj):
# Load grasp database
graspModel = GraspModel(self.robot, obj)
graspModel.order_grasps()
for i in range(6):
print 'Showing grasp ', i
graspModel.show_grasp(graspModel.grasps_ordered[i], delay=self.delay)
print "using grasp 0 to generate Transform "
Tgrasp = graspModel.gmodel.getGlobalGraspTransform(graspModel.grasps_ordered[i], collisionfree=True) # get the grasp transform
if Tgrasp != None:
break
base_pose = None
grasp_config = None
###################################################################
# TODO: Here you will fill in the function to compute
# a base pose and associated grasp config for the
# grasping the bottle
###################################################################
self.irmodel = openravepy.databases.inversereachability.InverseReachabilityModel(self.robot)
loaded = self.irmodel.load()
if loaded:
densityfn,samplerfn,bounds = self.irmodel.computeBaseDistribution(Tgrasp)
# densityfn: gaussian kernel density function taking poses of openrave quaternion type, returns probabilities
# samplerfn: gaussian kernel sampler function taking number of sample and weight, returns robot base poses and joint states
# bounds: 2x3 array, bounds of samples, [[min rotation, min x, min y],[max rotation, max x, max y]]
goals = []
numfailures = 0
starttime = time.time()
timeout = 5000
N = 5
with self.robot:
while len(goals) < N:
if time.time()-starttime > timeout:
break
poses,jointstate = samplerfn(N-len(goals))
for pose in poses:
self.robot.SetTransform(pose)
self.robot.SetDOFValues(*jointstate)
# validate that base is not in collision
if not self.irmodel.manip.CheckIndependentCollision(openravepy.CollisionReport()):
q = self.irmodel.manip.FindIKSolution(Tgrasp,filteroptions=openravepy.IkFilterOptions.CheckEnvCollisions)
if q is not None:
values = self.robot.GetDOFValues()
values[self.irmodel.manip.GetArmIndices()] = q
goals.append((Tgrasp,pose,values))
elif self.irmodel.manip.FindIKSolution(Tgrasp,0) is None:
numfailures += 1
self.armmanip = self.robot.GetManipulator('right_wam')
self.robot.SetActiveManipulator('right_wam')
print 'showing %d results'%N
for ind,goal in enumerate(goals):
raw_input('press ENTER to show goal %d'%ind)
Tgrasp,base_pose, all_config = goal
self.robot.SetTransform(base_pose)
self.robot.SetDOFValues(all_config)
# IPython.embed()
idx = raw_input("Choose from goal index 0, 1, 2, 3, 4: ")
goal_chosen = goals[int(idx)]
Tgrasp, pose, all_config = goal_chosen
matrix = openravepy.matrixFromPose(pose)
aa = openravepy.axisAngleFromRotationMatrix(matrix)
base_pose = [matrix[0,3], matrix[1,3], aa[2]] # x, y , theta
return base_pose, all_config[11:18] # base_pose [x, y] grasp_config [7 dof values]
def GetBasePoseForObjectGrasp(self, obj):
# Load grasp database
self.gmodel = openravepy.databases.grasping.GraspingModel(
self.robot, obj)
if not self.gmodel.load():
self.gmodel.autogenerate()
base_pose = None
grasp_config = None
###################################################################
# TODO: Here you will fill in the function to compute
# a base pose and associated grasp config for the
# grasping the bottle
###################################################################
self.graspindices = self.gmodel.graspindices
self.grasps = self.gmodel.grasps
grasp_config = self.order_grasps_noisy()
# Store the initial robot pose
init_pose = self.robot.GetTransform()
# Format grasp transform in global frame into 4x4 numpy.array
Tgrasp = self.gmodel.getGlobalGraspTransform(grasp_config,
collisionfree=True)
print 'Gripper transform', Tgrasp
# load inverserechability database
self.irmodel = openravepy.databases.inversereachability.InverseReachabilityModel(
robot=self.robot)
print 'loading irmodel'
if not self.irmodel.load():
class IrmodelOption:
self.irmodel.autogenerate()
self.irmodel.load()
densityfn, samplerfn, bounds = self.irmodel.computeBaseDistribution(
Tgrasp)
# initialize sampling parameters
with self.robot:
while base_pose is None:
pose, jointstate = samplerfn(1)
self.robot.SetTransform(pose[0])
pose_config = self.base_planner.planning_env.herb.GetCurrentConfiguration(
)
pid = self.base_planner.planning_env.discrete_env.ConfigurationToNodeId(
pose_config)
pose_config = self.base_planner.planning_env.discrete_env.NodeIdToConfiguration(
pid)
self.base_planner.planning_env.herb.SetCurrentConfiguration(
pose_config)
# print self.robot.GetTransform()
self.robot.SetDOFValues(*jointstate)
# validate that base is not in collision
if not self.robot.ikmodel.manip.CheckIndependentCollision(
openravepy.CollisionReport()):
q = self.robot.ikmodel.manip.FindIKSolution(
Tgrasp,
filteroptions=openravepy.IkFilterOptions.
CheckEnvCollisions)
if q is not None:
print "Found valid pose"
base_pose = self.base_planner.planning_env.herb.GetCurrentConfiguration(
)
print 'base pose', base_pose
grasp_config = q
print "grasp_config:"
print grasp_config
# Once after finding out the final base pose, change the robot back to its initial pose
self.robot.SetTransform(init_pose)
return base_pose, grasp_config
def GetBasePoseForObjectGrasp(self, obj):
# Load grasp database
self.gmodel = openravepy.databases.grasping.GraspingModel(
self.robot, obj)
if not self.gmodel.load():
self.gmodel.autogenerate()
#load model of base
self.irmodel = openravepy.databases.inversereachability.InverseReachabilityModel(
self.robot)
if not self.irmodel.load():
self.irmodel.autogenerate()
print "models are loaded"
base_pose = None
grasp_config = None
###################################################################
# TODO: Here you will fill in the function to compute
# a base pose and associated grasp config for the
# grasping the bottle
###################################################################
print "start calculate grasp and base!!!!!!"
self.graspindices = self.gmodel.graspindices
self.grasps = self.gmodel.grasps
self.order_grasps()
Tgrasp = self.gmodel.getGlobalGraspTransform(self.grasps_ordered[10],
collisionfree=True)
densityfn, samplerfn, bounds = self.irmodel.computeBaseDistribution(
Tgrasp)
# pickle.dump([ Tgrasp,densityfn,samplerfn,bounds ], open( "save.p", "wb" ) )
# Tgrasp: 4x4 numpy.array, row major matrix, the grasp transform in global frame
# equals manip.GetTransform() in the goal state
print "got irmodel {}".format(Tgrasp)
goals = []
numfailures = 0
starttime = time.time()
timeout = 10000
N = 5
print "start time {}".format(starttime)
with self.robot:
while len(goals) < N:
if time.time() - starttime > timeout:
break
poses, jointstate = samplerfn(N - len(goals))
for pose in poses:
self.robot.SetTransform(pose)
self.robot.SetDOFValues(*jointstate)
# validate that base is not in collision
if not self.manip.CheckIndependentCollision(
openravepy.CollisionReport()):
q = self.manip.FindIKSolution(
Tgrasp,
filteroptions=openravepy.IkFilterOptions.
CheckEnvCollisions)
if q is not None:
values = self.robot.GetDOFValues()
values[self.manip.GetArmIndices()] = q
goals.append((Tgrasp, pose, values))
print "q: {}".format(q)
elif self.manip.FindIKSolution(Tgrasp, 0) is None:
numfailures += 1
print 'showing {} results'.format(N)
for ind, goal in enumerate(goals):
raw_input('press ENTER to show goal %d' % ind)
Tgrasp, pose, values = goal
self.robot.SetTransform(pose)
self.robot.SetDOFValues(values)
base_pose = np.array(
self.base_planner.planning_env.herb.GetCurrentConfiguration())
grasp_config = np.array(
self.arm_planner.planning_env.herb.GetCurrentConfiguration())
print "found goals!"
IPython.embed()
theta = -numpy.pi / 4.
start_pose = numpy.array(
[[numpy.cos(theta), -numpy.sin(theta), 0, -1.25],
[numpy.sin(theta), numpy.cos(theta), 0, 0.82], [0., 0., 1, 0.],
[0., 0., 0, 1.]])
right_relaxed = [5.65, -1.76, -0.26, 1.96, -1.15, 0.87, -1.43]
left_relaxed = [0.64, -1.76, 0.26, 1.96, 1.16, 0.87, 1.43]
self.robot.SetTransform(start_pose)
self.robot.SetActiveDOFValues(left_relaxed)
return base_pose, grasp_config
def _Snap(self, robot, goal, **kw_args):
from prpy.util import CheckJointLimits
from prpy.util import GetLinearCollisionCheckPts
from prpy.planning.exceptions import CollisionPlanningError
from prpy.planning.exceptions import SelfCollisionPlanningError
# Create a two-point trajectory between the
# current configuration and the goal.
# (a straight line in joint space)
traj = openravepy.RaveCreateTrajectory(self.env, '')
cspec = robot.GetActiveConfigurationSpecification('linear')
active_indices = robot.GetActiveDOFIndices()
# Check the start position is within joint limits,
# this can throw a JointLimitError
start = robot.GetActiveDOFValues()
CheckJointLimits(robot, start)
# Add the start waypoint
start_waypoint = numpy.zeros(cspec.GetDOF())
cspec.InsertJointValues(start_waypoint, start, robot,
active_indices, False)
traj.Init(cspec)
traj.Insert(0, start_waypoint.ravel())
# Make the trajectory end at the goal configuration, as
# long as it is not in collision and is not identical to
# the start configuration.
CheckJointLimits(robot, goal)
if not numpy.allclose(start, goal):
goal_waypoint = numpy.zeros(cspec.GetDOF())
cspec.InsertJointValues(goal_waypoint, goal, robot,
active_indices, False)
traj.Insert(1, goal_waypoint.ravel())
# Get joint configurations to check
# Note: this returns a python generator, and if the
# trajectory only has one waypoint then only the
# start configuration will be collisioned checked.
#
# Sampling function:
# 'linear'
# from prpy.util import SampleTimeGenerator
# linear = SampleTimeGenerator
# 'Van der Corput'
from prpy.util import VanDerCorputSampleGenerator
vdc = VanDerCorputSampleGenerator
checks = GetLinearCollisionCheckPts(robot, traj,
norm_order=2,
sampling_func=vdc)
# Run constraint checks at DOF resolution:
for t, q in checks:
# Set the joint positions
# Note: the planner is using a cloned 'robot' object
robot.SetActiveDOFValues(q)
# Check for collisions
report = openravepy.CollisionReport()
if self.env.CheckCollision(robot, report=report):
raise CollisionPlanningError.FromReport(report)
elif robot.CheckSelfCollision(report=report):
raise SelfCollisionPlanningError.FromReport(report)
# Tag the return trajectory as smooth (in joint space).
SetTrajectoryTags(traj, {Tags.SMOOTH: True}, append=True)
return traj
