class TrajectoryGenerator(object):
"""
Makes calls to the motion planner and checks the resulting
trajectory for collisions.
"""
def __init__(self, env):
self.env = env
self.robot = self.env.GetRobots()[0]
self.manip = self.robot.GetActiveManipulator()
self.motion_planner = TrajoptPlanner(self.env)
self.collision_checker = CollisionChecker(self.env)
def traj_from_pose(self, pos, rot,
collisionfree=True,
joint_targets=None,
n_steps=None):
traj = self.motion_planner.plan_with_pose(pos, rot, collisionfree,
joint_targets, n_steps)
collisions = self.collision_checker.get_collisions(traj)
if collisionfree and collisions:
return None, collisions
else:
return traj, collisions
def traj_from_joints(self, joint_targets,
collisionfree=True,
n_steps=None):
traj = self.motion_planner.plan_with_joints(joint_targets, collisionfree,
n_steps)
collisions = self.collision_checker.get_collisions(traj)
if collisionfree and collisions:
return None, collisions
else:
return traj, collisions
def __init__(self, env): self.env = env self.robot = self.env.GetRobots()[0] self.manip = self.robot.GetActiveManipulator() self.motion_planner = TrajoptPlanner(self.env) self.collision_checker = CollisionChecker(self.env)
