def draw_point(env, point, size=10, color=(0, 1, 0)):
"""
Draw a colored point into the OpenRAVE environment.
Parameters
----------
env: orpy.Environment
The OpenRAVE environment
point: array_like
XYZ position of the point
size: float
Size of the point (pixels)
color: array_like
Color of the point in the format `(red, green, blue, alpha)`
Returns
-------
h: orpy.GraphHandle
Handles of the plot. This is require for the point to stay on the
environment
"""
iktype = orpy.IkParameterizationType.Translation3D
ikparam = orpy.IkParameterization(point, iktype)
h = orpy.misc.DrawIkparam2(env, ikparam, linewidth=size, coloradd=color)
return h
def draw_ray(env, ray, dist=0.03, linewidth=2, color=None):
"""
Draw a ray as an arrow + line into the OpenRAVE environment.
Parameters
----------
env: orpy.Environment
The OpenRAVE environment
ray: orpy.ray
The input ray with the position and direction of the arrow
dist: float
Length of the line
linewidth: float
Linewidth of the arrow and line (pixels)
color: array_like
Color of the arrow in the format `(red, green, blue, alpha)`
Returns
-------
h: orpy.GraphHandle
Handles holding the plot.
"""
if dist < 0:
newpos = ray.pos() + dist * ray.dir()
newray = orpy.Ray(newpos, ray.dir())
else:
newray = ray
iktype = orpy.IkParameterizationType.TranslationDirection5D
ikparam = orpy.IkParameterization(ray, iktype)
h = orpy.misc.DrawIkparam2(env,
ikparam,
dist=dist,
linewidth=linewidth,
coloradd=color)
return h
def PlanToTransform(env, robot, transform):
handle = openravepy.misc.DrawAxes(env, transform)
iksolver = robot.arm.GetIkSolver()
param = openravepy.IkParameterization(
transform, openravepy.IkParameterizationType.Transform6D)
solution = iksolver.Solve(param, robot.GetActiveDOFValues(), 0)
traj = robot.PlanToConfiguration(solution.GetSolution())
return traj
def FindIK(self, target):
"""Find an IK solution that is looking at a desired position.
@param target target position
@return IK solution
"""
ik_params = openravepy.IkParameterization(
target, openravepy.IkParameterization.Type.Lookat3D)
return self.ikmodel.manip.FindIKSolution(ik_params, 0)
def get_pos_ik_solns(robot, x):
ik_param = orpy.IkParameterization(
x, orpy.IkParameterizationType.Translation3D)
ik_solution = robot.arm.FindIKSolutions(
ik_param,
orpy.IkFilterOptions.CheckEnvCollisions,
ikreturn=False,
releasegil=True)
return ik_solution
def get_ik_solns(robot, goal_T):
with robot.GetEnv():
ik_param = orpy.IkParameterization(
goal_T, orpy.IkParameterizationType.Transform6D)
ik_solutions = robot.arm.FindIKSolutions(
ik_param,
orpy.IkFilterOptions.CheckEnvCollisions,
ikreturn=False,
releasegil=True)
return ik_solutions
def get_ik_parameterization(goal, iktype):
target = goal
if iktype == orpy.IkParameterizationType.TranslationDirection5D:
if type(goal) is not orpy.Ray:
target = criros.conversions.to_ray(goal)
elif iktype == orpy.IkParameterizationType.Transform6D:
if type(goal) is orpy.Ray:
target = criros.conversions.from_ray(goal)
else:
return None
return orpy.IkParameterization(target, iktype)
def find_ik_solutions(robot, target, iktype, collision_free=True, freeinc=0.1):
"""
Find all the possible IK solutions
Parameters
----------
robot: orpy.Robot
The OpenRAVE robot
target: array_like or orpy.Ray
The target in the task space (Cartesian). A target can be defined as a Ray
(position and direction) which is 5D or as an homogeneous transformation
which is 6D.
iktype: orpy.IkParameterizationType
Inverse kinematics type to use. Supported values:
`orpy.IkParameterizationType.Transform6D` and
`orpy.IkParameterizationType.TranslationDirection5D`
collision_free: bool, optional
If true, find only collision-free solutions
freeinc: float, optional
The free increment (discretization) to be used for the free DOF when the
target is 5D.
Returns
-------
solutions: list
The list of IK solutions found
"""
# Populate the target list
target_list = []
if iktype == orpy.IkParameterizationType.TranslationDirection5D:
if type(target) is not orpy.Ray:
ray = ru.conversions.to_ray(target)
target_list.append(ray)
else:
target_list.append(target)
elif iktype == orpy.IkParameterizationType.Transform6D:
if type(target) is orpy.Ray:
Tray = ru.conversions.from_ray(target)
for angle in np.arange(0, 2 * np.pi, freeinc):
Toffset = orpy.matrixFromAxisAngle(angle * br.Z_AXIS)
target_list.append(np.dot(Tray, Toffset))
else:
target_list.append(target)
# Concatenate all the solutions
solutions = []
for goal in target_list:
ikparam = orpy.IkParameterization(goal, iktype)
manipulator = robot.GetActiveManipulator()
opt = 0
if collision_free:
opt = orpy.IkFilterOptions.CheckEnvCollisions
solutions += list(manipulator.FindIKSolutions(ikparam, opt))
return solutions
def PlanToEndEffectorPose(self, robot, goal_pose, **kw_args):
"""
Attempt to plan a straight line trajectory from the robot's
current configuration to a desired end-effector pose. This
happens by finding the closest IK solution to the robot's
current configuration and attempts to snap there
(using PlanToConfiguration) if possible.
In the case of a redundant manipulator, no attempt is
made to check other IK solutions.
@param robot
@param goal_pose desired end-effector pose
@return traj
"""
from prpy.planning.exceptions import CollisionPlanningError
from prpy.planning.exceptions import SelfCollisionPlanningError
ikp = openravepy.IkParameterizationType
ikfo = openravepy.IkFilterOptions
# Find an IK solution. OpenRAVE tries to return a solution that is
# close to the configuration of the arm, so we don't need to do any
# custom IK ranking.
manipulator = robot.GetActiveManipulator()
ik_param = openravepy.IkParameterization(goal_pose, ikp.Transform6D)
ik_solution = manipulator.FindIKSolution(
ik_param, ikfo.CheckEnvCollisions,
ikreturn=False, releasegil=True
)
if ik_solution is None:
# FindIKSolutions is slower than FindIKSolution,
# so call this only to identify and raise error when
# there is no solution
ik_solutions = manipulator.FindIKSolutions(
ik_param, ikfo.IgnoreSelfCollisions,
ikreturn=False, releasegil=True)
for q in ik_solutions:
robot.SetActiveDOFValues(q)
report = openravepy.CollisionReport()
if self.env.CheckCollision(robot, report=report):
raise CollisionPlanningError.FromReport(report)
elif robot.CheckSelfCollision(report=report):
raise SelfCollisionPlanningError.FromReport(report)
raise PlanningError('There is no IK solution at the goal pose.')
return self._Snap(robot, ik_solution, **kw_args)
def lookat_ik(self, target, current_angles=None):
# Update robot state to current angles (or best guess from current model position)
if current_angles is None:
current_angles = self.robot.GetDOFValues(self.arm_indices)
self.robot.SetDOFValues(current_angles, self.arm_indices)
# Solve IK
sols = self.manip.FindIKSolutions(
op.IkParameterization(target, op.IkParameterizationType.Lookat3D),
op.IkFilterOptions.CheckEnvCollisions)
if not np.any(sols):
raise IkError("[{0}] Could not find an IK solution.".format(
rospy.get_name()))
# Weed out solutions which don't point directly at the target point
good_sols = np.array(
[s for s in sols if self.verify_solution(target, s)])
# Get the solution with the (weighted) nearest joint angles to the current angles
# (Argmin over weighted sum of squared error)
rospy.loginfo("[%s] Found LookatIK solution." % rospy.get_name())
return good_sols[np.argmin(
np.sum(self.weights * np.sqrt((good_sols - current_angles)**2),
1))]
def draw_axes(env, transform, dist=0.03, linewidth=2):
"""
Draw an RGB set of axes into the OpenRAVE environment.
Parameters
----------
env: orpy.Environment
The OpenRAVE environment
transform: array_like
The homogeneous transformation where to draw the axes
dist: float
Length of the axes (meters)
linewidth: float
Line width of the axes (pixels)
Returns
-------
h: orpy.GraphHandle
The total axes added to the scene.
"""
iktype = orpy.IkParameterizationType.Transform6D
ikparam = orpy.IkParameterization(transform, iktype)
h = orpy.misc.DrawIkparam2(env, ikparam, dist=dist, linewidth=linewidth)
return h
def get_old_joint_traj_ik(sim_env, ee_hmats, prev_vals, i_start, i_end):
old_joint_trajs = {}
for lr in 'lr':
old_joint_traj = []
link_name = "%s_gripper_tool_frame" % lr
manip_name = {"l": "leftarm", "r": "rightarm"}[lr]
manip = sim_env.robot.GetManipulator(manip_name)
ik_type = openravepy.IkParameterizationType.Transform6D
all_x = []
x = []
for (i, pose_matrix) in enumerate(ee_hmats[link_name]):
rot_pose_matrix = pose_matrix.dot(TFM_GTF_EE)
sols = manip.FindIKSolutions(
openravepy.IkParameterization(rot_pose_matrix, ik_type),
openravepy.IkFilterOptions.CheckEnvCollisions)
all_x.append(i)
if sols != []:
x.append(i)
reference_sol = None
for sol in reversed(old_joint_traj):
if sol != None:
reference_sol = sol
break
if reference_sol is None:
if prev_vals[lr] is not None:
reference_sol = prev_vals[lr]
else:
reference_sol = sim_util.PR2_L_POSTURES[
'side'] if lr == 'l' else sim_util.mirror_arm_joints(
sim_util.PR2_L_POSTURES['side'])
sols = [
sim_util.closer_angs(sol, reference_sol) for sol in sols
]
norm_differences = [
norm(np.asarray(reference_sol) - np.asarray(sol), 2)
for sol in sols
]
min_index = norm_differences.index(min(norm_differences))
old_joint_traj.append(sols[min_index])
#blueprint("Openrave IK succeeds")
#else:
#redprint("Openrave IK fails")
if len(x) == 0:
if prev_vals[lr] is not None:
vals = prev_vals[lr]
else:
vals = sim_util.PR2_L_POSTURES[
'side'] if lr == 'l' else sim_util.mirror_arm_joints(
sim_util.PR2_L_POSTURES['side'])
old_joint_traj_interp = np.tile(vals, (i_end + 1 - i_start, 1))
else:
if prev_vals[lr] is not None:
old_joint_traj_interp = sim_util.lerp(all_x,
x,
old_joint_traj,
first=prev_vals[lr])
else:
old_joint_traj_interp = sim_util.lerp(all_x, x, old_joint_traj)
yellowprint("Openrave IK found %i solutions out of %i." %
(len(x), len(all_x)))
init_traj_close = sim_util.close_traj(old_joint_traj_interp.tolist())
old_joint_trajs[lr] = np.asarray(init_traj_close)
return old_joint_trajs
if not manip.ik_database.load():
print 'Generating IK database for %s.' % manip.GetName()
manip.ik_database.autogenerate()
target = np.array([0.42, 0, 0.13])
#target = np.array([ 0.39, -0.06220808, 0.2342928])
#target = np.array([ 0.39, 0.0, 0.2342928])
direction = np.array([-1, 0, 0])
zstep = 0.001
prevtarget = None
# move up to find highest point with an ik.
while True:
solution = manip.FindIKSolution(
orpy.IkParameterization(
orpy.Ray(target, direction),
orpy.IkParameterization.Type.TranslationDirection5D),
orpy.IkFilterOptions.CheckEnvCollisions)
if solution is None:
print 'highest: ', prevtarget
target = prevtarget.copy()
break
else:
prevtarget = target.copy()
target[2] = target[2] + zstep
path = []
while True:
solution = manip.FindIKSolution(
orpy.IkParameterization(
orpy.Ray(target, direction),
# Generate IKFast if needed
iktype = orpy.IkParameterization.Type.Transform6D
ikmodel = orpy.databases.inversekinematics.InverseKinematicsModel(robot, iktype=iktype)
if not ikmodel.load():
print 'Generating IKFast {0}. It will take few minutes...'.format(iktype.name)
ikmodel.autogenerate()
print 'IKFast {0} has been successfully generated'.format(iktype.name)
# Find a valid IK solution for grasping the cube
cube = env.GetKinBody('cube01')
cube_centroid = cube.ComputeAABB().pos()
Tgrasp = tr.euler_matrix(0, np.pi, 0)
Tgrasp[:3,3] = cube_centroid
qgrasp = None
ikparam = orpy.IkParameterization(Tgrasp, iktype)
solutions = manipulator.FindIKSolutions(ikparam, orpy.IkFilterOptions.CheckEnvCollisions)
if len(solutions) == 0:
raise Exception('Failed to find a valid IK for grasping the cube')
qgrasp = solutions[0]
axes = []
axes.append( orpy.misc.DrawAxes(env, Tgrasp, dist=0.05) )
# Move to the grasping pose
def plan_to_joint_values(qgoal):
# Create the planner
planner = orpy.RaveCreatePlanner(env,'birrt') # Using bidirectional RRT
params = orpy.Planner.PlannerParameters()
params.SetRobotActiveJoints(robot)
params.SetGoalConfig(qgoal)
params.SetExtraParameters('<_postprocessing planner="ParabolicSmoother"><_nmaxiterations>40</_nmaxiterations></_postprocessing>')
