예제 #1
0
def VerifyIKSolver(youbot, desired_eepose, visualize=False):
    if visualize:
        h = orpy.misc.DrawAxes(youbot.GetEnv(), desired_eepose, 0.25, 2)
        time.sleep(1)
    base_poses,arm_configs = FindIKAndBaseSolutions(youbot,desired_eepose,returnfirst=False,checkenvcollision=False,randomize=False)
    print 'Found %d IK solutions. Verifying...'%(len(base_poses))
    youbot.GetController().Reset()
    for pose,q,i in izip(base_poses,arm_configs,range(len(base_poses))):
        with youbot:
            youbot.SetTransform(pose)
            youbot.SetDOFValues(q,youbot.GetManipulators()[0].GetArmIndices())
            eepose = youbot.GetManipulators()[0].GetEndEffectorTransform()
            if np.linalg.norm(eepose[:3,3]-desired_eepose[:3,3]) > 0.005:
                print 'EE is not at the desired position. index: %d.'%(i)
                return base_poses,arm_configs
            quat1 = orpy.quatFromRotationMatrix(desired_eepose[:3,:3])
            quat2 = orpy.quatFromRotationMatrix(eepose[:3,:3])
            quatinnerproduct = quat1[0]*quat2[0]+quat1[1]*quat2[1]+quat1[2]*quat2[2]+quat1[3]*quat2[3] 
            quatdist = np.arccos(2.0* (quatinnerproduct*quatinnerproduct) - 1.0) 
            if quatdist > 0.001:
                print 'EE is not at the desired orientation. index: %d.'%(i)
                return base_poses,arm_configs
            if visualize:
                youbot.GetEnv().UpdatePublishedBodies()
                time.sleep(0.001)
    print 'All IK solutions verified.'
    h = None
    return base_poses,arm_configs
예제 #2
0
def VerifyIKSolver(youbot, desired_eepose, visualize=False):
    if visualize:
        h = orpy.misc.DrawAxes(youbot.GetEnv(), desired_eepose, 0.25, 2)
        time.sleep(1)
    base_poses, arm_configs = FindIKAndBaseSolutions(youbot,
                                                     desired_eepose,
                                                     returnfirst=False,
                                                     checkenvcollision=False,
                                                     randomize=False)
    print 'Found %d IK solutions. Verifying...' % (len(base_poses))
    youbot.GetController().Reset()
    for pose, q, i in izip(base_poses, arm_configs, range(len(base_poses))):
        with youbot:
            youbot.SetTransform(pose)
            youbot.SetDOFValues(q, youbot.GetManipulators()[0].GetArmIndices())
            eepose = youbot.GetManipulators()[0].GetEndEffectorTransform()
            if np.linalg.norm(eepose[:3, 3] - desired_eepose[:3, 3]) > 0.005:
                print 'EE is not at the desired position. index: %d.' % (i)
                return base_poses, arm_configs
            quat1 = orpy.quatFromRotationMatrix(desired_eepose[:3, :3])
            quat2 = orpy.quatFromRotationMatrix(eepose[:3, :3])
            quatinnerproduct = quat1[0] * quat2[0] + quat1[1] * quat2[
                1] + quat1[2] * quat2[2] + quat1[3] * quat2[3]
            quatdist = np.arccos(2.0 * (quatinnerproduct * quatinnerproduct) -
                                 1.0)
            if quatdist > 0.001:
                print 'EE is not at the desired orientation. index: %d.' % (i)
                return base_poses, arm_configs
            if visualize:
                youbot.GetEnv().UpdatePublishedBodies()
                time.sleep(0.001)
    print 'All IK solutions verified.'
    h = None
    return base_poses, arm_configs
예제 #3
0
파일: mk.py 프로젝트: DavidB-CMU/prpy
    def GetStraightVelocity(self, manip, velocity, initial_hand_pose, nullspace_fn, step_size, sign_flipper = 1):
        robot = manip.GetRobot()
        current_hand_pose = manip.GetEndEffectorTransform()
        initial_position = initial_hand_pose[0:3, 3]
        current_position = current_hand_pose[0:3, 3]

        # Simulate a position goal step_size distance further along the velocity vector.
        moved_already = velocity * numpy.dot(current_position - initial_position, velocity)
        desired_position = initial_position + moved_already + velocity * step_size
        error_pos = desired_position - current_position

        # Append the desired quaternion to create the error vector. There is a
        # sign ambiguity on quaternions, so we'll always choose the shortest path.
        initial_ori = openravepy.quatFromRotationMatrix(initial_hand_pose)
        current_ori = openravepy.quatFromRotationMatrix(current_hand_pose)
        choices_ori = [ current_ori - initial_ori, current_ori + initial_ori ]
        error_ori = sign_flipper*min(choices_ori, key=lambda q: numpy.linalg.norm(q))

        # Jacobian pseudo-inverse.
        jacobian_spatial = manip.CalculateJacobian()
        jacobian_angular = manip.CalculateRotationJacobian() #this function seems very buggy/wrong
        jacobian = numpy.vstack((jacobian_spatial, jacobian_angular))
        jacobian_pinv = numpy.linalg.pinv(jacobian)

        # Null-space projector
        nullspace_projector = numpy.eye(jacobian.shape[1]) - numpy.dot(jacobian_pinv, jacobian)
        nullspace_goal = nullspace_fn(robot)
        pose_error = numpy.hstack((error_pos, error_ori))

        return numpy.dot(jacobian_pinv, pose_error) + numpy.dot(nullspace_projector, nullspace_goal)
  def straight_line_jacobian(self, distance):

    graph = np.array([])
    points =np.array([self.robot.GetActiveDOFValues()])
    delta_d = 0.001
    distance_to_goal = 1
 
    #constructing the jacobian
    j_spatial = self.manip.CalculateJacobian()
    j_rot = self.manip.CalculateRotationJacobian()
    J = np.vstack((j_spatial, j_rot))
    Jinv = np.linalg.pinv(J) #pseudo-inverse

    #end effector data
    ee_trans = self.end_effector.GetTransform()
    ee_pos_init = ee_trans[0:3,3] #position of the end effector
    ee_quat_init = openravepy.quatFromRotationMatrix(ee_trans[0:3,0:3])
    
    
    x_present = np.append(ee_pos_init, ee_quat_init)
    x_add = [delta_d, 0, 0, 0, 0, 0, 0]
    x_goal = x_present + x_add 
    x_present_x = ee_pos_init[0]
    
    while np.abs(distance_to_goal) > delta_d:
      q_present = self.robot.GetActiveDOFValues()
      delta_x = x_goal - x_present
      delta_q = np.array(np.dot(Jinv,delta_x))

      q_new = np.array(q_present + delta_q)
      check = self.check_collision(q_new)
      if(check):
        print ' the final positon of the end effector is:'
        print x_present[0:3]
        break
      else:
        with self.env:
          self.robot.SetActiveDOFValues(q_new)
        points = np.append(points, np.array([q_present + delta_q]), axis=0)
        
        j_spatial = self.manip.CalculateJacobian()
        j_rot = self.manip.CalculateRotationJacobian()
        J = np.vstack((j_spatial, j_rot))
        Jinv = np.linalg.pinv(J) 
 
        ee_trans = self.end_effector.GetTransform()
        ee_quat = openravepy.quatFromRotationMatrix(ee_trans[0:3,0:3])
        ee_pos = ee_trans[0:3,3]
        x_present = np.append(ee_pos, ee_quat)
        x_present_x = x_present_x + delta_d
        x_goal = np.append([x_present_x, x_present[1], x_present[2]], ee_quat)
        
        distance_to_goal = x_goal[0]-(ee_pos_init[0]+distance) 
        
    traj = self.points_to_traj(np.array(points))
    #print points
    return traj
예제 #5
0
def rotation_interpolation(r1, r2, t):
    quat1 = rave.quatFromRotationMatrix(r1)
    quat2 = rave.quatFromRotationMatrix(r2)
    # print(r1)
    # print(r2)
    t = min(max(t, 0), 1)

    quat_interpolate = rave.quatSlerp(quat1, quat2, t, True)
    return rave.matrixFromQuat(quat_interpolate)[0:3, 0:3]
 def IsSameGrasp(self,g1,g2):
     diff = g1[:3,3] - g2[:3,3]
     #if np.linalg.norm(diff) < 0.001:
     if (diff[0] ** 2 + diff[1] ** 2 + diff[2] ** 2) > 0.00001:
         return False
     qd=orpy.quatMult(orpy.quatInverse(orpy.quatFromRotationMatrix(g1[:3,:3])),orpy.quatFromRotationMatrix(g2[:3,:3]))
     shortest_arc_angle = 2.0*np.arctan2(np.linalg.norm(qd[1:4]),qd[0])
     if shortest_arc_angle > 0.001:
         return False
     return True
예제 #7
0
def IsSameTransform(tr1, tr2):
    # FIXME just check element-by-element similarity?
    if np.linalg.norm(tr1[:3,3]-tr2[:3,3]) > 0.0001:
        return False
    quat1 = orpy.quatFromRotationMatrix(tr1[:3,:3])
    quat2 = orpy.quatFromRotationMatrix(tr2[:3,:3])
    quatinnerproduct = quat1[0]*quat2[0]+quat1[1]*quat2[1]+quat1[2]*quat2[2]+quat1[3]*quat2[3] 
    quatdist = np.arccos(2.0* (quatinnerproduct*quatinnerproduct) - 1.0) 
    if quatdist > 0.00001:
        return False
    return True
예제 #8
0
def IsSameTransform(tr1, tr2):
    # FIXME just check element-by-element similarity?
    if np.linalg.norm(tr1[:3, 3] - tr2[:3, 3]) > 0.0001:
        return False
    quat1 = orpy.quatFromRotationMatrix(tr1[:3, :3])
    quat2 = orpy.quatFromRotationMatrix(tr2[:3, :3])
    quatinnerproduct = quat1[0] * quat2[0] + quat1[1] * quat2[1] + quat1[
        2] * quat2[2] + quat1[3] * quat2[3]
    quatdist = np.arccos(2.0 * (quatinnerproduct * quatinnerproduct) - 1.0)
    if quatdist > 0.00001:
        return False
    return True
예제 #9
0
 def IsSameGrasp(self, g1, g2):
     diff = g1[:3, 3] - g2[:3, 3]
     #if np.linalg.norm(diff) < 0.001:
     if (diff[0]**2 + diff[1]**2 + diff[2]**2) > 0.00001:
         return False
     qd = orpy.quatMult(
         orpy.quatInverse(orpy.quatFromRotationMatrix(g1[:3, :3])),
         orpy.quatFromRotationMatrix(g2[:3, :3]))
     shortest_arc_angle = 2.0 * np.arctan2(np.linalg.norm(qd[1:4]), qd[0])
     if shortest_arc_angle > 0.001:
         return False
     return True
예제 #10
0
    def RegisterBody(self, or_body, tf_id, openrave_frame_in_tf_frame=None, planar_tracking=False, fixed_translation_z=-1.0):
        x = 0.0
        y = 0.0
        z = 0.0
        qw = 1.0
        qx = 0.0
        qy = 0.0
        qz = 0.0

        if openrave_frame_in_tf_frame is not None:
            x = openrave_frame_in_tf_frame[0,3]
            y = openrave_frame_in_tf_frame[1,3]
            z = openrave_frame_in_tf_frame[2,3]
            quat = orpy.quatFromRotationMatrix(openrave_frame_in_tf_frame[:3,:3])
            qw = quat[0]
            qx = quat[1]
            qy = quat[2]
            qz = quat[3]

        cmd = 'RegisterBody ' + or_body.GetName() + ' ' + tf_id + ' openrave_frame_in_tf_frame ' + str(x) + ' ' + str(y) + ' ' + str(z) + ' ' + str(qw) + ' ' + str(qx) + ' ' + str(qy) + ' ' + str(qz)

        if planar_tracking:
            cmd +=' planar_tracking' + ' ' + 'fixed_translation_z' + ' ' + str(fixed_translation_z)

        self.or_tf.SendCommand(cmd)
예제 #11
0
def serialize_transform(t):
    from openravepy import quatFromRotationMatrix

    return {
        'position': list(map(float,t[0:3, 3])),
        'orientation': list(map(float,quatFromRotationMatrix(t[0:3, 0:3]))),
    }
예제 #12
0
def inverse_kinematics(robot, manip_name, ee_link_name, ee_hmat):

    init_guess = np.zeros(7)

    xyz_target = ee_hmat[:3, 3]
    quat_target = openravepy.quatFromRotationMatrix(ee_hmat[:3, :3])

    request = {
        "basic_info": {
            "n_steps": 10,
            "manip": manip_name,  # see below for valid values
            "start_fixed": False,  # i.e., DOF values at first timestep are fixed based on current robot state
        },
        "costs": [
            {
                "type": "joint_vel",  # joint-space velocity cost
                "params": {"coeffs": [1]},  # a list of length one is automatically expanded to a list of length n_dofs
            },
            {
                "type": "collision",
                "name": "cont_coll",  # shorten name so printed table will be prettier
                "params": {
                    "continuous": False,
                    "coeffs": [
                        50
                    ],  # penalty coefficients. list of length one is automatically expanded to a list of length n_timesteps
                    "dist_pen": [0.05],  # robot-obstacle distance that penalty kicks in. expands to length n_timesteps
                },
            },
        ],
        "constraints": [
            # BEGIN pose_constraint
            {
                "type": "pose",
                "params": {
                    "xyz": xyz_target.tolist(),
                    "wxyz": quat_target.tolist(),
                    "link": ee_link_name,
                    "pos_coeffs": [10, 10, 10],
                    "rot_coeffs": [10, 10, 10],
                },
            }
            # END pose_constraint
        ],
        # BEGIN init
        "init_info": {
            "type": "straight_line",  # straight line in joint space.
            "endpoint": init_guess.tolist(),  # need to convert numpy array to list
        }
        # END init
    }

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

    return traj[-1]
예제 #13
0
def getPoseMsg(transform):  #Convert transform into ROS poseStamped message
    robotPose = Pose()
    q = openravepy.quatFromRotationMatrix(transform[0:3, 0:3])
    robotPose.position.x = transform[0][3]
    robotPose.position.y = transform[1][3]
    robotPose.position.z = transform[2][3]
    robotPose.orientation.x = q[1]
    robotPose.orientation.y = q[2]
    robotPose.orientation.z = q[3]
    robotPose.orientation.w = q[0]
    return robotPose
예제 #14
0
파일: hw3.py 프로젝트: mingchesu/1
  def straight_line_jacobian(self, distance):

    j_spatial = self.manip.CalculateJacobian()
    j_rot = self.manip.CalculateRotationJacobian()
    J = np.vstack((j_spatial, j_rot))
    Jinv = np.linalg.pinv(J) #pseudo-inverse
    u,s,v=np.linalg.svd(J)
    quat = openravepy.quatFromRotationMatrix(trans)
    vh=v.T
    jac_null=vh[:,-1:]
    return jac_null
예제 #15
0
파일: mk.py 프로젝트: rsinnet/prpy
    def GetStraightVelocity(self,
                            manip,
                            velocity,
                            initial_hand_pose,
                            nullspace_fn,
                            step_size,
                            sign_flipper=1):
        robot = manip.GetRobot()
        current_hand_pose = manip.GetEndEffectorTransform()
        initial_position = initial_hand_pose[0:3, 3]
        current_position = current_hand_pose[0:3, 3]

        # Simulate a position goal step_size distance further along the velocity vector.
        moved_already = velocity * numpy.dot(
            current_position - initial_position, velocity)
        desired_position = initial_position + moved_already + velocity * step_size
        error_pos = desired_position - current_position

        # Append the desired quaternion to create the error vector. There is a
        # sign ambiguity on quaternions, so we'll always choose the shortest path.
        initial_ori = openravepy.quatFromRotationMatrix(initial_hand_pose)
        current_ori = openravepy.quatFromRotationMatrix(current_hand_pose)
        choices_ori = [current_ori - initial_ori, current_ori + initial_ori]
        error_ori = sign_flipper * min(choices_ori,
                                       key=lambda q: numpy.linalg.norm(q))

        # Jacobian pseudo-inverse.
        jacobian_spatial = manip.CalculateJacobian()
        jacobian_angular = manip.CalculateRotationJacobian(
        )  #this function seems very buggy/wrong
        jacobian = numpy.vstack((jacobian_spatial, jacobian_angular))
        jacobian_pinv = numpy.linalg.pinv(jacobian)

        # Null-space projector
        nullspace_projector = numpy.eye(jacobian.shape[1]) - numpy.dot(
            jacobian_pinv, jacobian)
        nullspace_goal = nullspace_fn(robot)
        pose_error = numpy.hstack((error_pos, error_ori))

        return numpy.dot(jacobian_pinv, pose_error) + numpy.dot(
            nullspace_projector, nullspace_goal)
예제 #16
0
def check_traj_pose(env, result, target_left, target_right, threshold=0.01):
    #check trajectory with Cartesian target
    robot = env.GetRobots()[0]
    traj = result.GetTraj()

    #check for collision
    if traj_is_safe(traj, robot) is not True:
        print "There is a collision within the trajectory!"
        return False

    #check optimal solver status
    if (result.GetStatus()[0] is not 0):
        print "Optimization is not converged!"
        return False

    robot.SetActiveDOFValues(traj[-1])
    T_left = robot.GetLink('l_gripper_tool_frame').GetTransform()
    T_right = robot.GetLink('r_gripper_tool_frame').GetTransform()
    pose_left = np.concatenate(
        [T_left[0:3, 3].flatten(),
         openravepy.quatFromRotationMatrix(T_left)])
    pose_right = np.concatenate([
        T_right[0:3, 3].flatten(),
        openravepy.quatFromRotationMatrix(T_right)
    ])

    #check target for joint constraints
    if (np.linalg.norm(target_left - pose_left) > threshold):
        print target_left, pose_left
        print('Left pose is not reached!')
        return False

    if (np.linalg.norm(target_right - pose_right) > threshold):
        print target_right, pose_right
        print('Right pose is not reached!')
        return False

    #Otherwise, return True
    return True
예제 #17
0
    def AddGaussianNoise(self, orientationSigma, positionSigma):
        '''Returns a new grasp with noise added to the current position and orientation.'''

        descriptor = self if rand() < 0.5 else self.Flip()

        q = openravepy.quatFromRotationMatrix(descriptor.T)
        q += orientationSigma * randn(4)
        q /= norm(q)
        T = openravepy.matrixFromQuat(q)

        c = descriptor.center + positionSigma * descriptor.center
        T[0:3, 3] = c

        return HandDescriptor(\
          T, image=None, function=descriptor.function, objectName=descriptor.objectName)
예제 #18
0
def calculate_sim_depths(xyz, rod, f):
    T_h_k = calc_Thk(xyz, rod)
    T_w_k = robot.GetLink("head_plate_frame").GetTransform().dot(T_h_k)
    
    sensor.SetPose(T_w_k[:3,3], openravepy.quatFromRotationMatrix(T_w_k[:3,:3]))
    sensor.SetIntrinsics(f)    
    out = []
    
    for dofs in rave_values:
        robot.SetActiveDOFValues(dofs)
        viewer.UpdateSceneData()
        sensor.Update()
        out.append(sensor.GetDepthImage())
        
    return out
예제 #19
0
파일: util.py 프로젝트: athackst/prpy
    def update(self):
        import rospy

        with self.body.GetEnv():
            or_pose = self.body.GetTransform()
            or_quaternion = openravepy.quatFromRotationMatrix(or_pose)

        position = tuple(or_pose[0:3, 3])
        orientation = (or_quaternion[1], or_quaternion[2], or_quaternion[3], or_quaternion[0])
        self.broadcaster.sendTransform(position, orientation, rospy.Time.now(),
                                       self.child_frame, self.parent_frame)

        self.timer = threading.Timer(self.period, self.update)
        self.timer.daemon = True
        self.timer.start()
예제 #20
0
def calculate_sim_depths(xyz, rod, f):
    T_h_k = calc_Thk(xyz, rod)
    T_w_k = robot.GetLink("head_plate_frame").GetTransform().dot(T_h_k)

    sensor.SetPose(T_w_k[:3, 3],
                   openravepy.quatFromRotationMatrix(T_w_k[:3, :3]))
    sensor.SetIntrinsics(f)
    out = []

    for dofs in rave_values:
        robot.SetActiveDOFValues(dofs)
        viewer.UpdateSceneData()
        sensor.Update()
        out.append(sensor.GetDepthImage())

    return out
예제 #21
0
 def RegisterBody(self,
                  or_body,
                  tf_id,
                  openrave_frame_in_tf_frame=None,
                  planar_tracking=False):
     if openrave_frame_in_tf_frame is None:
         self.tfplugin.SendCommand('RegisterBody ' + or_body.GetName() +
                                   ' ' + tf_id)
     else:
         x = openrave_frame_in_tf_frame[0, 3]
         y = openrave_frame_in_tf_frame[1, 3]
         z = openrave_frame_in_tf_frame[2, 3]
         quat = orpy.quatFromRotationMatrix(
             openrave_frame_in_tf_frame[:3, :3])
         cmd = 'RegisterBody ' + or_body.GetName(
         ) + ' ' + tf_id + ' openrave_frame_in_tf_frame ' + str(
             x) + ' ' + str(y) + ' ' + str(z) + ' ' + str(
                 quat[0]) + ' ' + str(quat[1]) + ' ' + str(
                     quat[2]) + ' ' + str(quat[3])
         if planar_tracking:
             cmd = cmd + ' planar_tracking'
         self.tfplugin.SendCommand(cmd)
예제 #22
0
def quat_from_rotation_matrix(R):
    return quatFromRotationMatrix(R)
예제 #23
0
def RosTransformFromRaveMatrix(mat):
    out = gm.Transform()
    out.translation = gm.Vector3(*mat[:3, 3])
    qw, qx, qy, qz = openravepy.quatFromRotationMatrix(mat[:3, :3])
    out.rotation = gm.Quaternion(qx, qy, qz, qw)
    return out
예제 #24
0
    def PlanToEndEffectorOffset(self,
                                robot,
                                direction,
                                distance,
                                max_distance=None,
                                position_tolerance=0.01,
                                angular_tolerance=0.15,
                                **kwargs):
        """
        Plan to an end-effector offset with a move-hand-straight constraint.

        This function plans a trajectory to purely translate a hand in the
        given direction as far as possible before collision. Movement less
        than min_distance will return failure. The motion will not also move
        further than max_distance.

        @param robot the robot whose active DOFs will be used
        @param direction unit vector in the direction of motion
        @param distance minimum distance in meters
        @param max_distance maximum distance in meters
        @param position_tolerance constraint tolerance in meters
        @param angular_tolerance constraint tolerance in radians
        @return traj a trajectory following specified direction
        """
        # Plan using the active manipulator.
        with robot.GetEnv():
            manipulator = robot.GetActiveManipulator()
            pose = manipulator.GetEndEffectorTransform()

        # Convert IK endpoint transformation to pose.
        ee_position = pose[0:3, 3].tolist()
        ee_rotation = openravepy.quatFromRotationMatrix(pose).tolist()

        # Create dummy frame that has direction vector along Z.
        z = direction
        y = numpy.cross(z, pose[0:3, 0])  # Cross with EE-frame Y.
        y /= numpy.linalg.norm(y)
        x = numpy.cross(y, z)

        cost_frame = numpy.vstack((x, y, z)).T
        cost_rotation = openravepy.quatFromRotationMatrix(cost_frame).tolist()

        # Construct a planning request with these constraints.
        n_steps = 10
        request = {
            "basic_info": {
                "n_steps": n_steps,
                "manip": "active",
                "start_fixed": True
            },
            "costs": [{
                "type": "joint_vel",
                "params": {
                    "coeffs": [1]
                }
            }, {
                "type": "collision",
                "params": {
                    "coeffs": [20],
                    "dist_pen": [0.025]
                },
            }, {
                "type": "pose",
                "params": {
                    "xyz": [0, 0, 0],
                    "wxyz": cost_rotation,
                    "link": manipulator.GetEndEffector().GetName(),
                    "rot_coeffs": [0, 0, 0],
                    "pos_coeffs": [0, 0, 10]
                }
            }],
            "constraints": [{
                "type": "pose",
                "params": {
                    "xyz": ee_position,
                    "wxyz": ee_rotation,
                    "link": manipulator.GetEndEffector().GetName(),
                    # "first_step": 0,
                    # "last_step": n_steps-1,
                }
            }],
            "init_info": {
                "type": "stationary"
            }
        }

        # Set active DOFs to match active manipulator and plan.
        p = openravepy.KinBody.SaveParameters
        with robot.CreateRobotStateSaver(p.ActiveDOF):
            robot.SetActiveDOFs(manipulator.GetArmIndices())
            return self._Plan(robot, request, **kwargs)
예제 #25
0
    LINK_NAME = "l_gripper_tool_frame"
    #0.0640945 0.704273 -0.147528 0.691468 -0.168806 -0.0452678 0.762821
    ##################

    ### Env setup ####
    env = rave.Environment()
    env.StopSimulation()
    env.Load(ENV_FILE)
    robot = env.GetRobots()[0]
    robot.SetDOFValues([
        0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, -0.119839, 0.861339,
        -2.22045e-16, -1.87834, -3.00095, -1.02143, 3.0678, 0.54, -1.01863e-14,
        0, -1.59595e-16, 0, 0, 0, 0, 0, 0, 0, 0, 0, -8.16014e-15, 0,
        -1.11022e-16, 0
    ])
    manip = robot.GetManipulator(MANIP_NAME)
    ##################
    T_gripper = robot.GetLink(LINK_NAME).GetTransform()
    T_grasp = T_gripper.copy()
    T_grasp[:3, 3] += np.array([0, 0.1, 0])
    xyz_targ = T_grasp[:3, 3]
    success = mk.create_cylinder(env, xyz_targ - np.array([.03, 0, 0]), .02,
                                 .5)
    quat_targ = rave.quatFromRotationMatrix(T_grasp[:3, :3])

    request = move_arm_to_grasp(xyz_targ, quat_targ, LINK_NAME, MANIP_NAME)
    s = json.dumps(request)
    print "REQUEST:", s
    trajoptpy.SetInteractive(args.interactive)
    prob = trajoptpy.ConstructProblem(s, env)
    result = trajoptpy.OptimizeProblem(prob)
def RosTransformFromRaveMatrix(mat):
    out = gm.Transform()
    out.translation = gm.Vector3(*mat[:3,3])
    qw,qx,qy,qz = openravepy.quatFromRotationMatrix(mat[:3,:3])
    out.rotation = gm.Quaternion(qx,qy,qz, qw)
    return out
def RosPoseFromRaveMatrix(mat):
    out = gm.Pose()
    out.position = gm.Point(*mat[:3,3])
    qw,qx,qy,qz = openravepy.quatFromRotationMatrix(mat[:3,:3])
    out.orientation = gm.Quaternion(qx,qy,qz, qw)
    return out
예제 #28
0
	#robot.WaitForController(0) # wait
	#raw_input('Hit ENTER to continue.')	

    ##################
    #T_gripper = robot.GetLink(LINK_NAME).GetTransform()
    #T_grasp = T_gripper.copy()
    #T_grasp[:3,3]  += np.array([-0.5,0.2,0])
    #T_grasp = T_grasp.dot(rave.matrixFromAxisAngle([0,0,1],np.pi/2))
    #xyz_targ = T_grasp[:3,3]
    #success = mk.create_cylinder(env, xyz_targ-np.array([.03,0,0]), .02, .5)
    #quat_targ = rave.quatFromRotationMatrix(T_grasp[:3,:3])
    #success = mk.create_cylinder(env, T_gripper[:3,3]-np.array([.1,-.1,0]), .02, .5)

    Tgoal = np.array([[0,0,1,0.6],[0,1,0,0.4],[-1,0,0,0.6],[0,0,0,1]])
    xyz_targ = Tgoal[:3,3]
    quat_targ = rave.quatFromRotationMatrix(Tgoal[:3,:3])

    request = move_arm_to_grasp(xyz_targ, quat_targ, LINK_NAME, MANIP_NAME)

    path_init = np.load("../data/barrett_traj3.npy")
    robot.SetActiveDOFValues(path_init[0,:])

    s = json.dumps(request)
    print "REQUEST:",s
    trajoptpy.SetInteractive(args.interactive) 
    prob = trajoptpy.ConstructProblem(s, env)
    result = trajoptpy.OptimizeProblem(prob)
    
    #stat = trajoptpy.SimulateAndReplan(s, env, 4)
    
#    stats = np.zeros((8,100))
예제 #29
0
파일: point.py 프로젝트: alexlee-gk/trajopt
    manip = robot.GetManipulator(MANIP_NAME)
    viewer = trajoptpy.GetViewer(env)
    viewer.SetCameraTransformation([0,0,20], [0,0,0], [0,1,0])
    ##################
    
    T_gripper = np.eye(4)
    T_gripper[:3,3] = np.array([2,2,0])
    robot.GetLink(LINK_NAME).SetTransform(T_gripper)
    T_grasp = np.eye(4)
    xyz_targ = T_grasp[:3,3]
#    mk.create_mesh_box(env, np.array([5+0.01,0,0]), np.array([0.01,10,0.01]))
    mk.create_mesh_box(env, np.array([3.5,3,0]), np.array([1,1,2]), "box1")
    mk.create_mesh_box(env, np.array([3.5,-0.5,0]), np.array([1,1,2]), "box2")
#    mk.create_mesh_box(env, np.array([3.5,1.25,0]), np.array([0.2,0.2,1]), "box3")
#    mk.create_mesh_box(env, np.array([3.5,2,0]), np.array([0.2,0.2,1]), "box4")
    quat_targ = rave.quatFromRotationMatrix(T_grasp[:3,:3])

    request = move_arm_to_grasp(xyz_targ, quat_targ, LINK_NAME, MANIP_NAME)
    
    ##################
    '''
    # first optimize ignoring collision costs
    all_costs = request["costs"]
    noncollision_costs = [cost for cost in request["costs"] if "collision" not in cost["type"]]
    
    request["costs"] = noncollision_costs
    
    saver = rave.Robot.RobotStateSaver(robot)
    s = json.dumps(request)
    print "REQUEST:",s
    trajoptpy.SetInteractive(False);
예제 #30
0
    def PlanToEndEffectorOffset(self, robot, direction, distance,
                                max_distance=None,
                                position_tolerance=0.01,
                                angular_tolerance=0.15, **kwargs):
        """
        Plan to an end-effector offset with a move-hand-straight constraint.

        This function plans a trajectory to purely translate a hand in the
        given direction as far as possible before collision. Movement less
        than min_distance will return failure. The motion will not also move
        further than max_distance.

        @param robot the robot whose active DOFs will be used
        @param direction unit vector in the direction of motion
        @param distance minimum distance in meters
        @param max_distance maximum distance in meters
        @param position_tolerance constraint tolerance in meters
        @param angular_tolerance constraint tolerance in radians
        @return traj a trajectory following specified direction
        """
        # Plan using the active manipulator.
        with robot.GetEnv():
            manipulator = robot.GetActiveManipulator()
            pose = manipulator.GetEndEffectorTransform()

        # Convert IK endpoint transformation to pose.
        ee_position = pose[0:3, 3].tolist()
        ee_rotation = openravepy.quatFromRotationMatrix(pose).tolist()

        # Create dummy frame that has direction vector along Z.
        z = direction
        y = numpy.cross(z, pose[0:3, 0])  # Cross with EE-frame Y.
        y /= numpy.linalg.norm(y)
        x = numpy.cross(y, z)

        cost_frame = numpy.vstack((x, y, z)).T
        cost_rotation = openravepy.quatFromRotationMatrix(cost_frame).tolist()

        # Construct a planning request with these constraints.
        n_steps = 10
        request = {
            "basic_info": {
                "n_steps": n_steps,
                "manip": "active",
                "start_fixed": True
            },
            "costs": [
                {
                    "type": "joint_vel",
                    "params": {"coeffs": [1]}
                },
                {
                    "type": "collision",
                    "params": {
                        "coeffs": [20],
                        "dist_pen": [0.025]
                    },
                },
                {
                    "type": "pose",
                    "params": {
                        "xyz": [0, 0, 0],
                        "wxyz": cost_rotation,
                        "link": manipulator.GetEndEffector().GetName(),
                        "rot_coeffs": [0, 0, 0],
                        "pos_coeffs": [0, 0, 10]
                    }
                }
            ],
            "constraints": [
                {
                    "type": "pose",
                    "params": {
                        "xyz": ee_position,
                        "wxyz": ee_rotation,
                        "link": manipulator.GetEndEffector().GetName(),
                        # "first_step": 0,
                        # "last_step": n_steps-1,
                    }
                }
            ],
            "init_info": {
                "type": "stationary"
            }
        }

        # Set active DOFs to match active manipulator and plan.
        p = openravepy.KinBody.SaveParameters
        with robot.CreateRobotStateSaver(p.ActiveDOF):
            robot.SetActiveDOFs(manipulator.GetArmIndices())
            return self._Plan(robot, request, **kwargs)
예제 #31
0
def RosPoseFromRaveMatrix(mat):
    out = gm.Pose()
    out.position = gm.Point(*mat[:3, 3])
    qw, qx, qy, qz = openravepy.quatFromRotationMatrix(mat[:3, :3])
    out.orientation = gm.Quaternion(qx, qy, qz, qw)
    return out
예제 #32
0
def quat_from_rot(rot):
    return quatFromRotationMatrix(rot)
예제 #33
0
    def _PlanToIK(self, robot, pose,
                  ranker=None, **kwargs):

        # Plan using the active manipulator.
        with robot.GetEnv():
            manipulator = robot.GetActiveManipulator()

            # Distance from current configuration is default ranking.
            if ranker is None:
                from prpy.ik_ranking import NominalConfiguration
                ranker = NominalConfiguration(manipulator.GetArmDOFValues())

        # Find initial collision-free IK solution.
        from openravepy import (IkFilterOptions,
                                IkParameterization,
                                IkParameterizationType)
        ik_param = IkParameterization(
            pose, IkParameterizationType.Transform6D)
        ik_solutions = manipulator.FindIKSolutions(
            ik_param, IkFilterOptions.CheckEnvCollisions)
        if not len(ik_solutions):
            raise PlanningError('No collision-free IK solution.')

        # Sort the IK solutions in ascending order by the costs returned by the
        # ranker. Lower cost solutions are better and infinite cost solutions
        # are assumed to be infeasible.
        scores = ranker(robot, ik_solutions)
        best_idx = numpy.argmin(scores)
        init_joint_config = ik_solutions[best_idx]

        # Convert IK endpoint transformation to pose.
        goal_position = pose[0:3, 3].tolist()
        goal_rotation = openravepy.quatFromRotationMatrix(pose).tolist()

        # Settings for TrajOpt
        num_steps = 10

        # Construct a planning request with these constraints.
        request = {
            "basic_info": {
                "n_steps": num_steps,
                "manip": "active",
                "start_fixed": True
            },
            "costs": [
                {
                    "type": "joint_vel",
                    "params": {"coeffs": [1]}
                },
                {
                    "type": "collision",
                    "params": {
                        "coeffs": [20],
                        "dist_pen": [0.025]
                    },
                }
            ],
            "constraints": [
                {
                    "type": "pose",
                    "params": {
                        "xyz": goal_position,
                        "wxyz": goal_rotation,
                        "link": manipulator.GetEndEffector().GetName(),
                        "timestep": num_steps-1
                    }
                }
            ],
            "init_info": {
                "type": "straight_line",
                "endpoint": init_joint_config.tolist()
            }
        }

        # Set active DOFs to match active manipulator and plan.
        p = openravepy.KinBody.SaveParameters
        with robot.CreateRobotStateSaver(p.ActiveDOF):
            robot.SetActiveDOFs(manipulator.GetArmIndices())
            return self._Plan(robot, request, **kwargs)
예제 #34
0
    def _PlanToIK(self, robot, robot_checker, pose, ranker=None, **kwargs):
        # Plan using the active manipulator.
        manipulator = robot.GetActiveManipulator()

        # Distance from current configuration is default ranking.
        if ranker is None:
            from prpy.ik_ranking import NominalConfiguration
            ranker = NominalConfiguration(manipulator.GetArmDOFValues())

        # Find initial collision-free IK solution.
        ik_param = IkParameterization(
            pose, IkParameterizationType.Transform6D)
        ik_solutions = manipulator.FindIKSolutions(
            ik_param, IkFilterOptions.CheckEnvCollisions)
        if not len(ik_solutions):
            # Identify collision and raise error.
            self._raiseCollisionErrorForPose(robot, robot_checker, pose)

        # Sort the IK solutions in ascending order by the costs returned by the
        # ranker. Lower cost solutions are better and infinite cost solutions
        # are assumed to be infeasible.
        scores = ranker(robot, ik_solutions)
        best_idx = numpy.argmin(scores)
        init_joint_config = ik_solutions[best_idx]

        # Convert IK endpoint transformation to pose. OpenRAVE operates on
        # GetEndEffectorTransform(), which is equivalent to:
        #
        #   GetEndEffector().GetTransform() * GetLocalToolTransform()
        #
        link_pose = numpy.dot(
            pose, numpy.linalg.inv(
                manipulator.GetLocalToolTransform()))
        goal_position = link_pose[0:3, 3].tolist()
        goal_rotation = openravepy.quatFromRotationMatrix(link_pose).tolist()

        # Settings for TrajOpt
        num_steps = 10

        # Construct a planning request with these constraints.
        request = {
            "basic_info": {
                "n_steps": num_steps
            },
            "costs": [
                {
                    "type": "joint_vel",
                    "params": {"coeffs": [1]}
                },
                {
                    "type": "collision",
                    "params": {
                        "coeffs": [20],
                        "dist_pen": [0.025]
                    },
                }
            ],
            "constraints": [
                {
                    "type": "pose",
                    "params": {
                        "xyz": goal_position,
                        "wxyz": goal_rotation,
                        "link": manipulator.GetEndEffector().GetName(),
                        "timestep": num_steps-1
                    }
                }
            ],
            "init_info": {
                "type": "straight_line",
                "endpoint": init_joint_config.tolist()
            }
        }


        p = openravepy.KinBody.SaveParameters
        with robot.CreateRobotStateSaver(p.ActiveDOF):
            robot.SetActiveDOFs(manipulator.GetArmIndices())
            return self._Plan(robot, robot_checker, request, **kwargs)
예제 #35
0
def quat_from_rot(rot):
  return quatFromRotationMatrix(rot)