def trajopt_plan(self,
target_pose,
json_config_str=None,
json_config_name=None,
target_joints=None):
with self.lock:
if (json_config_str is None and json_config_name is not None):
json_config_str = self.load_json_config(json_config_name)
robot = self.controller_commander.rox_robot
#vel_upper_lim = np.array(robot.joint_vel_limit) * speed_scalar
#vel_lower_lim = -vel_upper_lim
#joint_lower_limit = np.array(robot.joint_lower_limit)
#joint_upper_limit = np.array(robot.joint_upper_limit)
joint_names = robot.joint_names
joint_positions = self.controller_commander.get_current_joint_values(
)
if target_pose is not None:
p = PoseArray()
p.header.frame_id = "world"
p.header.stamp = rospy.Time.now()
p.poses.append(
rox_msg.transform2pose_msg(
self.controller_commander.compute_fk(joint_positions)))
p.poses.append(rox_msg.transform2pose_msg(target_pose))
self.waypoint_plotter.publish(p)
self.tesseract_env.setState(joint_names, joint_positions)
init_pos = self.tesseract_env.getCurrentJointValues()
self.tesseract_plotter.plotTrajectory(
self.tesseract_env.getJointNames(),
np.reshape(init_pos, (1, 6)))
planner = tesseract.TrajOptPlanner()
manip = "move_group"
end_effector = "vacuum_gripper_tool"
pci = tesseract.ProblemConstructionInfo(self.tesseract_env)
pci.fromJson(json_config_str)
pci.kin = self.tesseract_env.getManipulator(manip)
pci.init_info.type = tesseract.InitInfo.STATIONARY
#pci.init_info.dt=0.5
if target_pose is not None:
#Final target_pose constraint
pose_constraint = tesseract.CartPoseTermInfo()
pose_constraint.term_type = tesseract.TT_CNT
pose_constraint.link = end_effector
pose_constraint.timestep = pci.basic_info.n_steps - 1
q = rox.R2q(target_pose.R)
pose_constraint.wxyz = np.array(q)
pose_constraint.xyz = np.array(target_pose.p)
pose_constraint.pos_coefs = np.array(
[1000000, 1000000, 1000000], dtype=np.float64)
pose_constraint.rot_coefs = np.array([10000, 10000, 10000],
dtype=np.float64)
pose_constraint.name = "final_pose"
pci.cnt_infos.push_back(pose_constraint)
elif target_joints is not None:
joint_constraint = tesseract.JointPosTermInfo()
joint_constraint.term_type = tesseract.TT_CNT
joint_constraint.link = end_effector
joint_constraint.first_step = pci.basic_info.n_steps - 2
joint_constraint.last_step = pci.basic_info.n_steps - 1
#joint_constraint.coeffs = tesseract.DblVec([10000]*6)
joint_constraint.targets = tesseract.DblVec(
list(target_joints))
print target_joints
pci.cnt_infos.push_back(joint_constraint)
else:
assert False
prob = tesseract.ConstructProblem(pci)
config = tesseract.TrajOptPlannerConfig(prob)
config.params.max_iter = 1000
planning_response = planner.solve(config)
if (planning_response.status_code != 0):
raise Exception(
"TrajOpt trajectory planning failed with code: %d" %
planning_response.status_code)
self.tesseract_plotter.plotTrajectory(
self.tesseract_env.getJointNames(),
planning_response.trajectory[:, 0:6])
jt = JointTrajectory()
jt.header.stamp = rospy.Time.now()
jt.joint_names = joint_names
trajectory_time = np.cumsum(1.0 /
planning_response.trajectory[:, 6])
trajectory_time = trajectory_time - trajectory_time[0]
for i in xrange(planning_response.trajectory.shape[0]):
jt_p = JointTrajectoryPoint()
jt_p.time_from_start = rospy.Duration(trajectory_time[i])
jt_p.positions = planning_response.trajectory[i, 0:6]
jt.points.append(jt_p)
return jt
viewer = TesseractViewer() viewer.update_environment(t_env, [0,0,0]) joint_names = ["joint_%d" % (i+1) for i in range(6)] viewer.update_joint_positions(joint_names, np.array([1,-.2,.01,.3,-.5,1])) viewer.start_serve_background() t_env.setState(joint_names, np.ones(6)*0.1) #while True: # time.sleep(2) # viewer.update_joint_positions(joint_names, np.random.rand(6)*.4) pci = tesseract.ProblemConstructionInfo(t) pci.init_info.type = tesseract.InitInfo.STATIONARY #pci.init_info.data = np.array([0.0,0,0,0,0,0]) pci.basic_info.n_steps = 10 pci.basic_info.manip = "manipulator" pci.basic_info.start_fixed = False pci.basic_info.use_time = False pci.basic_info.dt_upper_lim = 1 pci.basic_info.dt_lower_lim = 0.9999 pci.opt_info.max_iter = 200 pci.opt_info.min_approx_improve = 1e-3 pci.opt_info.min_trust_box_size = 1e-3
def trajopt_smooth_trajectory(self,
trajectory_in,
json_config_str=None,
json_config_name=None):
with self.lock:
if (json_config_str is None and json_config_name is not None):
json_config_str = self.load_json_config(json_config_name)
robot = self.controller_commander.rox_robot
#vel_upper_lim = np.array(robot.joint_vel_limit) * speed_scalar
#vel_lower_lim = -vel_upper_lim
#joint_lower_limit = np.array(robot.joint_lower_limit)
#joint_upper_limit = np.array(robot.joint_upper_limit)
joint_names = trajectory_in.joint_names
joint_positions = trajectory_in.points[0].positions
self.tesseract_env.setState(joint_names, joint_positions)
init_pos = self.tesseract_env.getCurrentJointValues()
self.tesseract_plotter.plotTrajectory(
self.tesseract_env.getJointNames(),
np.reshape(init_pos, (1, 6)))
planner = tesseract.TrajOptPlanner()
manip = "move_group"
end_effector = "vacuum_gripper_tool"
pci = tesseract.ProblemConstructionInfo(self.tesseract_env)
pci.fromJson(json_config_str)
pci.kin = self.tesseract_env.getManipulator(manip)
pci.init_info.type = tesseract.InitInfo.STATIONARY
#pci.init_info.dt=0.5
for i in xrange(14):
joint_constraint = tesseract.JointPosTermInfo()
joint_constraint.targets = tesseract.DblVec(
trajectory_in.points[i].positions)
joint_constraint.first_step = i * 10
joint_constraint.last_step = i * 10
joint_constraint.upper_tols = tesseract.DblVec(
np.array([np.deg2rad(0.1)] * 6))
joint_constraint.lower_tols = tesseract.DblVec(
np.array([np.deg2rad(-0.1)] * 6))
joint_constraint.term_type = tesseract.TT_COST
joint_constraint.coeffs = tesseract.DblVec([1000] * 6)
pci.cost_infos.push_back(joint_constraint)
joint_constraint = tesseract.JointPosTermInfo()
print trajectory_in.points[-1].positions
joint_constraint.targets = tesseract.DblVec(
trajectory_in.points[-1].positions)
joint_constraint.first_step = pci.basic_info.n_steps - 1
joint_constraint.last_step = pci.basic_info.n_steps - 1
#joint_constraint.upper_tols = tesseract.DblVec(np.array([np.deg2rad(0.1)]*6))
#joint_constraint.lower_tols = tesseract.DblVec(np.array([np.deg2rad(-0.1)]*6))
joint_constraint.coeffs = tesseract.DblVec([100000] * 6)
joint_constraint.term_type = tesseract.TT_COST
pci.cost_infos.push_back(joint_constraint)
prob = tesseract.ConstructProblem(pci)
config = tesseract.TrajOptPlannerConfig(prob)
config.params.max_iter = 1000
planning_response = planner.solve(config)
if (planning_response.status_code != 0):
raise Exception(
"TrajOpt trajectory planning failed with code: %d" %
planning_response.status_code)
self.tesseract_plotter.plotScene()
self.tesseract_plotter.plotTrajectory(
self.tesseract_env.getJointNames(),
planning_response.trajectory[:, 0:6])
jt = JointTrajectory()
jt.header.stamp = rospy.Time.now()
jt.joint_names = joint_names
trajectory_time = np.cumsum(1.0 /
planning_response.trajectory[:, 6])
trajectory_time = trajectory_time - trajectory_time[0]
for i in xrange(planning_response.trajectory.shape[0]):
jt_p = JointTrajectoryPoint()
jt_p.time_from_start = rospy.Duration(trajectory_time[i])
jt_p.positions = planning_response.trajectory[i, 0:6]
jt.points.append(jt_p)
return jt
def test_trajopt_motion_planner_pci():
t = _load_tesseract()
pci = tesseract.ProblemConstructionInfo(t)
pci.init_info.type = tesseract.InitInfo.STATIONARY
#pci.init_info.data = np.array([0.0,0,0,0,0,0])
pci.basic_info.n_steps = 10
pci.basic_info.manip = "manipulator"
pci.basic_info.start_fixed = True
pci.basic_info.use_time = False
pci.opt_info.max_iter = 10000
pci.opt_info.min_approx_improve = 1e-3
pci.opt_info.min_trust_box_size = 1e-3
pci.kin = pci.getManipulator(pci.basic_info.manip)
start_pos_terminfo = tesseract.JointPosTermInfo()
start_pos_terminfo.name = "start"
start_pos_terminfo.term_type = tesseract.TT_CNT
#start_pos_terminfo.coeffs=np.ones(6)
start_pos_terminfo.first_step = 0
start_pos_terminfo.last_step = 1
start_pos_terminfo.targets = np.random.rand(7) * 0.0
end_pos_terminfo = tesseract.JointPosTermInfo()
end_pos_terminfo.name = "end"
end_pos_terminfo.term_type = tesseract.TT_CNT
#end_pos_terminfo.coeffs=np.ones(6)
end_pos_terminfo.targets = np.random.rand(7) * 0.1
end_pos_terminfo.first_step = pci.basic_info.n_steps - 1
end_pos_terminfo.last_step = pci.basic_info.n_steps - 1
joint_vel = tesseract.JointVelTermInfo()
joint_vel.coeffs = np.ones(7)
joint_vel.targets = np.zeros(7)
joint_vel.first_step = 0
joint_vel.last_step = pci.basic_info.n_steps - 1
joint_vel.name = "Joint_vel"
joint_vel.term_type = tesseract.TT_COST
time_terminfo = tesseract.TotalTimeTermInfo()
time_terminfo.coeff = 1
time_terminfo.term_type = tesseract.TT_COST | tesseract.TT_USE_TIME
time_terminfo.name = "time"
collision = tesseract.CollisionTermInfo()
collision.name = "collision"
collision.term_type = tesseract.TT_CNT
collision.continuous = True
collision.first_step = 0
collision.last_step = pci.basic_info.n_steps - 2
collision.gap = 1
collision_info = tesseract.createSafetyMarginDataVector(
pci.basic_info.n_steps, .0001, 40)
for i in collision_info:
collision.info.append(i)
pci.cnt_infos.append(start_pos_terminfo)
pci.cnt_infos.append(end_pos_terminfo)
#pci.cost_infos.append(time_terminfo)
pci.cnt_infos.push_back(collision)
pci.cost_infos.push_back(joint_vel)
prob = tesseract.ConstructProblem(pci)
config = tesseract.TrajOptPlannerConfig(prob)
planner = tesseract.TrajOptMotionPlanner()
planner.setConfiguration(config)
planner_status_code, planner_response = planner.solve(True)
assert planner_status_code.value() == 0, "Planning failed"
print(planner_response)