def GetHardGoal(params, yname):
    goal = params[yname+'_goal_arm_config']
    base_goal = [np.array(params[yname+'_goal_base_pose'])[0,3], 
                 np.array(params[yname+'_goal_base_pose'])[1,3], 
                 orpy.axisAngleFromRotationMatrix(np.array(params[yname+'_goal_base_pose'])[:3,:3])[2]]
    goal.extend(base_goal)
    return goal
Esempio n. 2
0
 def GetHardGoal(self, params, yname):
     goal = params[yname+'_goal_arm_config']
     base_goal = [np.array(params[yname+'_goal_base_pose'])[0,3], 
                  np.array(params[yname+'_goal_base_pose'])[1,3], 
                  orpy.axisAngleFromRotationMatrix(np.array(params[yname+'_goal_base_pose'])[:3,:3])[2]]
     goal.extend(base_goal)
     return goal
Esempio n. 3
0
def get_angles(hmats):
    """
    returns the angles of a list of 4x4 transformation matrices
    """
    assert hmats.ndim ==3 and hmats.shape[1:]==(4,4)
    angles = np.empty(hmats.shape[0])
    for i in xrange(hmats.shape[0]):
        angles[i] = np.linalg.norm(rave.axisAngleFromRotationMatrix(hmats[i,0:3,0:3]))
    return angles
Esempio n. 4
0
def get_angles(hmats):
    """
    returns the angles of a list of 4x4 transformation matrices
    """
    assert hmats.ndim == 3 and hmats.shape[1:] == (4, 4)
    angles = np.empty(hmats.shape[0])
    for i in xrange(hmats.shape[0]):
        angles[i] = np.linalg.norm(
            rave.axisAngleFromRotationMatrix(hmats[i, 0:3, 0:3]))
    return angles
  def move_straight(self, dist):
    #TODO Fill in, remove sleep
    #time.sleep(0)

    with self.env:
	T1 = self.robot.GetTransform() # Get the current Transform
	T2 = openravepy.axisAngleFromRotationMatrix(T1[0:3,0:3]) # Get the current axis angles of the robot
        T1[0,3] = dist*np.cos(T2[2]) # Distance movied in X-direction
        T1[1,3] = dist*np.sin(T2[2]) # Distance moved in Y-direction
        self.robot.SetTransform(T1) # Setting the new transform  
Esempio n. 6
0
def rpy2axang(rpy):
    """
    Converts a matrix of rpy (nx3) into a matrix of 
    axis-angles (nx3). 
    """
    n = rpy.shape[0]
    assert rpy.shape[1]==3, "unknown shape."
    ax_ang = np.empty((n,3))
    for i in xrange(n):
        th = rpy[i,:]
        ax_ang[i,:] = rave.axisAngleFromRotationMatrix(tfms.euler_matrix(th[0], th[1], th[2]))
    return ax_ang
    def performNavigationPlanning(self, goal, execute=True, draw_marker=False, steplength=0.1):
        goal = np.array(goal)
        if goal.ndim == 2:
            # assume a 4x4 transformation matrix
            angle = openravepy.axisAngleFromRotationMatrix(goal)[-1]
            goal = np.array([goal[0, -1], goal[1, -1], angle])

        # find the boundaries of the environment
        envmin, envmax = utils.get_environment_limits(self.env, self.robot)

        with self.env:
            self.robot.SetAffineTranslationLimits(envmin, envmax)
            self.robot.SetAffineTranslationMaxVels([0.3, 0.3, 0.3])
            self.robot.SetAffineRotationAxisMaxVels(np.ones(4))
            self.robot.SetActiveDOFs(
                [], openravepy.DOFAffine.X | openravepy.DOFAffine.Y | openravepy.DOFAffine.RotationAxis, [0, 0, 1]
            )

        # draw the marker
        if draw_marker:
            center = np.r_[goal[0:2], 0.2]
            xaxis = 0.5 * np.array((np.cos(goal[2]), np.sin(goal[2]), 0))
            yaxis = 0.25 * np.array((-np.sin(goal[2]), np.cos(goal[2]), 0))
            h = self.env.drawlinelist(
                np.transpose(
                    np.c_[
                        center - xaxis,
                        center + xaxis,
                        center - yaxis,
                        center + yaxis,
                        center + xaxis,
                        center + 0.5 * xaxis + 0.5 * yaxis,
                        center + xaxis,
                        center + 0.5 * xaxis - 0.5 * yaxis,
                    ]
                ),
                linewidth=5.0,
                colors=np.array((0, 1, 0)),
            )

        openravepy.RaveLogInfo("Planning to goal " + str(goal))
        res = self.basemanip.MoveActiveJoints(
            goal=goal, maxiter=3000, steplength=steplength, execute=execute, outputtrajobj=True
        )
        if res is None:
            raise ValueError("Could not find a trajectory")

        if execute:
            openravepy.RaveLogInfo("Waiting for controller to finish")
            self.robot.WaitForController(0)
            self.robot.GetController().Reset()
        return res
Esempio n. 8
0
def GeodesicError(t1, t2):
    '''
    Computes the error in global coordinates between two transforms

    @param t1 current transform
    @param t2 goal transform
    @return a 4-vector of [dx, dy, dz, solid angle]
    '''
    trel = numpy.dot(numpy.linalg.inv(t1), t2)
    trans = numpy.dot(t1[0:3, 0:3], trel[0:3, 3])
    omega = openravepy.axisAngleFromRotationMatrix(trel[0:3, 0:3])
    angle = numpy.linalg.norm(omega)
    return numpy.hstack((trans, angle))
Esempio n. 9
0
    def performNavigationPlanning(self,
                                  goal,
                                  execute=True,
                                  draw_marker=False,
                                  steplength=0.1):
        goal = np.array(goal)
        if goal.ndim == 2:
            #assume a 4x4 transformation matrix
            angle = openravepy.axisAngleFromRotationMatrix(goal)[-1]
            goal = np.array([goal[0, -1], goal[1, -1], angle])

        # find the boundaries of the environment
        envmin, envmax = utils.get_environment_limits(self.env, self.robot)

        with self.env:
            self.robot.SetAffineTranslationLimits(envmin, envmax)
            self.robot.SetAffineTranslationMaxVels([0.3, 0.3, 0.3])
            self.robot.SetAffineRotationAxisMaxVels(np.ones(4))
            self.robot.SetActiveDOFs(
                [], openravepy.DOFAffine.X | openravepy.DOFAffine.Y
                | openravepy.DOFAffine.RotationAxis, [0, 0, 1])

        # draw the marker
        if draw_marker:
            center = np.r_[goal[0:2], 0.2]
            xaxis = 0.5 * np.array((np.cos(goal[2]), np.sin(goal[2]), 0))
            yaxis = 0.25 * np.array((-np.sin(goal[2]), np.cos(goal[2]), 0))
            h = self.env.drawlinelist(np.transpose(
                np.c_[center - xaxis, center + xaxis, center - yaxis,
                      center + yaxis, center + xaxis,
                      center + 0.5 * xaxis + 0.5 * yaxis, center + xaxis,
                      center + 0.5 * xaxis - 0.5 * yaxis]),
                                      linewidth=5.0,
                                      colors=np.array((0, 1, 0)))

        openravepy.RaveLogInfo("Planning to goal " + str(goal))
        res = self.basemanip.MoveActiveJoints(goal=goal,
                                              maxiter=3000,
                                              steplength=steplength,
                                              execute=execute,
                                              outputtrajobj=True)
        if res is None:
            raise ValueError("Could not find a trajectory")

        if execute:
            openravepy.RaveLogInfo("Waiting for controller to finish")
            self.robot.WaitForController(0)
            self.robot.GetController().Reset()
        return res
Esempio n. 10
0
def GeodesicTwist(t1, t2):
    '''
    Computes the twist in global coordinates that corresponds
    to the gradient of the geodesic distance between two transforms.

    @param t1 current transform
    @param t2 goal transform
    @return twist in se(3)
    '''
    trel = numpy.dot(numpy.linalg.inv(t1), t2)
    trans = numpy.dot(t1[0:3, 0:3], trel[0:3, 3])
    omega = numpy.dot(t1[0:3, 0:3],
                      openravepy.axisAngleFromRotationMatrix(
                        trel[0:3, 0:3]))
    return numpy.hstack((trans, omega))
Esempio n. 11
0
def test_drive_base(pose_targ):
    
    wxyz_targ = pose_targ[:4]
    xyz_targ = pose_targ[4:]
    

    
    robot.SetDOFValues([0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0])
    robot.SetTransform(rave.matrixFromPose([1, 0, 0, 0, -3.4, -1.4, 0.05]))
    
    robot.SetActiveDOFs(np.r_[robot.GetManipulator("rightarm").GetArmIndices(), robot.GetLink("torso_lift_link").GetIndex()], 
                        rave.DOFAffine.X + rave.DOFAffine.Y + rave.DOFAffine.RotationAxis, [0,0,1])
    
    angle_init = np.random.rand() * 2*np.pi
    x_init = xyz_targ[0] - .5*np.cos(angle_init)
    y_init = xyz_targ[1] - .5*np.sin(angle_init)
    dofvals_init = np.r_[np.zeros(7), 0, x_init, y_init, angle_init]    
    #start = robot.
    
    start = robot.GetActiveDOFValues();
    end = dofvals_init
    n_steps = 10;
    init_traj = mu.linspace2d(start, end, n_steps)

    
    manipname = "active"
    prob_desc = make_request_skeleton(manipname, n_steps);
    prob_desc["basic_info"]["start_fixed"] = True
    prob_desc["costs"].append(
    {"type" : "pose",
    "name" : "pose",
    "params" : {
        "pos_coeffs" : [10,10,10],
        "rot_coeffs" : [0,0,0],
        "xyz" : xyz_targ,
        "wxyz" : wxyz_targ,
        "link" : "r_gripper_tool_frame",
    }})

    prob_desc["init_info"]["type"] = "given_traj"
    angle = np.linalg.norm(rave.axisAngleFromRotationMatrix(robot.GetTransform()))

    prob_desc["init_info"]["data"] = [row.tolist() for row in init_traj]
    s = json.dumps(prob_desc)

    traj.SetInteractive(True);
    prob = traj.ConstructProblem(s, env)
    return traj.OptimizeProblem(prob)
Esempio n. 12
0
 def __init__(self, robot):
     self.robot = robot
     for joint in robot.GetJoints():
         lower, upper = joint.GetLimits()
         lower = np.clip(lower, -np.pi, np.inf)
         upper = np.clip(upper, -np.inf, np.pi)
         self.add_trait(joint.GetName(), tr.Range(lower[0].item(), upper[0].item(), joint.GetValues()[0]))
     T = robot.GetTransform()
     rx,ry,rz = rave.axisAngleFromRotationMatrix(T[:3,:3])
     tx,ty,tz = T[:3,3]
     self.add_trait("tftx", tr.Range(-5.,5,tx.item()))
     self.add_trait("tfty", tr.Range(-5.,5,ty.item()))
     self.add_trait("tftz", tr.Range(-5.,5,tz.item()))
     self.add_trait("tfrx", tr.Range(-5.,5,rx.item()))
     self.add_trait("tfry", tr.Range(-5.,5,ry.item()))
     self.add_trait("tfrz", tr.Range(-5.,5,rz.item()))
     self.on_trait_change(self.update_robot, 'anytrait')
     tr.HasTraits.__init__(self)
Esempio n. 13
0
 def __init__(self, robot):
     self.robot = robot
     for joint in robot.GetJoints():
         lower, upper = joint.GetLimits()
         lower = np.clip(lower, -np.pi, np.inf)
         upper = np.clip(upper, -np.inf, np.pi)
         self.add_trait(joint.GetName(), tr.Range(lower[0].item(), upper[0].item(), joint.GetValues()[0]))
     T = robot.GetTransform()
     rx,ry,rz = rave.axisAngleFromRotationMatrix(T[:3,:3])
     tx,ty,tz = T[:3,3]
     self.add_trait("tftx", tr.Range(-5.,5,tx.item()))
     self.add_trait("tfty", tr.Range(-5.,5,ty.item()))
     self.add_trait("tftz", tr.Range(-5.,5,tz.item()))
     self.add_trait("tfrx", tr.Range(-5.,5,rx.item()))
     self.add_trait("tfry", tr.Range(-5.,5,ry.item()))
     self.add_trait("tfrz", tr.Range(-5.,5,rz.item()))
     self.on_trait_change(self.update_robot, 'anytrait')
     tr.HasTraits.__init__(self)
Esempio n. 14
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])
Esempio n. 15
0
    def PlanToEndEffectorOffset(self, robot, direction, distance, max_distance=None,
                                nullspace=JointLimitAvoidance, timelimit=5.0, step_size=0.001,
                                position_tolerance=0.01, angular_tolerance=0.15, **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
        @param stepsize step size in meters for the Jacobian pseudoinverse controller
        @param position_tolerance constraint tolerance in meters
        @param angular_tolerance constraint tolerance in radians
        @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 step_size <= 0:
            raise ValueError('Step size must be positive.')
        elif position_tolerance < 0:
            raise ValueError('Position tolerance must be non-negative.')
        elif angular_tolerance < 0:
            raise ValueError('Angular tolerance must be non-negative.')

        # save all active bodies so we only check collision with those
        active_bodies = []
        for body in self.env.GetBodies():
            if body.IsEnabled():
                active_bodies.append(body)
        

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

        # Default to moving an exact distance.
        if max_distance is None:
            max_distance = distance

        with robot:
            manip = robot.GetActiveManipulator()
            traj = openravepy.RaveCreateTrajectory(self.env, '')
            traj.Init(manip.GetArmConfigurationSpecification())

            active_dof_indices = manip.GetArmIndices()
            limits_lower, limits_upper = robot.GetDOFLimits(active_dof_indices)
            initial_pose = manip.GetEndEffectorTransform()
            q = robot.GetDOFValues(active_dof_indices)
            traj.Insert(0, q)

            start_time = time.time()
            current_distance = 0.0
            sign_flipper = 1
            last_rot_error = 9999999999.0
            try:
                while current_distance < max_distance:
                    # Check for a timeout.
                    current_time = time.time()
                    if timelimit is not None and current_time - start_time > timelimit:
                        raise PlanningError('Reached time limit.')

                    # Compute joint velocities using the Jacobian pseudoinverse.
                    q_dot = self.GetStraightVelocity(manip, direction, initial_pose, nullspace, step_size, sign_flipper=sign_flipper)
                    q += q_dot
                    robot.SetDOFValues(q, active_dof_indices)

                    # Check for collisions.
                    #if self.env.CheckCollision(robot):
                    for body in active_bodies:
                        if self.env.CheckCollision(robot, body):
                            raise PlanningError('Encountered collision.')
                    if robot.CheckSelfCollision():
                        raise PlanningError('Encountered self-collision.')
                    # Check for joint limits.
                    elif not (limits_lower < q).all() or not (q < limits_upper).all():
                        raise PlanningError('Encountered joint limit during Jacobian move.')

                    # Check our distance from the constraint.
                    current_pose = manip.GetEndEffectorTransform()
                    a = initial_pose[0:3, 3]
                    p = current_pose[0:3, 3]
                    orthogonal_proj = (a - p) - numpy.dot(a - p, direction) * direction
                    if numpy.linalg.norm(orthogonal_proj) > position_tolerance:
                        raise PlanningError('Deviated from a straight line constraint.')

                    # Check our orientation against the constraint.
                    offset_pose = numpy.dot(numpy.linalg.inv(current_pose), initial_pose)
                    offset_angle = openravepy.axisAngleFromRotationMatrix(offset_pose)
                    offset_angle_norm = numpy.linalg.norm(offset_angle)
                    if offset_angle_norm > last_rot_error + 0.0005:
                        sign_flipper *= -1
                    last_rot_error = offset_angle_norm
                    if offset_angle_norm > angular_tolerance:
                        raise PlanningError('Deviated from orientation constraint.')

                    traj.Insert(traj.GetNumWaypoints(), q)

                    # Check if we've exceeded the maximum distance by projecting our
                    # displacement along the direction.
                    hand_pose = manip.GetEndEffectorTransform()
                    displacement = hand_pose[0:3, 3] - initial_pose[0:3, 3]
                    current_distance = numpy.dot(displacement, direction)
            except PlanningError as e:
                # Throw an error if we haven't reached the minimum distance.
                if current_distance < distance:
                    raise
                # Otherwise we'll gracefully terminate.
                else:
                    logger.warning('Terminated early at distance %f < %f: %s',
                                   current_distance, max_distance, e.message)

        SetTrajectoryTags(output_traj, {Tags.CONSTRAINED: True}, append=True)
        return traj
Esempio n. 16
0

def vec2args(x):
    return x[0:3], x[3:6]


def args2vec(xyz, rod):
    out = np.empty(6)
    out[0:3] = xyz
    out[3:6] = rod
    return out


T_init = np.eye(4)
xyz_init = T_init[:3, 3]
rod_init = rave.axisAngleFromRotationMatrix(T_init[:3, :3])

print "> optimizing..."
soln = opt.fmin(calc_error_wrapper, args2vec(xyz_init, rod_init), xtol=1e-2, ftol=1e-1, callback=plot_point)
print "\t optimization done."

(best_xyz, best_rod) = vec2args(soln)
print "xyz, rod:", best_xyz, best_rod
T_best = calc_T(best_xyz, best_rod)
print "T_h_k:", T_best

## final display:
pc2 = (np.c_[pc2, np.ones((pc2.shape[0], 1))].dot(T_best.T))[:, :3]
clearreq = gen_mlab_request(mlab.clf)
plotter.request(clearreq)
c1req = gen_mlab_request(mlab.points3d, pc1[:, 0], pc1[:, 1], pc1[:, 2], color=(1, 0, 0), scale_factor=0.001)
Esempio n. 17
0
File: mk.py Progetto: rsinnet/prpy
    def PlanToEndEffectorOffset(self,
                                robot,
                                direction,
                                distance,
                                max_distance=None,
                                nullspace=JointLimitAvoidance,
                                timelimit=5.0,
                                step_size=0.001,
                                position_tolerance=0.01,
                                angular_tolerance=0.15,
                                **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
        @param stepsize step size in meters for the Jacobian pseudoinverse controller
        @param position_tolerance constraint tolerance in meters
        @param angular_tolerance constraint tolerance in radians
        @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 step_size <= 0:
            raise ValueError('Step size must be positive.')
        elif position_tolerance < 0:
            raise ValueError('Position tolerance must be non-negative.')
        elif angular_tolerance < 0:
            raise ValueError('Angular tolerance must be non-negative.')

        # save all active bodies so we only check collision with those
        active_bodies = []
        for body in self.env.GetBodies():
            if body.IsEnabled():
                active_bodies.append(body)

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

        # Default to moving an exact distance.
        if max_distance is None:
            max_distance = distance

        with robot:
            manip = robot.GetActiveManipulator()
            traj = openravepy.RaveCreateTrajectory(self.env, '')
            traj.Init(manip.GetArmConfigurationSpecification())

            active_dof_indices = manip.GetArmIndices()
            limits_lower, limits_upper = robot.GetDOFLimits(active_dof_indices)
            initial_pose = manip.GetEndEffectorTransform()
            q = robot.GetDOFValues(active_dof_indices)
            traj.Insert(0, q)

            start_time = time.time()
            current_distance = 0.0
            sign_flipper = 1
            last_rot_error = 9999999999.0
            try:
                while current_distance < max_distance:
                    # Check for a timeout.
                    current_time = time.time()
                    if timelimit is not None and current_time - start_time > timelimit:
                        raise PlanningError('Reached time limit.')

                    # Compute joint velocities using the Jacobian pseudoinverse.
                    q_dot = self.GetStraightVelocity(manip,
                                                     direction,
                                                     initial_pose,
                                                     nullspace,
                                                     step_size,
                                                     sign_flipper=sign_flipper)
                    q += q_dot
                    robot.SetDOFValues(q, active_dof_indices)

                    # Check for collisions.
                    #if self.env.CheckCollision(robot):
                    for body in active_bodies:
                        if self.env.CheckCollision(robot, body):
                            raise PlanningError('Encountered collision.')
                    if robot.CheckSelfCollision():
                        raise PlanningError('Encountered self-collision.')
                    # Check for joint limits.
                    elif not (limits_lower <
                              q).all() or not (q < limits_upper).all():
                        raise PlanningError(
                            'Encountered joint limit during Jacobian move.')

                    # Check our distance from the constraint.
                    current_pose = manip.GetEndEffectorTransform()
                    a = initial_pose[0:3, 3]
                    p = current_pose[0:3, 3]
                    orthogonal_proj = (
                        a - p) - numpy.dot(a - p, direction) * direction
                    if numpy.linalg.norm(orthogonal_proj) > position_tolerance:
                        raise PlanningError(
                            'Deviated from a straight line constraint.')

                    # Check our orientation against the constraint.
                    offset_pose = numpy.dot(numpy.linalg.inv(current_pose),
                                            initial_pose)
                    offset_angle = openravepy.axisAngleFromRotationMatrix(
                        offset_pose)
                    offset_angle_norm = numpy.linalg.norm(offset_angle)
                    if offset_angle_norm > last_rot_error + 0.0005:
                        sign_flipper *= -1
                    last_rot_error = offset_angle_norm
                    if offset_angle_norm > angular_tolerance:
                        raise PlanningError(
                            'Deviated from orientation constraint.')

                    traj.Insert(traj.GetNumWaypoints(), q)

                    # Check if we've exceeded the maximum distance by projecting our
                    # displacement along the direction.
                    hand_pose = manip.GetEndEffectorTransform()
                    displacement = hand_pose[0:3, 3] - initial_pose[0:3, 3]
                    current_distance = numpy.dot(displacement, direction)
            except PlanningError as e:
                # Throw an error if we haven't reached the minimum distance.
                if current_distance < distance:
                    raise
                # Otherwise we'll gracefully terminate.
                else:
                    logger.warning('Terminated early at distance %f < %f: %s',
                                   current_distance, max_distance, e.message)

        SetTrajectoryTags(output_traj, {Tags.CONSTRAINED: True}, append=True)
        return traj
Esempio n. 18
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
Esempio n. 19
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])
Esempio n. 20
0
def axis_angle_from_rot(rot):
    return axisAngleFromRotationMatrix(rot)
    def GetBasePoseForObjectGrasp(self, obj):

        # Load grasp database
        self.gmodel = openravepy.databases.grasping.GraspingModel(
            self.robot, obj)
        if not self.gmodel.load():
            self.gmodel.autogenerate()

        base_pose = None
        grasp_config = None

        ###################################################################
        # TODO: Here you will fill in the function to compute
        #  a base pose and associated grasp config for the
        #  grasping the bottle
        ###################################################################
        #get the ordered valid grasp from homework1

        print "robot start transformation -----------------"
        print self.robot.GetTransform()
        self.graspindices = self.gmodel.graspindices
        self.grasps = self.gmodel.grasps
        self.order_grasps()

        # get the grasp transform
        Tgrasp = self.gmodel.getGlobalGraspTransform(self.grasps_ordered[10],
                                                     collisionfree=True)

        # load inverserechability database
        irmodel = openravepy.databases.inversereachability.InverseReachabilityModel(
            robot=self.robot)
        starttime = time.time()
        print 'loading irmodel'
        if not irmodel.load():
            irmodel.autogenerate()
        loaded = irmodel.load()
        print "irmodel loaded? {}".format(loaded)

        densityfn, samplerfn, bounds = irmodel.computeBaseDistribution(Tgrasp)

        #find the valid pose and joint states
        # initialize sampling parameters
        goals = []
        numfailures = 0
        N = 3
        with self.robot.GetEnv():
            while len(goals) < N:
                poses, jointstate = samplerfn(N - len(goals))
                for pose in poses:
                    self.robot.SetTransform(pose)
                    self.robot.SetDOFValues(*jointstate)
                    # validate that base is not in collision
                    if not self.manip.CheckIndependentCollision(
                            openravepy.CollisionReport()):
                        q = self.manip.FindIKSolution(
                            Tgrasp,
                            filteroptions=IkFilterOptions.CheckEnvCollisions)
                        if q is not None:
                            values = self.robot.GetDOFValues()
                            values[self.manip.GetArmIndices()] = q
                            goals.append((Tgrasp, pose, values))
                        elif self.manip.FindIKSolution(Tgrasp, 0) is None:
                            numfailures += 1
        # To do still
        #base_pose = goals[0][1]
        #grasp_config = goals[0][2]
        for i, goal in enumerate(goals):
            grasp_with_pose, pose, values = goal
            self.robot.SetTransform(pose)
            self.robot.SetJointValues(values)
        trans_pose = self.robot.GetTransform()
        angle_pose = openravepy.axisAngleFromRotationMatrix(trans_pose)
        pose = [trans_pose[0, 3], trans_pose[1, 3], angle_pose[2]]
        base_pose = numpy.array(pose)

        grasp_config = q

        #import IPython
        #IPython.embed()
        print "grasping result"
        print base_pose
        print grasp_config
        return base_pose, grasp_config
def tf_to_pose(tf):
	return np.r_[tf[:3,3], rave.axisAngleFromRotationMatrix(tf[:3,:3])]
 def GetCurrentConfiguration(self):
     t = self.robot.GetTransform()
     aa = openravepy.axisAngleFromRotationMatrix(t)
     pose = [t[0,3], t[1,3], aa[2]]
     return numpy.array(pose)
Esempio n. 24
0
#o = T_w_k[:3,3]
#x = T_w_k[:3,0]
#y = T_w_k[:3,1]
#z = T_w_k[:3,2]
#handles = []
#handles.append(env.drawarrow(o, o+.2*x, .005,(1,0,0,1)))
#handles.append(env.drawarrow(o, o+.2*y, .005,(0,1,0,1)))
#handles.append(env.drawarrow(o, o+.2*z, .005,(0,0,1,1)))
#viewer.Idle()


T_h_k_init = berkeley_pr2.T_h_k

xyz_init = T_h_k_init[:3,3]
rod_init = openravepy.axisAngleFromRotationMatrix(T_h_k_init[:3,:3])
f_init = 525

#from rapprentice.math_utils import linspace2d



#sim_depths = calculate_sim_depths(xyz_init, rod_init, f_init)    
#cv2.imshow('hi',np.clip(sim_depths[0]/3,0,1))
#cv2.waitKey()
#plot_real_and_sim(args2vec(xyz_init, rod_init, f_init))
#calculate_depth_error(xyz_init, rod_init, f_init)
soln = opt.fmin(calc_error_wrapper, args2vec(xyz_init, rod_init, f_init),callback=plot_real_and_sim)
(best_xyz, best_rod, best_f) = vec2args(soln)
print "xyz, rod:", best_xyz, best_rod
print "T_h_k:", calc_Thk(best_xyz, best_rod)
    def GetBasePoseForObjectGrasp(self, obj):
        # Load grasp database
        graspModel = GraspModel(self.robot, obj)
        graspModel.order_grasps()

        for i in range(6):
            print 'Showing grasp ', i
            graspModel.show_grasp(graspModel.grasps_ordered[i], delay=self.delay)
            print "using grasp 0 to generate Transform "
            Tgrasp = graspModel.gmodel.getGlobalGraspTransform(graspModel.grasps_ordered[i], collisionfree=True) # get the grasp transform
            if Tgrasp != None:
                break

        base_pose = None
        grasp_config = None

        ###################################################################
        # TODO: Here you will fill in the function to compute
        #  a base pose and associated grasp config for the
        #  grasping the bottle
        ###################################################################
        self.irmodel = openravepy.databases.inversereachability.InverseReachabilityModel(self.robot)

        loaded = self.irmodel.load()
        if loaded:
            densityfn,samplerfn,bounds = self.irmodel.computeBaseDistribution(Tgrasp)
        # densityfn: gaussian kernel density function taking poses of openrave quaternion type, returns probabilities
        # samplerfn: gaussian kernel sampler function taking number of sample and weight, returns robot base poses and joint states
        # bounds: 2x3 array, bounds of samples, [[min rotation, min x, min y],[max rotation, max x, max y]]

        goals = []
        numfailures = 0
        starttime = time.time()
        timeout = 5000
        N = 5

        with self.robot:
            while len(goals) < N:
                if time.time()-starttime > timeout:
                    break
                poses,jointstate = samplerfn(N-len(goals))
                for pose in poses:
                    self.robot.SetTransform(pose)
                    self.robot.SetDOFValues(*jointstate)
                    # validate that base is not in collision
                    if not self.irmodel.manip.CheckIndependentCollision(openravepy.CollisionReport()):
                        q = self.irmodel.manip.FindIKSolution(Tgrasp,filteroptions=openravepy.IkFilterOptions.CheckEnvCollisions)
                        if q is not None:
                            values = self.robot.GetDOFValues()
                            values[self.irmodel.manip.GetArmIndices()] = q
                            goals.append((Tgrasp,pose,values))
                        elif self.irmodel.manip.FindIKSolution(Tgrasp,0) is None:
                            numfailures += 1

        self.armmanip = self.robot.GetManipulator('right_wam')
        self.robot.SetActiveManipulator('right_wam')
        print 'showing %d results'%N
        for ind,goal in enumerate(goals):
            raw_input('press ENTER to show goal %d'%ind)
            Tgrasp,base_pose, all_config = goal

            self.robot.SetTransform(base_pose)
            self.robot.SetDOFValues(all_config)

        # IPython.embed()
        idx = raw_input("Choose from goal index 0, 1, 2, 3, 4: ")
        goal_chosen = goals[int(idx)]
        Tgrasp, pose, all_config = goal_chosen
        matrix = openravepy.matrixFromPose(pose)
        aa = openravepy.axisAngleFromRotationMatrix(matrix)
        base_pose = [matrix[0,3], matrix[1,3], aa[2]] # x, y , theta

        return base_pose, all_config[11:18] # base_pose [x, y] grasp_config [7 dof values]
Esempio n. 26
0
  def PlotImages(self, o, a, desc):
    '''Produces plots of the robot's observation and selected action.
    - Input o: Image where 1st channel is the target sensed volume and the 2nd channel is the hand
      contents.
    - Input a: Index into the Q-function output which corresponds to the selected action.
    - Input desc: Descriptor corresponding to the current action in the base frame.
    - Returns None.
    '''

    # setup
    fig = pyplot.figure()
    It = o[:, :, 0]
    Ih = zeros(It.shape) if self.IsGraspImage(o) else o[:, :, 1]
    Ir = copy(It); Ig = copy(It); Ib = copy(It)
    
    if self.IsOrientationLevel():
      
      # determine rotation angle
      R = desc.T[0:3, 0:3]
      axisAngle = openravepy.axisAngleFromRotationMatrix(R)
      angle = norm(axisAngle)
      axis = axisAngle / angle if angle > 0 else array([0.0, 0.0, 1.0])
      angle *= sign(sum(axis))
      
      # draw axis indicator
      c = self.imP / 2
      majorRadius = self.imP / 8
      minorRadius = majorRadius if self.IsGraspImage else majorRadius / 2
      xx, yy = ellipse_perimeter(c, c, minorRadius, majorRadius, orientation=0)
      Ir[xx, yy] = 1.0
      
      # draw angle indicator
      length = self.imP / 5
      x = -int(length * sin(angle))
      y = int(length * cos(angle))
      xx, yy = line(c, c, c + x, c + y)
      Ir[xx, yy] = 1.0
      xx, yy = line(c, c, c, c + length)
      Ir[xx, yy] = 1.0
    
    else:

      # draw the selection area
      halfWidth = (It.shape[0] * (self.selW / self.imW)) / 2.0
      middle = It.shape[0] / 2.0
      start = int(round(middle - halfWidth))
      end = int(round(middle + halfWidth))
      pixels = arange(start, end + 1)
      if start >= 0 and end < It.shape[0]:
        Ib[start, pixels] = 1.0
        Ib[end, pixels] = 1.0
        Ib[pixels, start] = 1.0
        Ib[pixels, end] = 1.0
  
      # draw robot's selection
      xh = self.actionsInHandFrame[a]
      xi = round(((xh[0:2] * self.imP) / self.imW) + ((self.imP - 1.0) / 2.0)).astype('int32')
      value = (xh[2] + (self.imD / 2.0)) / self.imD
      if xi[0] >= 0 and xi[1] < self.imP and xi[1] >= 0 and xi[1] < self.imP:
        Ir[xi[0], xi[1]] = value
        Ig[xi[0], xi[1]] = 0
        Ib[xi[0], xi[1]] = 0

    # show image
    Irgb = stack((Ir, Ig, Ib), 2)
    pyplot.subplot(1, 2, 1)
    pyplot.imshow(Irgb, vmin=0.00, vmax=1.00, interpolation="none")
    pyplot.subplot(1, 2, 2)
    pyplot.imshow(Ih, vmin=0.00, vmax=1.00, interpolation="none", cmap="gray")
    fig.suptitle("(Left.) Sensed volume. (Right.) Hand contents.")
    for i in xrange(2):
      fig.axes[i].set_xticks([])
      fig.axes[i].set_yticks([])
    pyplot.show(block=True)
Esempio n. 27
0
T_w_k = berkeley_pr2.get_kinect_transform(robot)

#o = T_w_k[:3,3]
#x = T_w_k[:3,0]
#y = T_w_k[:3,1]
#z = T_w_k[:3,2]
#handles = []
#handles.append(env.drawarrow(o, o+.2*x, .005,(1,0,0,1)))
#handles.append(env.drawarrow(o, o+.2*y, .005,(0,1,0,1)))
#handles.append(env.drawarrow(o, o+.2*z, .005,(0,0,1,1)))
#viewer.Idle()

T_h_k_init = berkeley_pr2.T_h_k

xyz_init = T_h_k_init[:3, 3]
rod_init = openravepy.axisAngleFromRotationMatrix(T_h_k_init[:3, :3])
f_init = 525

#from rapprentice.math_utils import linspace2d

#sim_depths = calculate_sim_depths(xyz_init, rod_init, f_init)
#cv2.imshow('hi',np.clip(sim_depths[0]/3,0,1))
#cv2.waitKey()
#plot_real_and_sim(args2vec(xyz_init, rod_init, f_init))
#calculate_depth_error(xyz_init, rod_init, f_init)
soln = opt.fmin(calc_error_wrapper,
                args2vec(xyz_init, rod_init, f_init),
                callback=plot_real_and_sim)
(best_xyz, best_rod, best_f) = vec2args(soln)
print "xyz, rod:", best_xyz, best_rod
print "T_h_k:", calc_Thk(best_xyz, best_rod)
Esempio n. 28
0
    def get_object_list(self, req):
        #rospy.loginfo('Getting object list')
        resp = GetObjectListResponse()

        resp.in_goal = False
        resp.num_objects = 0
        resp.objects = []
        bodyNum = 0

        for body in self.env_.GetBodies():
            body_name = body.GetName().lower()
            resp.num_objects += 1
            obj = ReconfigurationObject()
            s_pose = numpy.array([[0., -1., 0., 0.], [0., 0., 1., 0.],
                                  [-1., 0., 0., 0.], [0., 0., 0., 1.]])
            s_inv = numpy.linalg.inv(s_pose)
            transform = body.GetTransform()
            q = transform[:3, :3]
            aa = openravepy.axisAngleFromRotationMatrix(q)
            state = self.module.GetState()
            obj_bb = body.ComputeAABB()

            if body_name == 'target':
                obj.type = 'target'
                obj.x = 1.0479 - state['movables'][bodyNum][0]
                obj.y = state['movables'][bodyNum][1]
                if self.initialize == True:
                    self.target_off = aa[2]
                    self.target_extents = [
                        obj_bb.extents()[0],
                        obj_bb.extents()[1],
                        obj_bb.extents()[2]
                    ]
                obj.rot = state['movables'][bodyNum][2] + self.target_off
                obj.xextent = self.target_extents[0]
                obj.yextent = self.target_extents[1]
                obj.zextent = self.target_extents[2]
                in_goal = self.module.CheckGoal(state)
                if in_goal:
                    resp.in_goal = True
                bodyNum = bodyNum + 1

            elif body_name == 'fuze_bottle':
                obj.type = 'fuze_bottle'
                obj.x = state['movables'][bodyNum][0]
                obj.y = state['movables'][bodyNum][1]
                if self.initialize == True:
                    self.bottle_radius = obj_bb.extents()[0]
                obj.xextent = self.bottle_radius
                bodyNum = bodyNum + 1

            elif body_name in [
                    'plastic_glass', 'plastic_glass2', 'plastic_glass3'
            ]:
                obj.type = 'plastic_glass'
                obj.x = state['movables'][bodyNum][0]
                obj.y = state['movables'][bodyNum][1]
                if self.initialize == True:
                    self.glass_radius = obj_bb.extents()[0]
                obj.xextent = self.glass_radius
                bodyNum = bodyNum + 1

            elif body_name in ['pop_tarts', 'pop_tarts2']:
                obj.type = 'pop_tarts'
                obj.x = 1.0479 - state['movables'][bodyNum][0]
                obj.y = state['movables'][bodyNum][1]
                if self.initialize == True:
                    self.pop_tarts_off = aa[2]
                    self.pop_tarts_extents = [
                        obj_bb.extents()[0],
                        obj_bb.extents()[1],
                        obj_bb.extents()[2]
                    ]
                obj.rot = state['movables'][bodyNum][2] + self.pop_tarts_off
                obj.xextent = self.pop_tarts_extents[0]
                obj.yextent = self.pop_tarts_extents[1]
                obj.zextent = self.pop_tarts_extents[2]
                bodyNum = bodyNum + 1

            elif body_name == 'bh280':
                obj.type = 'bh280'
                obj.x = state['manip'][0]
                obj.y = state['manip'][1]
                n_pose = body.GetTransform()
                now_in_start = numpy.dot(n_pose[:3, :3], s_inv[:3, :3])
                obj.rot = numpy.arctan2(now_in_start[1, 0], now_in_start[0, 0])
                if self.initialize == True:
                    self.hand_off = aa[2]
                    self.hand_extents = [
                        obj_bb.extents()[0],
                        obj_bb.extents()[1],
                        obj_bb.extents()[2]
                    ]
                obj.xextent = self.hand_extents[0]
                obj.yextent = self.hand_extents[1]
                obj.zextent = self.hand_extents[2]

            resp.objects.append(obj)

        if self.initialize:
            self.startTime = datetime.now()
            self.initialize = False

        return resp
Esempio n. 29
0
  def PlotImagesContrastAdjusted(self, o, a, desc):
    '''Same as PlotImages but with contrast adjusted for visualization.'''

    # setup
    fig = pyplot.figure()
    It = o[:, :, 0] / max(o[:, :, 0])
    Ih = zeros(It.shape) if self.IsGraspImage(o) else o[:, :, 1] / max(o[:, :, 1])
    Ir = copy(It); Ig = copy(It); Ib = copy(It)
    
    if self.IsOrientationLevel():
      
      # determine rotation angle
      R = desc.T[0:3, 0:3]
      axisAngle = openravepy.axisAngleFromRotationMatrix(R)
      angle = norm(axisAngle)
      axis = axisAngle / angle if angle > 0 else array([0.0, 0.0, 1.0])
      angle *= sign(sum(axis))
      
      # draw axis indicator
      c = self.imP / 2
      majorRadius = self.imP / 8
      minorRadius = majorRadius if self.IsGraspImage else majorRadius / 2
      xx, yy = ellipse_perimeter(c, c, minorRadius, majorRadius, orientation=0)
      Ir[xx, yy] = 1.0
      Ig[xx, yy] = 0.0
      Ib[xx, yy] = 0.0
      
      # draw angle indicator
      length = self.imP / 5
      x = -int(length * sin(angle))
      y = int(length * cos(angle))
      xx, yy = line(c, c, c + x, c + y)
      Ir[xx, yy] = 1.0
      Ig[xx, yy] = 0.0
      Ib[xx, yy] = 0.0
      xx, yy = line(c, c, c, c + length)
      Ir[xx, yy] = 1.0
      Ig[xx, yy] = 0.0
      Ib[xx, yy] = 0.0
    
    else:

      # draw the selection area
      halfWidth = (It.shape[0] * (self.selW / self.imW)) / 2.0
      middle = It.shape[0] / 2.0
      start = int(round(middle - halfWidth))
      end = int(round(middle + halfWidth))
      pixels = arange(start, end + 1)
      if start >= 0 and end < It.shape[0]:
        Ir[start, pixels] = 0.0
        Ir[end, pixels] = 0.0
        Ir[pixels, start] = 0.0
        Ir[pixels, end] = 0.0
        Ig[start, pixels] = 0.0
        Ig[end, pixels] = 0.0
        Ig[pixels, start] = 0.0
        Ig[pixels, end] = 0.0
        Ib[start, pixels] = 1.0
        Ib[end, pixels] = 1.0
        Ib[pixels, start] = 1.0
        Ib[pixels, end] = 1.0
  
      # draw robot's selection
      xh = self.actionsInHandFrame[a]
      xi = round(((xh[0:2] * self.imP) / self.imW) + ((self.imP - 1.0) / 2.0)).astype('int32')
      cross = [(xi[0], xi[1]), (xi[0] + 1, xi[1]), (xi[0] - 1, xi[1]), (xi[0] + 2, xi[1]),
      (xi[0] - 2, xi[1]), (xi[0], xi[1]+ 1), (xi[0], xi[1] - 1), (xi[0], xi[1] + 2),
      (xi[0], xi[1] - 2)]
      for p in cross:
        if p[0] >= 0 and p[1] < self.imP and p[1] >= 0 and p[1] < self.imP:
          Ir[p[0], p[1]] = 1
          Ig[p[0], p[1]] = 0
          Ib[p[0], p[1]] = 0

    # show image
    Irgb = stack((Ir, Ig, Ib), 2)
    pyplot.subplot(1, 2, 1)
    pyplot.imshow(Irgb, vmin=0.00, vmax=1.00, interpolation="none")
    pyplot.subplot(1, 2, 2)
    pyplot.imshow(Ih, vmin=0.00, vmax=1.00, interpolation="none", cmap="gray")
    fig.suptitle("(Left.) Sensed volume. (Right.) Hand contents.")
    for i in xrange(2):
      fig.axes[i].set_xticks([])
      fig.axes[i].set_yticks([])
    pyplot.show(block=True)
 def GetCurrentConfiguration(self):
     t = self.robot.GetTransform()
     aa = openravepy.axisAngleFromRotationMatrix(t)
     pose = [t[0, 3], t[1, 3], aa[2]]
     return numpy.array(pose)
Esempio n. 31
0
def axis_angle_from_rot(rot):
  return axisAngleFromRotationMatrix(rot)