def testFK(): rospy.init_node('test_IK',anonymous=True) rp = RavenPlanner(MyConstants.Arm.Both) rp.env.SetViewer('qtcoin') rospy.sleep(2) leftPose = rp.getCurrentPose(MyConstants.Arm.Left) rightPose = rp.getCurrentPose(MyConstants.Arm.Right) leftGrasp = rp.getCurrentGrasp(MyConstants.Arm.Left) rightGrasp = rp.getCurrentGrasp(MyConstants.Arm.Right) leftJoints = invArmKin(MyConstants.Arm.Left, leftPose, leftGrasp) rightJoints = invArmKin(MyConstants.Arm.Right, rightPose, rightGrasp) rp.updateOpenraveJoints(MyConstants.Arm.Left, joints1=leftJoints, grasp=leftGrasp) rp.updateOpenraveJoints(MyConstants.Arm.Right, joints1=rightJoints, grasp=rightGrasp) raveLeftMatrix = Util.openraveTransformFromTo(rp.robot, np.eye(4), MyConstants.Frames.LeftTool, MyConstants.Frames.Link0) raveRightMatrix = Util.openraveTransformFromTo(rp.robot, np.eye(4), MyConstants.Frames.RightTool, MyConstants.Frames.Link0) raveLeftPose = tfx.pose(raveLeftMatrix,frame=leftPose.frame) raveRightPose = tfx.pose(raveRightMatrix,frame=rightPose.frame) deltaLeft = Util.deltaPose(leftPose, raveLeftPose) deltaRight = Util.deltaPose(rightPose, raveRightPose) g = [] #g += Util.plot_transform(rp.env, Util.openraveTransformFromTo(rp.robot, np.eye(4), MyConstants.Frames.LeftTool, 'world')) #g += Util.plot_transform(rp.env, Util.openraveTransformFromTo(rp.robot, np.eye(4), MyConstants.Frames.RightTool, 'world')) g += Util.plot_transform(rp.env, Util.openraveTransformFromTo(rp.robot, leftPose.matrix, leftPose.frame[1:], 'world')) g += Util.plot_transform(rp.env, Util.openraveTransformFromTo(rp.robot, rightPose.matrix, rightPose.frame[1:], 'world')) IPython.embed()
def test(): rospy.init_node('test_IK',anonymous=True) rp = RavenPlanner(Constants.Arm.Right) rospy.sleep(2) startJoints = {0:0.51091998815536499, 1:1.6072717905044558, 2:-0.049991328269243247, 4:0.14740140736103061, 5:0.10896652936935426, 8:-0.31163200736045837} endJoints = {0:0.45221099211619786, 1:2.2917657932075581, 2:-0.068851854076958902, 4:0.44096283117933965, 5:0.32085205361054148, 8:-0.82079765727524379} rp.updateOpenraveJoints(endJoints) endEE = rp.manip.GetEndEffectorTransform() rp.updateOpenraveJoints(startJoints) startEE = rp.manip.GetEndEffectorTransform() rp.env.SetViewer('qtcoin') endJointPositions = [] for raveJointType in rp.manip.GetArmIndices(): rosJointType = rp.raveJointTypesToRos[raveJointType] endJointPositions.append(endJoints[rosJointType]) n_steps = 15 endEEPose = tfx.pose(endEE) desPos = endEEPose.position.list endEEQuat = endEEPose.orientation wxyzQuat = [endEEQuat.w, endEEQuat.x, endEEQuat.y, endEEQuat.z] request = { "basic_info" : { "n_steps" : n_steps, "manip" : rp.manip.GetName(), "start_fixed" : True }, "costs" : [ { "type" : "joint_vel", "params": {"coeffs" : [1]} }, { "type" : "collision", "params" : { "coeffs" : [100], "continuous" : True, "dist_pen" : [0.001] } }, ], "constraints" : [ { "type" : "pose", "name" : "target_pose", "params" : {"xyz" : desPos, "wxyz" : wxyzQuat, "link" : rp.toolFrame, "rot_coeffs" : [5,5,5], "pos_coeffs" : [100,100,100] } } ], "init_info" : { "type" : "straight_line", "endpoint" : endJointPositions } } # convert dictionary into json-formatted string s = json.dumps(request) # create object that stores optimization problem prob = trajoptpy.ConstructProblem(s, rp.env) # do optimization result = trajoptpy.OptimizeProblem(prob) # check trajectory safety from trajoptpy.check_traj import traj_is_safe prob.SetRobotActiveDOFs() if not traj_is_safe(result.GetTraj(), rp.robot): rospy.loginfo('Trajopt trajectory is not safe. Trajopt failed!') rospy.loginfo('Still returning trajectory') #return j = rp.trajoptTrajToDicts(result.GetTraj()) Util.plot_transform(rp.env, startEE) Util.plot_transform(rp.env, endEE) IPython.embed()
def testRavenCpp(): trajoptpy.SetInteractive(True) rospy.init_node('testRavenCpp',anonymous=True) arm = MyConstants.Arm.Right rp = RavenPlanner([arm],thread=True) manip = rp.robot.GetManipulators()[1] #rp.env.SetViewer('qtcoin') #endJoints = [0.5672114866971969, 1.5318772029876708, -0.12326233461499214, -0.18351784765720369, -0.55512279719114299, -0.10556840419769287] #rp.robot.SetActiveDOFValues(endJoints) startJoints = [0.15336623787879944, 1.5377614498138428, -0.11949782818555832, -0.23602290451526642, -0.14983920753002167, -0.10172462463378906] rp.robot.SetActiveDOFValues(startJoints) #print('press enter') #raw_input() startPose = tfx.pose(Util.openraveTransformFromTo(rp.robot, np.eye(4), 'tool_R', '0_link'),frame=MyConstants.Frames.Link0) endPose = tfx.pose(startPose + [0,-.1,0]) raveStartEE = manip.GetEndEffectorTransform() raveEndEE = np.array(raveStartEE) raveEndEE[0,3] += .1 g = [] g += Util.plot_transform(rp.env, raveStartEE) g += Util.plot_transform(rp.env, raveEndEE) box = rave.RaveCreateKinBody(rp.env,'') box.SetName('testbox') box.InitFromBoxes(np.array([[0,0,0,0.01,0.04,0.015]]),True) #code.interact(local=locals()) ee = .5*(raveStartEE + raveEndEE) ee[:3,:3] = np.identity(3) box.SetTransform(ee) rp.env.Add(box,True) IPython.embed() rp.getTrajectoryFromPose(arm, endPose, startPose=startPose, n_steps=50) print('waiting...') rp.waitForTrajReady() print('traj ready!') rightTraj = rp.jointTraj[MyConstants.Arm.Right] for right in rightTraj: if rospy.is_shutdown(): break rp.updateOpenraveJoints('R', right) rospy.loginfo('Press enter to go to next step') raw_input() rospy.spin()