class GenerateTraj():
def __init__(self, arm=MyConstants.Arm.Right):
self.arm = arm
self.ravenArm = RavenArm(self.arm)
self.ravenPlanner = RavenPlanner(self.arm)
def generateTraj(self, deltaPose):
startPose = tfx.pose(self.ravenArm.getGripperPose())
deltaPose = tfx.pose(deltaPose)
#code.interact(local=locals())
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}
endPose = Util.endPose(startPose, deltaPose)
#endPose = startPose
#IPython.embed()
jointTraj = self.ravenPlanner.getTrajectoryFromPose(endPose, startJoints=startJoints, n_steps=15)
"""
n_steps = 15
self.ravenPlanner.updateOpenraveJoints(startJoints)
endJointPositions = endJoints.values()
request = Request.joints(n_steps, self.ravenPlanner.manip.GetName(), endJointPositions)
# convert dictionary into json-formatted string
s = json.dumps(request)
# create object that stores optimization problem
prob = trajoptpy.ConstructProblem(s, self.ravenPlanner.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(), self.ravenPlanner.robot):
rospy.loginfo('Trajopt trajectory is not safe. Trajopt failed!')
rospy.loginfo('Still returning trajectory')
#return
IPython.embed()
return self.ravenPlanner.trajoptTrajToDicts(result.GetTraj())
"""
return jointTraj
def __init__(self, arm=MyConstants.Arm.Right):
self.arm = arm
self.ravenArm = RavenArm(self.arm)
self.ravenPlanner = RavenPlanner(self.arm)
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()
