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