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>')