def GetHardGoal(params, yname):
goal = params[yname+'_goal_arm_config']
base_goal = [np.array(params[yname+'_goal_base_pose'])[0,3],
np.array(params[yname+'_goal_base_pose'])[1,3],
orpy.axisAngleFromRotationMatrix(np.array(params[yname+'_goal_base_pose'])[:3,:3])[2]]
goal.extend(base_goal)
return goal
def GetHardGoal(self, params, yname):
goal = params[yname+'_goal_arm_config']
base_goal = [np.array(params[yname+'_goal_base_pose'])[0,3],
np.array(params[yname+'_goal_base_pose'])[1,3],
orpy.axisAngleFromRotationMatrix(np.array(params[yname+'_goal_base_pose'])[:3,:3])[2]]
goal.extend(base_goal)
return goal
def get_angles(hmats):
"""
returns the angles of a list of 4x4 transformation matrices
"""
assert hmats.ndim ==3 and hmats.shape[1:]==(4,4)
angles = np.empty(hmats.shape[0])
for i in xrange(hmats.shape[0]):
angles[i] = np.linalg.norm(rave.axisAngleFromRotationMatrix(hmats[i,0:3,0:3]))
return angles
def get_angles(hmats):
"""
returns the angles of a list of 4x4 transformation matrices
"""
assert hmats.ndim == 3 and hmats.shape[1:] == (4, 4)
angles = np.empty(hmats.shape[0])
for i in xrange(hmats.shape[0]):
angles[i] = np.linalg.norm(
rave.axisAngleFromRotationMatrix(hmats[i, 0:3, 0:3]))
return angles
def move_straight(self, dist):
#TODO Fill in, remove sleep
#time.sleep(0)
with self.env:
T1 = self.robot.GetTransform() # Get the current Transform
T2 = openravepy.axisAngleFromRotationMatrix(T1[0:3,0:3]) # Get the current axis angles of the robot
T1[0,3] = dist*np.cos(T2[2]) # Distance movied in X-direction
T1[1,3] = dist*np.sin(T2[2]) # Distance moved in Y-direction
self.robot.SetTransform(T1) # Setting the new transform
def rpy2axang(rpy):
"""
Converts a matrix of rpy (nx3) into a matrix of
axis-angles (nx3).
"""
n = rpy.shape[0]
assert rpy.shape[1]==3, "unknown shape."
ax_ang = np.empty((n,3))
for i in xrange(n):
th = rpy[i,:]
ax_ang[i,:] = rave.axisAngleFromRotationMatrix(tfms.euler_matrix(th[0], th[1], th[2]))
return ax_ang
def performNavigationPlanning(self, goal, execute=True, draw_marker=False, steplength=0.1):
goal = np.array(goal)
if goal.ndim == 2:
# assume a 4x4 transformation matrix
angle = openravepy.axisAngleFromRotationMatrix(goal)[-1]
goal = np.array([goal[0, -1], goal[1, -1], angle])
# find the boundaries of the environment
envmin, envmax = utils.get_environment_limits(self.env, self.robot)
with self.env:
self.robot.SetAffineTranslationLimits(envmin, envmax)
self.robot.SetAffineTranslationMaxVels([0.3, 0.3, 0.3])
self.robot.SetAffineRotationAxisMaxVels(np.ones(4))
self.robot.SetActiveDOFs(
[], openravepy.DOFAffine.X | openravepy.DOFAffine.Y | openravepy.DOFAffine.RotationAxis, [0, 0, 1]
)
# draw the marker
if draw_marker:
center = np.r_[goal[0:2], 0.2]
xaxis = 0.5 * np.array((np.cos(goal[2]), np.sin(goal[2]), 0))
yaxis = 0.25 * np.array((-np.sin(goal[2]), np.cos(goal[2]), 0))
h = self.env.drawlinelist(
np.transpose(
np.c_[
center - xaxis,
center + xaxis,
center - yaxis,
center + yaxis,
center + xaxis,
center + 0.5 * xaxis + 0.5 * yaxis,
center + xaxis,
center + 0.5 * xaxis - 0.5 * yaxis,
]
),
linewidth=5.0,
colors=np.array((0, 1, 0)),
)
openravepy.RaveLogInfo("Planning to goal " + str(goal))
res = self.basemanip.MoveActiveJoints(
goal=goal, maxiter=3000, steplength=steplength, execute=execute, outputtrajobj=True
)
if res is None:
raise ValueError("Could not find a trajectory")
if execute:
openravepy.RaveLogInfo("Waiting for controller to finish")
self.robot.WaitForController(0)
self.robot.GetController().Reset()
return res
def GeodesicError(t1, t2):
'''
Computes the error in global coordinates between two transforms
@param t1 current transform
@param t2 goal transform
@return a 4-vector of [dx, dy, dz, solid angle]
'''
trel = numpy.dot(numpy.linalg.inv(t1), t2)
trans = numpy.dot(t1[0:3, 0:3], trel[0:3, 3])
omega = openravepy.axisAngleFromRotationMatrix(trel[0:3, 0:3])
angle = numpy.linalg.norm(omega)
return numpy.hstack((trans, angle))
def performNavigationPlanning(self,
goal,
execute=True,
draw_marker=False,
steplength=0.1):
goal = np.array(goal)
if goal.ndim == 2:
#assume a 4x4 transformation matrix
angle = openravepy.axisAngleFromRotationMatrix(goal)[-1]
goal = np.array([goal[0, -1], goal[1, -1], angle])
# find the boundaries of the environment
envmin, envmax = utils.get_environment_limits(self.env, self.robot)
with self.env:
self.robot.SetAffineTranslationLimits(envmin, envmax)
self.robot.SetAffineTranslationMaxVels([0.3, 0.3, 0.3])
self.robot.SetAffineRotationAxisMaxVels(np.ones(4))
self.robot.SetActiveDOFs(
[], openravepy.DOFAffine.X | openravepy.DOFAffine.Y
| openravepy.DOFAffine.RotationAxis, [0, 0, 1])
# draw the marker
if draw_marker:
center = np.r_[goal[0:2], 0.2]
xaxis = 0.5 * np.array((np.cos(goal[2]), np.sin(goal[2]), 0))
yaxis = 0.25 * np.array((-np.sin(goal[2]), np.cos(goal[2]), 0))
h = self.env.drawlinelist(np.transpose(
np.c_[center - xaxis, center + xaxis, center - yaxis,
center + yaxis, center + xaxis,
center + 0.5 * xaxis + 0.5 * yaxis, center + xaxis,
center + 0.5 * xaxis - 0.5 * yaxis]),
linewidth=5.0,
colors=np.array((0, 1, 0)))
openravepy.RaveLogInfo("Planning to goal " + str(goal))
res = self.basemanip.MoveActiveJoints(goal=goal,
maxiter=3000,
steplength=steplength,
execute=execute,
outputtrajobj=True)
if res is None:
raise ValueError("Could not find a trajectory")
if execute:
openravepy.RaveLogInfo("Waiting for controller to finish")
self.robot.WaitForController(0)
self.robot.GetController().Reset()
return res
def GeodesicTwist(t1, t2):
'''
Computes the twist in global coordinates that corresponds
to the gradient of the geodesic distance between two transforms.
@param t1 current transform
@param t2 goal transform
@return twist in se(3)
'''
trel = numpy.dot(numpy.linalg.inv(t1), t2)
trans = numpy.dot(t1[0:3, 0:3], trel[0:3, 3])
omega = numpy.dot(t1[0:3, 0:3],
openravepy.axisAngleFromRotationMatrix(
trel[0:3, 0:3]))
return numpy.hstack((trans, omega))
def test_drive_base(pose_targ):
wxyz_targ = pose_targ[:4]
xyz_targ = pose_targ[4:]
robot.SetDOFValues([0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0])
robot.SetTransform(rave.matrixFromPose([1, 0, 0, 0, -3.4, -1.4, 0.05]))
robot.SetActiveDOFs(np.r_[robot.GetManipulator("rightarm").GetArmIndices(), robot.GetLink("torso_lift_link").GetIndex()],
rave.DOFAffine.X + rave.DOFAffine.Y + rave.DOFAffine.RotationAxis, [0,0,1])
angle_init = np.random.rand() * 2*np.pi
x_init = xyz_targ[0] - .5*np.cos(angle_init)
y_init = xyz_targ[1] - .5*np.sin(angle_init)
dofvals_init = np.r_[np.zeros(7), 0, x_init, y_init, angle_init]
#start = robot.
start = robot.GetActiveDOFValues();
end = dofvals_init
n_steps = 10;
init_traj = mu.linspace2d(start, end, n_steps)
manipname = "active"
prob_desc = make_request_skeleton(manipname, n_steps);
prob_desc["basic_info"]["start_fixed"] = True
prob_desc["costs"].append(
{"type" : "pose",
"name" : "pose",
"params" : {
"pos_coeffs" : [10,10,10],
"rot_coeffs" : [0,0,0],
"xyz" : xyz_targ,
"wxyz" : wxyz_targ,
"link" : "r_gripper_tool_frame",
}})
prob_desc["init_info"]["type"] = "given_traj"
angle = np.linalg.norm(rave.axisAngleFromRotationMatrix(robot.GetTransform()))
prob_desc["init_info"]["data"] = [row.tolist() for row in init_traj]
s = json.dumps(prob_desc)
traj.SetInteractive(True);
prob = traj.ConstructProblem(s, env)
return traj.OptimizeProblem(prob)
def __init__(self, robot):
self.robot = robot
for joint in robot.GetJoints():
lower, upper = joint.GetLimits()
lower = np.clip(lower, -np.pi, np.inf)
upper = np.clip(upper, -np.inf, np.pi)
self.add_trait(joint.GetName(), tr.Range(lower[0].item(), upper[0].item(), joint.GetValues()[0]))
T = robot.GetTransform()
rx,ry,rz = rave.axisAngleFromRotationMatrix(T[:3,:3])
tx,ty,tz = T[:3,3]
self.add_trait("tftx", tr.Range(-5.,5,tx.item()))
self.add_trait("tfty", tr.Range(-5.,5,ty.item()))
self.add_trait("tftz", tr.Range(-5.,5,tz.item()))
self.add_trait("tfrx", tr.Range(-5.,5,rx.item()))
self.add_trait("tfry", tr.Range(-5.,5,ry.item()))
self.add_trait("tfrz", tr.Range(-5.,5,rz.item()))
self.on_trait_change(self.update_robot, 'anytrait')
tr.HasTraits.__init__(self)
def __init__(self, robot):
self.robot = robot
for joint in robot.GetJoints():
lower, upper = joint.GetLimits()
lower = np.clip(lower, -np.pi, np.inf)
upper = np.clip(upper, -np.inf, np.pi)
self.add_trait(joint.GetName(), tr.Range(lower[0].item(), upper[0].item(), joint.GetValues()[0]))
T = robot.GetTransform()
rx,ry,rz = rave.axisAngleFromRotationMatrix(T[:3,:3])
tx,ty,tz = T[:3,3]
self.add_trait("tftx", tr.Range(-5.,5,tx.item()))
self.add_trait("tfty", tr.Range(-5.,5,ty.item()))
self.add_trait("tftz", tr.Range(-5.,5,tz.item()))
self.add_trait("tfrx", tr.Range(-5.,5,rx.item()))
self.add_trait("tfry", tr.Range(-5.,5,ry.item()))
self.add_trait("tfrz", tr.Range(-5.,5,rz.item()))
self.on_trait_change(self.update_robot, 'anytrait')
tr.HasTraits.__init__(self)
def mat_to_base_pose(mat):
pose = poseFromMatrix(mat)
x = pose[4]
y = pose[5]
rot = axisAngleFromRotationMatrix(mat)[2]
return np.array([x,y,rot])
def PlanToEndEffectorOffset(self, robot, direction, distance, max_distance=None,
nullspace=JointLimitAvoidance, timelimit=5.0, step_size=0.001,
position_tolerance=0.01, angular_tolerance=0.15, **kw_args):
"""
Plan to a desired end-effector offset with move-hand-straight
constraint. movement less than distance will return failure. The motion
will not move further than max_distance.
@param robot
@param direction unit vector in the direction of motion
@param distance minimum distance in meters
@param max_distance maximum distance in meters
@param timelimit timeout in seconds
@param stepsize step size in meters for the Jacobian pseudoinverse controller
@param position_tolerance constraint tolerance in meters
@param angular_tolerance constraint tolerance in radians
@return traj
"""
if distance < 0:
raise ValueError('Distance must be non-negative.')
elif numpy.linalg.norm(direction) == 0:
raise ValueError('Direction must be non-zero')
elif max_distance is not None and max_distance < distance:
raise ValueError('Max distance is less than minimum distance.')
elif step_size <= 0:
raise ValueError('Step size must be positive.')
elif position_tolerance < 0:
raise ValueError('Position tolerance must be non-negative.')
elif angular_tolerance < 0:
raise ValueError('Angular tolerance must be non-negative.')
# save all active bodies so we only check collision with those
active_bodies = []
for body in self.env.GetBodies():
if body.IsEnabled():
active_bodies.append(body)
# Normalize the direction vector.
direction = numpy.array(direction, dtype='float')
direction /= numpy.linalg.norm(direction)
# Default to moving an exact distance.
if max_distance is None:
max_distance = distance
with robot:
manip = robot.GetActiveManipulator()
traj = openravepy.RaveCreateTrajectory(self.env, '')
traj.Init(manip.GetArmConfigurationSpecification())
active_dof_indices = manip.GetArmIndices()
limits_lower, limits_upper = robot.GetDOFLimits(active_dof_indices)
initial_pose = manip.GetEndEffectorTransform()
q = robot.GetDOFValues(active_dof_indices)
traj.Insert(0, q)
start_time = time.time()
current_distance = 0.0
sign_flipper = 1
last_rot_error = 9999999999.0
try:
while current_distance < max_distance:
# Check for a timeout.
current_time = time.time()
if timelimit is not None and current_time - start_time > timelimit:
raise PlanningError('Reached time limit.')
# Compute joint velocities using the Jacobian pseudoinverse.
q_dot = self.GetStraightVelocity(manip, direction, initial_pose, nullspace, step_size, sign_flipper=sign_flipper)
q += q_dot
robot.SetDOFValues(q, active_dof_indices)
# Check for collisions.
#if self.env.CheckCollision(robot):
for body in active_bodies:
if self.env.CheckCollision(robot, body):
raise PlanningError('Encountered collision.')
if robot.CheckSelfCollision():
raise PlanningError('Encountered self-collision.')
# Check for joint limits.
elif not (limits_lower < q).all() or not (q < limits_upper).all():
raise PlanningError('Encountered joint limit during Jacobian move.')
# Check our distance from the constraint.
current_pose = manip.GetEndEffectorTransform()
a = initial_pose[0:3, 3]
p = current_pose[0:3, 3]
orthogonal_proj = (a - p) - numpy.dot(a - p, direction) * direction
if numpy.linalg.norm(orthogonal_proj) > position_tolerance:
raise PlanningError('Deviated from a straight line constraint.')
# Check our orientation against the constraint.
offset_pose = numpy.dot(numpy.linalg.inv(current_pose), initial_pose)
offset_angle = openravepy.axisAngleFromRotationMatrix(offset_pose)
offset_angle_norm = numpy.linalg.norm(offset_angle)
if offset_angle_norm > last_rot_error + 0.0005:
sign_flipper *= -1
last_rot_error = offset_angle_norm
if offset_angle_norm > angular_tolerance:
raise PlanningError('Deviated from orientation constraint.')
traj.Insert(traj.GetNumWaypoints(), q)
# Check if we've exceeded the maximum distance by projecting our
# displacement along the direction.
hand_pose = manip.GetEndEffectorTransform()
displacement = hand_pose[0:3, 3] - initial_pose[0:3, 3]
current_distance = numpy.dot(displacement, direction)
except PlanningError as e:
# Throw an error if we haven't reached the minimum distance.
if current_distance < distance:
raise
# Otherwise we'll gracefully terminate.
else:
logger.warning('Terminated early at distance %f < %f: %s',
current_distance, max_distance, e.message)
SetTrajectoryTags(output_traj, {Tags.CONSTRAINED: True}, append=True)
return traj
def vec2args(x):
return x[0:3], x[3:6]
def args2vec(xyz, rod):
out = np.empty(6)
out[0:3] = xyz
out[3:6] = rod
return out
T_init = np.eye(4)
xyz_init = T_init[:3, 3]
rod_init = rave.axisAngleFromRotationMatrix(T_init[:3, :3])
print "> optimizing..."
soln = opt.fmin(calc_error_wrapper, args2vec(xyz_init, rod_init), xtol=1e-2, ftol=1e-1, callback=plot_point)
print "\t optimization done."
(best_xyz, best_rod) = vec2args(soln)
print "xyz, rod:", best_xyz, best_rod
T_best = calc_T(best_xyz, best_rod)
print "T_h_k:", T_best
## final display:
pc2 = (np.c_[pc2, np.ones((pc2.shape[0], 1))].dot(T_best.T))[:, :3]
clearreq = gen_mlab_request(mlab.clf)
plotter.request(clearreq)
c1req = gen_mlab_request(mlab.points3d, pc1[:, 0], pc1[:, 1], pc1[:, 2], color=(1, 0, 0), scale_factor=0.001)
def PlanToEndEffectorOffset(self,
robot,
direction,
distance,
max_distance=None,
nullspace=JointLimitAvoidance,
timelimit=5.0,
step_size=0.001,
position_tolerance=0.01,
angular_tolerance=0.15,
**kw_args):
"""
Plan to a desired end-effector offset with move-hand-straight
constraint. movement less than distance will return failure. The motion
will not move further than max_distance.
@param robot
@param direction unit vector in the direction of motion
@param distance minimum distance in meters
@param max_distance maximum distance in meters
@param timelimit timeout in seconds
@param stepsize step size in meters for the Jacobian pseudoinverse controller
@param position_tolerance constraint tolerance in meters
@param angular_tolerance constraint tolerance in radians
@return traj
"""
if distance < 0:
raise ValueError('Distance must be non-negative.')
elif numpy.linalg.norm(direction) == 0:
raise ValueError('Direction must be non-zero')
elif max_distance is not None and max_distance < distance:
raise ValueError('Max distance is less than minimum distance.')
elif step_size <= 0:
raise ValueError('Step size must be positive.')
elif position_tolerance < 0:
raise ValueError('Position tolerance must be non-negative.')
elif angular_tolerance < 0:
raise ValueError('Angular tolerance must be non-negative.')
# save all active bodies so we only check collision with those
active_bodies = []
for body in self.env.GetBodies():
if body.IsEnabled():
active_bodies.append(body)
# Normalize the direction vector.
direction = numpy.array(direction, dtype='float')
direction /= numpy.linalg.norm(direction)
# Default to moving an exact distance.
if max_distance is None:
max_distance = distance
with robot:
manip = robot.GetActiveManipulator()
traj = openravepy.RaveCreateTrajectory(self.env, '')
traj.Init(manip.GetArmConfigurationSpecification())
active_dof_indices = manip.GetArmIndices()
limits_lower, limits_upper = robot.GetDOFLimits(active_dof_indices)
initial_pose = manip.GetEndEffectorTransform()
q = robot.GetDOFValues(active_dof_indices)
traj.Insert(0, q)
start_time = time.time()
current_distance = 0.0
sign_flipper = 1
last_rot_error = 9999999999.0
try:
while current_distance < max_distance:
# Check for a timeout.
current_time = time.time()
if timelimit is not None and current_time - start_time > timelimit:
raise PlanningError('Reached time limit.')
# Compute joint velocities using the Jacobian pseudoinverse.
q_dot = self.GetStraightVelocity(manip,
direction,
initial_pose,
nullspace,
step_size,
sign_flipper=sign_flipper)
q += q_dot
robot.SetDOFValues(q, active_dof_indices)
# Check for collisions.
#if self.env.CheckCollision(robot):
for body in active_bodies:
if self.env.CheckCollision(robot, body):
raise PlanningError('Encountered collision.')
if robot.CheckSelfCollision():
raise PlanningError('Encountered self-collision.')
# Check for joint limits.
elif not (limits_lower <
q).all() or not (q < limits_upper).all():
raise PlanningError(
'Encountered joint limit during Jacobian move.')
# Check our distance from the constraint.
current_pose = manip.GetEndEffectorTransform()
a = initial_pose[0:3, 3]
p = current_pose[0:3, 3]
orthogonal_proj = (
a - p) - numpy.dot(a - p, direction) * direction
if numpy.linalg.norm(orthogonal_proj) > position_tolerance:
raise PlanningError(
'Deviated from a straight line constraint.')
# Check our orientation against the constraint.
offset_pose = numpy.dot(numpy.linalg.inv(current_pose),
initial_pose)
offset_angle = openravepy.axisAngleFromRotationMatrix(
offset_pose)
offset_angle_norm = numpy.linalg.norm(offset_angle)
if offset_angle_norm > last_rot_error + 0.0005:
sign_flipper *= -1
last_rot_error = offset_angle_norm
if offset_angle_norm > angular_tolerance:
raise PlanningError(
'Deviated from orientation constraint.')
traj.Insert(traj.GetNumWaypoints(), q)
# Check if we've exceeded the maximum distance by projecting our
# displacement along the direction.
hand_pose = manip.GetEndEffectorTransform()
displacement = hand_pose[0:3, 3] - initial_pose[0:3, 3]
current_distance = numpy.dot(displacement, direction)
except PlanningError as e:
# Throw an error if we haven't reached the minimum distance.
if current_distance < distance:
raise
# Otherwise we'll gracefully terminate.
else:
logger.warning('Terminated early at distance %f < %f: %s',
current_distance, max_distance, e.message)
SetTrajectoryTags(output_traj, {Tags.CONSTRAINED: True}, append=True)
return traj
def mat_to_base_pose(mat):
pose = openravepy.poseFromMatrix(mat)
x = pose[4]
y = pose[5]
rot = openravepy.axisAngleFromRotationMatrix(mat)[2]
return x, y, rot
def mat_to_base_pose(mat):
pose = poseFromMatrix(mat)
x = pose[4]
y = pose[5]
rot = axisAngleFromRotationMatrix(mat)[2]
return np.array([x, y, rot])
def axis_angle_from_rot(rot):
return axisAngleFromRotationMatrix(rot)
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 tf_to_pose(tf): return np.r_[tf[:3,3], rave.axisAngleFromRotationMatrix(tf[:3,:3])]
def GetCurrentConfiguration(self):
t = self.robot.GetTransform()
aa = openravepy.axisAngleFromRotationMatrix(t)
pose = [t[0,3], t[1,3], aa[2]]
return numpy.array(pose)
#o = T_w_k[:3,3]
#x = T_w_k[:3,0]
#y = T_w_k[:3,1]
#z = T_w_k[:3,2]
#handles = []
#handles.append(env.drawarrow(o, o+.2*x, .005,(1,0,0,1)))
#handles.append(env.drawarrow(o, o+.2*y, .005,(0,1,0,1)))
#handles.append(env.drawarrow(o, o+.2*z, .005,(0,0,1,1)))
#viewer.Idle()
T_h_k_init = berkeley_pr2.T_h_k
xyz_init = T_h_k_init[:3,3]
rod_init = openravepy.axisAngleFromRotationMatrix(T_h_k_init[:3,:3])
f_init = 525
#from rapprentice.math_utils import linspace2d
#sim_depths = calculate_sim_depths(xyz_init, rod_init, f_init)
#cv2.imshow('hi',np.clip(sim_depths[0]/3,0,1))
#cv2.waitKey()
#plot_real_and_sim(args2vec(xyz_init, rod_init, f_init))
#calculate_depth_error(xyz_init, rod_init, f_init)
soln = opt.fmin(calc_error_wrapper, args2vec(xyz_init, rod_init, f_init),callback=plot_real_and_sim)
(best_xyz, best_rod, best_f) = vec2args(soln)
print "xyz, rod:", best_xyz, best_rod
print "T_h_k:", calc_Thk(best_xyz, best_rod)
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 PlotImages(self, o, a, desc):
'''Produces plots of the robot's observation and selected action.
- Input o: Image where 1st channel is the target sensed volume and the 2nd channel is the hand
contents.
- Input a: Index into the Q-function output which corresponds to the selected action.
- Input desc: Descriptor corresponding to the current action in the base frame.
- Returns None.
'''
# setup
fig = pyplot.figure()
It = o[:, :, 0]
Ih = zeros(It.shape) if self.IsGraspImage(o) else o[:, :, 1]
Ir = copy(It); Ig = copy(It); Ib = copy(It)
if self.IsOrientationLevel():
# determine rotation angle
R = desc.T[0:3, 0:3]
axisAngle = openravepy.axisAngleFromRotationMatrix(R)
angle = norm(axisAngle)
axis = axisAngle / angle if angle > 0 else array([0.0, 0.0, 1.0])
angle *= sign(sum(axis))
# draw axis indicator
c = self.imP / 2
majorRadius = self.imP / 8
minorRadius = majorRadius if self.IsGraspImage else majorRadius / 2
xx, yy = ellipse_perimeter(c, c, minorRadius, majorRadius, orientation=0)
Ir[xx, yy] = 1.0
# draw angle indicator
length = self.imP / 5
x = -int(length * sin(angle))
y = int(length * cos(angle))
xx, yy = line(c, c, c + x, c + y)
Ir[xx, yy] = 1.0
xx, yy = line(c, c, c, c + length)
Ir[xx, yy] = 1.0
else:
# draw the selection area
halfWidth = (It.shape[0] * (self.selW / self.imW)) / 2.0
middle = It.shape[0] / 2.0
start = int(round(middle - halfWidth))
end = int(round(middle + halfWidth))
pixels = arange(start, end + 1)
if start >= 0 and end < It.shape[0]:
Ib[start, pixels] = 1.0
Ib[end, pixels] = 1.0
Ib[pixels, start] = 1.0
Ib[pixels, end] = 1.0
# draw robot's selection
xh = self.actionsInHandFrame[a]
xi = round(((xh[0:2] * self.imP) / self.imW) + ((self.imP - 1.0) / 2.0)).astype('int32')
value = (xh[2] + (self.imD / 2.0)) / self.imD
if xi[0] >= 0 and xi[1] < self.imP and xi[1] >= 0 and xi[1] < self.imP:
Ir[xi[0], xi[1]] = value
Ig[xi[0], xi[1]] = 0
Ib[xi[0], xi[1]] = 0
# show image
Irgb = stack((Ir, Ig, Ib), 2)
pyplot.subplot(1, 2, 1)
pyplot.imshow(Irgb, vmin=0.00, vmax=1.00, interpolation="none")
pyplot.subplot(1, 2, 2)
pyplot.imshow(Ih, vmin=0.00, vmax=1.00, interpolation="none", cmap="gray")
fig.suptitle("(Left.) Sensed volume. (Right.) Hand contents.")
for i in xrange(2):
fig.axes[i].set_xticks([])
fig.axes[i].set_yticks([])
pyplot.show(block=True)
T_w_k = berkeley_pr2.get_kinect_transform(robot)
#o = T_w_k[:3,3]
#x = T_w_k[:3,0]
#y = T_w_k[:3,1]
#z = T_w_k[:3,2]
#handles = []
#handles.append(env.drawarrow(o, o+.2*x, .005,(1,0,0,1)))
#handles.append(env.drawarrow(o, o+.2*y, .005,(0,1,0,1)))
#handles.append(env.drawarrow(o, o+.2*z, .005,(0,0,1,1)))
#viewer.Idle()
T_h_k_init = berkeley_pr2.T_h_k
xyz_init = T_h_k_init[:3, 3]
rod_init = openravepy.axisAngleFromRotationMatrix(T_h_k_init[:3, :3])
f_init = 525
#from rapprentice.math_utils import linspace2d
#sim_depths = calculate_sim_depths(xyz_init, rod_init, f_init)
#cv2.imshow('hi',np.clip(sim_depths[0]/3,0,1))
#cv2.waitKey()
#plot_real_and_sim(args2vec(xyz_init, rod_init, f_init))
#calculate_depth_error(xyz_init, rod_init, f_init)
soln = opt.fmin(calc_error_wrapper,
args2vec(xyz_init, rod_init, f_init),
callback=plot_real_and_sim)
(best_xyz, best_rod, best_f) = vec2args(soln)
print "xyz, rod:", best_xyz, best_rod
print "T_h_k:", calc_Thk(best_xyz, best_rod)
def get_object_list(self, req):
#rospy.loginfo('Getting object list')
resp = GetObjectListResponse()
resp.in_goal = False
resp.num_objects = 0
resp.objects = []
bodyNum = 0
for body in self.env_.GetBodies():
body_name = body.GetName().lower()
resp.num_objects += 1
obj = ReconfigurationObject()
s_pose = numpy.array([[0., -1., 0., 0.], [0., 0., 1., 0.],
[-1., 0., 0., 0.], [0., 0., 0., 1.]])
s_inv = numpy.linalg.inv(s_pose)
transform = body.GetTransform()
q = transform[:3, :3]
aa = openravepy.axisAngleFromRotationMatrix(q)
state = self.module.GetState()
obj_bb = body.ComputeAABB()
if body_name == 'target':
obj.type = 'target'
obj.x = 1.0479 - state['movables'][bodyNum][0]
obj.y = state['movables'][bodyNum][1]
if self.initialize == True:
self.target_off = aa[2]
self.target_extents = [
obj_bb.extents()[0],
obj_bb.extents()[1],
obj_bb.extents()[2]
]
obj.rot = state['movables'][bodyNum][2] + self.target_off
obj.xextent = self.target_extents[0]
obj.yextent = self.target_extents[1]
obj.zextent = self.target_extents[2]
in_goal = self.module.CheckGoal(state)
if in_goal:
resp.in_goal = True
bodyNum = bodyNum + 1
elif body_name == 'fuze_bottle':
obj.type = 'fuze_bottle'
obj.x = state['movables'][bodyNum][0]
obj.y = state['movables'][bodyNum][1]
if self.initialize == True:
self.bottle_radius = obj_bb.extents()[0]
obj.xextent = self.bottle_radius
bodyNum = bodyNum + 1
elif body_name in [
'plastic_glass', 'plastic_glass2', 'plastic_glass3'
]:
obj.type = 'plastic_glass'
obj.x = state['movables'][bodyNum][0]
obj.y = state['movables'][bodyNum][1]
if self.initialize == True:
self.glass_radius = obj_bb.extents()[0]
obj.xextent = self.glass_radius
bodyNum = bodyNum + 1
elif body_name in ['pop_tarts', 'pop_tarts2']:
obj.type = 'pop_tarts'
obj.x = 1.0479 - state['movables'][bodyNum][0]
obj.y = state['movables'][bodyNum][1]
if self.initialize == True:
self.pop_tarts_off = aa[2]
self.pop_tarts_extents = [
obj_bb.extents()[0],
obj_bb.extents()[1],
obj_bb.extents()[2]
]
obj.rot = state['movables'][bodyNum][2] + self.pop_tarts_off
obj.xextent = self.pop_tarts_extents[0]
obj.yextent = self.pop_tarts_extents[1]
obj.zextent = self.pop_tarts_extents[2]
bodyNum = bodyNum + 1
elif body_name == 'bh280':
obj.type = 'bh280'
obj.x = state['manip'][0]
obj.y = state['manip'][1]
n_pose = body.GetTransform()
now_in_start = numpy.dot(n_pose[:3, :3], s_inv[:3, :3])
obj.rot = numpy.arctan2(now_in_start[1, 0], now_in_start[0, 0])
if self.initialize == True:
self.hand_off = aa[2]
self.hand_extents = [
obj_bb.extents()[0],
obj_bb.extents()[1],
obj_bb.extents()[2]
]
obj.xextent = self.hand_extents[0]
obj.yextent = self.hand_extents[1]
obj.zextent = self.hand_extents[2]
resp.objects.append(obj)
if self.initialize:
self.startTime = datetime.now()
self.initialize = False
return resp
def PlotImagesContrastAdjusted(self, o, a, desc):
'''Same as PlotImages but with contrast adjusted for visualization.'''
# setup
fig = pyplot.figure()
It = o[:, :, 0] / max(o[:, :, 0])
Ih = zeros(It.shape) if self.IsGraspImage(o) else o[:, :, 1] / max(o[:, :, 1])
Ir = copy(It); Ig = copy(It); Ib = copy(It)
if self.IsOrientationLevel():
# determine rotation angle
R = desc.T[0:3, 0:3]
axisAngle = openravepy.axisAngleFromRotationMatrix(R)
angle = norm(axisAngle)
axis = axisAngle / angle if angle > 0 else array([0.0, 0.0, 1.0])
angle *= sign(sum(axis))
# draw axis indicator
c = self.imP / 2
majorRadius = self.imP / 8
minorRadius = majorRadius if self.IsGraspImage else majorRadius / 2
xx, yy = ellipse_perimeter(c, c, minorRadius, majorRadius, orientation=0)
Ir[xx, yy] = 1.0
Ig[xx, yy] = 0.0
Ib[xx, yy] = 0.0
# draw angle indicator
length = self.imP / 5
x = -int(length * sin(angle))
y = int(length * cos(angle))
xx, yy = line(c, c, c + x, c + y)
Ir[xx, yy] = 1.0
Ig[xx, yy] = 0.0
Ib[xx, yy] = 0.0
xx, yy = line(c, c, c, c + length)
Ir[xx, yy] = 1.0
Ig[xx, yy] = 0.0
Ib[xx, yy] = 0.0
else:
# draw the selection area
halfWidth = (It.shape[0] * (self.selW / self.imW)) / 2.0
middle = It.shape[0] / 2.0
start = int(round(middle - halfWidth))
end = int(round(middle + halfWidth))
pixels = arange(start, end + 1)
if start >= 0 and end < It.shape[0]:
Ir[start, pixels] = 0.0
Ir[end, pixels] = 0.0
Ir[pixels, start] = 0.0
Ir[pixels, end] = 0.0
Ig[start, pixels] = 0.0
Ig[end, pixels] = 0.0
Ig[pixels, start] = 0.0
Ig[pixels, end] = 0.0
Ib[start, pixels] = 1.0
Ib[end, pixels] = 1.0
Ib[pixels, start] = 1.0
Ib[pixels, end] = 1.0
# draw robot's selection
xh = self.actionsInHandFrame[a]
xi = round(((xh[0:2] * self.imP) / self.imW) + ((self.imP - 1.0) / 2.0)).astype('int32')
cross = [(xi[0], xi[1]), (xi[0] + 1, xi[1]), (xi[0] - 1, xi[1]), (xi[0] + 2, xi[1]),
(xi[0] - 2, xi[1]), (xi[0], xi[1]+ 1), (xi[0], xi[1] - 1), (xi[0], xi[1] + 2),
(xi[0], xi[1] - 2)]
for p in cross:
if p[0] >= 0 and p[1] < self.imP and p[1] >= 0 and p[1] < self.imP:
Ir[p[0], p[1]] = 1
Ig[p[0], p[1]] = 0
Ib[p[0], p[1]] = 0
# show image
Irgb = stack((Ir, Ig, Ib), 2)
pyplot.subplot(1, 2, 1)
pyplot.imshow(Irgb, vmin=0.00, vmax=1.00, interpolation="none")
pyplot.subplot(1, 2, 2)
pyplot.imshow(Ih, vmin=0.00, vmax=1.00, interpolation="none", cmap="gray")
fig.suptitle("(Left.) Sensed volume. (Right.) Hand contents.")
for i in xrange(2):
fig.axes[i].set_xticks([])
fig.axes[i].set_yticks([])
pyplot.show(block=True)
def GetCurrentConfiguration(self):
t = self.robot.GetTransform()
aa = openravepy.axisAngleFromRotationMatrix(t)
pose = [t[0, 3], t[1, 3], aa[2]]
return numpy.array(pose)
def axis_angle_from_rot(rot): return axisAngleFromRotationMatrix(rot)