def no_collision(self, config):
pose = openravepy.quatFromAxisAngle([0, 0, config[2]])
pose = numpy.append(pose, [config[0], config[1], 0])
self.robot.SetTransform(pose)
#IPython.embed()
collision = self.robot.GetEnv().CheckCollision(self.robot)
return not collision
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 add_drop_table(env, dist_from_robot): thickness = 0.1 legheight = 0.4 table = object_models.create_table(env, "table", 0.5, 0.5, thickness, 0.1, 0.1, legheight) x, y = dist_from_robot z = thickness / 2 + legheight rot = openravepy.quatFromAxisAngle((0, 0, 0)) table.SetTransform(openravepy.matrixFromPose(list(rot) + [x, y, z])) env.AddKinBody(table) return table
def base_pose_to_mat(pose):
# x, y, rot = pose
assert len(pose) == 3
x = pose[0]
y = pose[1]
rot = pose[2]
q = quatFromAxisAngle((0, 0, rot)).tolist()
pos = [x, y, 0]
# pos = np.vstack((x,y,np.zeros(1)))
matrix = matrixFromPose(q + pos)
return matrix
def base_pose_to_mat(pose):
# x, y, rot = pose
assert len(pose) == 3
x = pose[0]
y = pose[1]
rot = pose[2]
q = quatFromAxisAngle((0, 0, rot)).tolist()
pos = [x, y, 0]
# pos = np.vstack((x,y,np.zeros(1)))
matrix = matrixFromPose(q + pos)
return matrix
def step_forward_request(robot, n_steps, stepping_foot, dx, dy):
"""
Sets up the problem to step forward with one foot (stepping_foot)
Suppose stepping_foot = "r_foot"
Then problem constrains r_foot to move by (dx, dy), while the center
of mass lies above the support polygon of l_foot
"""
planted_foot = opposite_foot(stepping_foot)
request = request_skeleton(n_steps)
# fixed pose of planted foot
planted_link = robot.GetLink(planted_foot)
planted_transform = planted_link.GetTransform()
planted_xyz, planted_wxyz = xyzQuatFromMatrix(planted_transform)
# final pose of stepping foot
stepping_link = robot.GetLink(stepping_foot)
stepping_init_transform = stepping_link.GetTransform()
stepping_init_xyz = stepping_init_transform[:3,3]
z_angle = np.arctan2(dy, dx)
stepping_final_xyz = stepping_init_xyz + np.array([dx, dy, 0])
stepping_final_wxyz = rave.quatFromAxisAngle([0,0,1], z_angle)
for i in xrange(1, n_steps):
request["constraints"].extend([
{
"type":"pose",
"name":"planted_pose",
"params":{
"xyz":list(planted_xyz),
"wxyz":list(planted_wxyz),
"link":planted_foot,
"timestep":i
}
},
{
"type":"zmp","name":"zmp_%i"%i,
"params":{"planted_links":[planted_foot], "timestep":i}
}
])
#if i < n_steps-1:
#request["constraints"].append({
#"type":"foot_height",
#"params":{
#"height": .1,
#"timestep": i,
#"link":stepping_foot}})
request["constraints"].append(
{
"type":"pose",
"name":"step_pose",
"params":{
"xyz" : list(stepping_final_xyz),
"wxyz" : list(stepping_final_wxyz),
"link" : stepping_foot,
"timestep":(n_steps-1)
}
}
)
return request
def quat_from_axis_angle(x_angle, y_angle, z_angle):
return quatFromAxisAngle(np.array((x_angle, y_angle, z_angle)))
def angle_pose_to_mat(pose):
assert len(pose) == 1
q = quatFromAxisAngle((0, 0, pose)).tolist()
matrix = matrixFromPose(q + pos)
return matrix
def step_forward_request(robot, n_steps, stepping_foot, dx, dy):
"""
Sets up the problem to step forward with one foot (stepping_foot)
Suppose stepping_foot = "r_foot"
Then problem constrains r_foot to move by (dx, dy), while the center
of mass lies above the support polygon of l_foot
"""
planted_foot = opposite_foot(stepping_foot)
request = request_skeleton(n_steps)
# fixed pose of planted foot
planted_link = robot.GetLink(planted_foot)
planted_transform = planted_link.GetTransform()
planted_xyz, planted_wxyz = xyzQuatFromMatrix(planted_transform)
# final pose of stepping foot
stepping_link = robot.GetLink(stepping_foot)
stepping_init_transform = stepping_link.GetTransform()
stepping_init_xyz = stepping_init_transform[:3,3]
z_angle = np.arctan2(dy, dx)
stepping_final_xyz = stepping_init_xyz + np.array([dx, dy, 0])
stepping_final_wxyz = rave.quatFromAxisAngle([0,0,1], z_angle)
for i in xrange(1, n_steps):
request["constraints"].extend([
{
"type":"pose",
"name":"planted_pose",
"params":{
"xyz":list(planted_xyz),
"wxyz":list(planted_wxyz),
"link":planted_foot,
"timestep":i
}
},
{
"type":"zmp","name":"zmp_%i"%i,
"params":{"planted_links":[planted_foot], "timestep":i}
}
])
#if i < n_steps-1:
#request["constraints"].append({
#"type":"foot_height",
#"params":{
#"height": .1,
#"timestep": i,
#"link":stepping_foot}})
request["constraints"].append(
{
"type":"pose",
"name":"step_pose",
"params":{
"xyz" : list(stepping_final_xyz),
"wxyz" : list(stepping_final_wxyz),
"link" : stepping_foot,
"timestep":(n_steps-1)
}
}
)
return request
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 base_pose_to_mat(pose):
x, y, rot = pose
q = openravepy.quatFromAxisAngle((0, 0, rot)).tolist()
pos = [x, y, 0]
matrix = openravepy.matrixFromPose(q + pos)
return matrix
def quat_from_axis_angle(x_angle, y_angle, z_angle): return quatFromAxisAngle(np.array((x_angle, y_angle, z_angle)))
