コード例 #1
0
def get_random_pose(env, limits, manip, frame_name="r_gripper_tool_frame"):
    robot = env.GetRobots()[0]
    is_success = False
    while is_success is not True:
        #Select which input space
        index = np.random.choice(limits.keys())
        x, y, z = generate_random_xyz(limits[index])

        #simple check for collision with a box
        if check_col_with_box(env, x, y, z):
            continue

        #do IK here
        xyz = [x, y, z]
        quat = [1, 0, 0, 0]  # wxyz
        pose = openravepy.matrixFromPose(np.r_[quat, xyz])
        joint_angle = ku.ik_for_link(
            pose,
            manip,
            frame_name,
            filter_options=openravepy.IkFilterOptions.CheckEnvCollisions)
        if joint_angle is None: continue

        #further check for collision
        robot.SetDOFValues(joint_angle, manip.GetArmIndices())
        if env.CheckCollision(robot) or robot.CheckSelfCollision(): continue

        is_success = True

    xyz = np.array([x, y, z])
    return joint_angle, xyz
コード例 #2
0
def test_plan_to_pose(xyz, xyzw, leftright, robot, initial_state = build_robot_state(), planner_id='', target_link="%s_gripper_tool_frame"):
    manip = robot.GetManipulator(leftright + "arm")
    
    base_pose = initial_state.multi_dof_joint_state.poses[0]
    base_q = base_pose.orientation
    base_p = base_pose.position
    t = rave.matrixFromPose([base_q.w, base_q.x, base_q.y, base_q.z, base_p.x, base_p.y, base_p.z])
    robot.SetTransform(t)

    joint_solutions = ku.ik_for_link(rave.matrixFromPose(np.r_[xyzw[3], xyzw[:3], xyz]), manip, target_link%leftright[0], 1, True)

    if len(joint_solutions) == 0:
        print "pose is not reachable"
        return None

    m = build_joint_request(joint_solutions[0], leftright, robot, initial_state, planner_id=planner_id)

    try:
        response = get_motion_plan(m)
        if args.pause_after_response:
            raw_input("press enter to continue")
    except rospy.ServiceException:
        return dict(returned = False)

    mpr = response.motion_plan_response
    if mpr.error_code.val != 1:
        print "Planner returned error code", mpr.error_code.val
        return dict(returned = True, error_code = mpr.error_code.val, safe=False)
    else:
        traj =  [list(jtp.positions) for jtp in mpr.trajectory.joint_trajectory.points]
        return dict(returned = True, safe = not check_traj(traj, manip, 100), traj = traj, planning_time = mpr.planning_time.to_sec(), error_code = mpr.error_code.val)
コード例 #3
0
    def planViewTarget(self, viewCenter, samplePair, configStart, keepout,
                       maxCSpaceJump, mover, viewEffector, visualize):
        '''Calls trajectory optimization with viewing constraints for a pair of reachable sample positions.
    
    - Input viewCenter: Point targeted for viewing.
    - Input samplePair: A pair of w-space positions critical to viewing the object.
    - Input configStart: The current configuration of the arm.
    - Input keepout: Distance the sensor should keep from the center of the hand (meters).
    - Input maxCSpaceJump: Estimated c-space distance between waypoints in the trajectory (radians).
    - Input mover: ArmMover object for computing IK and checking collisions.
    - Input viewEffector: Name of the sensor link in OpenRAVE.
    - Input cloud: Point cloud (nx3 array) to add as obstacle. Removes before function returns. Ignores if None.
    - Input visualize: True to run the OpenRAVE visualization for the trajopt run.
    - Returns traj: List of 7-DOF configurations according to OpenRAVE ordering.
    - Returns essIdxs: Indices in the trajectory that correspond to beginning, viewing (x2), and ending points.
    - Returns trajData: Metadata for running this algorithm such as time, cost, and trajectory quality.
    '''

        timeStart = time_module.time()

        #import pdb; pdb.set_trace()

        # solve ik the two samples samples
        configsPair = []
        for i in xrange(2):
            T = motion_planner.GeneratePose(samplePair[i], viewCenter)
            solutions = ku.ik_for_link(T, mover.manip, viewEffector, return_all_solns=True, filter_options= \
              openravepy.IkFilterOptions.CheckEnvCollisions)
            if len(solutions) == 0:
                raise Exception(
                    "No IK solutions found for input sample point.")
            configsPair.append(solutions)

        # switch order of pair if 2nd sample is closer
        if motion_planner.ClosestDistance([configStart], configsPair[1]) < \
           motion_planner.ClosestDistance([configStart], configsPair[0]):
            samplePair = (samplePair[1], samplePair[0])
            configsPair = (configsPair[1], configsPair[0])

        # create constraints
        constraints = [None, None]
        for i in xrange(2):
            constraints[i] = ViewConstraint(viewCenter, keepout, mover,
                                            viewEffector)

        # run trajopt
        traj, essIdxs, cost, inCollision, meetsPosition, meetsKeepout, meetsOrient = \
          self.smoothTrajectory(configStart, configsPair, constraints, maxCSpaceJump, mover, viewEffector, visualize)

        # return result
        time = time_module.time() - timeStart
        trajData = TrajectoryData(time, cost, inCollision, meetsPosition,
                                  meetsKeepout, meetsOrient)
        return traj, essIdxs, trajData
コード例 #4
0
ファイル: arm.py プロジェクト: gkahn13/pr2_eih_ros
 def ik(self, pose):
     """
     :param pose: tfx.pose
     :return list of joints
     """
     assert pose.frame.count('base_link') == 1
     
     self.sim.update()
     joints = ku.ik_for_link(np.array(pose.matrix), self.manip, self.tool_frame, 0)
     
     return joints
コード例 #5
0
ファイル: arm.py プロジェクト: olov-andersson/bsp
 def ik(self, pose):
     """
     :param pose: tfx.pose
     :return list of joints
     """
     assert pose.frame.count('base_link') == 1
     
     self.sim.update()
     joints = ku.ik_for_link(np.array(pose.matrix), self.manip, self.tool_frame, 0)
     
     return joints
コード例 #6
0
    def calcIKForTGivenLink(
            self,
            T,
            linkName,
            opts=openravepy.IkFilterOptions.CheckEnvCollisions):
        '''TODO'''

        return ku.ik_for_link(T,
                              self.manip,
                              linkName,
                              return_all_solns=True,
                              filter_options=opts)
コード例 #7
0
def robot_state_from_pose_goal(xyz, xyzw, arm, robot, initial_state = build_robot_state()):
    manip = robot.GetManipulator(arm + "arm")
    joint_solutions = ku.ik_for_link(rave.matrixFromPose(np.r_[xyzw[3], xyzw[:3], xyz]),
                                     manip, "%s_wrist_roll_link"%arm[0], 1, True)

    if len(joint_solutions) == 0:
        raise Exception
    joints = robot.GetJoints()
    joint_inds = robot.GetManipulator("%sarm"%arm).GetArmIndices()
    joint_names = [joints[joint_inds[i]].GetName() for i in xrange(len(joint_solutions[0]))]
    joint_values = [joint_solutions[0][i] for i in xrange(len(joint_solutions[0]))]
    new_state = alter_robot_state(initial_state, joint_names, joint_values)
    return new_state
コード例 #8
0
def GetIKSolutions(env,
                   Tgoal,
                   manip=None,
                   getAll=True):  #manip is OpenRAVE manip
    sol = None
    if (env == None or robot == None or manip == None):
        print('\n Error: Undefined env, robot, or manip \n')
        return sol

    with env:
        #sol = manip.FindIKSolutions(Tgoal, IkFilterOptions.CheckEnvCollisions) # get collision-free solution
        if (getAll):
            sol = ku.ik_for_link(
                Tgoal,
                manip,
                manip.GetEndEffector().GetName(),
                filter_options=openravepy.IkFilterOptions.CheckEnvCollisions,
                return_all_solns=True)
        else:
            sol = manip.FindIKSolution(Tgoal,
                                       IkFilterOptions.CheckEnvCollisions)
    return sol
コード例 #9
0
# cc = trajoptpy.GetCollisionChecker(env)
# root = ET.parse("/opt/ros/groovy/share/pr2_moveit_config/config/pr2.srdf").getroot()
# disabled_elems=root.findall("disable_collisions")
# for elem in disabled_elems:
#     linki = robot.GetLink(elem.get("link1"))
#     linkj = robot.GetLink(elem.get("link2"))
#     if linki and linkj:
#          cc.ExcludeCollisionPair(linki, linkj)

xs, ys, zs = np.mgrid[.35:.85:.05, 0:.5:.05, .8:.9:.1]
wxyz = [1, 0, 0, 0]
results = []
for (x, y, z) in zip(xs.flat, ys.flat, zs.flat):
    wxyzxyz = np.r_[wxyz, x, y, z]
    joint_solutions = ku.ik_for_link(openravepy.matrixFromPose(wxyzxyz),
                                     robot.GetManipulator(manip),
                                     "%s_gripper_tool_frame" % manip[0], 1,
                                     True)
    if len(joint_solutions) > 0:
        target_joints = joint_solutions[0]
        results.append(
            plan(robot, manip, target_joints,
                 wxyzxyz if args.pose_goal else None))

success_frac = np.mean([result.success for result in results])
print "success frac:", success_frac
print "avg total time:", np.mean([result.t_total for result in results])
print "avg optimization time:", np.mean([result.t_opt for result in results])
print "avg verification time:", np.mean(
    [result.t_verify for result in results])
コード例 #10
0
robot = env.GetRobots()[0]
manip = robot.GetManipulator("rightarm")
robot.SetActiveManipulator(manip)
ikmodel = openravepy.databases.inversekinematics.InverseKinematicsModel(
    robot, iktype=openravepy.IkParameterization.Type.Transform6D)
if not ikmodel.load():
    ikmodel.autogenerate()

xyz_target = [.6, -.6, 1]

n_steps = 15
hmat_target = openravepy.matrixFromPose(np.r_[(0, 1, 0, 0), xyz_target])

init_joint_target = ku.ik_for_link(
    hmat_target,
    manip,
    "r_gripper_tool_frame",
    filter_options=openravepy.IkFilterOptions.CheckEnvCollisions)

request = {
    "basic_info": {
        "n_steps": n_steps,
        "manip": "rightarm",
        "start_fixed": True
    },
    "costs": [{
        "type": "joint_vel",
        "params": {
            "coeffs": [1]
        }
    }, {
コード例 #11
0
    new_head_transform[0, 3] += 0.2
    new_head_transform[2, 3] -= 0.1

    baxter = env.GetRobots()[1]
    b_manip = baxter.SetActiveManipulator("rightarm")
    b_sol = []

    goggles_trans = np.array(
        [[-6.12323400e-17, -7.49879891e-33, 1.00000000e+00, 1.15732000e+00],
         [1.22464680e-16, -1.00000000e+00, 0.00000000e+00, 0.00000000e+00],
         [1.00000000e+00, 1.22464680e-16, 6.12323400e-17, 7.32410000e-01],
         [0.00000000e+00, 0.00000000e+00, 0.00000000e+00, 1.00000000e+00]])
    with env:
        b_sol = ku.ik_for_link(
            goggles_trans,
            b_manip,
            b_manip.GetEndEffector().GetName(),
            filter_options=IkFilterOptions.IgnoreEndEffectorCollisions,
            return_all_solns=True)
    if (len(b_sol) > 0):
        baxter.SetActiveDOFs(b_manip.GetArmIndices())
        baxter.SetActiveDOFValues(b_sol[0])

    # sol = []
    # with env:
    #     sol = ku.ik_for_link(new_head_transform, manip, manip.GetEndEffector().GetName(),
    #                          filter_options = IkFilterOptions.IgnoreSelfCollisions | IkFilterOptions.CheckEnvCollisions,
    #                          return_all_solns = True)
    #     print "solutions",sol
    # for s in sol:
    #     with env:
    #         robot.SetActiveDOFs(manip.GetArmIndices())
コード例 #12
0
ファイル: point_cloud_test.py プロジェクト: hcrlab/push_pull
    print "Joint start: " + str(joint_start)
    robot.SetDOFValues(joint_start) # robot.GetManipulator('rightarm').GetArmIndices())
    time.sleep(3)

    roll = 0.0
    pitch = 0.0
    yaw = 0
    quat = tf.transformations.quaternion_from_euler(roll, pitch, yaw)
    quat_target = [quat[3], quat[0], quat[1], quat[2]] # wxyz
    xyz_target = [0.5, -0.2, 0.5]
    hmat_target = openravepy.matrixFromPose( np.r_[quat_target, xyz_target] )

    # BEGIN ik
    init_joint_target = None
    manip = robot.GetManipulator("rightarm")
    init_joint_target = ku.ik_for_link(hmat_target, manip, "r_gripper_tool_frame",
            filter_options = openravepy.IkFilterOptions.CheckEnvCollisions)

    tries = 0
    while ((init_joint_target == None) and (tries < 100)):
        tries = tries + 1
        xyz_target[0] = xyz_target[0] + random.uniform(-0.05, 0.05)
        xyz_target[1] = xyz_target[1] + random.uniform(-0.05, 0.05)
        xyz_target[2] = xyz_target[2] + random.uniform(-0.05, 0.05)
        init_joint_target = ku.ik_for_link(hmat_target, manip, "r_gripper_tool_frame",
            filter_options = openravepy.IkFilterOptions.CheckEnvCollisions)

    print "Final target: " + str(xyz_target) + " Took " + str(tries) + " tries."
    # END ik
    print "Target: " + str(init_joint_target)
    robot.SetDOFValues(init_joint_target, robot.GetManipulator('rightarm').GetArmIndices())
    time.sleep(3)
コード例 #13
0
ファイル: benchmark.py プロジェクト: Hongxiao321321/trajopt-1
# import xml.etree.ElementTree as ET
# cc = trajoptpy.GetCollisionChecker(env)
# root = ET.parse("/opt/ros/groovy/share/pr2_moveit_config/config/pr2.srdf").getroot()
# disabled_elems=root.findall("disable_collisions")
# for elem in disabled_elems:
#     linki = robot.GetLink(elem.get("link1"))
#     linkj = robot.GetLink(elem.get("link2"))
#     if linki and linkj:
#          cc.ExcludeCollisionPair(linki, linkj)



xs, ys, zs = np.mgrid[.35:.85:.05, 0:.5:.05, .8:.9:.1]
wxyz = [1,0,0,0]
results = []
for (x,y,z) in zip(xs.flat, ys.flat, zs.flat):
    wxyzxyz = np.r_[wxyz, x,y,z]
    joint_solutions = ku.ik_for_link(openravepy.matrixFromPose(wxyzxyz), robot.GetManipulator(manip), "%s_gripper_tool_frame"%manip[0], 1, True)
    if len(joint_solutions) > 0:
        target_joints = joint_solutions[0]
        results.append( plan(robot, manip, target_joints, wxyzxyz if args.pose_goal else None) )

success_frac = np.mean([result.success for result in results])
print "success frac:", success_frac
print "avg total time:", np.mean([result.t_total for result in results])
print "avg optimization time:", np.mean([result.t_opt for result in results])
print "avg verification time:", np.mean([result.t_verify for result in results])


  
コード例 #14
0
robot.SetDOFValues([GRIPPER_OPEN_CODE], manip.GetGripperIndices())

ikmodel = databases.inversekinematics.InverseKinematicsModel(
    robot, iktype=IkParameterization.Type.Transform6D)
if not ikmodel.load():
    ikmodel.autogenerate()
ikmodel.setrobot()

vec_target = [0.125, -0.0274, 0.990, 0.052, -0.078, -0.898, 0.263]
quat_target, xyz_target = vec_target[:4], vec_target[4:]
mat_target = matrixFromPose(vec_target)

manip = robot.GetManipulator('grip')
init_joint_target = ku.ik_for_link(
    mat_target,
    manip,
    "Palm",
    filter_options=IkFilterOptions.CheckEnvCollisions)
request = {
    "basic_info": {
        "n_steps": 10,
        "manip": "grip",
        "start_fixed": True
    },
    "costs": [{
        "type": "joint_vel",
        "params": {
            "coeffs": [1]
        }
    }, {
        "type": "collision",
コード例 #15
0
ファイル: arm_interface.py プロジェクト: gkahn13/bsp
    def goToArmPose(self, pose, isPlanned, reqName=Constants.Request.noRequest):
        """
        Go to PoseStamped pose

        If isPlanned is True, then trajopt is used to plan the trajectory
        Otherwise, just IK is used to plan trajectory

        If IK fails, arm doesn't move
        
        The methods for executing the trajectories are in pr2/pr2.py
        """
        # must convert to BaseLink frame
        if self.listener != None:
            try:
                commonTime = self.listener.getLatestCommonTime(Constants.BaseLink,pose.header.frame_id)
                pose.header.stamp = commonTime
                pose = self.listener.transformPose(Constants.BaseLink,pose)
            except tf.Exception:
                return
            
        if pose.header.frame_id != Constants.BaseLink:
            return

        # get the current joint state for joint_names
        rospy.wait_for_service("return_joint_states")
        s = rospy.ServiceProxy("return_joint_states", ReturnJointStates)
        resp = s(self.joint_names)
        
        # set the start joint position
        joint_start = resp.position
        self.robot.SetDOFValues(joint_start, self.robot.GetManipulator(self.armName).GetArmIndices())

        # initialize trajopt inputs
        quat = pose.pose.orientation
        xyz = pose.pose.position
        quat_target = [quat.w, quat.x, quat.y, quat.z]
        xyz_target = [xyz.x, xyz.y, xyz.z]
        hmat_target = openravepy.matrixFromPose( np.r_[quat_target, xyz_target] )
        
        # if no planning
        if not isPlanned:
            self.arm.goto_pose_matrix(hmat_target, Constants.BaseLink, self.toolframe)
            return True

        # inverse kinematics
        manip = self.robot.GetManipulator(self.armName)
        init_joint_target = ku.ik_for_link(hmat_target, manip, self.toolframe, filter_options = openravepy.IkFilterOptions.CheckEnvCollisions)
        
        if init_joint_target == None:
            # inverse kinematics failed
            # will do nothing for now, may want to alter xyz_target a little
            rospy.loginfo('IK failed')
            return False




        request = self.getRequest(reqName, xyz_target, quat_target, init_joint_target)
        
        if request == None:
            return
               
        # convert dictionary into json-formatted string
        s = json.dumps(request) 
        # create object that stores optimization problem
        prob = trajoptpy.ConstructProblem(s, self.env)
        # do optimization
        result = trajoptpy.OptimizeProblem(prob)

        self.arm.follow_joint_trajectory(result.GetTraj())

        return True
コード例 #16
0
def grab_test(tb, robot, env, human, goggles, forehead):

    h = human.GetTransform()
    h[0, 3] -= 0.5
    t = goggles.GetTransform()
    t[0, 3] -= 0.5
    f = forehead.GetTransform()
    f[0, 3] -= 0.5

    with env:
        human.SetTransform(h)
        goggles.SetTransform(t)
        forehead.SetTransform(f)

    t = goggles.GetTransform()
    t[0, 3] -= 0.1
    handles.append(
        env.plot3(points=t[0:3, 3], pointsize=5.0, colors=array(((0, 0, 1)))))
    rot_mat = np.array([[np.cos(np.pi), -np.sin(np.pi), 0],
                        [np.sin(np.pi), np.cos(np.pi), 0], [0, 0, 1]])
    t[0:3, 0:3] = np.dot(t[0:3, 0:3], rot_mat)
    direction = [0, 1, 0]
    angle = np.pi * 0.5
    rot_mat = transformations.rotation_matrix(angle, direction)
    t[0:3, 0:3] = np.dot(rot_mat[0:3, 0:3], t[0:3, 0:3])
    ikSolutions = ku.ik_for_link(
        t,
        tb.r_manip,
        tb.r_manip.GetEndEffector().GetName(),
        filter_options=openravepy.IkFilterOptions.CheckEnvCollisions,
        return_all_solns=True)

    from_vec = t[0:3, 3]

    to_vec_1 = from_vec + 0.05 * (t[0:3, 0])
    to_vec_2 = from_vec + 0.05 * (t[0:3, 1])
    to_vec_3 = from_vec + 0.05 * (t[0:3, 2])

    handles.append(env.drawarrow(from_vec, to_vec_1, 0.005, [1, 0, 0]))
    handles.append(env.drawarrow(from_vec, to_vec_2, 0.005, [0, 1, 0]))
    handles.append(env.drawarrow(from_vec, to_vec_3, 0.005, [0, 0, 1]))

    raw_input("Press enter to move to ready...")

    print len(ikSolutions)

    with env:
        robot.SetActiveManipulator(tb.r_manip)
        robot.SetActiveDOFs(tb.r_manip.GetArmIndices())
        robot.SetActiveDOFValues(ikSolutions[0])

    raw_input("Press enter to grab...")

    with env:
        robot.Grab(env.GetKinBody('goggles'))

    # # tb.TestInterp()
    raw_input("Press enter to continue...")

    tuckarms(env, robot)

    raw_input("Press enter to continue...")
コード例 #17
0
    def goToArmPose(self, lr, pose):

        body = rave.RaveCreateKinBody(self.env,'')
        body.SetName('testbody')
        body.InitFromBoxes(np.array([[0,0,0,20,20,pose.pose.position.z - .05]]),True) # set geometry as one box of extents 0.1, 0.2, 0.3
        #self.env.Add(body,True)
        print(pose.pose.position.z)

        if lr == 'l':
            arm = self.larm
            armName = 'leftarm'
        else:
            arm = self.rarm
            armName = 'rightarm'

        joint_names = [lr+suffix for suffix in self.joint_name_suffixes]
        toolframe = lr + self.tool_frame_suffix
        
        rospy.wait_for_service("return_joint_states")
        s = rospy.ServiceProxy("return_joint_states", ReturnJointStates)
        resp = s(joint_names)
        #print(pplist(resp.position))
        
        joint_start = resp.position
        self.robot.SetDOFValues(joint_start, self.robot.GetManipulator(armName).GetArmIndices())

        quat = pose.pose.orientation
        xyz = pose.pose.position
        quat_target = [quat.w, quat.x, quat.y, quat.z]
        xyz_target = [xyz.x, xyz.y, xyz.z]
        elbow_target = [0, 0, xyz.z + .1]
        #quat_target = [1,0,0,0] # wxyz
        #xyz_target = [6.51073449e-01,  -1.87673551e-01, 4.91061915e-01]
        hmat_target = openravepy.matrixFromPose( np.r_[quat_target, xyz_target] )

# BEGIN ik
        manip = self.robot.GetManipulator(armName)
        init_joint_target = ku.ik_for_link(hmat_target, manip, toolframe, filter_options = openravepy.IkFilterOptions.CheckEnvCollisions)
        # if init_joint_target is NONE, return failure. or try a nearby point
# END ik
        n_steps = 10

        request = {
            "basic_info" : {
                "n_steps" : n_steps,
                "manip" : armName, 
                "start_fixed" : True 
                },
            "costs" : [
                {
                    "type" : "joint_vel",
                    "params": {"coeffs" : [1]} 
                    },
                {
                    "type" : "collision",
                    "params" : {
                        "coeffs" : [20],
                        "dist_pen" : [0.025] 
                        }
                    },
                {
                    "type" : "pose",
                    "params" : {
                        "coeffs" : [20],
                        "xyz" : elbow_target,
                        "wxyz" : quat_target,
                        "link": "l_elbow_flex_link",
                        "rot_coeffs" : [0,0,0],
                        "pos_coeffs" : [0,0,1]
                        }
                    },
                ],
            "constraints" : [
                # BEGIN pose_target
                {
                    "type" : "pose",
                    "name" : "target_pose",
                    "params" : {"xyz" : xyz_target, 
                                "wxyz" : quat_target,  # unused
                                "link": toolframe,
                                "rot_coeffs" : [1,1,0],
                                "pos_coeffs" : [1,1,1]
                                }
                    
                    },
                #END pose_target

                ],
            "init_info" : {
                "type" : "straight_line",
                "endpoint" : init_joint_target.tolist()
                }
            }

        """
        request = {
            "basic_info" : {
                "n_steps" : 10,
                "manip" : armName, # see below for valid values
                "start_fixed" : True # 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
                    # Also valid: "coeffs" : [7,6,5,4,3,2,1]
                    },
                {
                    "type" : "continuous_collision",
                    "name" :"cont_coll", # shorten name so printed table will be prettier
                    "params" : {
                        "coeffs" : [20], # 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
                        }
                    }
                ],
            "constraints" : [
                # BEGIN pose_constraint
                {
                    "type" : "pose", 
                    "params" : {"xyz" : xyz_target, 
                                "wxyz" : quat_target, 
                                "link": toolframe,
                                # "timestep" : 9
                                # omitted because timestep = n_steps-1 is default
                                # "pos_coeffs" : [1,1,1], # omitted because that's default
                                "pos_coeffs" : [1,1,1],
                                "rot_coeffs" : [1,1,1]
                                }


                    
                    } ,
                # 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, self.env) # create object that stores optimization problem
        result = trajoptpy.OptimizeProblem(prob) # do optimization
        #print(result.GetTraj())
        arm.follow_joint_trajectory(result.GetTraj())
コード例 #18
0
ファイル: trajopt_server.py プロジェクト: hcrlab/push_pull
    
    joint_start = resp.position

    print "Joint start: " + str(joint_start)
    robot.SetDOFValues(joint_start) # robot.GetManipulator('rightarm').GetArmIndices())
    #time.sleep(3)

    quat_target = [req.goal.pose.orientation.w, req.goal.pose.orientation.x, req.goal.pose.orientation.y, req.goal.pose.orientation.z] # wxyz
    xyz_target = [req.goal.pose.position.x, req.goal.pose.position.y, req.goal.pose.position.z]
    hmat_target = openravepy.matrixFromPose( np.r_[quat_target, xyz_target] )

    # BEGIN ik
    init_joint_target = None
    manip = robot.GetManipulator("rightarm")
    init_joint_target = ku.ik_for_link(hmat_target, manip, "r_gripper_tool_frame")

    tries = 0
    """
    while ((init_joint_target == None) and (tries < 100)):
        tries = tries + 1
        xyz_target[0] = xyz_target[0] + random.uniform(-0.05, 0.05)
        xyz_target[1] = xyz_target[1] + random.uniform(-0.05, 0.05)
        xyz_target[2] = xyz_target[2] + random.uniform(-0.05, 0.05)
        init_joint_target = ku.ik_for_link(hmat_target, manip, "r_gripper_tool_frame",
            filter_options = openravepy.IkFilterOptions.CheckEnvCollisions)
    """

    print "Final target: " + str(xyz_target) + " Took " + str(tries) + " tries."
    # END ik
    print "Target: " + str(init_joint_target)
コード例 #19
0
ファイル: arm_interface.py プロジェクト: olov-andersson/bsp
    def goToArmPose(self,
                    pose,
                    isPlanned,
                    reqName=Constants.Request.noRequest):
        """
        Go to PoseStamped pose

        If isPlanned is True, then trajopt is used to plan the trajectory
        Otherwise, just IK is used to plan trajectory

        If IK fails, arm doesn't move
        
        The methods for executing the trajectories are in pr2/pr2.py
        """
        # must convert to BaseLink frame
        if self.listener != None:
            try:
                commonTime = self.listener.getLatestCommonTime(
                    Constants.BaseLink, pose.header.frame_id)
                pose.header.stamp = commonTime
                pose = self.listener.transformPose(Constants.BaseLink, pose)
            except tf.Exception:
                return

        if pose.header.frame_id != Constants.BaseLink:
            return

        # get the current joint state for joint_names
        rospy.wait_for_service("return_joint_states")
        s = rospy.ServiceProxy("return_joint_states", ReturnJointStates)
        resp = s(self.joint_names)

        # set the start joint position
        joint_start = resp.position
        self.robot.SetDOFValues(
            joint_start,
            self.robot.GetManipulator(self.armName).GetArmIndices())

        # initialize trajopt inputs
        quat = pose.pose.orientation
        xyz = pose.pose.position
        quat_target = [quat.w, quat.x, quat.y, quat.z]
        xyz_target = [xyz.x, xyz.y, xyz.z]
        hmat_target = openravepy.matrixFromPose(np.r_[quat_target, xyz_target])

        # if no planning
        if not isPlanned:
            self.arm.goto_pose_matrix(hmat_target, Constants.BaseLink,
                                      self.toolframe)
            return True

        # inverse kinematics
        manip = self.robot.GetManipulator(self.armName)
        init_joint_target = ku.ik_for_link(
            hmat_target,
            manip,
            self.toolframe,
            filter_options=openravepy.IkFilterOptions.CheckEnvCollisions)

        if init_joint_target == None:
            # inverse kinematics failed
            # will do nothing for now, may want to alter xyz_target a little
            rospy.loginfo('IK failed')
            return False

        request = self.getRequest(reqName, xyz_target, quat_target,
                                  init_joint_target)

        if request == None:
            return

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

        self.arm.follow_joint_trajectory(result.GetTraj())

        return True
コード例 #20
0
 def ikfunc(hmat):
     return ku.ik_for_link(hmat, manip, link_name, return_all_solns=True)
コード例 #21
0
robot.SetDOFValues(joint_start, manip.GetArmIndices())
robot.SetDOFValues([GRIPPER_OPEN_CODE], manip.GetGripperIndices())

ikmodel = databases.inversekinematics.InverseKinematicsModel(robot,iktype=IkParameterization.Type.Transform6D)
if not ikmodel.load():
	ikmodel.autogenerate()
ikmodel.setrobot()



vec_target = [0.125, -0.0274, 0.990, 0.052, -0.078, -0.898, 0.263]
quat_target, xyz_target = vec_target[:4], vec_target[4:]
mat_target = matrixFromPose(vec_target)

manip = robot.GetManipulator('grip')
init_joint_target = ku.ik_for_link(mat_target, manip, "Palm",
    filter_options = IkFilterOptions.CheckEnvCollisions)
request = {
  "basic_info" : {
    "n_steps" : 10,
    "manip" : "grip",
    "start_fixed" : True
  },
  "costs" : [
  {
    "type" : "joint_vel",
    "params": {"coeffs" : [1]}
  },
  {
    "type" : "collision",
    "name" : "cont_coll",
    "params" : {