def getEnv(self, key, iter=0, first=False):
val = self.base.getModel(key)
print key, val
obj = None
obj_list = []
parent_tf = tf.identity_matrix()
if self.base.typeOf(key) == self.base.ROBOTS.ident:
obj = self._getRobot(key)
obj_list.append(obj)
parent_tf = obj.GetTransform()
elif self.base.typeOf(key) == self.base.OBJECTS.ident:
obj = self._getObject(key)
obj_list.append(obj)
parent_tf = obj.GetTransform()
elif self.base.typeOf(key) == self.base.LOCATIONS.ident:
obj = self._getLocation(key)
obj_list.append(obj)
parent_tf = obj.GetTransform()
elif self.base.typeOf(key) == self.base.SENSORS.ident:
obj = self._getSensor(key)
obj_list.append(obj)
parent_tf = obj.GetTransform()
else:
if val.has_key("translation"):
translation = map(lambda x: float(x), val["translation"].split(" "))
parent_tf = tf.concatenate_matrices(parent_tf, tf.compose_matrix(translate = translation))
if val.has_key("quat"):
quat = map(lambda x: float(x), val["quat"].split(" "))
rot = rave.axisAngleFromQuat(quat)
m2 = tf.compose_matrix(angles = rot)
parent_tf = tf.concatenate_matrices(parent_tf, m2)
if first:
parent_tf = tf.identity_matrix()
if obj != None:
obj.SetTransform(parent_tf)
if iter==0:
return obj_list
# search for ancestors
child_expr = pycassa.create_index_expression('base', key)
clild_clause = pycassa.create_index_clause([child_expr])
for child_key, _ in self.base.col.get_indexed_slices(clild_clause):
child_obj = self.getEnv(child_key, iter-1)
for obj in child_obj:
if type(obj) != type(None):
obj.SetTransform(tf.concatenate_matrices(parent_tf, obj.GetTransform()))
obj_list.append(obj)
return obj_list
def handle_conf_generator(req):
# request
gp = req.grasp_poses[0]
m_gp = openravepy.matrixFromPose((gp.orientation.w, gp.orientation.x, gp.orientation.y, gp.orientation.z,\
gp.position.x, gp.position.y, gp.position.z))
bps = cg._find_base_conf(m_gp, req.base_num)
# response
pcr = PossibleConfResponse()
i = 0
if bps is not None:
for b in bps[0]:
axisAngle = openravepy.axisAngleFromQuat(b[0:4])
bm = [b[4], b[5], axisAngle[2]]
bc = BaseConf()
bc.b_c.append(bm[0])
bc.b_c.append(bm[1])
bc.b_c.append(bm[2])
pcr.base_confs.append(bc)
jc = JointConf()
if (i < req.joint_num):
robot.SetActiveDOFValues(bm)
joint_conf = manip.FindIKSolution(
m_gp,
filteroptions=openravepy.IkFilterOptions.CheckEnvCollisions
)
if joint_conf is not None:
jc.j_c = joint_conf
i = i + 1
pcr.joint_confs.append(jc)
return pcr
def cnst_evaluate(q0, qu=qu):
with self.robot:
self.robot.SetActiveDOFValues(q0)
qu0 = self.manip.GetTransformPose()[0:4]
quat_err = quatMultiply(qu, quatInverse(qu0))
axangle_err = axisAngleFromQuat(quat_err)
return np.linalg.norm(axangle_err)
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
self.order_grasps()
self.show_grasp(self.grasps_ordered[0])
graspTransform = self.gmodel.getGlobalGraspTransform(self.grasps_ordered[0], collisionfree = True)
irmodel = openravepy.databases.inversereachability.InverseReachabilityModel(robot = self.robot)
irmodel.load()
densityfn, samplerfn, bounds = irmodel.computeBaseDistribution(graspTransform)
poses, jointstate = samplerfn(100)
self.manip = self.robot.GetActiveManipulator()
start_pose = self.robot.GetTransform()
handles = []
init_config = self.base_planner.planning_env.herb.GetCurrentConfiguration()
for pose in poses:
self.robot.SetTransform(pose)
angle = openravepy.axisAngleFromQuat(pose)
newpose = numpy.array([pose[4], pose[5], angle[2]])
node = self.base_planner.planning_env.discrete_env.ConfigurationToNodeId(newpose)
discrete_pose = self.base_planner.planning_env.discrete_env.NodeIdToConfiguration(node)
handles.append(self.robot.GetEnv().plot3(points=numpy.array(numpy.append(discrete_pose[:2], [0])), pointsize = 15.0))
quat = openravepy.quatFromAxisAngle([0, 0, discrete_pose[2]])
quat = numpy.append(quat, discrete_pose[0])
quat = numpy.append(quat, discrete_pose[1])
quat = numpy.append(quat, 0)
self.robot.SetTransform(quat)
self.base_planner.planning_env.herb.SetCurrentConfiguration(discrete_pose)
obs = self.robot.GetEnv().GetBodies()
table = obs[1]
grasp_config = self.manip.FindIKSolution(graspTransform, filteroptions = openravepy.IkFilterOptions.CheckEnvCollisions.IgnoreEndEffectorCollisions)
if not self.robot.GetEnv().CheckCollision(self.robot, table) and not grasp_config == None:
print grasp_config
self.base_planner.planning_env.herb.SetCurrentConfiguration(init_config)
return discrete_pose, grasp_config
def cnst_b(q0, qu=qu):
with self.robot:
self.robot.SetActiveDOFValues(q0)
qu0 = self.manip.GetTransformPose()[0:4]
quat_err = quatMultiply(qu, quatInverse(qu0))
axangle_err = axisAngleFromQuat(quat_err)
temp = np.dot(cnst_A(q0), q0)
b = axangle_err + temp
return b.reshape(3, 1)
def transformOpenrave(self, openrave_obj, complex_data):
if complex_data.has_key("translation"):
translation = map(lambda x: float(x), complex_data["translation"].split(" "))
m1 = openrave_obj.GetTransform()
m2 = tf.compose_matrix(translate = translation)
openrave_obj.SetTransform(tf.concatenate_matrices(m1, m2))
if complex_data.has_key("quat"):
quat = map(lambda x: float(x), complex_data["quat"].split(" "))
m1 = openrave_obj.GetTransform()
rot = rave.axisAngleFromQuat(quat)
m2 = tf.compose_matrix(angles = rot)
openrave_obj.SetTransform(tf.concatenate_matrices(m1, m2))
return openrave_obj
def ros_quat_to_aa(q):
return openravepy.axisAngleFromQuat([q.w, q.x, q.y, q.z])
def GetBasePoseForObjectGrasp(self, obj):
# Load grasp database
# change gmodel to sel.gmodel since eval_grasp need it
self.gmodel = openravepy.databases.grasping.GraspingModel(
self.robot, obj)
if not self.gmodel.load():
self.gmodel.autogenerate()
basePose = None
graspConfig = None
self.graspindices = self.gmodel.graspindices
self.grasps = self.gmodel.grasps
self.order_grasps()
#uncomment orgConfig
orgConfig = self.base_planner.planning_env.herb.GetCurrentConfiguration(
)
# get the highest score grasping pose
# self.show_grasp(self.grasps_ordered[0]) # visualize solution, seems ok
#print "top grasping {}".format(self.grasps_ordered[0])
graspTransform = self.gmodel.getGlobalGraspTransform(
self.grasps_ordered[0], collisionfree=True)
irModel = openravepy.databases.inversereachability.InverseReachabilityModel(
self.robot)
if not irModel.load():
irModel.autogenerate()
loaded = irModel.load()
print "irModel loaded? {}".format(loaded)
densityFN, samplerFN, bounds = irModel.computeBaseDistribution(
graspTransform)
#print "bounds {}".format(bounds)
poses, joint = samplerFN(500)
#print "pose number: {}".format(len(poses))
#print "pose: {}".format(poses[0])
#print "jointstate: {}".format(jointstate[0])
#angle = openravepy.axisAngleFromQuat(poses[0])
#print "angle: {}".format(angle)
#trans = openravepy.matrixFromPose(poses[0])
#print "trans: {}".format(trans)
for pose in poses:
#self.robot.SetTransform(pose) # pose format [s, vx, vy, vz, x, y, z]
angle = openravepy.axisAngleFromQuat(pose)
continuousPose = copy.deepcopy([pose[4], pose[5], angle[2]
]) # 2D location with orientation
#convert continuous pose to discrete pose
node = self.base_planner.planning_env.discrete_env.ConfigurationToNodeId(
continuousPose)
discretePose = self.base_planner.planning_env.discrete_env.NodeIdToConfiguration(
node)
basePose = openravepy.quatFromAxisAngle([0, 0, discretePose[2]])
basePose = numpy.append(basePose,
[discretePose[0], discretePose[1], 0])
#basePose = numpy.array(discretePose)
obstacles = self.robot.GetEnv().GetBodies()
#print('gettransform=',self.robot.GetTransform())
self.robot.SetTransform(basePose)
#get grasp joing config from IK
tableTransform = obstacles[1].GetTransform()
tablePosition = (tableTransform[0][3], tableTransform[1][3])
tableDist = la.norm(
numpy.array([discretePose[0], discretePose[1]]) -
numpy.array(tablePosition))
bottleTransform = obstacles[2].GetTransform()
bottlePosition = (bottleTransform[0][3], bottleTransform[1][3])
bottleDist = la.norm(
numpy.array([discretePose[0], discretePose[1]]) -
numpy.array(bottlePosition))
#IPython.embed()
graspConfig = self.manip.FindIKSolution(
graspTransform,
filteroptions=openravepy.IkFilterOptions.CheckEnvCollisions)
#if self.robot.GetEnv().CheckCollision(self.robot, obstacles[1]) != True and graspConfig != None and dist > 0.9 and dist < 1.1:
if self.robot.GetEnv().CheckCollision(
self.robot, obstacles[1]) != True and graspConfig != None:
print "discretePose: {}".format(discretePose)
print "graspConfig: {}".format(graspConfig)
print "tableDist: {}".format(tableDist)
print "bottleDist: {}".format(bottleDist)
#IPython.embed()
#restore robot position before return
self.base_planner.planning_env.herb.SetCurrentConfiguration(
orgConfig)
#Peter,change
return discretePose, graspConfig
print "Fail to find solution!!!"
return discretePose, graspConfig
def ros_quat_to_aa(q):
return openravepy.axisAngleFromQuat([q.w, q.x, q.y, q.z])