Example #1
0
    def PlanToEndEffectorPose(self, robot, goal_pose, timelimit=5.0,
                              **kw_args):
        """
        Plan to an end effector pose by first creating a geodesic
        trajectory in SE(3) from the starting end-effector pose to the goal
        end-effector pose, and then attempting to follow it exactly
        using PlanWorkspacePath.

        @param robot
        @param goal_pose desired end-effector pose
        @return traj
        """

        with robot:
            # Create geodesic trajectory in SE(3)
            manip = robot.GetActiveManipulator()
            start_pose = manip.GetEndEffectorTransform()
            traj = openravepy.RaveCreateTrajectory(self.env, '')
            spec = openravepy.IkParameterization.\
                GetConfigurationSpecificationFromType(
                        openravepy.IkParameterizationType.Transform6D,
                        'linear')
            traj.Init(spec)
            traj.Insert(traj.GetNumWaypoints(),
                        openravepy.poseFromMatrix(start_pose))
            traj.Insert(traj.GetNumWaypoints(),
                        openravepy.poseFromMatrix(goal_pose))
            openravepy.planningutils.RetimeAffineTrajectory(
                traj,
                maxvelocities=0.1*numpy.ones(7),
                maxaccelerations=0.1*numpy.ones(7)
            )

        return self.PlanWorkspacePath(robot, traj, timelimit)
Example #2
0
    def PlanToEndEffectorOffset(self,
                                robot,
                                direction,
                                distance,
                                max_distance=None,
                                timelimit=5.0,
                                **kw_args):
        """
        Plan to a desired end-effector offset with move-hand-straight
        constraint. movement less than distance will return failure.
        The motion will not move further than max_distance.
        @param robot
        @param direction unit vector in the direction of motion
        @param distance minimum distance in meters
        @param max_distance maximum distance in meters
        @param timelimit timeout in seconds
        @return traj
        """

        if distance < 0:
            raise ValueError('Distance must be non-negative.')
        elif numpy.linalg.norm(direction) == 0:
            raise ValueError('Direction must be non-zero')
        elif max_distance is not None and max_distance < distance:
            raise ValueError('Max distance is less than minimum distance.')
        elif max_distance is not None and not numpy.isfinite(max_distance):
            raise ValueError('Max distance must be finite.')

        # Normalize the direction vector.
        direction = numpy.array(direction, dtype='float')
        direction /= numpy.linalg.norm(direction)

        with robot:
            manip = robot.GetActiveManipulator()
            start_pose = manip.GetEndEffectorTransform()
            traj = openravepy.RaveCreateTrajectory(self.env, '')
            spec = openravepy.IkParameterization.\
                GetConfigurationSpecificationFromType(
                    openravepy.IkParameterizationType.Transform6D, 'linear')
            traj.Init(spec)
            traj.Insert(traj.GetNumWaypoints(),
                        openravepy.poseFromMatrix(start_pose))
            min_pose = numpy.copy(start_pose)
            min_pose[0:3, 3] += distance * direction
            traj.Insert(traj.GetNumWaypoints(),
                        openravepy.poseFromMatrix(min_pose))
            if max_distance is not None:
                max_pose = numpy.copy(start_pose)
                max_pose[0:3, 3] += max_distance * direction
                traj.Insert(traj.GetNumWaypoints(),
                            openravepy.poseFromMatrix(max_pose))
            openravepy.planningutils.RetimeAffineTrajectory(
                traj,
                maxvelocities=0.1 * numpy.ones(7),
                maxaccelerations=0.1 * numpy.ones(7))

        return self.PlanWorkspacePath(robot,
                                      traj,
                                      timelimit,
                                      min_waypoint_index=1)
Example #3
0
    def PlanToEndEffectorPose(self,
                              robot,
                              goal_pose,
                              timelimit=5.0,
                              **kw_args):
        """
        Plan to an end effector pose by first creating a geodesic
        trajectory in SE(3) from the starting end-effector pose to the goal
        end-effector pose, and then attempting to follow it exactly
        using PlanWorkspacePath.

        @param robot
        @param goal_pose desired end-effector pose
        @return traj
        """

        with robot:
            # Create geodesic trajectory in SE(3)
            manip = robot.GetActiveManipulator()
            start_pose = manip.GetEndEffectorTransform()
            traj = openravepy.RaveCreateTrajectory(self.env, '')
            spec = openravepy.IkParameterization.\
                GetConfigurationSpecificationFromType(
                    openravepy.IkParameterizationType.Transform6D, 'linear')
            traj.Init(spec)
            traj.Insert(traj.GetNumWaypoints(),
                        openravepy.poseFromMatrix(start_pose))
            traj.Insert(traj.GetNumWaypoints(),
                        openravepy.poseFromMatrix(goal_pose))
            openravepy.planningutils.RetimeAffineTrajectory(
                traj,
                maxvelocities=0.1 * numpy.ones(7),
                maxaccelerations=0.1 * numpy.ones(7))

        return self.PlanWorkspacePath(robot, traj, timelimit)
Example #4
0
    def PlanToEndEffectorOffset(self, robot, direction, distance,
                                max_distance=None, timelimit=5.0,
                                **kw_args):

        """
        Plan to a desired end-effector offset with move-hand-straight
        constraint. movement less than distance will return failure.
        The motion will not move further than max_distance.
        @param robot
        @param direction unit vector in the direction of motion
        @param distance minimum distance in meters
        @param max_distance maximum distance in meters
        @param timelimit timeout in seconds
        @return traj
        """

        if distance < 0:
            raise ValueError('Distance must be non-negative.')
        elif numpy.linalg.norm(direction) == 0:
            raise ValueError('Direction must be non-zero')
        elif max_distance is not None and max_distance < distance:
            raise ValueError('Max distance is less than minimum distance.')
        elif max_distance is not None and not numpy.isfinite(max_distance):
            raise ValueError('Max distance must be finite.')

        # Normalize the direction vector.
        direction = numpy.array(direction, dtype='float')
        direction /= numpy.linalg.norm(direction)

        with robot:
            manip = robot.GetActiveManipulator()
            start_pose = manip.GetEndEffectorTransform()
            traj = openravepy.RaveCreateTrajectory(self.env, '')
            spec = openravepy.IkParameterization.\
                GetConfigurationSpecificationFromType(
                    openravepy.IkParameterizationType.Transform6D, 'linear')
            traj.Init(spec)
            traj.Insert(traj.GetNumWaypoints(),
                        openravepy.poseFromMatrix(start_pose))
            min_pose = numpy.copy(start_pose)
            min_pose[0:3, 3] += distance * direction
            traj.Insert(traj.GetNumWaypoints(),
                        openravepy.poseFromMatrix(min_pose))
            if max_distance is not None:
                max_pose = numpy.copy(start_pose)
                max_pose[0:3, 3] += max_distance * direction
                traj.Insert(traj.GetNumWaypoints(),
                            openravepy.poseFromMatrix(max_pose))
            openravepy.planningutils.RetimeAffineTrajectory(
                traj,
                maxvelocities=0.1 * numpy.ones(7),
                maxaccelerations=0.1 * numpy.ones(7)
            )

        return self.PlanWorkspacePath(robot, traj,
                                      timelimit, min_waypoint_index=1)
    def PlanToEndEffectorOffset(self, robot, direction,
                                distance,
                                threshold,
                                matching_fn_class,
                                distance_metric_fn,
                                ignore_segments=True,
                                max_recursion_depth=10,
                                terminate_on_max_recursion=True,
                                **kwargs):
        """
        Add docstring
        """

        print "in RecursiveSnap: PlanToEndEffectorOffset() \n"

        if distance <= 0.0:
            raise ValueError("Distance must be non-negative.")
        elif numpy.linalg.norm(direction) == 0:
            raise ValueError("Direction must be non-zero")

        # Normalize the direction vector.
        direction = numpy.array(direction, dtype='float')
        direction /= numpy.linalg.norm(direction)

        with robot:
            manip = robot.GetActiveManipulator()
            start_pose = manip.GetEndEffectorTransform()
            traj = openravepy.RaveCreateTrajectory(self.env, '')
            spec = openravepy.IkParameterization.\
                            GetConfigurationSpecificationFromType(
                                 openravepy.IkParameterizationType.Transform6D,
                                 'linear')
            traj.Init(spec)
            traj.Insert(traj.GetNumWaypoints(),
                        openravepy.poseFromMatrix(start_pose))
            min_pose = numpy.copy(start_pose)
            min_pose[0:3, 3] += distance*direction
            traj.Insert(traj.GetNumWaypoints(),
                        openravepy.poseFromMatrix(min_pose))

        return self.PlanWorkspacePath(robot, traj,
                                      threshold,
                                      matching_fn_class,
                                      distance_metric_fn,
                                      ignore_segments,
                                      max_recursion_depth,
                                      terminate_on_max_recursion,
                                      **kwargs
                                      )
Example #6
0
    def PlanToEndEffectorPose(self,
                              robot,
                              goal_pose,
                              timelimit=5.0,
                              **kw_args):
        """
        Plan to an end effector pose by first creating a geodesic
        trajectory in SE(3) from the starting end-effector pose to the goal
        end-effector pose, and then attempting to follow it exactly
        using PlanWorkspacePath.

        @param robot
        @param goal_pose desired end-effector pose
        @return traj
        """

        with robot:
            # Create geodesic trajectory in SE(3)
            env = robot.GetEnv()
            manip = robot.GetActiveManipulator()
            start_pose = manip.GetEndEffectorTransform()
            traj = openravepy.RaveCreateTrajectory(env, '')
            spec = openravepy.IkParameterization.\
                GetConfigurationSpecificationFromType(
                    openravepy.IkParameterizationType.Transform6D, 'linear')
            traj.Init(spec)
            traj.Insert(traj.GetNumWaypoints(),
                        openravepy.poseFromMatrix(start_pose))
            traj.Insert(traj.GetNumWaypoints(),
                        openravepy.poseFromMatrix(goal_pose))

            with robot.CreateRobotStateSaver(
                    Robot.SaveParameters.LinkTransformation):
                openravepy.planningutils.RetimeAffineTrajectory(
                    traj,
                    maxvelocities=0.1 * numpy.ones(7),
                    maxaccelerations=0.1 * numpy.ones(7))

        qtraj = self.PlanWorkspacePath(robot, traj, timelimit)
        # modify tags to reflect that we won't care about
        # the entire path, but only the final pose
        SetTrajectoryTags(qtraj, {
            Tags.CONSTRAINED: False,
            Tags.SMOOTH: True
        },
                          append=True)

        return qtraj
Example #7
0
    def sequence_goals(self):
        if not len(self.goal_array):
            self.goal = [None]
            return
        goals = numpy.array(deepcopy(self.goal_array))
        tasks_msg = PoseArray()
        for goal in goals:
            T = openravepy.transformLookat(
                goal + [0.1, 0.0, 0.0],
                goal - [self.starting_position_offset, 0.0, 0.0], [0, 0, -1])
            T = numpy.dot(T, numpy.linalg.inv(self.T_G2EE))
            pose = openravepy.poseFromMatrix(T)
            pose_msg = Pose()
            pose_msg.orientation.w = pose[0]
            pose_msg.orientation.x = pose[1]
            pose_msg.orientation.y = pose[2]
            pose_msg.orientation.z = pose[3]
            pose_msg.position.x = pose[4]
            pose_msg.position.y = pose[5]
            pose_msg.position.z = pose[6]
            tasks_msg.poses.append(pose_msg)
        resp = self.sequencer_client.call(tasks_msg, SEQUENCING_TYPE)
        self.sequenced_goals = [goals[i] for i in resp.sequence]
        self.sequenced_trajectories = resp.database_trajectories
        self.num_goals_history = len(self.sequenced_goals)
        print("sequenced goals: " + str(self.sequenced_goals))
        print("resp.sequence: " + str(resp.sequence))
        if self.sim:
            self.goal = numpy.array(self.sequenced_goals[0])
            self.goal_off = self.goal - self.go_to_goal_offset * self.normalize(
                self.goal - self.ee_position)

            self.starting_position = self.goal - numpy.array(
                [self.starting_position_offset, 0.0, 0.0])
            self.starting_direction = numpy.array([1.0, 0.0, 0.0])
Example #8
0
def get_quat_from_matrix(transform):
    """
    OpenRAVE Convention: Quaternions are defined with the scalar value as the first component. For example [w x y z]
    """
    quat = openravepy.poseFromMatrix(transform)[0:4]
    quat.tolist().reverse()
    return tuple(quat)
Example #9
0
def joints2xyzquat(manip, link_name, joint_vals):
    robot, T = manip.GetRobot(), None
    with robot:
        robot.SetDOFValues(joint_vals, manip.GetArmIndices())
        T = robot.GetLink(link_name).GetTransform()
    quat_xyz = rave.poseFromMatrix(T)
    return quat_xyz[4:7], quat_xyz[:4]  # xyz, quat
Example #10
0
def joints2xyzquat(manip, link_name, joint_vals):
    robot, T = manip.GetRobot(), None
    with robot:
        robot.SetDOFValues(joint_vals, manip.GetArmIndices())
        T = robot.GetLink(link_name).GetTransform()
    quat_xyz = rave.poseFromMatrix(T)
    return quat_xyz[4:7], quat_xyz[:4] # xyz, quat
def CreateWorkspaceTrajectoryByLinearInterpBetweenConfigs(env, robot,
                                                          q0, q1):
    """
    Create a workspace trajectory by linearly interpolating
    between two joint configurations.
    """

    # Get a jointspace trajectory between the two configurations
    # using linear joint interpolation
    path = None
    samples = None
    with robot: # save robot state
        _,samples = InterpBetweenConfigs(env, robot, q0, q1)
    # restore robot state

    traj = openravepy.RaveCreateTrajectory(env, '')
    velocity_interp = 'linear'
    spec = openravepy.IkParameterization.\
                            GetConfigurationSpecificationFromType(
                                 openravepy.IkParameterizationType.Transform6D,
                                 velocity_interp)
    traj.Init(spec)

    # Create a workspace path of end-effector poses.
    # Each joint configuration 'q' sampled is L2 norm
    # from the previous configuration.
    for idx,(t,q) in enumerate(samples):
        T_current = prpy.util.GetForwardKinematics(robot, q)
        traj.Insert(idx, openravepy.poseFromMatrix(T_current))

    # If q0 and q1 are close together, only q0 will exist in 'samples'
    # and the workspace trajectory will only have one waypoint,
    # therefore we manually add q1 as a second waypoint.
    num_waypoints = traj.GetNumWaypoints()
    if num_waypoints == 1:
        T_goal = prpy.util.GetForwardKinematics(robot, q1)
        traj.Insert(num_waypoints, openravepy.poseFromMatrix(T_goal))

    # Add timing so we can continuously sample the trajectory
    openravepy.planningutils.RetimeAffineTrajectory(traj, maxvelocities=0.1*numpy.ones(7), maxaccelerations=0.1*numpy.ones(7))

    return traj
Example #12
0
 def callback(request):
     for t in pose_constraints:
         pose = orpy.poseFromMatrix(pose_constraints[t])
         quat, xyz = pose[:4], pose[4:]
         request['constraints'].append({
             'type': 'pose',
             'params': {
                 'timestep': t,
                 'xyz': xyz.tolist(),
                 'wxyz': quat.tolist(),
                 'link': 'j2s7s300_link_7'
             }
         })
    def get_body_markers(self):
        """ Returns a list of visualization_msgs/MarkerArray with all the links of each body in the environment """
        body_markers = []

        # Get all the bodies in the OpenRAVE environment
        bodies = self.env.GetBodies()
        for body in bodies:
            print("Found body with name: {}".format(body.GetName()))
            body_marker = MarkerArray()

            # Choose a random color for this body
            color_r = random.random()
            color_g = random.random()
            color_b = random.random()

            # Create a separate marker for each link
            for link in body.GetLinks():
                print("  Link name: {}".format(link.GetName()))
                link_transform = link.GetTransform()

                link_marker = Marker()
                link_marker.header.frame_id = '/root'
                link_marker.header.stamp = rospy.get_rostime()
                link_marker.ns = body.GetName() + '/link/' + link.GetName()
                link_marker.id = 0
                link_marker.type = Marker.SPHERE

                pose = openravepy.poseFromMatrix(link_transform)

                link_marker.pose.position.x = pose[4]
                link_marker.pose.position.y = pose[5]
                link_marker.pose.position.z = pose[6]
                link_marker.pose.orientation.x = pose[1]
                link_marker.pose.orientation.y = pose[2]
                link_marker.pose.orientation.z = pose[3]
                link_marker.pose.orientation.w = pose[0]

                link_marker.scale.x = 0.2
                link_marker.scale.y = 0.1
                link_marker.scale.z = 0.1
                link_marker.color.r = color_r
                link_marker.color.g = color_g
                link_marker.color.b = color_b
                link_marker.color.a = 0.50
                link_marker.lifetime = rospy.Duration(0)
                body_marker.markers.append(link_marker)

            body_markers.append(body_marker)

        return body_markers
Example #14
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:])
Example #15
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
Example #16
0
    def FindIKSolution(self, T, qref=None, checkcollision=True):
        self.ActivateIKSolver()
        # Use IKFast
        with self.robot:
            if qref is not None:
                self.robot.SetActiveDOFValues(qref)
            if checkcollision:
                qsol = self.manip.FindIKSolution(T, ikfilter_checkcollision)
            else:
                qsol = self.manip.FindIKSolution(T, ikfilter_ignorecollision)
        if qsol is not None:
            # IKFast works. Return the IKFast solution directly.
            return qsol

        # Here IKFast does not return anything. It is either that a
        # solution exists but IKFast fails or no solution exists.
        targetpose = orpy.poseFromMatrix(T)
        for i in xrange(self._ntrials):
            # Perturb the desired T
            Tnew = PerturbT(T)
            with self.robot:
                if qref is not None:
                    self.robot.SetActiveDOFValues(qref)
                if checkcollision:
                    qinit = self.manip.FindIKSolution(Tnew, ikfilter_checkcollision)
                else:
                    qinit = self.manip.FindIKSolution(Tnew, ikfilter_ignorecollision)
            if qinit is None:
                continue            
            
            # Since qinit is assumably close to a real solution (if
            # one exists), we set max_it to be only 20.
            [reached, _, qsol] = self.diffiksolver.solve\
            (targetpose, qinit, dt=1.0, max_it=20, conv_tol=1e-8, checkcollision=checkcollision)
            
            if not reached:
                continue
            
            # message = "Desired transformation reached"
            # self.logger.info(message)
            return qsol

        message = "Failed to find an IK solution after {0} trials".format(self._ntrials)
        self.logger.info(message)
        
        return None
Example #17
0
    def sample_traj(self, traj_obj):
        spec = traj_obj.GetConfigurationSpecification()
        traj = []
        step = traj_obj.GetDuration() / self.params.n_steps

        for i in range(self.params.n_steps):
            data = traj_obj.Sample(i * step)
            T = spec.ExtractTransform(None, data, self.robot)
            pose = rave.poseFromMatrix(T)  # wxyz, xyz
            euler = transformations.euler_from_quaternion(np.r_[pose[1:4], pose[0]])
            traj.append(np.r_[pose[4:7], euler[0:3]])

        with self.robot:
            if check_traj.traj_is_safe(traj, self.robot):
                return traj, traj_obj.GetDuration()

        return None, None
Example #18
0
def main():

    ### Parameters ###
    ENV_FILE = "../data/pr2_table.env.xml"
    MANIP_NAME = "leftarm"
    N_STEPS = 15
    LINK_NAME = "l_gripper_tool_frame"
    XYZ_TARGET = [0.5, 0, 0.9]
    QUAT_TARGET = [1, 0, 0, 0]
    INTERACTIVE = True
    ##################

    ### 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]
    manip = robot.GetManipulator(MANIP_NAME)
    robot.SetDOFValues(np.zeros(len(manip.GetArmIndices())), manip.GetArmIndices())
    ##################

    T_start = robot.GetLink(LINK_NAME).GetTransform()
    quat_xyz = rave.poseFromMatrix(T_start)
    quat_start = quat_xyz[:4]
    xyz_start = quat_xyz[4:7]

    request = move_arm_straight_request(
        manip,
        N_STEPS,
        LINK_NAME,
        xyz_start=xyz_start,
        xyz_end=XYZ_TARGET,
        quat_start=quat_start,
        quat_end=QUAT_TARGET,
        start_joints=robot.GetDOFValues(manip.GetArmJoints()),
    )
    s = json.dumps(request)
    print "REQUEST:", s
    trajoptpy.SetInteractive(INTERACTIVE)
    prob = trajoptpy.ConstructProblem(s, env)
    result = trajoptpy.OptimizeProblem(prob)
Example #19
0
 def _get_OpenRaveFK(self, config, link_name):
     """
     Calculate the forward kinematics using openRAVE for use in cost evaluation.
     Params
     ---
     config: Robot joint configuration (3,) numpy array
     link_name: Name of the link to calculate forward kinematics for
     """
     q = config.tolist()
     self.robot.SetDOFValues(q + [0.0, 0.0, 0.0])
     eef_link = self.robot.GetLink(link_name)
     if eef_link is None:
         rospy.logerror("Error: end-effector \"{}\" does not exist".format(
             self.params["eef_link_name"]))
         raise ValueError(
             "Error: end-effector \"{}\" does not exist".format(
                 self.params["eef_link_name"]))
     eef_pose = openravepy.poseFromMatrix(eef_link.GetTransform())
     return np.array([eef_pose[4], eef_pose[5], eef_pose[6]])
Example #20
0
def chomp_plan(robot, group_name, active_joint_names, active_affine,
               target_dof_values, init_trajs):
    datadir = 'chomp_data'
    n_points = args.n_steps

    assert active_affine == 0 or active_affine == 11
    use_base = active_affine == 11

    saver = openravepy.RobotStateSaver(robot)

    target_base_pose = None
    if use_base:
        with robot:
            robot.SetActiveDOFValues(target_dof_values)
            target_base_pose = openravepy.poseFromMatrix(robot.GetTransform())
        robot.SetActiveDOFs(
            robot.GetActiveDOFIndices(),
            0)  # turn of affine dofs; chomp takes that separately
        target_dof_values = target_dof_values[:-3]  # strip off the affine part

    openravepy.RaveSetDebugLevel(openravepy.DebugLevel.Warn)
    m_chomp = get_chomp_module(robot.GetEnv())

    env_hash = hash_env(robot.GetEnv())
    if active_affine != 0:
        env_hash += "_aa" + str(active_affine)

    # load distance field
    j1idxs = [m.GetArmIndices()[0] for m in robot.GetManipulators()]
    for link in robot.GetLinks():
        for j1idx in j1idxs:
            if robot.DoesAffect(j1idx, link.GetIndex()):
                link.Enable(False)
    try:
        aabb_padding = 1.0 if not use_base else 3.0  # base problems should need a distance field over a larger volume
        m_chomp.computedistancefield(kinbody=robot,
                                     aabb_padding=aabb_padding,
                                     cache_filename='%s/chomp-sdf-%s.dat' %
                                     (datadir, env_hash))
    except Exception, e:
        print 'Exception in computedistancefield:', repr(e)
def resolve_clusters(env, pattern_poses):
  num_bodies = sum([1 for body in env.GetBodies() if is_object(body)])
  if num_bodies < len(pattern_poses):
    rospy.logwarn("Detected fewer objects than markers!")
  if num_bodies > len(pattern_poses):
    rospy.logwarn("Detected more objects than markers!")
  listener = tf.TransformListener()
  detection_results = []

  for body in env.GetBodies():
    if is_object(body):
      """ REMOVED BECAUSE BBOXES ALREADY COMPUTED IN /BASE_LINK FRAME
      bbox_centroid_in = PointStamped()
      bbox_centroid_in.header.frame_id = '/head_mount_link'
      bbox_centroid_in.point.x, bbox_centroid_in.point.y, bbox_centroid_in.point.z = openravepy.poseFromMatrix(body.GetTransform())[4:7]
      bbox_centroid = PointStamped()
      tf.TransformListener().transformPoint('/base_link', bbox_centroid_in, bbox_centroid)"""
      bbox_centroid = openravepy.poseFromMatrix(body.GetTransform())[4:7]
      min_dist, min_dist_id, min_dist_pose = -1, None, None

      for (pattern_id, frame_id, pattern_pose) in pattern_poses:
        pattern_pose_in = PoseStamped()
        pattern_pose_in.header.frame_id = frame_id
        pattern_pose_in.pose = pattern_pose.pose
        listener.waitForTransform("/base_link", frame_id, rospy.Time(0), rospy.Duration(5.0))    
        pattern_pose_out = listener.transformPose("/base_link", pattern_pose_in)

        curr_dist = np.linalg.norm(np.array(bbox_centroid) - \
                                     np.array((pattern_pose_out.pose.position.x, pattern_pose_out.pose.position.y, pattern_pose_out.pose.position.z)))
        if min_dist == -1 or curr_dist < min_dist:
          min_dist = curr_dist
          min_dist_id = pattern_id
          min_dist_pose = pattern_pose_out

      detection_results.append((get_pattern_name(min_dist_id), min_dist_pose, body.GetName()))

  return detection_results
def chomp_plan(robot, group_name, active_joint_names, active_affine, target_dof_values, init_trajs):
    datadir = 'chomp_data'
    n_points = args.n_steps

    assert active_affine == 0 or active_affine == 11
    use_base = active_affine == 11
    
    saver = openravepy.RobotStateSaver(robot)
    
    target_base_pose = None
    if use_base:
        with robot:
            robot.SetActiveDOFValues(target_dof_values)
            target_base_pose = openravepy.poseFromMatrix(robot.GetTransform())
        robot.SetActiveDOFs(robot.GetActiveDOFIndices(), 0) # turn of affine dofs; chomp takes that separately
        target_dof_values = target_dof_values[:-3] # strip off the affine part

    openravepy.RaveSetDebugLevel(openravepy.DebugLevel.Warn)
    m_chomp = get_chomp_module(robot.GetEnv())

    env_hash = hash_env(robot.GetEnv())
    if active_affine != 0:
        env_hash += "_aa" + str(active_affine)

    # load distance field
    j1idxs = [m.GetArmIndices()[0] for m in robot.GetManipulators()]
    for link in robot.GetLinks():
        for j1idx in j1idxs:
            if robot.DoesAffect(j1idx, link.GetIndex()):
                link.Enable(False)
    try:
        aabb_padding = 1.0 if not use_base else 3.0 # base problems should need a distance field over a larger volume
        m_chomp.computedistancefield(kinbody=robot, aabb_padding=aabb_padding,
          cache_filename='%s/chomp-sdf-%s.dat' % (datadir, env_hash))
    except Exception, e:
        print 'Exception in computedistancefield:', repr(e)
Example #23
0
def plan_follow_traj(robot, manip_name, ee_link, new_hmats, old_traj):
    handles = []

    def animate_traj(traj, robot):
        with robot:
            viewer = trajoptpy.GetViewer(robot.GetEnv())
            for i, traj_mats in enumerate(zip(traj, new_hmats)):
                dofs = traj_mats[0]
                mat = traj_mats[1]
                print mat
                robot.SetDOFValues(
                    dofs,
                    robot.GetManipulator(manip_name).GetArmIndices())
                colors = [(1, 0, 0, 1), (0, 1, 0, 1), (0, 0, 1, 1)]
                for i in range(3):
                    point = mat[:3, 3]
                    axis = mat[:3, i] / 10
                    handles.append(
                        Globals.env.drawarrow(p1=point,
                                              p2=point + axis,
                                              linewidth=0.004,
                                              color=colors[i]))
                viewer.Idle()

    n_steps = len(new_hmats)
    assert old_traj.shape[0] == n_steps
    assert old_traj.shape[1] == 7

    arm_inds = robot.GetManipulator(manip_name).GetArmIndices()

    ee_linkname = ee_link.GetName()

    init_traj = old_traj.copy()
    #animate_traj(init_traj, robot)
    #init_traj[0] = robot.GetDOFValues(arm_inds)

    request = {
        "basic_info": {
            "n_steps": n_steps,
            "manip": manip_name,
            "start_fixed": False
        },
        "costs": [{
            "type": "joint_vel",
            "params": {
                "coeffs": [1]
            }
        }, {
            "type": "collision",
            "params": {
                "coeffs": [10],
                "dist_pen": [0.005]
            }
        }],
        "constraints": [],
        "init_info": {
            "type": "given_traj",
            "data": [x.tolist() for x in init_traj]
        }
    }

    poses = [openravepy.poseFromMatrix(hmat) for hmat in new_hmats]
    for (i_step, pose) in enumerate(poses):
        request["costs"].append({
            "type": "pose",
            "params": {
                "xyz": pose[4:7].tolist(),
                "wxyz": pose[0:4].tolist(),
                "link": ee_linkname,
                "timestep": i_step,
                "pos_coeffs": [20, 20, 20],
                "rot_coeff": [20, 20, 20]
            }
        })

    s = json.dumps(request)
    prob = trajoptpy.ConstructProblem(
        s, Globals.env)  # create object that stores optimization problem
    result = trajoptpy.OptimizeProblem(prob)  # do optimization
    traj = result.GetTraj()
    #animate_traj(traj, robot)

    saver = openravepy.RobotStateSaver(robot)
    pos_errs = []
    for i_step in xrange(1, n_steps):
        row = traj[i_step]
        robot.SetDOFValues(row, arm_inds)
        tf = ee_link.GetTransform()
        pos = tf[:3, 3]
        pos_err = np.linalg.norm(poses[i_step][4:7] - pos)
        pos_errs.append(pos_err)
    pos_errs = np.array(pos_errs)

    print "planned trajectory for %s. max position error: %.3f. all position errors: %s" % (
        manip_name, pos_errs.max(), pos_errs)

    return traj
Example #24
0
def mat_to_base_pose(mat):
    pose = openravepy.poseFromMatrix(mat)
    x = pose[4]
    y = pose[5]
    rot = openravepy.axisAngleFromRotationMatrix(mat)[2]
    return x, y, rot
Example #25
0
def get_position_from_matrix(transform):
    return tuple(openravepy.poseFromMatrix(transform)[4:])
  def generate_grasping_traj(self, obj, grasp_pose_list, collisionfree=True):
    for grasp_pose, pre_grasp_pose in grasp_pose_list:
      # find IK for pregrasp
      if collisionfree:
        init_joints1 = self.manip.FindIKSolution(pre_grasp_pose,
          openravepy.IkFilterOptions.CheckEnvCollisions)
      else:
        init_joints1 = self.manip.FindIKSolution(pre_grasp_pose,
          openravepy.IkFilterOptions.IgnoreEndEffectorCollisions)

      with self.env:
        # find IK for grasp
        self.env.Remove(obj)
        if collisionfree:
          init_joints2 = self.manip.FindIKSolution(grasp_pose,
            openravepy.IkFilterOptions.CheckEnvCollisions)
        else:
          init_joints2 = self.manip.FindIKSolution(grasp_pose,
            openravepy.IkFilterOptions.IgnoreEndEffectorCollisions)
        self.env.AddKinBody(obj)

      if (init_joints1 is None) or (init_joints2 is None):
        continue

      # find traj for pregrasp
      gripper_pose1 = openravepy.poseFromMatrix(pre_grasp_pose).tolist()
      xyz_target1 = gripper_pose1[4:7]
      # quaternions are rotated by pi/2 around y for some reason...
      quat_target1 = openravepy.quatMultiply(gripper_pose1[:4],
                                            (0.7071, 0, -0.7071, 0)).tolist()

      traj1, collisions1 = self.traj_generator.traj_from_pose(
        xyz_target1, quat_target1,
        collisionfree=collisionfree, joint_targets=init_joints1.tolist())
      if traj1 is None:
        continue

      with self.env:
        # find trajectory to grasp
        orig_values = self.robot.GetDOFValues(
          self.robot.GetManipulator('rightarm').GetArmIndices())
        self.robot.SetDOFValues(traj1[-1],
          self.robot.GetManipulator('rightarm').GetArmIndices())

        gripper_pose2 = openravepy.poseFromMatrix(grasp_pose).tolist()
        xyz_target2 = gripper_pose2[4:7]
        # quaternions are rotated by pi/2 around y for some reason...
        quat_target2 = openravepy.quatMultiply(gripper_pose2[:4],
                                              (0.7071, 0, -0.7071, 0)).tolist()

        self.env.Remove(obj)
        traj2, collisions2 = self.traj_generator.traj_from_pose(
          xyz_target2, quat_target2, n_steps=2,
          collisionfree=collisionfree, joint_targets=init_joints2.tolist())
        self.env.AddKinBody(obj)

        # reset
        self.robot.SetDOFValues(orig_values,
          self.robot.GetManipulator('rightarm').GetArmIndices())

      if traj2 is None:
        continue

      collisions = collisions1.union(collisions2)
      if obj in collisions:
        collisions.remove(obj)
      if self.unmovable_objects.intersection(collisions):
        continue

      return traj1.tolist() + traj2.tolist(), collisions

    return None, set()
Example #27
0
        init_joint_target.tolist()  # need to convert numpy array to list
    }
    # END init
}

if args.position_only:
    request["constraints"][0]["params"]["rot_coeffs"] = [0, 0, 0]

s = json.dumps(request)  # convert dictionary into json-formatted string
prob = trajoptpy.ConstructProblem(
    s, env)  # create object that stores optimization problem
result = trajoptpy.OptimizeProblem(prob)  # do optimization
print result

from trajoptpy.check_traj import traj_is_safe
prob.SetRobotActiveDOFs()  # set robot DOFs to DOFs in optimization problem
assert traj_is_safe(result.GetTraj(),
                    robot)  # Check that trajectory is collision free

# Now we'll check to see that the final constraint was satisfied
robot.SetActiveDOFValues(result.GetTraj()[-1])
posevec = openravepy.poseFromMatrix(
    robot.GetLink("r_gripper_tool_frame").GetTransform())
quat, xyz = posevec[0:4], posevec[4:7]

quat *= np.sign(quat.dot(quat_target))
if args.position_only:
    assert (quat - quat_target).max() > 1e-3
else:
    assert (quat - quat_target).max() < 1e-3
Example #28
0
def plan_follow_trajs(robot,
                      manip_name,
                      ee_link_names,
                      ee_trajs,
                      old_traj,
                      no_collision_cost_first=False,
                      use_collision_cost=True,
                      start_fixed=False,
                      joint_vel_limits=None,
                      beta_pos=settings.BETA_POS,
                      beta_rot=settings.BETA_ROT,
                      gamma=settings.GAMMA):
    orig_dof_inds = robot.GetActiveDOFIndices()
    orig_dof_vals = robot.GetDOFValues()

    n_steps = len(ee_trajs[0])
    dof_inds = sim_util.dof_inds_from_name(robot, manip_name)
    assert old_traj.shape[0] == n_steps
    assert old_traj.shape[1] == len(dof_inds)
    assert len(ee_link_names) == len(ee_trajs)

    if no_collision_cost_first:
        init_traj, _, _ = plan_follow_trajs(robot,
                                            manip_name,
                                            ee_link_names,
                                            ee_trajs,
                                            old_traj,
                                            no_collision_cost_first=False,
                                            use_collision_cost=False,
                                            start_fixed=start_fixed,
                                            joint_vel_limits=joint_vel_limits,
                                            beta_pos=beta_pos,
                                            beta_rot=beta_rot,
                                            gamma=gamma)
    else:
        init_traj = old_traj.copy()

    if start_fixed:
        init_traj = np.r_[robot.GetDOFValues(dof_inds)[None, :], init_traj[1:]]
        sim_util.unwrap_in_place(init_traj, dof_inds)
        init_traj += robot.GetDOFValues(dof_inds) - init_traj[0, :]

    request = {
        "basic_info": {
            "n_steps": n_steps,
            "manip": manip_name,
            "start_fixed": start_fixed
        },
        "costs": [
            {
                "type": "joint_vel",
                "params": {
                    "coeffs": [gamma / (n_steps - 1)]
                }
            },
        ],
        "constraints": [],
        "init_info": {
            "type": "given_traj",
            "data": [x.tolist() for x in init_traj]
        }
    }

    if use_collision_cost:
        request["costs"].append({
            "type": "collision",
            "params": {
                "continuous": True,
                "coeffs": [
                    1000
                ],  # penalty coefficients. list of length one is automatically expanded to a list of length n_timesteps
                "dist_pen": [
                    0.025
                ]  # robot-obstacle distance that penalty kicks in. expands to length n_timesteps
            }
        })

    if joint_vel_limits is not None:
        request["constraints"].append({
            "type": "joint_vel_limits",
            "params": {
                "vals": joint_vel_limits,
                "first_step": 0,
                "last_step": n_steps - 1
            }
        })

    for (ee_link_name, ee_traj) in zip(ee_link_names, ee_trajs):
        poses = [openravepy.poseFromMatrix(hmat) for hmat in ee_traj]
        for (i_step, pose) in enumerate(poses):
            if start_fixed and i_step == 0:
                continue
            request["costs"].append({
                "type": "pose",
                "params": {
                    "xyz": pose[4:7].tolist(),
                    "wxyz": pose[0:4].tolist(),
                    "link": ee_link_name,
                    "timestep": i_step,
                    "pos_coeffs": [np.sqrt(beta_pos / n_steps)] * 3,
                    "rot_coeffs": [np.sqrt(beta_rot / n_steps)] * 3
                }
            })

    s = json.dumps(request)
    with openravepy.RobotStateSaver(robot):
        orig_dof_vals
        with util.suppress_stdout():
            prob = trajoptpy.ConstructProblem(s, robot.GetEnv(
            ))  # create object that stores optimization problem
            result = trajoptpy.OptimizeProblem(prob)  # do optimization
    traj = result.GetTraj()

    pose_costs = np.sum([
        cost_val for (cost_type, cost_val) in result.GetCosts()
        if cost_type == "pose"
    ])
    pose_err = []
    with openravepy.RobotStateSaver(robot):
        for (ee_link_name, ee_traj) in zip(ee_link_names, ee_trajs):
            ee_link = robot.GetLink(ee_link_name)
            for (i_step, hmat) in enumerate(ee_traj):
                if start_fixed and i_step == 0:
                    continue
                robot.SetDOFValues(traj[i_step], dof_inds)
                new_hmat = ee_link.GetTransform()
                pose_err.append(
                    openravepy.poseFromMatrix(
                        mu.invertHmat(hmat).dot(new_hmat)))
    pose_err = np.asarray(pose_err)
    pose_costs2 = (beta_rot / n_steps) * np.square(pose_err[:, 1:4]).sum() + (
        beta_pos / n_steps) * np.square(pose_err[:, 4:7]).sum()

    joint_vel_cost = np.sum([
        cost_val for (cost_type, cost_val) in result.GetCosts()
        if cost_type == "joint_vel"
    ])
    joint_vel_err = np.diff(traj, axis=0)
    joint_vel_cost2 = (gamma / (n_steps - 1)) * np.square(joint_vel_err).sum()
    sim_util.unwrap_in_place(traj, dof_inds)
    joint_vel_err = np.diff(traj, axis=0)

    collision_costs = [
        cost_val for (cost_type, cost_val) in result.GetCosts()
        if "collision" in cost_type
    ]
    if len(collision_costs) > 0:
        collision_err = np.asarray(collision_costs)
        collision_costs = np.sum(collision_costs)

    obj_value = np.sum(
        [cost_val for (cost_type, cost_val) in result.GetCosts()])

    print "{:>15} | {:>10} | {:>10}".format("", "trajopt", "computed")
    print "{:>15} | {:>10}".format("COSTS", "-" * 23)
    print "{:>15} | {:>10,.4} | {:>10,.4}".format("joint_vel", joint_vel_cost,
                                                  joint_vel_cost2)
    if np.isscalar(collision_costs):
        print "{:>15} | {:>10,.4} | {:>10}".format("collision(s)",
                                                   collision_costs, "-")
    print "{:>15} | {:>10,.4} | {:>10,.4}".format("pose(s)", pose_costs,
                                                  pose_costs2)
    print "{:>15} | {:>10,.4} | {:>10}".format("total_obj", obj_value, "-")
    print ""

    print "{:>15} | {:>10} | {:>10}".format("", "abs min", "abs max")
    print "{:>15} | {:>10}".format("ERRORS", "-" * 23)
    print "{:>15} | {:>10,.4} | {:>10,.4}".format(
        "joint_vel (deg)", np.rad2deg(np.abs(joint_vel_err).min()),
        np.rad2deg(np.abs(joint_vel_err).max()))
    if np.isscalar(collision_costs):
        print "{:>15} | {:>10,.4} | {:>10,.4}".format(
            "collision(s)",
            np.abs(-collision_err).min(),
            np.abs(-collision_err).max())
    print "{:>15} | {:>10,.4} | {:>10,.4}".format(
        "rot pose(s)",
        np.abs(pose_err[:, 1:4]).min(),
        np.abs(pose_err[:, 1:4]).max())
    print "{:>15} | {:>10,.4} | {:>10,.4}".format(
        "trans pose(s)",
        np.abs(pose_err[:, 4:7]).min(),
        np.abs(pose_err[:, 4:7]).max())
    print ""

    # make sure this function doesn't change state of the robot
    assert not np.any(orig_dof_inds - robot.GetActiveDOFIndices())
    assert not np.any(orig_dof_vals - robot.GetDOFValues())

    return traj, obj_value, pose_costs
Example #29
0
def test_move_pr2_with_obj_wrt_obj():
    env = cans_world_env()
    obj17 = env.GetKinBody('object17')
    tran = obj17.GetTransform()
    tran[0,3] = .49268
    tran[1,3] = -.51415
    obj17.SetTransform(tran)

    hl_plan = TestDomain(env)
    move_env = env.CloneSelf(1) # clones objects in the environment
    move_pr2 = move_env.GetKinBody('pr2')
    move_pr2.SetActiveDOFs(move_pr2.GetManipulator('rightarm').GetArmIndices())
    move_pr2.SetDOFValues([0.3],
                    [move_pr2.GetJoint("torso_lift_joint").GetDOFIndex()])

    robot = PR2('pr2')
    robot.tuck_arms(env)
    robot._set_active_dofs(env, ['rightarm'])
    pose = robot.get_pose(env)

    hl_plan = HLPlan(env)
    obj = Obj('object17')
    K = 7
    # obj_pose = np.array([[0.],[0.],[0.]])
    # obj.set_pose(env, obj_pose)

    # start = HLParam("pos", (self.K, 1), is_var=False, value=robot.get_pose(env))
    start_pose = np.array([[-1.4352011 ],\
       [ 0.63522797],\
       [-1.19804084],\
       [-1.55807855],\
       [-0.9962505 ],\
       [-0.75279673],\
       [-1.13988334]])
    end_pose = np.array([[-1.28868338],\
       [ 0.53921863],\
       [-1.41293145],\
       [-1.67185359],\
       [-0.74117023],\
       [-0.73603889],\
       [-1.29004773]])
    robot.set_pose(env, start_pose)
    # traj_val = lininterp(start_pose, end_pose, 10)
    traj_val = lininterp(start_pose, end_pose, 2)

    # getting grasp pose
    robot_body = robot.get_env_body(env)
    rarm = robot_body.GetManipulator('rightarm')
    # import ipdb; ipdb.set_trace()
    W_T_EE = rarm.GetEndEffectorTransform()
    EE_T_W = np.linalg.inv(W_T_EE)
    W_T_O = obj17.GetTransform()
    EE_T_O = np.dot(EE_T_W, W_T_O)
    EE_T_O_pose = poseFromMatrix(EE_T_O)
    # gp = HLParam("gp", (7, 1), is_var=False, value=EE_T_O_pose)
    gp_val = np.array([0,0,.125]).reshape((3,1))
    gp = HLParam("gp", (3, 1), is_var=False, value=gp_val)

    # start_pose = np.array([[-1.07265580e+00], [5.17875957e-01], [-1.30000000e+00], \
    #     [-1.08244363e+00], [-1.04143184e+00], [-9.06938766e-02], [-5.38403045e-01]])
    # obj trajectory to follow
    # obj_start = poseFromMatrix(tran).reshape((7,1))
    obj_start = obj_pose_from_transform(tran).reshape((6,1))
    end_tran = tran.copy()
    end_tran[1, 3] = -0.34
    obj_end = obj_pose_from_transform(end_tran).reshape((6,1))
    # print "obj_start:", obj_start
    # print "obj_end:", obj_end
    # obj_traj = lininterp(obj_start, obj_end, 10)
    obj_traj = lininterp(obj_start, obj_end, 2)

    start = HLParam("start", (K, 1), is_var=False, value=start_pose)
    end = HLParam("end", (K, 1), is_var=False, value=end_pose)
    # obj_pos = HLParam("obj_pos", (3, 1), is_var=False, value=obj.get_pose(env))

    move = PR2ObjMove(0, hl_plan, move_env, robot, start, end, traj_val, obj, gp)
    hlas = [move]
    ll_prob = LLProb(hlas)
    ll_prob.solve()
    import ipdb; ipdb.set_trace()
Example #30
0
def pose_from_trans(trans):
    return poseFromMatrix(trans)
Example #31
0
def plan_follow_traj(
    robot,
    manip_name,
    ee_link,
    new_hmats,
    old_traj,
    rope_cloud=None,
    rope_constraint_thresh=0.01,
    end_pose_constraint=False,
):

    n_steps = len(new_hmats)
    assert old_traj.shape[0] == n_steps
    assert old_traj.shape[1] == 7

    arm_inds = robot.GetManipulator(manip_name).GetArmIndices()

    ee_linkname = ee_link.GetName()

    init_traj = old_traj.copy()
    # init_traj[0] = robot.GetDOFValues(arm_inds)
    end_pose = openravepy.poseFromMatrix(new_hmats[-1])

    request = {
        "basic_info": {"n_steps": n_steps, "manip": manip_name, "start_fixed": False},
        "costs": [
            {"type": "joint_vel", "params": {"coeffs": [n_steps / 5.0]}},
            {"type": "collision", "params": {"coeffs": [1], "dist_pen": [0.01]}},
        ],
        "constraints": [],
        "init_info": {"type": "given_traj", "data": [x.tolist() for x in init_traj]},
    }

    request["costs"] += [
        {"type": "collision", "name": "cont_coll", "params": {"coeffs": [50], "dist_pen": [0.05]}},
        {"type": "collision", "name": "col", "params": {"continuous": False, "coeffs": [20], "dist_pen": [0.02]}},
    ]

    # impose that the robot goes to final ee tfm at last ts
    # the constraint works only when the arm is the 'grasp' arm; otherwise only cost is added

    # if end_pose_constraint or not is_fake_motion(new_hmats, 0.1):
    # hack to avoid missing grasp
    if rope_cloud != None:
        closest_point = find_closest_point(rope_cloud, end_pose[4:7])
        dist = np.linalg.norm(end_pose[4:7] - closest_point)
        if dist > rope_constraint_thresh and dist < 0.05:
            end_pose[4:7] = closest_point
            redprint("grasp hack is active, dist = %f" % dist)
        else:
            blueprint("grasp hack is inactive, dist = %f" % dist)
        # raw_input()

    request["constraints"] += [
        {
            "type": "pose",
            "params": {
                "xyz": end_pose[4:7].tolist(),
                "wxyz": end_pose[0:4].tolist(),
                "link": ee_linkname,
                "pos_coeffs": [100, 100, 100],
                "rot_coeffs": [10, 10, 10],
            },
        }
    ]

    poses = [openravepy.poseFromMatrix(hmat) for hmat in new_hmats]

    for (i_step, pose) in enumerate(poses):
        request["costs"].append(
            {
                "type": "pose",
                "params": {
                    "xyz": pose[4:7].tolist(),
                    "wxyz": pose[0:4].tolist(),
                    "link": ee_linkname,
                    "timestep": i_step,
                    "pos_coeffs": [100, 100, 100],
                    "rot_coeffs": [10, 10, 10],
                },
            }
        )

    s = json.dumps(request)
    prob = trajoptpy.ConstructProblem(s, robot.GetEnv())  # create object that stores optimization problem
    result = trajoptpy.OptimizeProblem(prob)  # do optimization
    traj = result.GetTraj()

    saver = openravepy.RobotStateSaver(robot)
    pos_errs = []
    with saver:
        for i_step in xrange(1, n_steps):
            row = traj[i_step]
            robot.SetDOFValues(row, arm_inds)
            tf = ee_link.GetTransform()
            pos = tf[:3, 3]
            pos_err = np.linalg.norm(poses[i_step][4:7] - pos)
            pos_errs.append(pos_err)
    pos_errs = np.array(pos_errs)
    print "planned trajectory for %s. max position error: %.3f. all position errors: %s" % (
        manip_name,
        pos_errs.max(),
        pos_errs,
    )

    return traj
Example #32
0
def pose_from_trans(trans):
  return poseFromMatrix(trans)
    def track(self, Tinit, direction, length, trackersteplength=0.0001, qinit=None):
        """
        Track a straight line in Cartesian space (3D).

        Tinit -- the initial transforamtion of the manipulator. Along
                 the path, the manipulator will stay pointing along 
                 Tinit[0:3, 2].
        direction -- a 3D-vector of workspace direction. The path is 
                     therefore starting from Tinit[0:3, 3] and ending at
                     Tinit[0:3, 3] + length*direction.
        length -- the length to go along direction
        trackersteplength -- a step size (in m.) for the tracker
        qinit -- an initial ik solution corresponding to Tinit.

        Note: Tinit[0:3, 2] and direction may be different from each other.

        Return a list of status and waypointslist.
        """
        if qinit is None:
            qinit = self._robustiksolver.FindIKSolution(Tinit)
            if qinit is None:
                message = "Failed to find an initial IK solution"
                self.logger.info(message)
                return [False, []]

        nsteps = int(length/trackersteplength)
        assert(nsteps > 0) # check soundness

        direction = direction / np.linalg.norm(direction)
        M = np.array(Tinit) # dummy transformation matrix
        waypointslist = [] # solution

        stepvector = trackersteplength * direction

        waypointslist.append(qinit)
        qprev = np.array(qinit)
        for i in xrange(nsteps):
            M[0:3, 3] += stepvector
            targetpose = orpy.poseFromMatrix(M)
            
            [reached, _, qsol] = self._robustiksolver.diffiksolver.solve\
            (targetpose, qprev, dt=1.0, max_it=100, conv_tol=1e-8)
            
            if not reached:
                message = 'Failed to track the path at step {0}'.format(i + 1)
                self.logger.info(message)
                
                if self._printextrainfo:
                    print 'qprev = np.' + repr(qprev)
                    print 'Ttarget = np.' + repr(M)                    
                
                return [False, []]

            waypointslist.append(qsol)
            qprev = np.array(qsol)

        if not np.allclose(M[0:3, 3], Tinit[0:3, 3] + length*direction):
            M[0:3, 3] = Tinit[0:3, 3] + length*direction
            targetpose = orpy.poseFromMatrix(M)
            
            [reached, _, qsol] = self._robustiksolver.diffiksolver.solve\
            (targetpose, qprev, dt=1.0, max_it=100, conv_tol=1e-8)
            
            if not reached:
                message = 'Failed to track the path at the last step'
                self.logger.info(message)
                
                if self._printextrainfo:
                    print 'qprev = np.' + repr(qprev)
                    print 'Ttarget = np.' + repr(M)                    
                
                return [False, []]

            waypointslist.append(qsol)
            qprev = np.array(qsol)

        return [True, waypointslist]
Example #34
0
def xyzQuatFromMatrix(T):
    wxyz_xyz = rave.poseFromMatrix(T)
    return wxyz_xyz[4:7], wxyz_xyz[0:4]
Example #35
0
def plan_follow_traj(robot, manip_name, ee_link, new_hmats, old_traj):

    n_steps = len(new_hmats)
    assert old_traj.shape[0] == n_steps
    assert old_traj.shape[1] == 7

    arm_inds = robot.GetManipulator(manip_name).GetArmIndices()

    ee_linkname = ee_link.GetName()

    init_traj = old_traj.copy()
    #init_traj[0] = robot.GetDOFValues(arm_inds)

    request = {
        "basic_info": {
            "n_steps": n_steps,
            "manip": manip_name,
            "start_fixed": False
        },
        "costs": [{
            "type": "joint_vel",
            "params": {
                "coeffs": [n_steps / 5.]
            }
        }, {
            "type": "collision",
            "params": {
                "coeffs": [10],
                "dist_pen": [0.01]
            }
        }],
        "constraints": [],
        "init_info": {
            "type": "given_traj",
            "data": [x.tolist() for x in init_traj]
        }
    }

    poses = [openravepy.poseFromMatrix(hmat) for hmat in new_hmats]
    for (i_step, pose) in enumerate(poses):
        request["costs"].append({
            "type": "pose",
            "params": {
                "xyz": pose[4:7].tolist(),
                "wxyz": pose[0:4].tolist(),
                "link": ee_linkname,
                "timestep": i_step,
                "pos_coeffs": [10, 10, 10],
                "rot_coeffs": [10, 10, 10]
            }
        })

    s = json.dumps(request)
    prob = trajoptpy.ConstructProblem(
        s, robot.GetEnv())  # create object that stores optimization problem
    result = trajoptpy.OptimizeProblem(prob)  # do optimization
    traj = result.GetTraj()

    saver = openravepy.RobotStateSaver(robot)
    pos_errs = []
    for i_step in xrange(1, n_steps):
        row = traj[i_step]
        robot.SetDOFValues(row, arm_inds)
        tf = ee_link.GetTransform()
        pos = tf[:3, 3]
        pos_err = np.linalg.norm(poses[i_step][4:7] - pos)
        pos_errs.append(pos_err)
    pos_errs = np.array(pos_errs)

    print "planned trajectory for %s. max position error: %.3f. all position errors: %s" % (
        manip_name, pos_errs.max(), pos_errs)

    return traj
Example #36
0
 def mat_to_base_pose_2D(mat):
     pose = poseFromMatrix(mat)
     x = pose[4]
     y = pose[5]
     return np.array([x,y])
Example #37
0
    def FindIKSolutions(self, point, direction, checkcollision=True):
        """
        point -- a 3D vector
        direction -- a 3D 'unit' vector
        """
        self.ActivateIKSolver()
        # Use IKFast
        ikparam = orpy.IkParameterization(orpy.Ray(point, direction), iktype5D)
        if checkcollision:
            sols = self.manip.FindIKSolutions(ikparam, ikfilter_checkcollision)
        else:
            sols = self.manip.FindIKSolutions(ikparam, ikfilter_ignorecollision)
        if len(sols) > 0:
            # IKFast works. Return the IKFast solution directly.
            return sols

        # Compute an initial rotation
        z = np.array(direction).reshape((3, 1))
        x = PerpendicularTo(z).reshape((3, 1))
        y = np.cross(z.T, x.T).reshape((3, 1))
        R = np.hstack([x, y, z])
        
        nattempts = 3
        sols = []
        
        for i in xrange(nattempts):
            # Sample a rotation
            theta = (2.0*rng.random() - 1.0) * np.pi
            cos = np.cos(theta)
            sin = np.sin(theta)
            Rtheta = np.array([[ cos, -sin,  0.0],
                               [ sin,  cos,  0.0],
                               [ 0.0,  0.0,  1.0]])
            Rnew = np.dot(R, Rtheta)
            T = CombineRotationTranslation(Rnew, point) # desired transformation
            targetpose = orpy.poseFromMatrix(T)  # desired pose

            # Add some perturbation
            Tnew = PerturbT(T)

            # Let IKFast provide an initial solution
            ray = orpy.Ray(Tnew[0:3, 3], Tnew[0:3, 2]/np.linalg.norm(Tnew[0:3, 2]))
            ikparam = orpy.IkParameterization(ray, iktype5D)
            if checkcollision:
                sols_init = self.manip.FindIKSolutions(ikparam, ikfilter_checkcollision)
            else:
                sols_init = self.manip.FindIKSolutions(ikparam, ikfilter_ignorecollision)
            if len(sols_init) == 0:
                continue
        
            for qinit in sols_init:
                # Since qinit is assumably close to a real solution (if
                # one exists), we set max_it to be only 20.
                [reached, _, qsol] = self.diffiksolver.solve\
                (targetpose, qinit, dt=1.0, max_it=20, conv_tol=1e-8, checkcollision=checkcollision)
              
                if reached:
                    sols.append(qsol)
            
        if len(sols) == 0:       
            message = "Failed to find an IK solution after {0} trials".format(self._ntrials)
            self.logger.info(message)
        return sols
Example #38
0
def plan_follow_traj(robot, manip_name, ee_link, new_hmats, old_traj):
        
    n_steps = len(new_hmats)
    assert old_traj.shape[0] == n_steps
    assert old_traj.shape[1] == 7
    
    arm_inds  = robot.GetManipulator(manip_name).GetArmIndices()

    ee_linkname = ee_link.GetName()
    
    init_traj = old_traj.copy()
    #init_traj[0] = robot.GetDOFValues(arm_inds)

    request = {
        "basic_info" : {
            "n_steps" : n_steps,
            "manip" : manip_name,
            "start_fixed" : False
        },
        "costs" : [
        {
            "type" : "joint_vel",
            "params": {"coeffs" : [1]}
        },
        {
            "type" : "collision",
            "params" : {"coeffs" : [10],"dist_pen" : [0.005]}
        }                
        ],
        "constraints" : [
        ],
        "init_info" : {
            "type":"given_traj",
            "data":[x.tolist() for x in init_traj]
        }
    }
        
    poses = [openravepy.poseFromMatrix(hmat) for hmat in new_hmats]
    for (i_step,pose) in enumerate(poses):
        request["costs"].append(
            {"type":"pose",
             "params":{
                "xyz":pose[4:7].tolist(),
                "wxyz":pose[0:4].tolist(),
                "link":ee_linkname,
                "timestep":i_step,
                "pos_coeffs":[20,20,20],
                "rot_coeff":[20,20,20]
             }
            })

    s = json.dumps(request)
    prob = trajoptpy.ConstructProblem(s, Globals.env) # create object that stores optimization problem
    result = trajoptpy.OptimizeProblem(prob) # do optimization
    traj = result.GetTraj()    
        
    saver = openravepy.RobotStateSaver(robot)
    pos_errs = []
    for i_step in xrange(1,n_steps):
        row = traj[i_step]
        robot.SetDOFValues(row, arm_inds)
        tf = ee_link.GetTransform()
        pos = tf[:3,3]
        pos_err = np.linalg.norm(poses[i_step][4:7] - pos)
        pos_errs.append(pos_err)
    pos_errs = np.array(pos_errs)
        
    print "planned trajectory for %s. max position error: %.3f. all position errors: %s"%(manip_name, pos_errs.max(), pos_errs)
            
    return traj         
Example #39
0
def xyzQuatFromMatrix(T):
    wxyz_xyz = rave.poseFromMatrix(T)
    return wxyz_xyz[4:7], wxyz_xyz[0:4]
Example #40
0
def plan_fullbody(
    robot,
    env,
    new_hmats_by_bodypart,
    old_traj_by_bodypart,
    end_pose_constraints,
    rope_cloud=None,
    rope_constraint_thresh=0.01,
    allow_base=True,
    init_data=None,
):

    collision_free = True
    for part_name in new_hmats_by_bodypart:
        n_steps = len(new_hmats_by_bodypart[part_name])
        break

    state = np.random.RandomState(None)

    init_dofs = []
    if "base" in old_traj_by_bodypart.keys():
        if "rightarm" in old_traj_by_bodypart.keys():
            init_dofs += old_traj_by_bodypart["rightarm"].tolist()
        else:
            init_dofs += robot.GetDOFValues(robot.GetManipulator("rightarm").GetArmIndices()).tolist()
        if "leftarm" in old_traj_by_bodypart.keys():
            init_dofs += old_traj_by_bodypart["leftarm"].tolist()
        else:
            init_dofs += robot.GetDOFValues(robot.GetManipulator("leftarm").GetArmIndices()).tolist()

        init_dofs += old_traj_by_bodypart["base"]
    else:
        arm_dofs = []
        if "rightarm" in old_traj_by_bodypart.keys():
            arm_dofs = old_traj_by_bodypart["rightarm"]
        if "leftarm" in old_traj_by_bodypart.keys():
            arm_dofs = np.c_[arm_dofs, old_traj_by_bodypart["leftarm"]]
        init_dofs = arm_dofs

        if allow_base:
            x, y, rot = mat_to_base_pose(robot.GetTransform())
            base_dofs = np.c_[x, y, rot]
            init_dofs = np.c_[init_dofs, np.repeat(base_dofs, init_dofs.shape[0], axis=0)]

        init_dofs = init_dofs.tolist()

    bodyparts = new_hmats_by_bodypart.keys()
    if "base" in bodyparts:  # problem
        bodyparts += ["rightarm", "leftarm"]
    elif allow_base:
        bodyparts.append("base")

    costs = []
    constraints = []
    if "base" in bodyparts:
        if old_traj_by_bodypart is None:
            raise Exception("Base motion planning must have init_dofs")

        n_dofs = len(init_dofs[0])
        cost_coeffs = n_dofs * [5]
        cost_coeffs[-1] = 500
        cost_coeffs[-2] = 500
        cost_coeffs[-3] = 500

        joint_vel_cost = {"type": "joint_vel", "params": {"coeffs": cost_coeffs}}

        costs.append(joint_vel_cost)

    else:
        joint_vel_cost = {
            "type": "joint_vel",  # joint-space velocity cost
            "params": {"coeffs": [5]},  # a list of length one is automatically expanded to a list of length n_dofs
        }
        costs.append(joint_vel_cost)

    for manip, poses in new_hmats_by_bodypart.items():
        if manip == "rightarm":
            link = "r_gripper_tool_frame"
        elif manip == "leftarm":
            link = "l_gripper_tool_frame"
        elif manip == "base":
            link = "base_footprint"

        if manip in ["rightarm", "leftarm"]:
            if not end_pose_constraints[manip] and is_fake_motion(poses, 0.1):
                continue

        if manip in ["rightarm", "leftarm"]:
            if end_pose_constraints[manip] or not is_fake_motion(poses, 0.1):
                end_pose = openravepy.poseFromMatrix(poses[-1])

                if rope_cloud != None:
                    closest_point = find_closest_point(rope_cloud, end_pose[4:7])
                    dist = np.linalg.norm(end_pose[4:7] - closest_point)
                    if dist > rope_constraint_thresh and dist < 0.2:
                        end_pose[4:7] = closest_point
                        redprint("grasp hack is active, dist = %f" % dist)

                constraints.append(
                    {
                        "type": "pose",
                        "params": {
                            "xyz": end_pose[4:7].tolist(),
                            "wxyz": end_pose[0:4].tolist(),
                            "link": link,
                            "pos_coeffs": [10, 10, 10],
                            "rot_coeffs": [10, 10, 10],
                        },
                    }
                )

        poses = [openravepy.poseFromMatrix(hmat) for hmat in poses]

        for t, pose in enumerate(poses):
            costs.append(
                {
                    "type": "pose",
                    "params": {
                        "xyz": pose[4:].tolist(),
                        "wxyz": pose[:4].tolist(),
                        "link": link,
                        "pos_coeffs": [20, 20, 20],
                        "rot_coeffs": [20, 20, 20],
                        "timestep": t,
                    },
                }
            )

    traj, total_cost = generate_traj(robot, env, costs, constraints, n_steps, init_dofs, collision_free)
    bodypart_traj = traj_to_bodypart_traj(traj, bodyparts)

    # Do multi init if base planning
    if "base" in bodypart_traj and not allow_base:
        waypoint_step = (n_steps - 1) // 2
        env_min, env_max = get_environment_limits(env, robot)
        current_dofs = robot.GetActiveDOFValues().tolist()
        waypoint_dofs = list(current_dofs)

        MAX_INITS = 10
        n = 0
        while True:
            collisions = get_traj_collisions(bodypart_traj, robot)
            if collisions == []:
                break

        n += 1
        if n > MAX_INITS:
            print "Max base multi init limit hit!"
            return bodypart_traj, total_cost  # return arbitrary traj with collisions

        print "Base planning failed! Trying random init. Iteration {} of {}".format(n, MAX_INITS)

        # randomly sample x, y within env limits + some padding to accommodate robot
        padding = 5.0
        env_min_x = env_min[0] - padding
        env_min_y = env_min[1] - padding
        env_max_x = env_max[0] + padding
        env_max_y = env_max[1] + padding

        waypoint_x = state.uniform(env_min_x, env_max_x)
        waypoint_y = state.uniform(env_min_y, env_max_y)

        waypoint_dofs[-3] = waypoint_x
        waypoint_dofs[-2] = waypoint_y

        init_data = np.empty((n_steps, robot.GetActiveDOF()))
        init_data[: waypoint_step + 1] = mu.linspace2d(current_dofs, waypoint_dofs, waypoint_step + 1)
        init_data[waypoint_step:] = mu.linspace2d(waypoint_dofs, init_dofs, n_steps - waypoint_step)

        traj, total_cost = generate_traj(robot, env, costs, constraints, n_steps, collision_free, init_dofs, init_data)
        bodypart_traj = traj_to_bodypart_traj(traj, bodyparts)

    return bodypart_traj, total_cost
Example #41
0
 def mat_to_base_pose(mat):
     pose = poseFromMatrix(mat)
     x = pose[4]
     y = pose[5]
     rot = axisAngleFromRotationMatrix(mat)[2]
     return np.array([x,y,rot])
Example #42
0
def plan_follow_traj(robot, manip_name, ee_link, new_hmats, old_traj):
    handles = []

    def animate_traj(traj, robot):
        with robot:
            viewer = trajoptpy.GetViewer(robot.GetEnv())
            for i, traj_mats in enumerate(zip(traj, new_hmats)):
                dofs = traj_mats[0]
                mat = traj_mats[1]
                print mat
                robot.SetDOFValues(dofs, robot.GetManipulator(manip_name).GetArmIndices())
                colors = [(1, 0, 0, 1), (0, 1, 0, 1), (0, 0, 1, 1)]
                for i in range(3):
                    point = mat[:3, 3]
                    axis = mat[:3, i] / 10
                    handles.append(Globals.env.drawarrow(p1=point, p2=point + axis, linewidth=0.004, color=colors[i]))
                viewer.Idle()

    n_steps = len(new_hmats)
    assert old_traj.shape[0] == n_steps
    assert old_traj.shape[1] == 7

    arm_inds = robot.GetManipulator(manip_name).GetArmIndices()

    ee_linkname = ee_link.GetName()

    init_traj = old_traj.copy()
    # animate_traj(init_traj, robot)
    # init_traj[0] = robot.GetDOFValues(arm_inds)

    request = {
        "basic_info": {"n_steps": n_steps, "manip": manip_name, "start_fixed": False},
        "costs": [
            {"type": "joint_vel", "params": {"coeffs": [1]}},
            {"type": "collision", "params": {"coeffs": [10], "dist_pen": [0.005]}},
        ],
        "constraints": [],
        "init_info": {"type": "given_traj", "data": [x.tolist() for x in init_traj]},
    }

    poses = [openravepy.poseFromMatrix(hmat) for hmat in new_hmats]
    for (i_step, pose) in enumerate(poses):
        request["costs"].append(
            {
                "type": "pose",
                "params": {
                    "xyz": pose[4:7].tolist(),
                    "wxyz": pose[0:4].tolist(),
                    "link": ee_linkname,
                    "timestep": i_step,
                    "pos_coeffs": [20, 20, 20],
                    "rot_coeff": [20, 20, 20],
                },
            }
        )

    s = json.dumps(request)
    prob = trajoptpy.ConstructProblem(s, Globals.env)  # create object that stores optimization problem
    result = trajoptpy.OptimizeProblem(prob)  # do optimization
    traj = result.GetTraj()
    # animate_traj(traj, robot)

    saver = openravepy.RobotStateSaver(robot)
    pos_errs = []
    for i_step in xrange(1, n_steps):
        row = traj[i_step]
        robot.SetDOFValues(row, arm_inds)
        tf = ee_link.GetTransform()
        pos = tf[:3, 3]
        pos_err = np.linalg.norm(poses[i_step][4:7] - pos)
        pos_errs.append(pos_err)
    pos_errs = np.array(pos_errs)

    print "planned trajectory for %s. max position error: %.3f. all position errors: %s" % (
        manip_name,
        pos_errs.max(),
        pos_errs,
    )

    return traj
Example #43
0
def plan_follow_trajs(robot, manip_name, ee_link_names, ee_trajs, old_traj, 
                     no_collision_cost_first=False, use_collision_cost=True, start_fixed=False, joint_vel_limits=None,
                     beta_pos=settings.BETA_POS, beta_rot=settings.BETA_ROT, gamma=settings.GAMMA):
    orig_dof_inds = robot.GetActiveDOFIndices()
    orig_dof_vals = robot.GetDOFValues()
    
    n_steps = len(ee_trajs[0])
    dof_inds = sim_util.dof_inds_from_name(robot, manip_name)
    assert old_traj.shape[0] == n_steps
    assert old_traj.shape[1] == len(dof_inds)
    assert len(ee_link_names) == len(ee_trajs)
        
    if no_collision_cost_first:
        init_traj, _, _ = plan_follow_trajs(robot, manip_name, ee_link_names, ee_trajs, old_traj, 
                                        no_collision_cost_first=False, use_collision_cost=False, start_fixed=start_fixed, joint_vel_limits=joint_vel_limits,
                                        beta_pos = beta_pos, beta_rot = beta_rot, gamma = gamma)
    else:
        init_traj = old_traj.copy()

    if start_fixed:
        init_traj = np.r_[robot.GetDOFValues(dof_inds)[None,:], init_traj[1:]]
        sim_util.unwrap_in_place(init_traj, dof_inds)
        init_traj += robot.GetDOFValues(dof_inds) - init_traj[0,:]

    request = {
        "basic_info" : {
            "n_steps" : n_steps,
            "manip" : manip_name,
            "start_fixed" : start_fixed
        },
        "costs" : [
        {
            "type" : "joint_vel",
            "params": {"coeffs" : [gamma/(n_steps-1)]}
        },            
        ],
        "constraints" : [
        ],
        "init_info" : {
            "type":"given_traj",
            "data":[x.tolist() for x in init_traj]
        }
    }
    
    if use_collision_cost:
        request["costs"].append(
            {
                "type" : "collision",
                "params" : {
                  "continuous" : True,
                  "coeffs" : [1000],  # penalty coefficients. list of length one is automatically expanded to a list of length n_timesteps
                  "dist_pen" : [0.025]  # robot-obstacle distance that penalty kicks in. expands to length n_timesteps
                }
            })
    
    if joint_vel_limits is not None:
        request["constraints"].append(
             {
                "type" : "joint_vel_limits",
                "params": {"vals" : joint_vel_limits,
                           "first_step" : 0,
                           "last_step" : n_steps-1
                           }
              })

    for (ee_link_name, ee_traj) in zip(ee_link_names, ee_trajs):
        poses = [openravepy.poseFromMatrix(hmat) for hmat in ee_traj]
        for (i_step,pose) in enumerate(poses):
            if start_fixed and i_step == 0:
                continue
            request["costs"].append(
                {"type":"pose",
                 "params":{
                    "xyz":pose[4:7].tolist(),
                    "wxyz":pose[0:4].tolist(),
                    "link":ee_link_name,
                    "timestep":i_step,
                    "pos_coeffs":[np.sqrt(beta_pos/n_steps)]*3,
                    "rot_coeffs":[np.sqrt(beta_rot/n_steps)]*3
                 }
                })

    s = json.dumps(request)
    with openravepy.RobotStateSaver(robot):
        orig_dof_vals
        with util.suppress_stdout():
            prob = trajoptpy.ConstructProblem(s, robot.GetEnv()) # create object that stores optimization problem
            result = trajoptpy.OptimizeProblem(prob) # do optimization
    traj = result.GetTraj()
    
    pose_costs = np.sum([cost_val for (cost_type, cost_val) in result.GetCosts() if cost_type == "pose"])
    pose_err = []
    with openravepy.RobotStateSaver(robot):
        for (ee_link_name, ee_traj) in zip(ee_link_names, ee_trajs):
            ee_link = robot.GetLink(ee_link_name)
            for (i_step, hmat) in enumerate(ee_traj):
                if start_fixed and i_step == 0:
                    continue
                robot.SetDOFValues(traj[i_step], dof_inds)
                new_hmat = ee_link.GetTransform()
                pose_err.append(openravepy.poseFromMatrix(mu.invertHmat(hmat).dot(new_hmat)))
    pose_err = np.asarray(pose_err)
    pose_costs2 = (beta_rot/n_steps) * np.square(pose_err[:,1:4]).sum() + (beta_pos/n_steps) * np.square(pose_err[:,4:7]).sum()

    joint_vel_cost = np.sum([cost_val for (cost_type, cost_val) in result.GetCosts() if cost_type == "joint_vel"])
    joint_vel_err = np.diff(traj, axis=0)
    joint_vel_cost2 = (gamma/(n_steps-1)) * np.square(joint_vel_err).sum()
    sim_util.unwrap_in_place(traj, dof_inds)
    joint_vel_err = np.diff(traj, axis=0)
    
    collision_costs = [cost_val for (cost_type, cost_val) in result.GetCosts() if "collision" in cost_type]
    if len(collision_costs) > 0:
        collision_err = np.asarray(collision_costs)
        collision_costs = np.sum(collision_costs)

    obj_value = np.sum([cost_val for (cost_type, cost_val) in result.GetCosts()])
    
    print "{:>15} | {:>10} | {:>10}".format("", "trajopt", "computed")
    print "{:>15} | {:>10}".format("COSTS", "-"*23)
    print "{:>15} | {:>10,.4} | {:>10,.4}".format("joint_vel", joint_vel_cost, joint_vel_cost2)
    if np.isscalar(collision_costs):
        print "{:>15} | {:>10,.4} | {:>10}".format("collision(s)", collision_costs, "-")
    print "{:>15} | {:>10,.4} | {:>10,.4}".format("pose(s)", pose_costs, pose_costs2)
    print "{:>15} | {:>10,.4} | {:>10}".format("total_obj", obj_value, "-")
    print ""

    print "{:>15} | {:>10} | {:>10}".format("", "abs min", "abs max")
    print "{:>15} | {:>10}".format("ERRORS", "-"*23)
    print "{:>15} | {:>10,.4} | {:>10,.4}".format("joint_vel (deg)", np.rad2deg(np.abs(joint_vel_err).min()), np.rad2deg(np.abs(joint_vel_err).max()))
    if np.isscalar(collision_costs):
        print "{:>15} | {:>10,.4} | {:>10,.4}".format("collision(s)", np.abs(-collision_err).min(), np.abs(-collision_err).max())
    print "{:>15} | {:>10,.4} | {:>10,.4}".format("rot pose(s)", np.abs(pose_err[:,1:4]).min(), np.abs(pose_err[:,1:4]).max())
    print "{:>15} | {:>10,.4} | {:>10,.4}".format("trans pose(s)", np.abs(pose_err[:,4:7]).min(), np.abs(pose_err[:,4:7]).max())
    print ""

    # make sure this function doesn't change state of the robot
    assert not np.any(orig_dof_inds - robot.GetActiveDOFIndices())
    assert not np.any(orig_dof_vals - robot.GetDOFValues())
    
    return traj, obj_value, pose_costs
Example #44
0
 def mat_to_base_pose_2D(mat):
     pose = poseFromMatrix(mat)
     x = pose[4]
     y = pose[5]
     return np.array([x, y])
Example #45
0
def read_text_traj(filename,robot,dt=.01,scale=1.0):
    """ Read in trajectory data stored in Youngbum's format (100Hz data):
        HPY LHY LHR ... RWP   (3-letter names)
        + - + ... +           (sign of joint about equivalent global axis + / -)
        0.0 5.0 2.0 ... -2.0  (Offset of joint from openHubo "zero" in YOUR sign
        convention and scale)
        1000 1000 pi/180 pi/180 ... pi/180 (scale of your units wrt openrave
        default)
        (data by row, single space separated)
    """
    #TODO: handle multiple spaces
    #Setup trajectory and source file
    ind=_oh.make_name_to_index_converter(robot)

    f=open(filename,'r')

    #Read in header row to find joint names
    header=f.readline().rstrip()
    #print header.split(' ')
    [traj,config]=create_trajectory(robot)
    k=0
    indices={}
    Tindices={}
    Tmap={'X':0,'Y':1,'Z':2,'R':3,'P':4,'W':5}
    for s in header.split(' '):
        j=ind(s)
        if j>=0:
            indices.setdefault(k,j)
        try:
            Tindices.setdefault(k,Tmap[s])
        except KeyError:
            pass
        except:
            raise
        k=k+1
    #Read in sign row
    signlist=f.readline().rstrip().split(' ')
    signs=[]
    for s in signlist:
        if s == '+':
            signs.append(1)
        else:
            signs.append(-1)

    #Read in offset row (fill with _np.zeros if not used)
    offsetlist=f.readline().rstrip().split(' ')
    offsets=[float(x) for x in offsetlist]
    #Read in scale row (fill with _np.ones if not used)
    scalelist=f.readline().rstrip().split(' ')
    scales=[float(x) for x in scalelist]

    pose=_oh.Pose(robot)
    for line in f:
        vals=[float(x) for x in line.rstrip().split(' ')]
        Tdata=_np.zeros(6)
        for i,v in enumerate(vals):
            if indices.has_key(i):
                pose[indices[i]]=(vals[i]+offsets[i])*scales[i]*signs[i]*scale
            elif Tindices.has_key(i):
                Tdata[Tindices[i]]=(vals[i]+offsets[i])*scales[i]*signs[i]*scale
        #TODO: clip joint vals at limits
        #TODO: use axis angle form for RPY data?
        T=array(_comps.Transform.make_transform(Tdata[3:],Tdata[0:3]))
        traj_append(traj,pose.to_waypt(dt,_rave.poseFromMatrix(T)))

    return traj
Example #46
0
 def mat_to_base_pose(mat):
     pose = poseFromMatrix(mat)
     x = pose[4]
     y = pose[5]
     rot = axisAngleFromRotationMatrix(mat)[2]
     return np.array([x, y, rot])
Example #47
0
def plan_follow_trajs(robot, manip_name, ee_link_names, ee_trajs, old_traj, 
                     no_collision_cost_first=False, use_collision_cost=True, start_fixed=False, joint_vel_limits=None,
                     beta_pos=settings.BETA_POS, beta_rot=settings.BETA_ROT, gamma=settings.GAMMA, pose_weights=1, grasp_cup=False, R=0.02, D=0.035, cup_xyz=None):
    orig_dof_inds = robot.GetActiveDOFIndices()
    orig_dof_vals = robot.GetDOFValues()
    
    n_steps = len(ee_trajs[0])
    dof_inds = sim_util.dof_inds_from_name(robot, manip_name)
    assert old_traj.shape[0] == n_steps
    assert old_traj.shape[1] == len(dof_inds)
    assert len(ee_link_names) == len(ee_trajs)
        
    if no_collision_cost_first:
        init_traj, _, _ = plan_follow_trajs(robot, manip_name, ee_link_names, ee_trajs, old_traj, 
                                        no_collision_cost_first=False, use_collision_cost=False, start_fixed=start_fixed, joint_vel_limits=joint_vel_limits,
                                        beta_pos = beta_pos, beta_rot = beta_rot, gamma = gamma)
    else:
        init_traj = old_traj.copy()

    if start_fixed:
        init_traj = np.r_[robot.GetDOFValues(dof_inds)[None,:], init_traj[1:]]
        sim_util.unwrap_in_place(init_traj, dof_inds)
        init_traj += robot.GetDOFValues(dof_inds) - init_traj[0,:]

    request = {
        "basic_info" : {
            "n_steps" : n_steps,
            "manip" : manip_name,
            "start_fixed" : start_fixed
        },
        "costs" : [
        {
            "type" : "joint_vel",
            #"params": {"coeffs" : [gamma/(n_steps-1)]}
            "params": {"coeffs" : [1]}
        },            
        ],
        "constraints" : [
        {                                                                             
            "type" : "cart_vel",                                                        
            "name" : "cart_vel",                                                        
            "params" : {                                                                
                "max_displacement" : .02,
                "first_step" : 0,                                                       
                "last_step" : n_steps-1, #inclusive                                     
                "link" : "r_gripper_tool_frame"                                         
            }
        },
        #{
        #    "type" : "collision",
        #    "params" : {
        #        "continuous" : True,
        #        "coeffs" : [100]*(n_steps-3) + [0]*3,
        #        "dist_pen" : [0.01]
        #    }
        #}
        ],
        "init_info" : {
            "type":"given_traj",
            "data":[x.tolist() for x in init_traj]
        }
    }

    if use_collision_cost:
        request["costs"].append(
            {
                "type" : "collision",
                "params" : {
                  "continuous" : True,
                  "coeffs" : [20000],  # penalty coefficients. list of length one is automatically expanded to a list of length n_timesteps
                  #"coeffs" : [0] * (len(ee_trajs[0]) - 3) + [100]*3,
                  "dist_pen" : [0.025]  # robot-obstacle distance that penalty kicks in. expands to length n_timesteps
                }
            })
    
    if joint_vel_limits is not None:
        request["constraints"].append(
             {
                "type" : "joint_vel_limits",
                "params": {"vals" : joint_vel_limits,
                           "first_step" : 0,
                           "last_step" : n_steps-1
                           }
              })

    #coeff_mult = [0.0] * (len(poses) - 1) + [100]
    #coeff_mult = np.linspace(1, 100, num=len(poses))
    #coeff_mult_trans = np.logspace(-3, 1, num=len(ee_trajs[0]))
    #coeff_mult_rot = np.logspace(-3, 1, num=len(ee_trajs[0]))
    coeff_mult_trans = np.array([0] * (len(ee_trajs[0])-1) + [100])
    coeff_mult_rot = np.array([0] * (len(ee_trajs[0])-1) + [100])
    if not grasp_cup:
        #coeff_mult_trans = np.array([0.1] * (len(ee_trajs[0])-1) + [100])
        #coeff_mult_rot = np.array([1] * len(ee_trajs[0]))
        for (ee_link_name, ee_traj) in zip(ee_link_names, ee_trajs):
            poses = [openravepy.poseFromMatrix(hmat) for hmat in ee_traj]
            for (i_step,pose) in enumerate(poses):
                if start_fixed and i_step == 0:
                    continue
                request["costs"].append(
                    {"type":"pose",
                     "params":{
                        "xyz":pose[4:7].tolist(),
                        "wxyz":pose[0:4].tolist(),
                        "link":ee_link_name,
                        "timestep":i_step,
                        #"pos_coeffs":list(pose_weights * [np.sqrt(beta_pos/n_steps)]*3),
                        "pos_coeffs":[np.sqrt(beta_pos/n_steps) * coeff_mult_trans[i_step]]*3,
                        #"rot_coeffs":list(pose_weights * [np.sqrt(beta_rot/n_steps)]*3)
                        "rot_coeffs":[np.sqrt(beta_rot/n_steps) * coeff_mult_rot[i_step]]*3
                     }
                    })


    print "Cup XYZ:", cup_xyz

    s = json.dumps(request)
    with openravepy.RobotStateSaver(robot):
        with util.suppress_stdout():
            prob = trajoptpy.ConstructProblem(s, robot.GetEnv()) # create object that stores optimization problem
            # for t in range(1,n_steps): 
            #     prob.AddCost(table_cost, [(t,j) for j in range(7)], "table%i"%t)






            if grasp_cup:
                tool_link = robot.GetLink("r_gripper_tool_frame")
                local_dir = np.array([0.,0.,1.])
                manip = robot.GetManipulator(manip_name)
                arm_inds = manip.GetArmIndices()
                arm_joints = [robot.GetJointFromDOFIndex(ind) for ind in arm_inds]
                f = lambda x: gripper_tilt(x, robot, arm_inds, tool_link, local_dir)
                f2 = lambda x: gripper_tilt2(x, robot, arm_inds, tool_link, local_dir)
                dfdx = lambda x: gripper_tilt_dx(x, robot, arm_inds, tool_link, local_dir, arm_joints)

                #for t in xrange(1,n_steps):
                for t in xrange(n_steps-2,n_steps):
                    prob.AddConstraint(f, dfdx, [(t,j) for j in xrange(7)], "EQ", "up%i"%t)

                #for t in xrange(1,n_steps-1):
                #    prob.AddConstraint(f2, [(t,j) for j in xrange(7)], "EQ", "up%i"%t)
                #prob.AddConstraint(f2, [(n_steps-1,j) for j in xrange(7)], "EQ", "up%i"%(n_steps-1))

                g = lambda x: gripper_around_cup(x, robot, arm_inds, tool_link, cup_xyz, R, D)
                prob.AddConstraint(g, [(n_steps-1,j) for j in xrange(7)], "EQ", "cup%i"%(n_steps-1))
            result = trajoptpy.OptimizeProblem(prob) # do optimization

    print [(_name, viol) for (_name, viol) in result.GetConstraints() if viol > 0.0001]
    #assert [viol <= 1e-4 for (_name,viol) in result.GetConstraints()]

    traj = result.GetTraj()

    pose_costs = np.sum([cost_val for (cost_type, cost_val) in result.GetCosts() if cost_type == "pose"])
    pose_err = []
    with openravepy.RobotStateSaver(robot):
        for (ee_link_name, ee_traj) in zip(ee_link_names, ee_trajs):
            ee_link = robot.GetLink(ee_link_name)
            for (i_step, hmat) in enumerate(ee_traj):
                if start_fixed and i_step == 0:
                    continue
                robot.SetDOFValues(traj[i_step], dof_inds)
                new_hmat = ee_link.GetTransform()
                pose_err.append(openravepy.poseFromMatrix(mu.invertHmat(hmat).dot(new_hmat)))
    pose_err = np.asarray(pose_err)
    pose_costs2 = (beta_rot/n_steps) * np.square(pose_err[:,1:4]).sum() + (beta_pos/n_steps) * np.square(pose_err[:,4:7]).sum()

    pose_costs3 = (beta_rot/n_steps) * np.dot(coeff_mult_rot[np.newaxis,1:], np.square(pose_err[:,1:4])).sum() + (beta_pos/n_steps) * np.dot(coeff_mult_trans[np.newaxis,1:], np.square(pose_err[:,4:7])).sum()

    joint_vel_cost = np.sum([cost_val for (cost_type, cost_val) in result.GetCosts() if cost_type == "joint_vel"])
    joint_vel_err = np.diff(traj, axis=0)
    joint_vel_cost2 = (gamma/(n_steps-1)) * np.square(joint_vel_err).sum()
    sim_util.unwrap_in_place(traj, dof_inds)
    joint_vel_err = np.diff(traj, axis=0)
    
    collision_costs = [cost_val for (cost_type, cost_val) in result.GetCosts() if "collision" in cost_type]
    #collision_costs = [cost_val for (cost_type, cost_val) in result.GetConstraints() if "collision" in cost_type]
    if len(collision_costs) > 0:
        collision_err = np.asarray(collision_costs)
        collision_costs = np.sum(collision_costs)

    obj_value = np.sum([cost_val for (cost_type, cost_val) in result.GetCosts()])
    
    print "{:>15} | {:>10} | {:>10}".format("", "trajopt", "computed")
    print "{:>15} | {:>10}".format("COSTS", "-"*23)
    print "{:>15} | {:>10,.4} | {:>10,.4}".format("joint_vel", joint_vel_cost, joint_vel_cost2)
    if np.isscalar(collision_costs):
        print "{:>15} | {:>10,.4} | {:>10}".format("collision(s)", collision_costs, "-")
    print "{:>15} | {:>10,.4} | {:>10,.4}".format("pose(s)", pose_costs, pose_costs2)
    print "{:>15} | {:>10,.4} | {:>10}".format("total_obj", obj_value, "-")
    print ""

    print "{:>15} | {:>10} | {:>10}".format("", "abs min", "abs max")
    print "{:>15} | {:>10}".format("ERRORS", "-"*23)
    print "{:>15} | {:>10,.4} | {:>10,.4}".format("joint_vel (deg)", np.rad2deg(np.abs(joint_vel_err).min()), np.rad2deg(np.abs(joint_vel_err).max()))
    if np.isscalar(collision_costs):
        print "{:>15} | {:>10,.4} | {:>10,.4}".format("collision(s)", np.abs(-collision_err).min(), np.abs(-collision_err).max())
    print "{:>15} | {:>10,.4} | {:>10,.4}".format("rot pose(s)", np.abs(pose_err[:,1:4]).min(), np.abs(pose_err[:,1:4]).max())
    print "{:>15} | {:>10,.4} | {:>10,.4}".format("trans pose(s)", np.abs(pose_err[:,4:7]).min(), np.abs(pose_err[:,4:7]).max())
    print ""

    # make sure this function doesn't change state of the robot
    assert not np.any(orig_dof_inds - robot.GetActiveDOFIndices())
    assert not np.any(orig_dof_vals - robot.GetDOFValues())

    rot_err = np.abs(pose_err[:,1:4]).sum() 
    pos_err = np.abs(pose_err[:,4:7]).sum()

    print collision_err
    
    return traj, obj_value, (pose_costs3, np.abs(collision_err[:-1]).sum())
Example #48
0
  }
  # END pose_constraint
  ],
  # BEGIN init
  "init_info" : {
      "type" : "straight_line", # straight line in joint space.
      "endpoint" : init_joint_target.tolist() # need to convert numpy array to list
  }
  # END init
}
s = json.dumps(request) # convert dictionary into json-formatted string
prob = trajoptpy.ConstructProblem(s, env) # create object that stores optimization problem
result = trajoptpy.OptimizeProblem(prob) # do optimization
print result

from trajoptpy.check_traj import traj_is_safe
prob.SetRobotActiveDOFs() # set robot DOFs to DOFs in optimization problem
assert traj_is_safe(result.GetTraj(), robot) # Check that trajectory is collision free

# Now we'll check to see that the final constraint was satisfied
robot.SetActiveDOFValues(result.GetTraj()[-1])
posevec = openravepy.poseFromMatrix(robot.GetLink("r_gripper_tool_frame").GetTransform())
quat, xyz = posevec[0:4], posevec[4:7]

quat *= np.sign(quat.dot(quat_target))
if args.position_only:
    assert (quat - quat_target).max() > 1e-3
else:
    assert (quat - quat_target).max() < 1e-3

Example #49
0
robot.SetDOFValues([rgj.GetLimits()[-1]], [rgj.GetDOFIndex()])
tlj = robot.GetJoint("torso_lift_joint")
robot.SetDOFValues([tlj.GetLimits()[-1]], [tlj.GetDOFIndex()])
    
print "processing point cloud"

if env.GetKinBody("convexsoup") is None:        
    cloud = cloudprocpy.readPCDXYZ("/home/joschu/Proj/trajoptrave/bigdata/laser_cloud.pcd")
    cloud = cloudprocpy.boxFilter(cloud, -1,5,-5,5,.1,2)
    aabb = robot.GetLink("base_link").ComputeAABB()
    (xmin,ymin,zmin) = aabb.pos() - aabb.extents()
    (xmax,ymax,zmax) = aabb.pos() + aabb.extents()
    cloud = cloudprocpy.boxFilterNegative(cloud, xmin,xmax,ymin,ymax,zmin,zmax)
    cloud = cloudprocpy.downsampleCloud(cloud, .015)
    convex_soup.create_convex_soup(cloud, env)

print "done processing"

T = np.eye(4)
T[:3,3] = [ 1.27   , -0.2   ,  0.790675]


##################

pose = rave.poseFromMatrix(T)
request = drive_to_reach_request(robot, "r_gripper_tool_frame", pose[4:7], pose[0:4])
s = json.dumps(request)
print "REQUEST:",s
trajoptpy.SetInteractive(True);
prob = trajoptpy.ConstructProblem(s, env)
result = trajoptpy.OptimizeProblem(prob)
Example #50
0
def read_text_traj(filename,robot,dt=.01,scale=1.0):
    """ Read in trajectory data stored in Youngbum's format (100Hz data):
        HPY LHY LHR ... RWP   (3-letter names)
        + - + ... +           (sign of joint about equivalent global axis + / -)
        0.0 5.0 2.0 ... -2.0  (Offset of joint from openHubo "zero" in YOUR sign
        convention and scale)
        1000 1000 pi/180 pi/180 ... pi/180 (scale of your units wrt openrave
        default)
        (data by row, single space separated)
    """
    #TODO: handle multiple spaces
    #Setup trajectory and source file
    ind=_oh.make_name_to_index_converter(robot)

    f=open(filename,'r')

    #Read in header row to find joint names
    header=f.readline().rstrip()
    #print header.split(' ')
    [traj,config]=create_trajectory(robot)
    k=0
    indices={}
    Tindices={}
    Tmap={'X':0,'Y':1,'Z':2,'R':3,'P':4,'W':5}
    for s in header.split(' '):
        j=ind(s)
        if j>=0:
            indices.setdefault(k,j)
        try:
            Tindices.setdefault(k,Tmap[s])
        except KeyError:
            pass
        except:
            raise
        k=k+1
    #Read in sign row
    signlist=f.readline().rstrip().split(' ')
    signs=[]
    for s in signlist:
        if s == '+':
            signs.append(1)
        else:
            signs.append(-1)

    #Read in offset row (fill with _np.zeros if not used)
    offsetlist=f.readline().rstrip().split(' ')
    offsets=[float(x) for x in offsetlist]
    #Read in scale row (fill with _np.ones if not used)
    scalelist=f.readline().rstrip().split(' ')
    scales=[float(x) for x in scalelist]

    pose=_oh.Pose(robot)
    for line in f:
        vals=[float(x) for x in line.rstrip().split(' ')]
        Tdata=_np.zeros(6)
        for i,v in enumerate(vals):
            if indices.has_key(i):
                pose[indices[i]]=(vals[i]+offsets[i])*scales[i]*signs[i]*scale
            elif Tindices.has_key(i):
                Tdata[Tindices[i]]=(vals[i]+offsets[i])*scales[i]*signs[i]*scale
        #TODO: clip joint vals at limits
        #TODO: use axis angle form for RPY data?
        T=array(_comps.Transform.make_transform(Tdata[3:],Tdata[0:3]))
        traj_append(traj,pose.to_waypt(dt,_rave.poseFromMatrix(T)))

    return traj