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)