def VerifyIKSolver(youbot, desired_eepose, visualize=False):
if visualize:
h = orpy.misc.DrawAxes(youbot.GetEnv(), desired_eepose, 0.25, 2)
time.sleep(1)
base_poses,arm_configs = FindIKAndBaseSolutions(youbot,desired_eepose,returnfirst=False,checkenvcollision=False,randomize=False)
print 'Found %d IK solutions. Verifying...'%(len(base_poses))
youbot.GetController().Reset()
for pose,q,i in izip(base_poses,arm_configs,range(len(base_poses))):
with youbot:
youbot.SetTransform(pose)
youbot.SetDOFValues(q,youbot.GetManipulators()[0].GetArmIndices())
eepose = youbot.GetManipulators()[0].GetEndEffectorTransform()
if np.linalg.norm(eepose[:3,3]-desired_eepose[:3,3]) > 0.005:
print 'EE is not at the desired position. index: %d.'%(i)
return base_poses,arm_configs
quat1 = orpy.quatFromRotationMatrix(desired_eepose[:3,:3])
quat2 = orpy.quatFromRotationMatrix(eepose[:3,:3])
quatinnerproduct = quat1[0]*quat2[0]+quat1[1]*quat2[1]+quat1[2]*quat2[2]+quat1[3]*quat2[3]
quatdist = np.arccos(2.0* (quatinnerproduct*quatinnerproduct) - 1.0)
if quatdist > 0.001:
print 'EE is not at the desired orientation. index: %d.'%(i)
return base_poses,arm_configs
if visualize:
youbot.GetEnv().UpdatePublishedBodies()
time.sleep(0.001)
print 'All IK solutions verified.'
h = None
return base_poses,arm_configs
def VerifyIKSolver(youbot, desired_eepose, visualize=False):
if visualize:
h = orpy.misc.DrawAxes(youbot.GetEnv(), desired_eepose, 0.25, 2)
time.sleep(1)
base_poses, arm_configs = FindIKAndBaseSolutions(youbot,
desired_eepose,
returnfirst=False,
checkenvcollision=False,
randomize=False)
print 'Found %d IK solutions. Verifying...' % (len(base_poses))
youbot.GetController().Reset()
for pose, q, i in izip(base_poses, arm_configs, range(len(base_poses))):
with youbot:
youbot.SetTransform(pose)
youbot.SetDOFValues(q, youbot.GetManipulators()[0].GetArmIndices())
eepose = youbot.GetManipulators()[0].GetEndEffectorTransform()
if np.linalg.norm(eepose[:3, 3] - desired_eepose[:3, 3]) > 0.005:
print 'EE is not at the desired position. index: %d.' % (i)
return base_poses, arm_configs
quat1 = orpy.quatFromRotationMatrix(desired_eepose[:3, :3])
quat2 = orpy.quatFromRotationMatrix(eepose[:3, :3])
quatinnerproduct = quat1[0] * quat2[0] + quat1[1] * quat2[
1] + quat1[2] * quat2[2] + quat1[3] * quat2[3]
quatdist = np.arccos(2.0 * (quatinnerproduct * quatinnerproduct) -
1.0)
if quatdist > 0.001:
print 'EE is not at the desired orientation. index: %d.' % (i)
return base_poses, arm_configs
if visualize:
youbot.GetEnv().UpdatePublishedBodies()
time.sleep(0.001)
print 'All IK solutions verified.'
h = None
return base_poses, arm_configs
def GetStraightVelocity(self, manip, velocity, initial_hand_pose, nullspace_fn, step_size, sign_flipper = 1):
robot = manip.GetRobot()
current_hand_pose = manip.GetEndEffectorTransform()
initial_position = initial_hand_pose[0:3, 3]
current_position = current_hand_pose[0:3, 3]
# Simulate a position goal step_size distance further along the velocity vector.
moved_already = velocity * numpy.dot(current_position - initial_position, velocity)
desired_position = initial_position + moved_already + velocity * step_size
error_pos = desired_position - current_position
# Append the desired quaternion to create the error vector. There is a
# sign ambiguity on quaternions, so we'll always choose the shortest path.
initial_ori = openravepy.quatFromRotationMatrix(initial_hand_pose)
current_ori = openravepy.quatFromRotationMatrix(current_hand_pose)
choices_ori = [ current_ori - initial_ori, current_ori + initial_ori ]
error_ori = sign_flipper*min(choices_ori, key=lambda q: numpy.linalg.norm(q))
# Jacobian pseudo-inverse.
jacobian_spatial = manip.CalculateJacobian()
jacobian_angular = manip.CalculateRotationJacobian() #this function seems very buggy/wrong
jacobian = numpy.vstack((jacobian_spatial, jacobian_angular))
jacobian_pinv = numpy.linalg.pinv(jacobian)
# Null-space projector
nullspace_projector = numpy.eye(jacobian.shape[1]) - numpy.dot(jacobian_pinv, jacobian)
nullspace_goal = nullspace_fn(robot)
pose_error = numpy.hstack((error_pos, error_ori))
return numpy.dot(jacobian_pinv, pose_error) + numpy.dot(nullspace_projector, nullspace_goal)
def straight_line_jacobian(self, distance):
graph = np.array([])
points =np.array([self.robot.GetActiveDOFValues()])
delta_d = 0.001
distance_to_goal = 1
#constructing the jacobian
j_spatial = self.manip.CalculateJacobian()
j_rot = self.manip.CalculateRotationJacobian()
J = np.vstack((j_spatial, j_rot))
Jinv = np.linalg.pinv(J) #pseudo-inverse
#end effector data
ee_trans = self.end_effector.GetTransform()
ee_pos_init = ee_trans[0:3,3] #position of the end effector
ee_quat_init = openravepy.quatFromRotationMatrix(ee_trans[0:3,0:3])
x_present = np.append(ee_pos_init, ee_quat_init)
x_add = [delta_d, 0, 0, 0, 0, 0, 0]
x_goal = x_present + x_add
x_present_x = ee_pos_init[0]
while np.abs(distance_to_goal) > delta_d:
q_present = self.robot.GetActiveDOFValues()
delta_x = x_goal - x_present
delta_q = np.array(np.dot(Jinv,delta_x))
q_new = np.array(q_present + delta_q)
check = self.check_collision(q_new)
if(check):
print ' the final positon of the end effector is:'
print x_present[0:3]
break
else:
with self.env:
self.robot.SetActiveDOFValues(q_new)
points = np.append(points, np.array([q_present + delta_q]), axis=0)
j_spatial = self.manip.CalculateJacobian()
j_rot = self.manip.CalculateRotationJacobian()
J = np.vstack((j_spatial, j_rot))
Jinv = np.linalg.pinv(J)
ee_trans = self.end_effector.GetTransform()
ee_quat = openravepy.quatFromRotationMatrix(ee_trans[0:3,0:3])
ee_pos = ee_trans[0:3,3]
x_present = np.append(ee_pos, ee_quat)
x_present_x = x_present_x + delta_d
x_goal = np.append([x_present_x, x_present[1], x_present[2]], ee_quat)
distance_to_goal = x_goal[0]-(ee_pos_init[0]+distance)
traj = self.points_to_traj(np.array(points))
#print points
return traj
def rotation_interpolation(r1, r2, t):
quat1 = rave.quatFromRotationMatrix(r1)
quat2 = rave.quatFromRotationMatrix(r2)
# print(r1)
# print(r2)
t = min(max(t, 0), 1)
quat_interpolate = rave.quatSlerp(quat1, quat2, t, True)
return rave.matrixFromQuat(quat_interpolate)[0:3, 0:3]
def IsSameGrasp(self,g1,g2):
diff = g1[:3,3] - g2[:3,3]
#if np.linalg.norm(diff) < 0.001:
if (diff[0] ** 2 + diff[1] ** 2 + diff[2] ** 2) > 0.00001:
return False
qd=orpy.quatMult(orpy.quatInverse(orpy.quatFromRotationMatrix(g1[:3,:3])),orpy.quatFromRotationMatrix(g2[:3,:3]))
shortest_arc_angle = 2.0*np.arctan2(np.linalg.norm(qd[1:4]),qd[0])
if shortest_arc_angle > 0.001:
return False
return True
def IsSameTransform(tr1, tr2):
# FIXME just check element-by-element similarity?
if np.linalg.norm(tr1[:3,3]-tr2[:3,3]) > 0.0001:
return False
quat1 = orpy.quatFromRotationMatrix(tr1[:3,:3])
quat2 = orpy.quatFromRotationMatrix(tr2[:3,:3])
quatinnerproduct = quat1[0]*quat2[0]+quat1[1]*quat2[1]+quat1[2]*quat2[2]+quat1[3]*quat2[3]
quatdist = np.arccos(2.0* (quatinnerproduct*quatinnerproduct) - 1.0)
if quatdist > 0.00001:
return False
return True
def IsSameTransform(tr1, tr2):
# FIXME just check element-by-element similarity?
if np.linalg.norm(tr1[:3, 3] - tr2[:3, 3]) > 0.0001:
return False
quat1 = orpy.quatFromRotationMatrix(tr1[:3, :3])
quat2 = orpy.quatFromRotationMatrix(tr2[:3, :3])
quatinnerproduct = quat1[0] * quat2[0] + quat1[1] * quat2[1] + quat1[
2] * quat2[2] + quat1[3] * quat2[3]
quatdist = np.arccos(2.0 * (quatinnerproduct * quatinnerproduct) - 1.0)
if quatdist > 0.00001:
return False
return True
def IsSameGrasp(self, g1, g2):
diff = g1[:3, 3] - g2[:3, 3]
#if np.linalg.norm(diff) < 0.001:
if (diff[0]**2 + diff[1]**2 + diff[2]**2) > 0.00001:
return False
qd = orpy.quatMult(
orpy.quatInverse(orpy.quatFromRotationMatrix(g1[:3, :3])),
orpy.quatFromRotationMatrix(g2[:3, :3]))
shortest_arc_angle = 2.0 * np.arctan2(np.linalg.norm(qd[1:4]), qd[0])
if shortest_arc_angle > 0.001:
return False
return True
def RegisterBody(self, or_body, tf_id, openrave_frame_in_tf_frame=None, planar_tracking=False, fixed_translation_z=-1.0):
x = 0.0
y = 0.0
z = 0.0
qw = 1.0
qx = 0.0
qy = 0.0
qz = 0.0
if openrave_frame_in_tf_frame is not None:
x = openrave_frame_in_tf_frame[0,3]
y = openrave_frame_in_tf_frame[1,3]
z = openrave_frame_in_tf_frame[2,3]
quat = orpy.quatFromRotationMatrix(openrave_frame_in_tf_frame[:3,:3])
qw = quat[0]
qx = quat[1]
qy = quat[2]
qz = quat[3]
cmd = 'RegisterBody ' + or_body.GetName() + ' ' + tf_id + ' openrave_frame_in_tf_frame ' + str(x) + ' ' + str(y) + ' ' + str(z) + ' ' + str(qw) + ' ' + str(qx) + ' ' + str(qy) + ' ' + str(qz)
if planar_tracking:
cmd +=' planar_tracking' + ' ' + 'fixed_translation_z' + ' ' + str(fixed_translation_z)
self.or_tf.SendCommand(cmd)
def serialize_transform(t):
from openravepy import quatFromRotationMatrix
return {
'position': list(map(float,t[0:3, 3])),
'orientation': list(map(float,quatFromRotationMatrix(t[0:3, 0:3]))),
}
def inverse_kinematics(robot, manip_name, ee_link_name, ee_hmat):
init_guess = np.zeros(7)
xyz_target = ee_hmat[:3, 3]
quat_target = openravepy.quatFromRotationMatrix(ee_hmat[:3, :3])
request = {
"basic_info": {
"n_steps": 10,
"manip": manip_name, # see below for valid values
"start_fixed": False, # i.e., DOF values at first timestep are fixed based on current robot state
},
"costs": [
{
"type": "joint_vel", # joint-space velocity cost
"params": {"coeffs": [1]}, # a list of length one is automatically expanded to a list of length n_dofs
},
{
"type": "collision",
"name": "cont_coll", # shorten name so printed table will be prettier
"params": {
"continuous": False,
"coeffs": [
50
], # penalty coefficients. list of length one is automatically expanded to a list of length n_timesteps
"dist_pen": [0.05], # robot-obstacle distance that penalty kicks in. expands to length n_timesteps
},
},
],
"constraints": [
# BEGIN pose_constraint
{
"type": "pose",
"params": {
"xyz": xyz_target.tolist(),
"wxyz": quat_target.tolist(),
"link": ee_link_name,
"pos_coeffs": [10, 10, 10],
"rot_coeffs": [10, 10, 10],
},
}
# END pose_constraint
],
# BEGIN init
"init_info": {
"type": "straight_line", # straight line in joint space.
"endpoint": init_guess.tolist(), # need to convert numpy array to list
}
# END init
}
s = json.dumps(request)
prob = trajoptpy.ConstructProblem(s, robot.GetEnv()) # create object that stores optimization problem
result = trajoptpy.OptimizeProblem(prob) # do optimization
traj = result.GetTraj()
return traj[-1]
def getPoseMsg(transform): #Convert transform into ROS poseStamped message
robotPose = Pose()
q = openravepy.quatFromRotationMatrix(transform[0:3, 0:3])
robotPose.position.x = transform[0][3]
robotPose.position.y = transform[1][3]
robotPose.position.z = transform[2][3]
robotPose.orientation.x = q[1]
robotPose.orientation.y = q[2]
robotPose.orientation.z = q[3]
robotPose.orientation.w = q[0]
return robotPose
def straight_line_jacobian(self, distance):
j_spatial = self.manip.CalculateJacobian()
j_rot = self.manip.CalculateRotationJacobian()
J = np.vstack((j_spatial, j_rot))
Jinv = np.linalg.pinv(J) #pseudo-inverse
u,s,v=np.linalg.svd(J)
quat = openravepy.quatFromRotationMatrix(trans)
vh=v.T
jac_null=vh[:,-1:]
return jac_null
def GetStraightVelocity(self,
manip,
velocity,
initial_hand_pose,
nullspace_fn,
step_size,
sign_flipper=1):
robot = manip.GetRobot()
current_hand_pose = manip.GetEndEffectorTransform()
initial_position = initial_hand_pose[0:3, 3]
current_position = current_hand_pose[0:3, 3]
# Simulate a position goal step_size distance further along the velocity vector.
moved_already = velocity * numpy.dot(
current_position - initial_position, velocity)
desired_position = initial_position + moved_already + velocity * step_size
error_pos = desired_position - current_position
# Append the desired quaternion to create the error vector. There is a
# sign ambiguity on quaternions, so we'll always choose the shortest path.
initial_ori = openravepy.quatFromRotationMatrix(initial_hand_pose)
current_ori = openravepy.quatFromRotationMatrix(current_hand_pose)
choices_ori = [current_ori - initial_ori, current_ori + initial_ori]
error_ori = sign_flipper * min(choices_ori,
key=lambda q: numpy.linalg.norm(q))
# Jacobian pseudo-inverse.
jacobian_spatial = manip.CalculateJacobian()
jacobian_angular = manip.CalculateRotationJacobian(
) #this function seems very buggy/wrong
jacobian = numpy.vstack((jacobian_spatial, jacobian_angular))
jacobian_pinv = numpy.linalg.pinv(jacobian)
# Null-space projector
nullspace_projector = numpy.eye(jacobian.shape[1]) - numpy.dot(
jacobian_pinv, jacobian)
nullspace_goal = nullspace_fn(robot)
pose_error = numpy.hstack((error_pos, error_ori))
return numpy.dot(jacobian_pinv, pose_error) + numpy.dot(
nullspace_projector, nullspace_goal)
def check_traj_pose(env, result, target_left, target_right, threshold=0.01):
#check trajectory with Cartesian target
robot = env.GetRobots()[0]
traj = result.GetTraj()
#check for collision
if traj_is_safe(traj, robot) is not True:
print "There is a collision within the trajectory!"
return False
#check optimal solver status
if (result.GetStatus()[0] is not 0):
print "Optimization is not converged!"
return False
robot.SetActiveDOFValues(traj[-1])
T_left = robot.GetLink('l_gripper_tool_frame').GetTransform()
T_right = robot.GetLink('r_gripper_tool_frame').GetTransform()
pose_left = np.concatenate(
[T_left[0:3, 3].flatten(),
openravepy.quatFromRotationMatrix(T_left)])
pose_right = np.concatenate([
T_right[0:3, 3].flatten(),
openravepy.quatFromRotationMatrix(T_right)
])
#check target for joint constraints
if (np.linalg.norm(target_left - pose_left) > threshold):
print target_left, pose_left
print('Left pose is not reached!')
return False
if (np.linalg.norm(target_right - pose_right) > threshold):
print target_right, pose_right
print('Right pose is not reached!')
return False
#Otherwise, return True
return True
def AddGaussianNoise(self, orientationSigma, positionSigma):
'''Returns a new grasp with noise added to the current position and orientation.'''
descriptor = self if rand() < 0.5 else self.Flip()
q = openravepy.quatFromRotationMatrix(descriptor.T)
q += orientationSigma * randn(4)
q /= norm(q)
T = openravepy.matrixFromQuat(q)
c = descriptor.center + positionSigma * descriptor.center
T[0:3, 3] = c
return HandDescriptor(\
T, image=None, function=descriptor.function, objectName=descriptor.objectName)
def calculate_sim_depths(xyz, rod, f):
T_h_k = calc_Thk(xyz, rod)
T_w_k = robot.GetLink("head_plate_frame").GetTransform().dot(T_h_k)
sensor.SetPose(T_w_k[:3,3], openravepy.quatFromRotationMatrix(T_w_k[:3,:3]))
sensor.SetIntrinsics(f)
out = []
for dofs in rave_values:
robot.SetActiveDOFValues(dofs)
viewer.UpdateSceneData()
sensor.Update()
out.append(sensor.GetDepthImage())
return out
def update(self):
import rospy
with self.body.GetEnv():
or_pose = self.body.GetTransform()
or_quaternion = openravepy.quatFromRotationMatrix(or_pose)
position = tuple(or_pose[0:3, 3])
orientation = (or_quaternion[1], or_quaternion[2], or_quaternion[3], or_quaternion[0])
self.broadcaster.sendTransform(position, orientation, rospy.Time.now(),
self.child_frame, self.parent_frame)
self.timer = threading.Timer(self.period, self.update)
self.timer.daemon = True
self.timer.start()
def calculate_sim_depths(xyz, rod, f):
T_h_k = calc_Thk(xyz, rod)
T_w_k = robot.GetLink("head_plate_frame").GetTransform().dot(T_h_k)
sensor.SetPose(T_w_k[:3, 3],
openravepy.quatFromRotationMatrix(T_w_k[:3, :3]))
sensor.SetIntrinsics(f)
out = []
for dofs in rave_values:
robot.SetActiveDOFValues(dofs)
viewer.UpdateSceneData()
sensor.Update()
out.append(sensor.GetDepthImage())
return out
def RegisterBody(self,
or_body,
tf_id,
openrave_frame_in_tf_frame=None,
planar_tracking=False):
if openrave_frame_in_tf_frame is None:
self.tfplugin.SendCommand('RegisterBody ' + or_body.GetName() +
' ' + tf_id)
else:
x = openrave_frame_in_tf_frame[0, 3]
y = openrave_frame_in_tf_frame[1, 3]
z = openrave_frame_in_tf_frame[2, 3]
quat = orpy.quatFromRotationMatrix(
openrave_frame_in_tf_frame[:3, :3])
cmd = 'RegisterBody ' + or_body.GetName(
) + ' ' + tf_id + ' openrave_frame_in_tf_frame ' + str(
x) + ' ' + str(y) + ' ' + str(z) + ' ' + str(
quat[0]) + ' ' + str(quat[1]) + ' ' + str(
quat[2]) + ' ' + str(quat[3])
if planar_tracking:
cmd = cmd + ' planar_tracking'
self.tfplugin.SendCommand(cmd)
def quat_from_rotation_matrix(R):
return quatFromRotationMatrix(R)
def RosTransformFromRaveMatrix(mat):
out = gm.Transform()
out.translation = gm.Vector3(*mat[:3, 3])
qw, qx, qy, qz = openravepy.quatFromRotationMatrix(mat[:3, :3])
out.rotation = gm.Quaternion(qx, qy, qz, qw)
return out
def PlanToEndEffectorOffset(self,
robot,
direction,
distance,
max_distance=None,
position_tolerance=0.01,
angular_tolerance=0.15,
**kwargs):
"""
Plan to an end-effector offset with a move-hand-straight constraint.
This function plans a trajectory to purely translate a hand in the
given direction as far as possible before collision. Movement less
than min_distance will return failure. The motion will not also move
further than max_distance.
@param robot the robot whose active DOFs will be used
@param direction unit vector in the direction of motion
@param distance minimum distance in meters
@param max_distance maximum distance in meters
@param position_tolerance constraint tolerance in meters
@param angular_tolerance constraint tolerance in radians
@return traj a trajectory following specified direction
"""
# Plan using the active manipulator.
with robot.GetEnv():
manipulator = robot.GetActiveManipulator()
pose = manipulator.GetEndEffectorTransform()
# Convert IK endpoint transformation to pose.
ee_position = pose[0:3, 3].tolist()
ee_rotation = openravepy.quatFromRotationMatrix(pose).tolist()
# Create dummy frame that has direction vector along Z.
z = direction
y = numpy.cross(z, pose[0:3, 0]) # Cross with EE-frame Y.
y /= numpy.linalg.norm(y)
x = numpy.cross(y, z)
cost_frame = numpy.vstack((x, y, z)).T
cost_rotation = openravepy.quatFromRotationMatrix(cost_frame).tolist()
# Construct a planning request with these constraints.
n_steps = 10
request = {
"basic_info": {
"n_steps": n_steps,
"manip": "active",
"start_fixed": True
},
"costs": [{
"type": "joint_vel",
"params": {
"coeffs": [1]
}
}, {
"type": "collision",
"params": {
"coeffs": [20],
"dist_pen": [0.025]
},
}, {
"type": "pose",
"params": {
"xyz": [0, 0, 0],
"wxyz": cost_rotation,
"link": manipulator.GetEndEffector().GetName(),
"rot_coeffs": [0, 0, 0],
"pos_coeffs": [0, 0, 10]
}
}],
"constraints": [{
"type": "pose",
"params": {
"xyz": ee_position,
"wxyz": ee_rotation,
"link": manipulator.GetEndEffector().GetName(),
# "first_step": 0,
# "last_step": n_steps-1,
}
}],
"init_info": {
"type": "stationary"
}
}
# Set active DOFs to match active manipulator and plan.
p = openravepy.KinBody.SaveParameters
with robot.CreateRobotStateSaver(p.ActiveDOF):
robot.SetActiveDOFs(manipulator.GetArmIndices())
return self._Plan(robot, request, **kwargs)
LINK_NAME = "l_gripper_tool_frame"
#0.0640945 0.704273 -0.147528 0.691468 -0.168806 -0.0452678 0.762821
##################
### Env setup ####
env = rave.Environment()
env.StopSimulation()
env.Load(ENV_FILE)
robot = env.GetRobots()[0]
robot.SetDOFValues([
0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, -0.119839, 0.861339,
-2.22045e-16, -1.87834, -3.00095, -1.02143, 3.0678, 0.54, -1.01863e-14,
0, -1.59595e-16, 0, 0, 0, 0, 0, 0, 0, 0, 0, -8.16014e-15, 0,
-1.11022e-16, 0
])
manip = robot.GetManipulator(MANIP_NAME)
##################
T_gripper = robot.GetLink(LINK_NAME).GetTransform()
T_grasp = T_gripper.copy()
T_grasp[:3, 3] += np.array([0, 0.1, 0])
xyz_targ = T_grasp[:3, 3]
success = mk.create_cylinder(env, xyz_targ - np.array([.03, 0, 0]), .02,
.5)
quat_targ = rave.quatFromRotationMatrix(T_grasp[:3, :3])
request = move_arm_to_grasp(xyz_targ, quat_targ, LINK_NAME, MANIP_NAME)
s = json.dumps(request)
print "REQUEST:", s
trajoptpy.SetInteractive(args.interactive)
prob = trajoptpy.ConstructProblem(s, env)
result = trajoptpy.OptimizeProblem(prob)
def RosTransformFromRaveMatrix(mat):
out = gm.Transform()
out.translation = gm.Vector3(*mat[:3,3])
qw,qx,qy,qz = openravepy.quatFromRotationMatrix(mat[:3,:3])
out.rotation = gm.Quaternion(qx,qy,qz, qw)
return out
def RosPoseFromRaveMatrix(mat):
out = gm.Pose()
out.position = gm.Point(*mat[:3,3])
qw,qx,qy,qz = openravepy.quatFromRotationMatrix(mat[:3,:3])
out.orientation = gm.Quaternion(qx,qy,qz, qw)
return out
#robot.WaitForController(0) # wait
#raw_input('Hit ENTER to continue.')
##################
#T_gripper = robot.GetLink(LINK_NAME).GetTransform()
#T_grasp = T_gripper.copy()
#T_grasp[:3,3] += np.array([-0.5,0.2,0])
#T_grasp = T_grasp.dot(rave.matrixFromAxisAngle([0,0,1],np.pi/2))
#xyz_targ = T_grasp[:3,3]
#success = mk.create_cylinder(env, xyz_targ-np.array([.03,0,0]), .02, .5)
#quat_targ = rave.quatFromRotationMatrix(T_grasp[:3,:3])
#success = mk.create_cylinder(env, T_gripper[:3,3]-np.array([.1,-.1,0]), .02, .5)
Tgoal = np.array([[0,0,1,0.6],[0,1,0,0.4],[-1,0,0,0.6],[0,0,0,1]])
xyz_targ = Tgoal[:3,3]
quat_targ = rave.quatFromRotationMatrix(Tgoal[:3,:3])
request = move_arm_to_grasp(xyz_targ, quat_targ, LINK_NAME, MANIP_NAME)
path_init = np.load("../data/barrett_traj3.npy")
robot.SetActiveDOFValues(path_init[0,:])
s = json.dumps(request)
print "REQUEST:",s
trajoptpy.SetInteractive(args.interactive)
prob = trajoptpy.ConstructProblem(s, env)
result = trajoptpy.OptimizeProblem(prob)
#stat = trajoptpy.SimulateAndReplan(s, env, 4)
# stats = np.zeros((8,100))
manip = robot.GetManipulator(MANIP_NAME)
viewer = trajoptpy.GetViewer(env)
viewer.SetCameraTransformation([0,0,20], [0,0,0], [0,1,0])
##################
T_gripper = np.eye(4)
T_gripper[:3,3] = np.array([2,2,0])
robot.GetLink(LINK_NAME).SetTransform(T_gripper)
T_grasp = np.eye(4)
xyz_targ = T_grasp[:3,3]
# mk.create_mesh_box(env, np.array([5+0.01,0,0]), np.array([0.01,10,0.01]))
mk.create_mesh_box(env, np.array([3.5,3,0]), np.array([1,1,2]), "box1")
mk.create_mesh_box(env, np.array([3.5,-0.5,0]), np.array([1,1,2]), "box2")
# mk.create_mesh_box(env, np.array([3.5,1.25,0]), np.array([0.2,0.2,1]), "box3")
# mk.create_mesh_box(env, np.array([3.5,2,0]), np.array([0.2,0.2,1]), "box4")
quat_targ = rave.quatFromRotationMatrix(T_grasp[:3,:3])
request = move_arm_to_grasp(xyz_targ, quat_targ, LINK_NAME, MANIP_NAME)
##################
'''
# first optimize ignoring collision costs
all_costs = request["costs"]
noncollision_costs = [cost for cost in request["costs"] if "collision" not in cost["type"]]
request["costs"] = noncollision_costs
saver = rave.Robot.RobotStateSaver(robot)
s = json.dumps(request)
print "REQUEST:",s
trajoptpy.SetInteractive(False);
def PlanToEndEffectorOffset(self, robot, direction, distance,
max_distance=None,
position_tolerance=0.01,
angular_tolerance=0.15, **kwargs):
"""
Plan to an end-effector offset with a move-hand-straight constraint.
This function plans a trajectory to purely translate a hand in the
given direction as far as possible before collision. Movement less
than min_distance will return failure. The motion will not also move
further than max_distance.
@param robot the robot whose active DOFs will be used
@param direction unit vector in the direction of motion
@param distance minimum distance in meters
@param max_distance maximum distance in meters
@param position_tolerance constraint tolerance in meters
@param angular_tolerance constraint tolerance in radians
@return traj a trajectory following specified direction
"""
# Plan using the active manipulator.
with robot.GetEnv():
manipulator = robot.GetActiveManipulator()
pose = manipulator.GetEndEffectorTransform()
# Convert IK endpoint transformation to pose.
ee_position = pose[0:3, 3].tolist()
ee_rotation = openravepy.quatFromRotationMatrix(pose).tolist()
# Create dummy frame that has direction vector along Z.
z = direction
y = numpy.cross(z, pose[0:3, 0]) # Cross with EE-frame Y.
y /= numpy.linalg.norm(y)
x = numpy.cross(y, z)
cost_frame = numpy.vstack((x, y, z)).T
cost_rotation = openravepy.quatFromRotationMatrix(cost_frame).tolist()
# Construct a planning request with these constraints.
n_steps = 10
request = {
"basic_info": {
"n_steps": n_steps,
"manip": "active",
"start_fixed": True
},
"costs": [
{
"type": "joint_vel",
"params": {"coeffs": [1]}
},
{
"type": "collision",
"params": {
"coeffs": [20],
"dist_pen": [0.025]
},
},
{
"type": "pose",
"params": {
"xyz": [0, 0, 0],
"wxyz": cost_rotation,
"link": manipulator.GetEndEffector().GetName(),
"rot_coeffs": [0, 0, 0],
"pos_coeffs": [0, 0, 10]
}
}
],
"constraints": [
{
"type": "pose",
"params": {
"xyz": ee_position,
"wxyz": ee_rotation,
"link": manipulator.GetEndEffector().GetName(),
# "first_step": 0,
# "last_step": n_steps-1,
}
}
],
"init_info": {
"type": "stationary"
}
}
# Set active DOFs to match active manipulator and plan.
p = openravepy.KinBody.SaveParameters
with robot.CreateRobotStateSaver(p.ActiveDOF):
robot.SetActiveDOFs(manipulator.GetArmIndices())
return self._Plan(robot, request, **kwargs)
def RosPoseFromRaveMatrix(mat):
out = gm.Pose()
out.position = gm.Point(*mat[:3, 3])
qw, qx, qy, qz = openravepy.quatFromRotationMatrix(mat[:3, :3])
out.orientation = gm.Quaternion(qx, qy, qz, qw)
return out
def quat_from_rot(rot):
return quatFromRotationMatrix(rot)
def _PlanToIK(self, robot, pose,
ranker=None, **kwargs):
# Plan using the active manipulator.
with robot.GetEnv():
manipulator = robot.GetActiveManipulator()
# Distance from current configuration is default ranking.
if ranker is None:
from prpy.ik_ranking import NominalConfiguration
ranker = NominalConfiguration(manipulator.GetArmDOFValues())
# Find initial collision-free IK solution.
from openravepy import (IkFilterOptions,
IkParameterization,
IkParameterizationType)
ik_param = IkParameterization(
pose, IkParameterizationType.Transform6D)
ik_solutions = manipulator.FindIKSolutions(
ik_param, IkFilterOptions.CheckEnvCollisions)
if not len(ik_solutions):
raise PlanningError('No collision-free IK solution.')
# Sort the IK solutions in ascending order by the costs returned by the
# ranker. Lower cost solutions are better and infinite cost solutions
# are assumed to be infeasible.
scores = ranker(robot, ik_solutions)
best_idx = numpy.argmin(scores)
init_joint_config = ik_solutions[best_idx]
# Convert IK endpoint transformation to pose.
goal_position = pose[0:3, 3].tolist()
goal_rotation = openravepy.quatFromRotationMatrix(pose).tolist()
# Settings for TrajOpt
num_steps = 10
# Construct a planning request with these constraints.
request = {
"basic_info": {
"n_steps": num_steps,
"manip": "active",
"start_fixed": True
},
"costs": [
{
"type": "joint_vel",
"params": {"coeffs": [1]}
},
{
"type": "collision",
"params": {
"coeffs": [20],
"dist_pen": [0.025]
},
}
],
"constraints": [
{
"type": "pose",
"params": {
"xyz": goal_position,
"wxyz": goal_rotation,
"link": manipulator.GetEndEffector().GetName(),
"timestep": num_steps-1
}
}
],
"init_info": {
"type": "straight_line",
"endpoint": init_joint_config.tolist()
}
}
# Set active DOFs to match active manipulator and plan.
p = openravepy.KinBody.SaveParameters
with robot.CreateRobotStateSaver(p.ActiveDOF):
robot.SetActiveDOFs(manipulator.GetArmIndices())
return self._Plan(robot, request, **kwargs)
def _PlanToIK(self, robot, robot_checker, pose, ranker=None, **kwargs):
# Plan using the active manipulator.
manipulator = robot.GetActiveManipulator()
# Distance from current configuration is default ranking.
if ranker is None:
from prpy.ik_ranking import NominalConfiguration
ranker = NominalConfiguration(manipulator.GetArmDOFValues())
# Find initial collision-free IK solution.
ik_param = IkParameterization(
pose, IkParameterizationType.Transform6D)
ik_solutions = manipulator.FindIKSolutions(
ik_param, IkFilterOptions.CheckEnvCollisions)
if not len(ik_solutions):
# Identify collision and raise error.
self._raiseCollisionErrorForPose(robot, robot_checker, pose)
# Sort the IK solutions in ascending order by the costs returned by the
# ranker. Lower cost solutions are better and infinite cost solutions
# are assumed to be infeasible.
scores = ranker(robot, ik_solutions)
best_idx = numpy.argmin(scores)
init_joint_config = ik_solutions[best_idx]
# Convert IK endpoint transformation to pose. OpenRAVE operates on
# GetEndEffectorTransform(), which is equivalent to:
#
# GetEndEffector().GetTransform() * GetLocalToolTransform()
#
link_pose = numpy.dot(
pose, numpy.linalg.inv(
manipulator.GetLocalToolTransform()))
goal_position = link_pose[0:3, 3].tolist()
goal_rotation = openravepy.quatFromRotationMatrix(link_pose).tolist()
# Settings for TrajOpt
num_steps = 10
# Construct a planning request with these constraints.
request = {
"basic_info": {
"n_steps": num_steps
},
"costs": [
{
"type": "joint_vel",
"params": {"coeffs": [1]}
},
{
"type": "collision",
"params": {
"coeffs": [20],
"dist_pen": [0.025]
},
}
],
"constraints": [
{
"type": "pose",
"params": {
"xyz": goal_position,
"wxyz": goal_rotation,
"link": manipulator.GetEndEffector().GetName(),
"timestep": num_steps-1
}
}
],
"init_info": {
"type": "straight_line",
"endpoint": init_joint_config.tolist()
}
}
p = openravepy.KinBody.SaveParameters
with robot.CreateRobotStateSaver(p.ActiveDOF):
robot.SetActiveDOFs(manipulator.GetArmIndices())
return self._Plan(robot, robot_checker, request, **kwargs)
def quat_from_rot(rot): return quatFromRotationMatrix(rot)
