def TerminateMove():
"""
Function defining the termination condition.
Fail if deviation larger than position and angular tolerance.
Succeed if distance moved is larger than max_distance.
Cache and continue if distance moved is larger than distance.
"""
from .exceptions import ConstraintViolationPlanningError
T_ee_curr = manip.GetEndEffectorTransform()
# Find where we are on the goal trajectory by finding
# the the closest point
(_, t, _) = util.GetMinDistanceBetweenTransformAndWorkspaceTraj(
T_ee_curr, traj, 0.0005)
# Get the desired end-effector transform from
# the goal trajectory
desired_T_ee = openravepy.matrixFromPose(traj.Sample(t)[0:7])
# Get the position vector tangent to the trajectory,
# using finite-differences
pose_ee_next = traj.Sample(t + t_step)[0:7]
desired_T_ee_next = openravepy.matrixFromPose(pose_ee_next)
tangent_vec = desired_T_ee_next[0:3, 3] - desired_T_ee[0:3, 3]
# Calculate error between current end-effector pose
# and where we should be on the goal trajectory
geodesic_error = util.GeodesicError(desired_T_ee, T_ee_curr)
orientation_error = geodesic_error[3]
position_error_vec = geodesic_error[0:3]
# Use only the translation error that is perpendicular
# to our current position
tangent_trans_error = \
position_error_vec - numpy.dot(
position_error_vec, util.NormalizeVector(tangent_vec))
tangent_trans_error = numpy.nan_to_num(tangent_trans_error)
position_error = tangent_trans_error
if numpy.fabs(orientation_error) > angular_tolerance:
raise ConstraintViolationPlanningError(
'Deviated from orientation constraint.')
position_deviation = numpy.linalg.norm(position_error)
if position_deviation > position_tolerance:
raise ConstraintViolationPlanningError(
'Deviated from straight line constraint.')
# Check if we have reached the end of the goal trajectory
error_to_goal = util.GeodesicError(T_ee_curr, T_ee_goal)
orientation_error = error_to_goal[3] # radians
position_error = error_to_goal[0:3] # x,y,z
if ((numpy.fabs(orientation_error) < angular_tolerance) and
(numpy.linalg.norm(position_error) < position_tolerance)):
return Status.CACHE_AND_TERMINATE
return Status.CONTINUE
def TerminateMove():
"""
Function defining the termination condition.
Fail if deviation larger than position and angular tolerance.
Succeed if distance moved is larger than max_distance.
Cache and continue if distance moved is larger than distance.
"""
from .exceptions import ConstraintViolationPlanningError
T_ee_curr = manip.GetEndEffectorTransform()
# Find where we are on the goal trajectory by finding
# the the closest point
(_, t, _) = util.GetMinDistanceBetweenTransformAndWorkspaceTraj(
T_ee_curr, traj, 0.0005)
# Get the desired end-effector transform from
# the goal trajectory
desired_T_ee = openravepy.matrixFromPose(traj.Sample(t)[0:7])
# Get the position vector tangent to the trajectory,
# using finite-differences
pose_ee_next = traj.Sample(t + t_step)[0:7]
desired_T_ee_next = openravepy.matrixFromPose(pose_ee_next)
tangent_vec = desired_T_ee_next[0:3, 3] - desired_T_ee[0:3, 3]
# Calculate error between current end-effector pose
# and where we should be on the goal trajectory
geodesic_error = util.GeodesicError(desired_T_ee, T_ee_curr)
orientation_error = geodesic_error[3]
position_error_vec = geodesic_error[0:3]
# Use only the translation error that is perpendicular
# to our current position
tangent_trans_error = \
position_error_vec - numpy.dot(
position_error_vec, util.NormalizeVector(tangent_vec))
tangent_trans_error = numpy.nan_to_num(tangent_trans_error)
position_error = tangent_trans_error
if numpy.fabs(orientation_error) > angular_tolerance:
raise ConstraintViolationPlanningError(
'Deviated from orientation constraint.')
position_deviation = numpy.linalg.norm(position_error)
if position_deviation > position_tolerance:
raise ConstraintViolationPlanningError(
'Deviated from straight line constraint.')
# Check if we have reached the end of the goal trajectory
error_to_goal = util.GeodesicError(T_ee_curr, T_ee_goal)
orientation_error = error_to_goal[3] # radians
position_error = error_to_goal[0:3] # x,y,z
if ((numpy.fabs(orientation_error) < angular_tolerance) and
(numpy.linalg.norm(position_error) < position_tolerance)):
return Status.CACHE_AND_TERMINATE
return Status.CONTINUE
def test_plan_to_pose(xyz, xyzw, leftright, robot, initial_state = build_robot_state(), planner_id='', target_link="%s_gripper_tool_frame"):
manip = robot.GetManipulator(leftright + "arm")
base_pose = initial_state.multi_dof_joint_state.poses[0]
base_q = base_pose.orientation
base_p = base_pose.position
t = rave.matrixFromPose([base_q.w, base_q.x, base_q.y, base_q.z, base_p.x, base_p.y, base_p.z])
robot.SetTransform(t)
joint_solutions = ku.ik_for_link(rave.matrixFromPose(np.r_[xyzw[3], xyzw[:3], xyz]), manip, target_link%leftright[0], 1, True)
if len(joint_solutions) == 0:
print "pose is not reachable"
return None
m = build_joint_request(joint_solutions[0], leftright, robot, initial_state, planner_id=planner_id)
try:
response = get_motion_plan(m)
if args.pause_after_response:
raw_input("press enter to continue")
except rospy.ServiceException:
return dict(returned = False)
mpr = response.motion_plan_response
if mpr.error_code.val != 1:
print "Planner returned error code", mpr.error_code.val
return dict(returned = True, error_code = mpr.error_code.val, safe=False)
else:
traj = [list(jtp.positions) for jtp in mpr.trajectory.joint_trajectory.points]
return dict(returned = True, safe = not check_traj(traj, manip, 100), traj = traj, planning_time = mpr.planning_time.to_sec(), error_code = mpr.error_code.val)
def vf_path():
"""
Function defining a joint-space vector field.
"""
T_ee_actual = manip.GetEndEffectorTransform()
# Find where we are on the goal trajectory by finding
# the the closest point
(_, t, _) = util.GetMinDistanceBetweenTransformAndWorkspaceTraj(
T_ee_actual, traj, 0.0005)
# Get the desired end-effector transform from
# the goal trajectory
desired_T_ee = openravepy.matrixFromPose(traj.Sample(t)[0:7])
# Get the next end-effector transform, using finite-differences
pose_ee_next = traj.Sample(t + t_step)[0:7]
desired_T_ee_next = openravepy.matrixFromPose(pose_ee_next)
# Get the translation tangent to current position
tangent_vec = desired_T_ee_next[0:3, 3] - desired_T_ee[0:3, 3]
# Get the translational error
position_error_vec = desired_T_ee[0:3, 3] - T_ee_actual[0:3, 3]
# Get the translational error perpendicular to the path
tangent_trans_error = \
position_error_vec - numpy.dot(
position_error_vec, util.NormalizeVector(tangent_vec))
tangent_trans_error = numpy.nan_to_num(tangent_trans_error)
# The twist between the actual end-effector position and
# where it should be on the goal trajectory
# (the error term)
twist_perpendicular = util.GeodesicTwist(T_ee_actual,
desired_T_ee)
twist_perpendicular[0:3] = tangent_trans_error
# The twist tangent to where the end-effector should be
# on the goal trajectory
# (the feed-forward term)
twist_parallel = util.GeodesicTwist(desired_T_ee,
desired_T_ee_next)
# Normalize the translational and angular velocity of
# the feed-forward twist
twist_parallel[0:3] = util.NormalizeVector(twist_parallel[0:3])
twist_parallel[3:6] = util.NormalizeVector(twist_parallel[3:6])
# Apply gains
twist = Kp_e * twist_perpendicular + Kp_ff * twist_parallel
# Calculate joint velocities using an optimized jacobian
dqout, _ = util.ComputeJointVelocityFromTwist(
robot, twist, joint_velocity_limits=numpy.PINF)
return dqout
def vf_path():
"""
Function defining a joint-space vector field.
"""
T_ee_actual = manip.GetEndEffectorTransform()
# Find where we are on the goal trajectory by finding
# the the closest point
(_, t, _) = util.GetMinDistanceBetweenTransformAndWorkspaceTraj(
T_ee_actual, traj, 0.0005)
# Get the desired end-effector transform from
# the goal trajectory
desired_T_ee = openravepy.matrixFromPose(traj.Sample(t)[0:7])
# Get the next end-effector transform, using finite-differences
pose_ee_next = traj.Sample(t + t_step)[0:7]
desired_T_ee_next = openravepy.matrixFromPose(pose_ee_next)
# Get the translation tangent to current position
tangent_vec = desired_T_ee_next[0:3, 3] - desired_T_ee[0:3, 3]
# Get the translational error
position_error_vec = desired_T_ee[0:3, 3] - T_ee_actual[0:3, 3]
# Get the translational error perpendicular to the path
tangent_trans_error = \
position_error_vec - numpy.dot(
position_error_vec, util.NormalizeVector(tangent_vec))
tangent_trans_error = numpy.nan_to_num(tangent_trans_error)
# The twist between the actual end-effector position and
# where it should be on the goal trajectory
# (the error term)
twist_perpendicular = util.GeodesicTwist(T_ee_actual,
desired_T_ee)
twist_perpendicular[0:3] = tangent_trans_error
# The twist tangent to where the end-effector should be
# on the goal trajectory
# (the feed-forward term)
twist_parallel = util.GeodesicTwist(desired_T_ee,
desired_T_ee_next)
# Normalize the translational and angular velocity of
# the feed-forward twist
twist_parallel[0:3] = util.NormalizeVector(twist_parallel[0:3])
twist_parallel[3:6] = util.NormalizeVector(twist_parallel[3:6])
# Apply gains
twist = Kp_e * twist_perpendicular + Kp_ff * twist_parallel
# Calculate joint velocities using an optimized jacobian
dqout, _ = util.ComputeJointVelocityFromTwist(
robot, twist, joint_velocity_limits=numpy.PINF)
return dqout
def warehouse_scene_pairwise_test(initial_state_id, goal_constraint_starts_with, robot, arm="right", planner_id=""):
from warehouse_utils import MoveitWarehouseDatabase
constraints_db = MoveitWarehouseDatabase('moveit_constraints')
states_db = MoveitWarehouseDatabase('moveit_robot_states')
initial_state = states_db.get_message(RobotState, state_id=initial_state_id)
query = {'constraints_id':{"$regex":"^%s.*" % goal_constraint_starts_with}}
constraints = constraints_db.get_messages(Constraints, query)
print "Got", len(constraints), "goal constraints from the warehouse"
initial_states = [initial_state]
positions, quats = [], []
results = []
base_pose = initial_state.multi_dof_joint_state.poses[0]
base_q = base_pose.orientation
base_p = base_pose.position
t = rave.matrixFromPose([base_q.w, base_q.x, base_q.y, base_q.z, base_p.x, base_p.y, base_p.z])
robot.SetTransform(t)
update_rave_from_ros(robot, initial_state.joint_state.position, initial_state.joint_state.name)
for c in constraints:
pos, quat = pose_constraints_to_xyzquat(c)
post = (pos.x, pos.y, pos.z)
quatt = (quat.x, quat.y, quat.z, quat.w)
positions.append(post)
quats.append(quatt)
state = robot_state_from_pose_goal(post, quatt, arm, robot, initial_state)
if state:
initial_states.append(state)
for start_state in initial_states:
update_rave_from_ros(robot, start_state.joint_state.position, start_state.joint_state.name)
for posit, quater in zip(positions, quats):
result = test_plan_to_pose(posit, quater, arm, robot, start_state, planner_id, "%s_wrist_roll_link")
if result is not None:
results.append(result)
process_results(results)
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 build_joint_request(jvals,
arm,
robot,
initial_state=build_robot_state(),
planner_id=''):
m = MotionPlanRequest()
m.group_name = "%s_arm" % arm
m.start_state = initial_state
m.planner_id = planner_id
c = Constraints()
joints = robot.GetJoints()
joint_inds = robot.GetManipulator("%sarm" % arm).GetArmIndices()
c.joint_constraints = [
JointConstraint(joint_name=joints[joint_inds[i]].GetName(),
position=jvals[i]) for i in xrange(len(jvals))
]
jc = JointConstraint()
m.goal_constraints = [c]
m.allowed_planning_time = rospy.Duration(args.max_planning_time)
base_pose = initial_state.multi_dof_joint_state.poses[0]
base_q = base_pose.orientation
base_p = base_pose.position
t = rave.matrixFromPose(
[base_q.w, base_q.x, base_q.y, base_q.z, base_p.x, base_p.y, base_p.z])
robot.SetTransform(t)
return m
def test_position_base(pose_targ):
wxyz_targ = pose_targ[:4]
xyz_targ = pose_targ[4:]
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]
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]))
manipname = "rightarm+torso_lift_joint+base"
n_steps = 1
prob_desc = make_request_skeleton(manipname, n_steps);
prob_desc["basic_info"]["start_fixed"] = False
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"
prob_desc["init_info"]["data"] = [dofvals_init.tolist()]
s = json.dumps(prob_desc)
traj.SetInteractive(True);
prob = traj.ConstructProblem(s, env)
return traj.OptimizeProblem(prob)
def test_ikfast_6d_case_1(self):
"""
Easy test. IKFast 6D
"""
i = 0
for (qseed, pose) in zip(self.qseeds, self.poses):
i += 1
T = orpy.matrixFromPose(pose)
with self.robot:
self.robot.SetActiveDOFValues(qseed)
ts = time.time()
sol = self.manip.FindIKSolution(T, ikfilter_checkcollision)
te = time.time()
if sol is not None:
self.total_time += te - ts
self.no_success += 1
with self.robot:
self.robot.SetActiveDOFValues(sol)
pose_actual = self.manip.GetTransformPose()
if pose_actual[0] < 0:
pose_actual[:4] *= -1.
np.testing.assert_allclose(pose_actual, pose, rtol=1e-5, atol=1e-5)
self.assertTrue((sol <= self.q_max).all(), msg="Violate joint limits")
self.assertTrue((self.q_min <= sol).all(), msg="Violate joint limits")
def SampleWorkspaceTrajectoryIntoArray(robot, traj, t_start, t_end, n):
"""
Takes n samples from a workspace trajectory, including the
end-points, and returns them in an array.
"""
if t_start < 0.0:
raise ValueError("t_start < 0.0 "
"in SampleWorkspaceTrajectoryIntoArray()")
if t_end <= t_start:
raise ValueError("t_end <= t_start "
"in SampleWorkspaceTrajectoryIntoArray()")
num_samples = n
if num_samples <= 3:
raise ValueError("Number of samples 'n' must be >= 3 "
"in SampleWorkspaceTrajectoryIntoArray()")
cspec = traj.GetConfigurationSpecification()
dof_indices, _ = cspec.ExtractUsedIndices(robot)
if t_end == None:
t_end = traj.GetDuration()
times_list = []
transforms_list = []
for t in numpy.linspace(t_start, t_end, num_samples):
times_list.append(t)
P = traj.Sample(t)[0:7] # first 7 values are pose
T_current = openravepy.matrixFromPose(P)
transforms_list.append(T_current)
times_array = numpy.asarray(times_list)
transforms_array = numpy.asarray(transforms_list)
return times_array,transforms_array
def create_polyhedron(self, contacts):
self.handle = None
self.nr_iter = 0
try:
self.polyhedron = stabilipy.StabilityPolygon(
robot.mass, dimension=3, radius=1.5)
stabilipy_contacts = []
for contact in contacts:
hmatrix = matrixFromPose(contact.pose)
X, Y = contact.shape
displacements = [array([[X, Y, 0]]).T,
array([[-X, Y, 0]]).T,
array([[-X, -Y, 0]]).T,
array([[X, -Y, 0]]).T]
for disp in displacements:
stabilipy_contacts.append(
stabilipy.Contact(
contact.friction,
hmatrix[:3, 3:] + hmatrix[:3, :3].dot(disp),
hmatrix[:3, 2:3]))
self.polyhedron.contacts = stabilipy_contacts
self.polyhedron.select_solver(self.method)
self.polyhedron.make_problem()
self.polyhedron.init_algo()
self.polyhedron.build_polys()
vertices = self.polyhedron.polyhedron()
self.handle = draw_polytope(
[(x[0], x[1], x[2]) for x in vertices])
except Exception as e:
print("SupportPolyhedronDrawer: {}".format(e))
def get_random_pose(env, limits, manip, frame_name="r_gripper_tool_frame"):
robot = env.GetRobots()[0]
is_success = False
while is_success is not True:
#Select which input space
index = np.random.choice(limits.keys())
x, y, z = generate_random_xyz(limits[index])
#simple check for collision with a box
if check_col_with_box(env, x, y, z):
continue
#do IK here
xyz = [x, y, z]
quat = [1, 0, 0, 0] # wxyz
pose = openravepy.matrixFromPose(np.r_[quat, xyz])
joint_angle = ku.ik_for_link(
pose,
manip,
frame_name,
filter_options=openravepy.IkFilterOptions.CheckEnvCollisions)
if joint_angle is None: continue
#further check for collision
robot.SetDOFValues(joint_angle, manip.GetArmIndices())
if env.CheckCollision(robot) or robot.CheckSelfCollision(): continue
is_success = True
xyz = np.array([x, y, z])
return joint_angle, xyz
def PublishWorkspaceTrajectoryPoses(rviz_tools, robot, traj, n=30, axis_length=0.02, axis_radius=0.001):
"""
Sample a workspace trajectory n number of times and publish an axes
Rviz Marker representing the end effector pose at each sampled
position.
@param instance rviz_tools: An instance of the rviz_tools.RvizMarkers class
@param openravepy.robot robot: The robot.
@param openravepy.Trajectory traj: A timed workspace trajectory.
@param n int: Number of axes to draw along the path of the
trajectory, including end-points.
@param axis_length float: The length of each of the 3 axis lines.
@param axis_radius float: The radius of each of the 3 axis lines.
"""
if n <= 0:
raise ValueError("Number of samples n must be a positive number"
" in PublishWorkspaceTrajectoryPoses()")
if not prpy.util.IsTimedTrajectory(traj):
raise ValueError(
'Trajectory must be timed. If you want to use this function on a'
' path, then consider using util.ComputeUnitTiming to compute its'
' arclength parameterization.')
# Draw specified number of axis markers along
# the path of the trajectory
duration = traj.GetDuration()
for t in numpy.linspace(0, duration, n):
P = traj.Sample(t)[0:7] # first 7 values are pose
T_ee_curr = openravepy.matrixFromPose(P)
duration = None # None = publish forever
rviz_tools.publishAxis(T_ee_curr, axis_length, axis_radius, duration)
def find_body_holes(body, radius, absolute=True):
import trimesh
mesh_holes = dict()
body_holes = dict()
Tbody = body.GetTransform()
for link in body.GetLinks():
link_holes = []
Tlink = link.GetTransform()
for geometry in link.GetGeometries():
if geometry.GetType() == orpy.GeometryType.Trimesh:
filename = geometry.GetRenderFilename()
scale = geometry.GetRenderScale()
pose = geometry.GetTransformPose()
if filename not in mesh_holes:
mesh = trimesh.load(filename)
mesh_holes[filename] = find_mesh_holes(
mesh.vertices, mesh.faces, radius, scale)
for h in mesh_holes[filename]:
hole = copy.deepcopy(h)
Tmesh = np.dot(Tlink, orpy.matrixFromPose(pose))
if absolute:
hole.transform(np.dot(Tbody, Tmesh))
else:
hole.transform(Tmesh)
link_holes.append(hole)
if len(link_holes) > 0:
body_holes[str(link.GetName())] = link_holes
return body_holes
def move_to_pregrasp(self, grasp_pose, eps=1e-2):
""" Move the robot to the pregrasp pose given by the grasp object """
# get grasp pose
gripper_position = grasp_pose.position
gripper_orientation = grasp_pose.orientation
gripper_pose = np.array([
gripper_orientation.w, gripper_orientation.x,
gripper_orientation.y, gripper_orientation.z, gripper_position.x,
gripper_position.y, gripper_position.z
])
if abs(np.linalg.norm(np.array(gripper_orientation.list)) - 1.0) > eps:
logging.warning('Illegal pose')
return None, None
# get grasp pose relative to object
T_gripper_obj = rave.matrixFromPose(gripper_pose)
T_obj_world = self.T_gripper_world_.dot(self.T_rviz_or_).dot(
np.linalg.inv(T_gripper_obj))
# set robot position as inverse of object (for viewing purposes)
T_robot_world = np.linalg.inv(T_obj_world)
self.robot.SetTransform(T_robot_world)
return T_gripper_obj, T_robot_world
def test_cart_traj():
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, 2, 0, 0]))
manipname = "rightarm"
manip = robot.GetManipulator(manipname)
n_steps = 5
prob_desc = make_request_skeleton(manipname, n_steps);
prob_desc["basic_info"]["start_fixed"] = False
decimation = 1
pts = mu.linspace2d([2.5,-.3,1],[2.5,.3,1],n_steps/decimation)
for (i,pt) in enumerate(pts):
prob_desc["costs"].append(
{"type" : "pose",
"name" : "posexxx",
"params" : {
"pos_coeffs" : [1,1,1],
"rot_coeffs" : [0,0,0],
"xyz" : pt.tolist(),
"wxyz" : [.8,.6,0,0],
"link" : "r_gripper_tool_frame",
"timestep" : i*decimation
}}
)
prob_desc["init_info"]["type"] = "stationary"
s = json.dumps(prob_desc)
traj.SetInteractive(True);
prob = traj.ConstructProblem(s, env)
return traj.OptimizeProblem(prob)
def _tf_to_rave_mat(tf):
""" Convert a RigidTransform to an OpenRAVE matrix """
position = tf.position
orientation = tf.quaternion
pose = np.array([orientation[0], orientation[1], orientation[2], orientation[3],
position[0], position[1], position[2]])
mat = rave.matrixFromPose(pose)
return mat
def get_lin_interp_poses(start_pos, end_pos, n_steps):
xyz_start, quat_start = juc.hmat_to_trans_rot(start_pos)
xyz_end, quat_end = juc.hmat_to_trans_rot(end_pos)
xyzs = math_utils.linspace2d(xyz_start, xyz_end, n_steps)
quats = [rave.quatSlerp(quat_start, quat_end, t) for t in np.linspace(0, 1, n_steps)]
hmats = [rave.matrixFromPose(np.r_[quat[3], quat[:3], xyz]) for (xyz, quat) in zip(xyzs, quats)]
gripper_poses = np.array([juc.hmat_to_pose(hmat) for hmat in hmats])
return gripper_poses
def calcIKForPQ(self,
pos,
quat,
opts=openravepy.IkFilterOptions.CheckEnvCollisions):
'''TODO'''
targetMat = openravepy.matrixFromPose(numpy.r_[quat, pos])
solutions = self.manip.FindIKSolutions(targetMat, opts)
return solutions
def main():
import yaml
import os
import os.path as osp
import sys
sys.path.insert(1, os.path.join(sys.path[0], '..'))
import planning_benchmark_common as pbc
import argparse
parser = argparse.ArgumentParser()
parser.add_argument("problemfile", type=argparse.FileType("r"))
parser.add_argument("--base_only", action="store_true")
args = parser.parse_args()
problemset = yaml.load(args.problemfile)
assert problemset["active_affine"] == 11
env = openravepy.Environment()
env.StopSimulation()
env.Load(osp.join(pbc.envfile_dir, problemset["env_file"]))
robot2file = {"pr2": "robots/pr2-beta-static.zae"}
env.Load(robot2file[problemset["robot_name"]])
robot = env.GetRobots()[0]
robot.SetTransform(
openravepy.matrixFromPose(problemset["default_base_pose"]))
rave_joint_names = [joint.GetName() for joint in robot.GetJoints()]
rave_inds, rave_values = [], []
for (name, val) in zip(problemset["joint_names"],
problemset["default_joint_values"]):
if name in rave_joint_names:
i = rave_joint_names.index(name)
rave_inds.append(i)
rave_values.append(val)
robot.SetDOFValues(rave_values, rave_inds)
active_joint_inds = [
rave_joint_names.index(name) for name in problemset["active_joints"]
]
robot.SetActiveDOFs(active_joint_inds, problemset["active_affine"])
env.SetViewer('qtcoin')
env.UpdatePublishedBodies()
if args.base_only:
poses = sample_base_positions(robot, num=100)
else:
poses = sample_all(robot, num=100)
while True:
for i, p in enumerate(poses):
robot.SetActiveDOFValues(poses[i])
env.UpdatePublishedBodies()
raw_input('showing %d/%d: joints = %s' %
(i + 1, len(poses), ', '.join(str(v) for v in poses[i])))
print '=================='
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 _init_robot(self):
""" Initialize the robot """
# set initial pose
self.robot.SetTransform(rave.matrixFromPose(np.array([1,0,0,0,0,0,0])))
self.robot.SetDOFValues([0.54,-1.57, 1.57, 0.54],[22,27,15,34])
# get robot manipulation tools
self.manip_ = self.robot.SetActiveManipulator("leftarm_torso")
self.maniprob_ = rave.interfaces.BaseManipulation(self.robot) # create the interface for task manipulation programs
self.taskprob_ = rave.interfaces.TaskManipulation(self.robot) # create the interface for task manipulation programs
self.finger_joint_ = self.robot.GetJoint('l_gripper_l_finger_joint')
def set_pose(self, pose):
"""
Set the 7D pose of the body orientation.
Parameters
----------
pose : (7,) array
Pose of the body, i.e. quaternion + position in world frame.
"""
T = openravepy.matrixFromPose(pose)
self.set_transform(T)
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 robot_state_from_pose_goal(xyz, xyzw, arm, robot, initial_state = build_robot_state()):
manip = robot.GetManipulator(arm + "arm")
joint_solutions = ku.ik_for_link(rave.matrixFromPose(np.r_[xyzw[3], xyzw[:3], xyz]),
manip, "%s_wrist_roll_link"%arm[0], 1, True)
if len(joint_solutions) == 0:
raise Exception
joints = robot.GetJoints()
joint_inds = robot.GetManipulator("%sarm"%arm).GetArmIndices()
joint_names = [joints[joint_inds[i]].GetName() for i in xrange(len(joint_solutions[0]))]
joint_values = [joint_solutions[0][i] for i in xrange(len(joint_solutions[0]))]
new_state = alter_robot_state(initial_state, joint_names, joint_values)
return new_state
def _init_openrave(self, viewer=False, envFile=None, pw_file=False):
if type(envFile) == str:
self.env = openravepy.Environment()
self.env.StopSimulation()
if viewer:
self.env.SetViewer('qtcoin')
self.env.Load(envFile)
else:
self.env = envFile
fname = "pw_file.txt"
if pw_file:
if os.path.isfile(fname):
with open(fname, "r") as f:
transforms = eval(f.read())
for b in self.env.GetBodies():
if b.GetName() in transforms.keys():
b.SetTransform(openravepy.matrixFromPose(eval(transforms[b.GetName()])))
else:
print("Could not find pw_file.txt, keeping transforms as is")
robot = self.env.GetRobots()[0]
# EnvManager.init_openrave_state(self.env)
# PR2 robot intialization
# # loading the IK models
# robot.SetActiveManipulator('leftarm')
# ikmodel = openravepy.databases.inversekinematics.InverseKinematicsModel(
# robot, iktype=openravepy.IkParameterization.Type.Transform6D)
# if not ikmodel.load():
# ikmodel.autogenerate()
# robot.SetActiveManipulator('rightarm')
# ikmodel = openravepy.databases.inversekinematics.InverseKinematicsModel(
# robot, iktype=openravepy.IkParameterization.Type.Transform6D)
# if not ikmodel.load():
# ikmodel.autogenerate()
# # utils.set_rave_limits_to_soft_joint_limits(robot)
# # start arms on the side
# left_joints = Arm.L_POSTURES['side2']
# right_joints = mirror_arm_joints(left_joints).tolist()
# # robot.SetDOFValues(
# # right_joints + left_joints,
# # robot.GetManipulator("rightarm").GetArmIndices().tolist() +
# # robot.GetManipulator("leftarm").GetArmIndices().tolist())
# # start torso up
# v = robot.GetActiveDOFValues()
# v[robot.GetJoint('torso_lift_joint').GetDOFIndex()] = 0.3
# # robot.SetDOFValues(v)
time.sleep(1)
def get_return_from_grasp_joint_trajectory(self, start_joints, target_pose, n_steps=40):
"""
Calls trajopt to plan return from grasp collision-free trajectory
:param start_joints: list of initial joints
:param target_pose: desired pose of tool_frame (tfx.pose)
:param n_steps: trajopt discretization
:return None if traj not collision free, else list of joint values
"""
assert len(start_joints) == len(self.joint_indices)
assert target_pose.frame.count('base_link') == 1
# set active manipulator and start joint positions
self.robot.SetDOFValues(start_joints, self.joint_indices)
# initialize trajopt inputs
rave_pose = tfx.pose(self.sim.transform_from_to(target_pose.matrix, target_pose.frame, 'world'))
quat = rave_pose.orientation
xyz = rave_pose.position
quat_target = [quat.w, quat.x, quat.y, quat.z]
xyz_target = [xyz.x, xyz.y, xyz.z]
rave_mat = rave.matrixFromPose(np.r_[quat_target, xyz_target])
request = self._get_return_from_grasp_trajopt_request(xyz_target, quat_target, n_steps)
# convert dictionary into json-formatted string
s = json.dumps(request)
# create object that stores optimization problem
prob = trajoptpy.ConstructProblem(s, self.sim.env)
tool_link = self.robot.GetLink(self.tool_frame)
def penalize_low_height(x):
self.robot.SetDOFValues(x, self.joint_indices, False)
z = tool_link.GetTransform()[2,3]
return max(0, 10.0 - z)
for t in xrange(n_steps-2):
prob.AddErrorCost(penalize_low_height, [(t,j) for j in xrange(len(self.joint_indices))], "ABS", "PENALIZE_LOW_HEIGHT_%i"%t)
# do optimization
result = trajoptpy.OptimizeProblem(prob)
self.robot.SetDOFValues(start_joints, self.joint_indices)
prob.SetRobotActiveDOFs() # set robot DOFs to DOFs in optimization problem
num_upsampled_collisions = self._num_collisions(result.GetTraj())
print('Number of collisions: {0}'.format(num_upsampled_collisions))
self.robot.SetDOFValues(start_joints, self.joint_indices)
if num_upsampled_collisions > 2:
return None
else:
return result.GetTraj()
def main():
import yaml
import os; import os.path as osp
import sys
sys.path.insert(1, os.path.join(sys.path[0], '..')); import planning_benchmark_common as pbc
import argparse
parser = argparse.ArgumentParser()
parser.add_argument("problemfile", type=argparse.FileType("r"))
parser.add_argument("--base_only", action="store_true")
args = parser.parse_args()
problemset = yaml.load(args.problemfile)
assert problemset["active_affine"] == 11
env = openravepy.Environment()
env.StopSimulation()
env.Load(osp.join(pbc.envfile_dir,problemset["env_file"]))
robot2file = {
"pr2":"robots/pr2-beta-static.zae"
}
env.Load(robot2file[problemset["robot_name"]])
robot = env.GetRobots()[0]
robot.SetTransform(openravepy.matrixFromPose(problemset["default_base_pose"]))
rave_joint_names = [joint.GetName() for joint in robot.GetJoints()]
rave_inds, rave_values = [],[]
for (name,val) in zip(problemset["joint_names"], problemset["default_joint_values"]):
if name in rave_joint_names:
i = rave_joint_names.index(name)
rave_inds.append(i)
rave_values.append(val)
robot.SetDOFValues(rave_values, rave_inds)
active_joint_inds = [rave_joint_names.index(name) for name in problemset["active_joints"]]
robot.SetActiveDOFs(active_joint_inds, problemset["active_affine"])
env.SetViewer('qtcoin')
env.UpdatePublishedBodies()
if args.base_only:
poses = sample_base_positions(robot, num=100)
else:
poses = sample_all(robot, num=100)
while True:
for i, p in enumerate(poses):
robot.SetActiveDOFValues(poses[i])
env.UpdatePublishedBodies()
raw_input('showing %d/%d: joints = %s' % (i+1, len(poses), ', '.join(str(v) for v in poses[i])))
print '=================='
def get_lin_interp_poses(start_pos, end_pos, n_steps):
xyz_start, quat_start = juc.hmat_to_trans_rot(start_pos)
xyz_end, quat_end = juc.hmat_to_trans_rot(end_pos)
xyzs = math_utils.linspace2d(xyz_start, xyz_end, n_steps)
quats = [
rave.quatSlerp(quat_start, quat_end, t)
for t in np.linspace(0, 1, n_steps)
]
hmats = [
rave.matrixFromPose(np.r_[quat[3], quat[:3], xyz])
for (xyz, quat) in zip(xyzs, quats)
]
gripper_poses = np.array([juc.hmat_to_pose(hmat) for hmat in hmats])
return gripper_poses
def _init_robot(self):
""" Initialize the robot """
# set initial pose
self.robot.SetTransform(
rave.matrixFromPose(np.array([1, 0, 0, 0, 0, 0, 0])))
self.robot.SetDOFValues([0.54, -1.57, 1.57, 0.54], [22, 27, 15, 34])
# get robot manipulation tools
self.manip_ = self.robot.SetActiveManipulator("leftarm_torso")
self.maniprob_ = rave.interfaces.BaseManipulation(
self.robot) # create the interface for task manipulation programs
self.taskprob_ = rave.interfaces.TaskManipulation(
self.robot) # create the interface for task manipulation programs
self.finger_joint_ = self.robot.GetJoint('l_gripper_l_finger_joint')
def PublishWorkspaceTrajectorySegments(rviz_tools, robot, traj,
color='red', line_width=0.005):
"""
Draw the linear segment between each waypoint of a workspace
trajectory as a line using Rviz Markers.
This works because the trajectory has linear interpolation for the
translation component.
@param instance rviz_tools: An instance of the rviz_tools.RvizMarkers class
@param openravepy.robot robot: The robot.
@param openravepy.Trajectory traj: A workspace trajectory.
@param color string: Color name to draw the line segments.
@param line_width float: Width of the line segments.
"""
num_waypoints = traj.GetNumWaypoints()
T_prev = openravepy.matrixFromPose(traj.GetWaypoint(0)[0:7])
for i in range(1, num_waypoints):
P = traj.GetWaypoint(i)[0:7] # first 7 values are pose
T = openravepy.matrixFromPose(P)
duration = None # None = publish forever
rviz_tools.publishLine(T_prev, T, color, line_width, duration)
T_prev = T
def get_robot_ee_position(self, msg):
# ee_orientation = pyquaternion.Quaternion(msg.pose.orientation.w, msg.pose.orientation.x, msg.pose.orientation.y, msg.pose.orientation.z)
# ee_position = numpy.array([msg.pose.position.x, msg.pose.position.y, msg.pose.position.z])
pose = [
msg.pose.orientation.w, msg.pose.orientation.x,
msg.pose.orientation.y, msg.pose.orientation.z,
msg.pose.position.x, msg.pose.position.y, msg.pose.position.z
]
pose = openravepy.matrixFromPose(pose)
ee_pose = numpy.dot(pose, self.T_G2EE)
ee_pose = openravepy.poseFromMatrix(ee_pose)
self.ee_orientation = pyquaternion.Quaternion(ee_pose[:4])
self.ee_position = numpy.array(ee_pose[4:])
def PublishWorkspaceTrajectoryClosestSegment(rviz_tools, robot, traj,
color='red', line_width=0.005,
t_min=None, t_max=None):
"""
Draw the linear segment of a workspace trajectory which is closest
to the segment from t_min to t_max.
"""
if (t_min == None) or (t_max == None):
raise ValueError("You must specify the desired segment using "
"t_min and t_max in "
"PublishWorkspaceTrajectoryClosestSegment()")
waypoint_times = GetWorkspaceTrajectoryWaypointTimes(traj)
epsilon = 0.000001
T_prev = openravepy.matrixFromPose(traj.GetWaypoint(0)[0:7])
waypoint_time_prev = 0.0
num_waypoints = traj.GetNumWaypoints()
for i in range(1, num_waypoints):
P = traj.GetWaypoint(i)[0:7] # first 7 values are pose
T = openravepy.matrixFromPose(P)
waypoint_time = waypoint_times[i]
diff_from_t_min = abs(waypoint_time_prev - t_min)
diff_from_t_max = abs(waypoint_time - t_max)
# If this segment starts and ends outside the desired time
# limits then draw it
if (t_min+epsilon >= waypoint_time_prev) \
and (t_max-epsilon <= waypoint_time):
duration = None # None = publish forever
rviz_tools.publishLine(T_prev, T, color, line_width, duration)
T_prev = T
waypoint_time_prev = waypoint_time
def cylinder_collisionfree_model(env, body_name, pos, dims):
iv_str = '#Inventor V2.1 ascii\nSeparator {\nCylinder {\nparts ALL\nradius %f\nheight %f\n}\n}'%(dims[0], dims[1]) # hardcode collision-free disk dimensions
with open("../models/cylinder.iv", "w+") as f:
f.write(iv_str)
xml_str = '<KinBody name="%s"> <Body name="surface" type="dynamic"> <Geom type="sphere"> <Render>cylinder.iv</Render> <Radius>0.0001</Radius> \
<Translation>0 0 %f</Translation> <RotationAxis>1 0 0 90></RotationAxis> </Geom> </Body> </KinBody>'%(body_name, dims[1] / 2)
with open("../models/temp_cylinder.xml", "w+") as f:
f.write(xml_str)
env.Load("../models/temp_cylinder.xml")
cylinder = env.GetKinBody(body_name)
x, y, z = pos
cylinder_t = openravepy.matrixFromPose([1, 0, 0, 0, x, y, z])
cylinder.SetTransform(cylinder_t)
return cylinder
def warehouse_scene_pairwise_test(initial_state_id,
goal_constraint_starts_with,
robot,
arm="right",
planner_id=""):
from warehouse_utils import MoveitWarehouseDatabase
constraints_db = MoveitWarehouseDatabase('moveit_constraints')
states_db = MoveitWarehouseDatabase('moveit_robot_states')
initial_state = states_db.get_message(RobotState,
state_id=initial_state_id)
query = {
'constraints_id': {
"$regex": "^%s.*" % goal_constraint_starts_with
}
}
constraints = constraints_db.get_messages(Constraints, query)
print "Got", len(constraints), "goal constraints from the warehouse"
initial_states = [initial_state]
positions, quats = [], []
results = []
base_pose = initial_state.multi_dof_joint_state.poses[0]
base_q = base_pose.orientation
base_p = base_pose.position
t = rave.matrixFromPose(
[base_q.w, base_q.x, base_q.y, base_q.z, base_p.x, base_p.y, base_p.z])
robot.SetTransform(t)
update_rave_from_ros(robot, initial_state.joint_state.position,
initial_state.joint_state.name)
for c in constraints:
pos, quat = pose_constraints_to_xyzquat(c)
post = (pos.x, pos.y, pos.z)
quatt = (quat.x, quat.y, quat.z, quat.w)
positions.append(post)
quats.append(quatt)
state = robot_state_from_pose_goal(post, quatt, arm, robot,
initial_state)
if state:
initial_states.append(state)
for start_state in initial_states:
update_rave_from_ros(robot, start_state.joint_state.position,
start_state.joint_state.name)
for posit, quater in zip(positions, quats):
result = test_plan_to_pose(posit, quater, arm, robot, start_state,
planner_id, "%s_wrist_roll_link")
if result is not None:
results.append(result)
process_results(results)
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_env(problemset):
env = openravepy.Environment()
env.StopSimulation()
robot2file = {"pr2": "robots/pr2-beta-static.zae"}
if args.planner == "trajopt":
if args.interactive: trajoptpy.SetInteractive(True)
plan_func = trajopt_plan
elif args.planner == "ompl":
setup_ompl(env)
plan_func = ompl_plan
elif args.planner in ["chomp", "chomp2"]:
setup_chomp(env)
plan_func = chomp_plan
# chomp needs a robot with spheres
chomp_pr2_file = "pr2_with_spheres.robot.xml" if problemset[
"active_affine"] == 0 else "pr2_with_spheres_fullbody.robot.xml"
robot2file["pr2"] = osp.join(pbc.envfile_dir, chomp_pr2_file)
env.Load(osp.join(pbc.envfile_dir, problemset["env_file"]))
env.Load(robot2file[problemset["robot_name"]])
robot = env.GetRobots()[0]
if args.planner == "trajopt":
try:
postsetup_trajopt(env)
except Exception:
print "can't find ros config file. skipping self collision ignore"
robot.SetTransform(
openravepy.matrixFromPose(problemset["default_base_pose"]))
rave_joint_names = [joint.GetName() for joint in robot.GetJoints()]
rave_inds, rave_values = [], []
for (name, val) in zip(problemset["joint_names"],
problemset["default_joint_values"]):
if name in rave_joint_names:
i = rave_joint_names.index(name)
rave_inds.append(i)
rave_values.append(val)
robot.SetDOFValues(rave_values, rave_inds)
active_joint_inds = [
rave_joint_names.index(name) for name in problemset["active_joints"]
]
robot.SetActiveDOFs(active_joint_inds, problemset["active_affine"])
return env, robot, plan_func
def move_to_pregrasp(self, T_gripper_obj, eps=1e-2):
""" Move the gripper to the pregrasp pose given by the grasp object """
# get grasp pose in rave
T_mesh_obj = self.gripper_.T_mesh_gripper.dot(T_gripper_obj)
gripper_position = T_mesh_obj.inverse().pose.position
gripper_orientation = T_mesh_obj.inverse().pose.orientation
gripper_pose = np.array([
gripper_orientation.w, gripper_orientation.x,
gripper_orientation.y, gripper_orientation.z, gripper_position.x,
gripper_position.y, gripper_position.z
])
T_mesh_obj = rave.matrixFromPose(gripper_pose)
self.gripper_obj_.SetTransform(T_mesh_obj)
return T_mesh_obj
def create_cylinder(env, table, radius, height, body_name, max_radial_distance, rand, color):
robot_pos = openravepy.poseFromMatrix(env.GetRobots()[0].GetTransform())[4:7]
min_x, max_x, min_y, max_y, table_height = utils.get_object_limits(table)
x = rand.uniform(min_x + radius, max_x - radius)
y = rand.uniform(min_y + radius, max_y - radius)
# while (x - robot_pos[0])**2 + (y - robot_pos[1])**2 > max_radial_distance:
# x = rand.uniform(min_x + radius, max_x - radius)
# y = rand.uniform(min_y + radius, max_y - radius)
z = table_height + height / 2
t = openravepy.matrixFromPose([1, 0, 0, 0, x, y, z])
cylinder = object_models.create_cylinder(env, body_name, t, [radius, height], color)
env.Add(cylinder, False)
return cylinder
def plot_body_com(link, handle=None, color=None):
""" efficiently plot the center of mass of a given link"""
if color is None:
color = _np.array([0, 1, 0])
origin = link.GetGlobalCOM()
mass = link.GetMass()
#Fetch environment from robot parent
env = link.GetParent().GetEnv()
if handle is None:
handle = env.plot3(points=origin, pointsize=5.0 * mass, colors=color)
else:
neworigin = [1, 0, 0, 0]
neworigin.extend(origin.tolist())
handle.SetTransform(_rave.matrixFromPose(neworigin))
return handle
def plot_body_com(link, handle=None, color=None):
""" efficiently plot the center of mass of a given link"""
if color is None:
color = _np.array([0, 1, 0])
origin = link.GetGlobalCOM()
mass = link.GetMass()
#Fetch environment from robot parent
env = link.GetParent().GetEnv()
if handle is None:
handle = env.plot3(
points=origin, pointsize=5.0 * mass, colors=color)
else:
neworigin = [1, 0, 0, 0]
neworigin.extend(origin.tolist())
handle.SetTransform(_rave.matrixFromPose(neworigin))
return handle
def init_env(problemset):
env = openravepy.Environment()
env.StopSimulation()
robot2file = {
"pr2":"robots/pr2-beta-static.zae"
}
if args.planner == "trajopt":
if args.interactive: trajoptpy.SetInteractive(True)
plan_func = trajopt_plan
elif args.planner == "ompl":
setup_ompl(env)
plan_func = ompl_plan
elif args.planner == "chomp":
setup_chomp(env)
plan_func = chomp_plan
# chomp needs a robot with spheres
chomp_pr2_file = "pr2_with_spheres.robot.xml" if problemset["active_affine"] == 0 else "pr2_with_spheres_fullbody.robot.xml"
robot2file["pr2"] = osp.join(pbc.envfile_dir, chomp_pr2_file)
env.Load(osp.join(pbc.envfile_dir,problemset["env_file"]))
env.Load(robot2file[problemset["robot_name"]])
robot = env.GetRobots()[0]
if args.planner == "trajopt":
try:
postsetup_trajopt(env)
except Exception:
print "can't find ros config file. skipping self collision ignore"
robot.SetTransform(openravepy.matrixFromPose(problemset["default_base_pose"]))
rave_joint_names = [joint.GetName() for joint in robot.GetJoints()]
rave_inds, rave_values = [],[]
for (name,val) in zip(problemset["joint_names"], problemset["default_joint_values"]):
if name in rave_joint_names:
i = rave_joint_names.index(name)
rave_inds.append(i)
rave_values.append(val)
robot.SetDOFValues(rave_values, rave_inds)
active_joint_inds = [rave_joint_names.index(name) for name in problemset["active_joints"]]
robot.SetActiveDOFs(active_joint_inds, problemset["active_affine"])
return env, robot, plan_func
def add_body(env, body_name, box_msg, scale=1.0, padding=0):
###@$!@#$!@#!!! openrave takes half extents
values = [0,0,0,\
box_msg.box_dims.x*scale/2 + padding,\
box_msg.box_dims.y*scale/2 + padding,\
box_msg.box_dims.z*scale/2 + padding]
# values = [box_msg.pose.pose.position.x, \
# box_msg.pose.pose.position.y,\
# box_msg.pose.pose.position.z,\
# box_msg.box_dims.x, \
# box_msg.box_dims.y,\
# box_msg.box_dims.z]
print(values)
print("box_msg: "+ repr(box_msg))
body = openravepy.RaveCreateKinBody(env,'')
body.SetName(body_name)
body.InitFromBoxes(np.array([values]), True)
T = body.GetTransform()
# T[:,-1] = [box_msg.pose.pose.position.x, \
# box_msg.pose.pose.position.y, \
# box_msg.pose.pose.position.z, 1]
obj_position = [box_msg.pose.pose.position.x,\
box_msg.pose.pose.position.y,\
box_msg.pose.pose.position.z + \
(box_msg.box_dims.z*scale/2 + padding - box_msg.box_dims.z/2)]
###@$!@#$!@#!!! Openrave expresses quaternions as wxyz!!!
obj_rot = [box_msg.pose.pose.orientation.w,\
box_msg.pose.pose.orientation.x,\
box_msg.pose.pose.orientation.y,\
box_msg.pose.pose.orientation.z]
poseForOpenrave = []
poseForOpenrave[0:4] = obj_rot
poseForOpenrave[4:6] = obj_position
# T = openravepy.matrixFromQuat(obj_rot)
# body.SetTransform(T)
T = openravepy.matrixFromPose(poseForOpenrave)
body.SetTransform(T)
env.Add(body, True)
def spawn_table_pr2(env, robot_dist_from_table, tabletype='rll'):
if tabletype == 'rll':
thickness = 0.2
legheight = 0.6
table = object_models.create_table(env, TABLE_NAMES[tabletype], 2.235, 0.94, thickness, 1.3, 0.6, legheight)
elif tabletype == 'small':
thickness = 0.2
legheight = 0.6
table = object_models.create_table(env, TABLE_NAMES[tabletype], 0.7 - robot_dist_from_table, 1.5, thickness, 0.2 - robot_dist_from_table, 0.2, legheight)
x = -utils.get_object_limits(table)[0] + 0.35 + robot_dist_from_table
y = 0
z = thickness / 2 + legheight
table.SetTransform(openravepy.matrixFromPose([1, 0, 0, 0, x, y, z]))
# env.AddKinBody(table)
env.AddRobot(table)
# robot = env.ReadRobotXMLFile("../models/pr2/pr2-head-kinect.xml")
robot = env.ReadRobotXMLFile("../models/pr2/pr2.zae")
env.Add(robot)
def move_to_pregrasp(self, grasp_pose, eps=1e-2):
""" Move the robot to the pregrasp pose given by the grasp object """
# get grasp pose
gripper_position = grasp_pose.position
gripper_orientation = grasp_pose.orientation
gripper_pose = np.array([gripper_orientation.w, gripper_orientation.x, gripper_orientation.y, gripper_orientation.z, gripper_position.x, gripper_position.y, gripper_position.z])
if abs(np.linalg.norm(np.array(gripper_orientation.list)) - 1.0) > eps:
logging.warning('Illegal pose')
return None, None
# get grasp pose relative to object
T_gripper_obj = rave.matrixFromPose(gripper_pose)
T_obj_world = self.T_gripper_world_.dot(self.T_rviz_or_).dot(np.linalg.inv(T_gripper_obj))
# set robot position as inverse of object (for viewing purposes)
T_robot_world = np.linalg.inv(T_obj_world)
self.robot.SetTransform(T_robot_world)
return T_gripper_obj, T_robot_world
def build_joint_request(jvals, arm, robot, initial_state=INITIAL_ROBOT_STATE, planner_id=''):
m = mm.MotionPlanRequest()
m.group_name = "%s_arm" % arm
m.start_state = initial_state
m.planner_id = planner_id
c = mm.Constraints()
joints = robot.GetJoints()
joint_inds = robot.GetManipulator("%sarm"%arm).GetArmIndices()
c.joint_constraints = [mm.JointConstraint(joint_name=joints[joint_inds[i]].GetName(), position = jvals[i])
for i in xrange(len(jvals))]
jc = mm.JointConstraint()
m.goal_constraints = [c]
m.allowed_planning_time = rospy.Duration(args.max_planning_time)
base_pose = initial_state.multi_dof_joint_state.poses[0]
base_q = base_pose.orientation
base_p = base_pose.position
t = rave.matrixFromPose([base_q.w, base_q.x, base_q.y, base_q.z, base_p.x, base_p.y, base_p.z])
robot.SetTransform(t)
return m
def get_joint_trajectory(self, start_joints, target_pose, n_steps=20):
"""
Calls trajopt to plan collision-free trajectory
:param start_joints: list of initial joints
:param target_pose: desired pose of tool_frame (tfx.pose)
:param n_steps: trajopt discretization
:return list of joint values
"""
assert len(start_joints) == len(self.joint_indices)
assert target_pose.frame.count('base_link') == 1
self.sim.update()
# set start joint positions
self.robot.SetDOFValues(start_joints, self.joint_indices)
# initialize trajopt inputs
rave_pose = tfx.pose(self.sim.transform_from_to(target_pose.matrix, target_pose.frame, 'world'))
quat = rave_pose.orientation
xyz = rave_pose.position
quat_target = [quat.w, quat.x, quat.y, quat.z]
xyz_target = [xyz.x, xyz.y, xyz.z]
rave_mat = rave.matrixFromPose(np.r_[quat_target, xyz_target])
#init_joint_target = ku.ik_for_link(rave_mat, self.manip, self.tool_frame, filter_options=rave.IkFilterOptions.CheckEnvCollisions)
#if init_joint_target is None:
# rospy.loginfo('get_traj: IK failed')
# return False
init_joint_target = None
request = self._get_trajopt_request(xyz_target, quat_target, init_joint_target, n_steps)
# convert dictionary into json-formatted string
s = json.dumps(request)
# create object that stores optimization problem
prob = trajoptpy.ConstructProblem(s, self.sim.env)
# do optimization
result = trajoptpy.OptimizeProblem(prob)
return result.GetTraj()
def main():
### Parameters ###
ENV_FILE = "data/pr2test1.env.xml"
N_STEPS = 15
XYZ_TARGET = [.5,0,.9]
QUAT_TARGET = [1,0,0,0]
INTERACTIVE = True
LINK_NAME = "r_gripper_tool_frame"
##################
### Env setup ####
env = rave.RaveGetEnvironment(1)
if env is None:
env = rave.Environment()
env.StopSimulation()
atexit.register(rave.RaveDestroy)
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, 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.GetJoint("torso_lift_joint").GetDOFIndex()],
rave.DOFAffine.X + rave.DOFAffine.Y + rave.DOFAffine.RotationAxis, [0,0,1])
##################
for i_try in xrange(100):
request = position_base_request(robot, LINK_NAME, XYZ_TARGET, QUAT_TARGET)
s = json.dumps(request)
print "REQUEST:",s
trajoptpy.SetInteractive(INTERACTIVE);
prob = trajoptpy.ConstructProblem(s, env)
result = trajoptpy.OptimizeProblem(prob)
if result.GetConstraints()[0][1] < 1e-3:
break
print "succeeded on try %i"%(i_try)
print result
def GeneratePose(sensorPosition, targetPosition, l=(1, 0, 0)):
'''Helps to determine a sensor pose just given a sensor position and view target.
- Input sensorPosition: 3-element desired position of sensor placement.
- Input targetPosition: 3-element position of object required to view.
- Input l: Sensor LOS axis in base frame given identity orientation.
- Returns T: 4x4 numpy array (transformation matrix) representing desired pose of end effector in the base frame.
'''
v = targetPosition - sensorPosition
vMag = norm(v)
v = [v[0] / vMag, v[1] / vMag, v[2] / vMag]
k = [
l[1] * v[2] - l[2] * v[1], l[2] * v[0] - l[0] * v[2],
l[0] * v[1] - l[1] * v[0]
]
theta = acos(l[0] * v[0] + l[1] * v[1] + l[2] * v[2])
q = tf.transformations.quaternion_about_axis(theta, k)
return openravepy.matrixFromPose(numpy.r_[[q[3], q[0], q[1], q[2]],
sensorPosition])
def get_return_from_grasp_joint_trajectory(self,
start_joints,
target_pose,
n_steps=40):
"""
Calls trajopt to plan return from grasp collision-free trajectory
:param start_joints: list of initial joints
:param target_pose: desired pose of tool_frame (tfx.pose)
:param n_steps: trajopt discretization
:return None if traj not collision free, else list of joint values
"""
assert len(start_joints) == len(self.joint_indices)
assert target_pose.frame.count('base_link') == 1
# set active manipulator and start joint positions
self.robot.SetDOFValues(start_joints, self.joint_indices)
# initialize trajopt inputs
rave_pose = tfx.pose(
self.sim.transform_from_to(target_pose.matrix, target_pose.frame,
'world'))
quat = rave_pose.orientation
xyz = rave_pose.position
quat_target = [quat.w, quat.x, quat.y, quat.z]
xyz_target = [xyz.x, xyz.y, xyz.z]
rave_mat = rave.matrixFromPose(np.r_[quat_target, xyz_target])
request = self._get_return_from_grasp_trajopt_request(
xyz_target, quat_target, n_steps)
# convert dictionary into json-formatted string
s = json.dumps(request)
# create object that stores optimization problem
prob = trajoptpy.ConstructProblem(s, self.sim.env)
tool_link = self.robot.GetLink(self.tool_frame)
def penalize_low_height(x):
self.robot.SetDOFValues(x, self.joint_indices, False)
z = tool_link.GetTransform()[2, 3]
return max(0, 10.0 - z)
for t in xrange(n_steps - 2):
prob.AddErrorCost(penalize_low_height,
[(t, j)
for j in xrange(len(self.joint_indices))],
"ABS", "PENALIZE_LOW_HEIGHT_%i" % t)
# do optimization
result = trajoptpy.OptimizeProblem(prob)
self.robot.SetDOFValues(start_joints, self.joint_indices)
prob.SetRobotActiveDOFs(
) # set robot DOFs to DOFs in optimization problem
num_upsampled_collisions = self._num_collisions(result.GetTraj())
print('Number of collisions: {0}'.format(num_upsampled_collisions))
self.robot.SetDOFValues(start_joints, self.joint_indices)
if num_upsampled_collisions > 2:
return None
else:
return result.GetTraj()
# import xml.etree.ElementTree as ET
# cc = trajoptpy.GetCollisionChecker(env)
# root = ET.parse("/opt/ros/groovy/share/pr2_moveit_config/config/pr2.srdf").getroot()
# disabled_elems=root.findall("disable_collisions")
# for elem in disabled_elems:
# linki = robot.GetLink(elem.get("link1"))
# linkj = robot.GetLink(elem.get("link2"))
# if linki and linkj:
# cc.ExcludeCollisionPair(linki, linkj)
xs, ys, zs = np.mgrid[.35:.85:.05, 0:.5:.05, .8:.9:.1]
wxyz = [1, 0, 0, 0]
results = []
for (x, y, z) in zip(xs.flat, ys.flat, zs.flat):
wxyzxyz = np.r_[wxyz, x, y, z]
joint_solutions = ku.ik_for_link(openravepy.matrixFromPose(wxyzxyz),
robot.GetManipulator(manip),
"%s_gripper_tool_frame" % manip[0], 1,
True)
if len(joint_solutions) > 0:
target_joints = joint_solutions[0]
results.append(
plan(robot, manip, target_joints,
wxyzxyz if args.pose_goal else None))
success_frac = np.mean([result.success for result in results])
print "success frac:", success_frac
print "avg total time:", np.mean([result.t_total for result in results])
print "avg optimization time:", np.mean([result.t_opt for result in results])
print "avg verification time:", np.mean(
[result.t_verify for result in results])
def PlanWorkspacePath(self,
robot,
traj,
timelimit=5.0,
min_waypoint_index=None,
norm_order=2,
**kw_args):
"""
Plan a configuration space path given a workspace path.
All timing information is ignored.
@param robot
@param traj workspace trajectory
represented as OpenRAVE AffineTrajectory
@param min_waypoint_index minimum waypoint index to reach
@param timelimit timeout in seconds
@param norm_order: 1 ==> The L1 norm
2 ==> The L2 norm
inf ==> The L_infinity norm
Used to determine the resolution of collision checked waypoints
in the trajectory
@return qtraj configuration space path
"""
env = robot.GetEnv()
from .exceptions import (TimeoutPlanningError, CollisionPlanningError,
SelfCollisionPlanningError)
from openravepy import (CollisionOptions, CollisionOptionsStateSaver,
CollisionReport)
p = openravepy.KinBody.SaveParameters
with robot, CollisionOptionsStateSaver(env.GetCollisionChecker(),
CollisionOptions.ActiveDOFs), \
robot.CreateRobotStateSaver(p.ActiveDOF | p.LinkTransformation):
manip = robot.GetActiveManipulator()
robot.SetActiveDOFs(manip.GetArmIndices())
# Create a new trajectory starting at current robot location.
qtraj = openravepy.RaveCreateTrajectory(env, '')
qtraj.Init(manip.GetArmConfigurationSpecification('linear'))
qtraj.Insert(0, robot.GetActiveDOFValues())
# Initial search for workspace path timing: one huge step.
t = 0.
dt = traj.GetDuration()
q_resolutions = robot.GetActiveDOFResolutions()
# Smallest CSpace step at which to give up
min_step = numpy.linalg.norm(robot.GetActiveDOFResolutions() /
100.,
ord=norm_order)
ik_options = openravepy.IkFilterOptions.CheckEnvCollisions
start_time = time.time()
epsilon = 1e-6
try:
while t < traj.GetDuration() + epsilon:
# Check for a timeout.
# TODO: This is not really deterministic because we do not
# have control over CPU time. However, it is exceedingly
# unlikely that running the query again will change the
# outcome unless there is a significant change in CPU load.
current_time = time.time()
if (timelimit is not None
and current_time - start_time > timelimit):
raise TimeoutPlanningError(timelimit,
deterministic=True)
# Hypothesize new configuration as closest IK to current
qcurr = robot.GetActiveDOFValues() # Configuration at t.
qnew = manip.FindIKSolution(openravepy.matrixFromPose(
traj.Sample(t + dt)[0:7]),
ik_options,
ikreturn=False,
releasegil=True)
# Check if the step was within joint DOF resolution.
infeasible_step = True
if qnew is not None:
# Found an IK
steps = abs(qnew - qcurr) / q_resolutions
norm = numpy.linalg.norm(steps, ord=norm_order)
if (norm < min_step) and qtraj:
raise PlanningError('Not making progress.')
infeasible_step = norm > 1.0
if infeasible_step:
# Backtrack and try half the step
dt = dt / 2.0
else:
# Move forward to new trajectory time.
robot.SetActiveDOFValues(qnew)
qtraj.Insert(qtraj.GetNumWaypoints(), qnew)
t = min(t + dt, traj.GetDuration())
dt = dt * 2.0
except PlanningError as e:
# Compute the min acceptable time from the min waypoint index.
if min_waypoint_index is None:
min_waypoint_index = traj.GetNumWaypoints() - 1
cspec = traj.GetConfigurationSpecification()
wpts = [
traj.GetWaypoint(i) for i in range(min_waypoint_index + 1)
]
dts = [cspec.ExtractDeltaTime(wpt) for wpt in wpts]
min_time = numpy.sum(dts)
# Throw an error if we haven't reached the minimum waypoint.
if t < min_time:
# FindIKSolutions is slower than FindIKSolution, so call
# this only to identify error when there is no solution.
ik_solutions = manip.FindIKSolutions(
openravepy.matrixFromPose(
traj.Sample(t + dt * 2.0)[0:7]),
openravepy.IkFilterOptions.IgnoreSelfCollisions,
ikreturn=False,
releasegil=True)
collision_error = None
# update collision_error to contain collision info.
with robot.CreateRobotStateSaver(p.LinkTransformation):
for q in ik_solutions:
robot.SetActiveDOFValues(q)
cr = CollisionReport()
if env.CheckCollision(robot, report=cr):
collision_error = \
CollisionPlanningError.FromReport(
cr, deterministic=True)
elif robot.CheckSelfCollision(report=cr):
collision_error = \
SelfCollisionPlanningError.FromReport(
cr, deterministic=True)
else:
collision_error = None
if collision_error is not None:
raise collision_error
else:
raise
# Otherwise we'll gracefully terminate.
else:
logger.warning('Terminated early at time %f < %f: %s', t,
traj.GetDuration(), str(e))
SetTrajectoryTags(qtraj, {
Tags.CONSTRAINED: True,
Tags.DETERMINISTIC_TRAJECTORY: True,
Tags.DETERMINISTIC_ENDPOINT: True,
},
append=True)
return qtraj
trajoptpy.SetInteractive(
args.interactive
) # pause every iteration, until you press 'p'. Press escape to disable further plotting
robot = env.GetRobots()[0]
manip = robot.GetManipulator("rightarm")
robot.SetActiveManipulator(manip)
ikmodel = openravepy.databases.inversekinematics.InverseKinematicsModel(
robot, iktype=openravepy.IkParameterization.Type.Transform6D)
if not ikmodel.load():
ikmodel.autogenerate()
xyz_target = [.6, -.6, 1]
n_steps = 15
hmat_target = openravepy.matrixFromPose(np.r_[(0, 1, 0, 0), xyz_target])
init_joint_target = ku.ik_for_link(
hmat_target,
manip,
"r_gripper_tool_frame",
filter_options=openravepy.IkFilterOptions.CheckEnvCollisions)
request = {
"basic_info": {
"n_steps": n_steps,
"manip": "rightarm",
"start_fixed": True
},
"costs": [{
"type": "joint_vel",
env.StopSimulation()
env.Load("robots/pr2-beta-static.zae")
env.Load("../data/table.xml")
trajoptpy.SetInteractive(
args.interactive
) # pause every iteration, until you press 'p'. Press escape to disable further plotting
robot = env.GetRobots()[0]
joint_start = [-1.832, -0.332, -1.011, -1.437, -1.1, -1.926, 3.074]
robot.SetDOFValues(joint_start,
robot.GetManipulator('rightarm').GetArmIndices())
quat_target = [1, 0, 0, 0] # wxyz
xyz_target = [6.51073449e-01, -1.87673551e-01, 4.91061915e-01]
hmat_target = openravepy.matrixFromPose(np.r_[quat_target, xyz_target])
# BEGIN ik
manip = robot.GetManipulator("rightarm")
robot.SetActiveManipulator(manip)
ikmodel = openravepy.databases.inversekinematics.InverseKinematicsModel(
robot, iktype=openravepy.IkParameterization.Type.Transform6D)
if not ikmodel.load():
ikmodel.autogenerate()
init_joint_target = ku.ik_for_link(
hmat_target,
manip,
"r_gripper_tool_frame",
filter_options=openravepy.IkFilterOptions.CheckEnvCollisions)
# END ik
def get_grasp_joint_trajectory(self,
start_joints,
target_pose,
n_steps=40,
ignore_orientation=False,
link_name=None):
"""
Calls trajopt to plan grasp collision-free trajectory
:param start_joints: list of initial joints
:param target_pose: desired pose of tool_frame (tfx.pose)
:param n_steps: trajopt discretization
:return None if traj not collision free, else list of joint values
"""
link_name = link_name if link_name is not None else self.tool_frame
assert len(start_joints) == len(self.joint_indices)
assert target_pose.frame.count('base_link') == 1
self.sim.update()
# set active manipulator and start joint positions
self.robot.SetDOFValues(start_joints, self.joint_indices)
# initialize trajopt inputs
rave_pose = tfx.pose(
self.sim.transform_from_to(target_pose.matrix, target_pose.frame,
'world'))
quat = rave_pose.orientation
xyz = rave_pose.position
quat_target = [quat.w, quat.x, quat.y, quat.z]
xyz_target = [xyz.x, xyz.y, xyz.z]
rave_mat = rave.matrixFromPose(np.r_[quat_target, xyz_target])
# init_joint_target = None
init_joint_target = self.sim.ik_for_link(rave_pose.matrix, self.manip,
link_name, 0)
if init_joint_target is not None:
init_joint_target = self._closer_joint_angles(
init_joint_target, start_joints)
init_traj = self.ik_point(start_joints,
xyz,
n_steps=n_steps,
link_name=link_name)
request = self._get_grasp_trajopt_request(
xyz_target,
quat_target,
n_steps,
ignore_orientation=ignore_orientation,
link_name=link_name,
init_traj=init_traj)
# convert dictionary into json-formatted string
s = json.dumps(request)
# create object that stores optimization problem
prob = trajoptpy.ConstructProblem(s, self.sim.env)
# TODO: worth doing?
# tool_link = self.robot.GetLink(link_name)
# def point_at(x):
# self.robot.SetDOFValues(x, self.joint_indices, False)
# T = tool_link.GetTransform()
# local_dir = xyz.array - T[:3,3]
# return T[1:3,:3].dot(local_dir)
#
# for t in xrange(int(0.8*n_steps), n_steps-1):
# #prob.AddConstraint(point_at, [(t,j) for j in xrange(len(self.joint_indices))], "EQ", "POINT_AT_%i"%t)
# prob.AddErrorCost(point_at, [(t,j) for j in xrange(len(self.joint_indices))], "ABS", "POINT_AT_%i"%t)
# do optimization
result = trajoptpy.OptimizeProblem(prob)
prob.SetRobotActiveDOFs(
) # set robot DOFs to DOFs in optimization problem
#num_upsampled_collisions = len(traj_collisions(result.GetTraj(), self.robot, n=100))
num_upsampled_collisions = self._num_collisions(result.GetTraj())
print('Number of collisions: {0}'.format(num_upsampled_collisions))
self.robot.SetDOFValues(start_joints, self.joint_indices)
if num_upsampled_collisions > 2:
#if not traj_is_safe(result.GetTraj()[:], self.robot): # Check that trajectory is collision free
return None
else:
return result.GetTraj()