def update_trajectory(self, tesseract_trajectory):
start_instruction_o = tesseract_trajectory[0]
assert isMoveInstruction(start_instruction_o)
start_waypoint_o = start_instruction_o.as_MoveInstruction(
).getWaypoint()
assert isStateWaypoint(start_waypoint_o)
start_waypoint = start_waypoint_o.as_StateWaypoint()
trajectory_json = dict()
trajectory_json["use_time"] = True
trajectory_json["loop_time"] = 20
trajectory_json["joint_names"] = list(start_waypoint.joint_names)
trajectory2 = []
for i in range(len(tesseract_trajectory)):
instr = tesseract_trajectory[i]
assert isMoveInstruction(instr)
wp = instr.as_MoveInstruction().getWaypoint()
assert isStateWaypoint(wp)
state_wp = wp.as_StateWaypoint()
trajectory2.append(state_wp.position.flatten().tolist() +
[state_wp.time])
trajectory_json["trajectory"] = trajectory2
self.trajectory_json = json.dumps(trajectory_json)
def test_trajopt_freespace_joint_cart():
env, manip = get_environment()
fwd_kin = env.getManipulatorManager().getFwdKinematicSolver(manip.manipulator)
inv_kin = env.getManipulatorManager().getInvKinematicSolver(manip.manipulator)
joint_names = fwd_kin.getJointNames()
cur_state = env.getCurrentState()
wp1 = JointWaypoint(joint_names, np.array([0,0,0,-1.57,0,0,0],dtype=np.float64))
wp2 = CartesianWaypoint(Isometry3d.Identity() * Translation3d(-.2,.4,0.2) * Quaterniond(0,0,1.0,0))
start_instruction = PlanInstruction(Waypoint(wp1), PlanInstructionType_START, "TEST_PROFILE")
plan_f1 = PlanInstruction(Waypoint(wp2), PlanInstructionType_FREESPACE, "TEST_PROFILE")
program = CompositeInstruction("TEST_PROFILE")
program.setStartInstruction(Instruction(start_instruction))
program.setManipulatorInfo(manip)
program.append(Instruction(plan_f1))
seed = generateSeed(program, cur_state, env, 3.14, 1.0, 3.14, 10)
plan_profile = TrajOptDefaultPlanProfile()
composite_profile = TrajOptDefaultCompositeProfile()
test_planner = TrajOptMotionPlanner()
test_planner.plan_profiles["TEST_PROFILE"] = plan_profile
test_planner.composite_profiles["TEST_PROFILE"] = composite_profile
problem_generator_fn = TrajOptProblemGeneratorFn(DefaultTrajoptProblemGenerator)
test_planner.problem_generator = problem_generator_fn
request = PlannerRequest()
request.seed = seed
request.instructions = program
request.env = env
request.env_state = env.getCurrentState()
problem = DefaultTrajoptProblemGenerator(test_planner.getName(), request, test_planner.plan_profiles, test_planner.composite_profiles, test_planner.solver_profiles)
response = PlannerResponse()
assert test_planner.solve(request, response)
assert response.status.value() == TrajOptMotionPlannerStatusCategory.SolutionFound
results = flatten(response.results)
assert len(results) == 11
for instr in results:
assert isMoveInstruction(instr)
wp1 = instr.as_MoveInstruction().getWaypoint()
assert isStateWaypoint(wp1)
wp = wp1.as_StateWaypoint()
assert len(wp.joint_names) == 7
assert isinstance(wp.position,np.ndarray)
assert len(wp.position) == 7
def test_planning_server_freespace():
env, manip = get_environment()
joint_names = [
"joint_a1", "joint_a2", "joint_a3", "joint_a4", "joint_a5", "joint_a6",
"joint_a7"
]
wp1 = JointWaypoint(joint_names,
np.array([0, 0, 0, -1.57, 0, 0, 0], dtype=np.float64))
wp2 = CartesianWaypoint(Isometry3d.Identity() *
Translation3d(-.2, .4, 0.2) *
Quaterniond(0, 0, 1.0, 0))
start_instruction = PlanInstruction(Waypoint(wp1),
PlanInstructionType_START, "DEFAULT")
plan_f1 = PlanInstruction(Waypoint(wp2), PlanInstructionType_FREESPACE,
"DEFAULT")
program = CompositeInstruction("DEFAULT")
program.setStartInstruction(Instruction(start_instruction))
program.setManipulatorInfo(manip)
program.append(Instruction(plan_f1))
planning_server = ProcessPlanningServer(env, 1)
planning_server.loadDefaultProcessPlanners()
request = ProcessPlanningRequest()
request.name = FREESPACE_PLANNER_NAME
request.instructions = Instruction(program)
response = planning_server.run(request)
planning_server.waitForAll()
assert response.interface.isSuccessful()
results = flatten(response.getResults().as_CompositeInstruction())
assert len(results) == 37
for instr in results:
assert isMoveInstruction(instr)
wp1 = instr.as_MoveInstruction().getWaypoint()
assert isStateWaypoint(wp1)
wp = wp1.as_StateWaypoint()
assert len(wp.joint_names) == 7
assert isinstance(wp.position, np.ndarray)
assert len(wp.position) == 7