Beispiel #1
0
        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
Beispiel #2
0
        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
Beispiel #3
0
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)
Beispiel #4
0
        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
Beispiel #5
0
        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
Beispiel #6
0
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)
Beispiel #7
0
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
Beispiel #9
0
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)
Beispiel #10
0
    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))
Beispiel #13
0
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)
Beispiel #15
0
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
Beispiel #16
0
    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
Beispiel #17
0
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
Beispiel #19
0
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
Beispiel #20
0
 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
Beispiel #21
0
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 '=================='
Beispiel #22
0
 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
Beispiel #23
0
 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')
Beispiel #24
0
    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)
Beispiel #25
0
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
Beispiel #26
0
 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
Beispiel #27
0
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
Beispiel #28
0
    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)
Beispiel #29
0
    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 '=================='
Beispiel #31
0
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
Beispiel #32
0
    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
Beispiel #34
0
    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
Beispiel #36
0
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)
Beispiel #38
0
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)
Beispiel #39
0
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
Beispiel #40
0
    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
Beispiel #41
0
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
Beispiel #42
0
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
Beispiel #43
0
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)
Beispiel #46
0
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)
Beispiel #47
0
    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
Beispiel #49
0
 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()
Beispiel #50
0
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
Beispiel #51
0
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])
Beispiel #52
0
    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])
Beispiel #54
0
    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
Beispiel #55
0
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
Beispiel #57
0
    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()