def test_retime_kinematics_ravetraj(robot_fixture, seed, solver_wrapper):
env = robot_fixture.GetEnv()
basemanip = orpy.interfaces.BaseManipulation(robot_fixture)
# Generate random trajectory from seed
arm_indices = robot_fixture.GetActiveDOFIndices()
qlim_lower, qlim_upper = robot_fixture.GetActiveDOFLimits()
np.random.seed(seed)
qseed = np.random.randint(1000000, size=(1000,)) # Try 1000 times
qsel = []
for seed in qseed:
np.random.seed(seed)
qrand = qlim_lower + (qlim_upper - qlim_lower) * np.random.rand(len(arm_indices))
with robot_fixture:
robot_fixture.SetActiveDOFValues(qrand)
incollision = env.CheckCollision(robot_fixture) or robot_fixture.CheckSelfCollision()
if not incollision:
qsel.append(qrand)
if len(qsel) == 2:
break
assert len(qsel) == 2
# Plan a path
robot_fixture.SetActiveDOFValues(qsel[0])
traj = basemanip.MoveActiveJoints(goal=qsel[1], execute=False, outputtrajobj=True)
assert traj is not None
# Rave traj
traj_new, interpolator = toppra.retime_active_joints_kinematics(
traj, robot_fixture, output_interpolator=True, vmult=1.0, solver_wrapper=solver_wrapper)
assert traj_new is not None
assert traj_new.GetDuration() < traj.GetDuration() + 1 # Should not be too far from the optimal value
def main(env=None, test=False):
"Main function."
parser = argparse.ArgumentParser(description="A simple example in which trajectories, which are planned using"
"OpenRAVE is retimed using toppra. The trajectories are found using"
"birrt, the default planner. Goals are generated randomly and "
"uniformly.")
parser.add_argument('-e', '--env', help='OpenRAVE Environment file', default="data/lab1.env.xml")
parser.add_argument('-v', '--verbose', help='Show DEBUG log and plot trajectories', action="store_true")
parser.add_argument('-N', '--Ngrid', help='Number of discretization step', default=100, type=int)
args = vars(parser.parse_args())
if args['verbose']:
toppra.setup_logging('DEBUG')
else:
toppra.setup_logging('INFO')
if env is None:
env = orpy.Environment()
env.SetDebugLevel(0)
env.Load(args['env'])
env.SetViewer('qtosg')
robot = env.GetRobots()[0]
robot.SetDOFAccelerationLimits(np.ones(11) * 3)
manipulator = robot.GetManipulators()[0]
robot.SetActiveManipulator(manipulator)
robot.SetActiveDOFs(manipulator.GetArmIndices())
controller = robot.GetController()
basemanip = orpy.interfaces.BaseManipulation(robot)
dof = robot.GetActiveDOF()
pc_torque = toppra.create_rave_torque_path_constraint(
robot, discretization_scheme=toppra.constraint.DiscretizationType.Interpolation)
it = 0
while True or (test and it > 5):
lower, upper = robot.GetActiveDOFLimits()
qrand = np.random.rand(dof) * (upper - lower) + lower
with robot:
robot.SetActiveDOFValues(qrand)
incollision = env.CheckCollision(robot) or robot.CheckSelfCollision()
if incollision:
continue
traj_original = basemanip.MoveActiveJoints(qrand, execute=False, outputtrajobj=True)
if traj_original is None:
continue
traj_retimed, trajra = toppra.retime_active_joints_kinematics(
traj_original, robot, output_interpolator=True, N=args['Ngrid'],
additional_constraints=[pc_torque])
print(("Original duration: {:.3f}. Retimed duration: {:3f}.".format(
traj_original.GetDuration(), traj_retimed.GetDuration())))
if args['verbose']:
plot_trajectories(traj_original, traj_retimed, robot)
time.sleep(1)
controller.SetPath(traj_retimed)
robot.WaitForController(0)
it += 1
def test_retime_kinematics_waypoints(robot_fixture, seed, solver_wrapper):
dof = robot_fixture.GetActiveDOF()
# Generate random trajectory from seed
qlim_lower, qlim_upper = robot_fixture.GetActiveDOFLimits()
np.random.seed(seed)
waypoints = np.random.rand(5, dof)
for i in range(5):
waypoints[i] = qlim_lower + waypoints[i] * (qlim_upper - qlim_lower)
traj_new, trajra = toppra.retime_active_joints_kinematics(
waypoints, robot_fixture, output_interpolator=True, solver_wrapper=solver_wrapper)
assert traj_new is not None
assert traj_new.GetDuration() < 30 and traj_new.GetDuration() > 0
def main(env=None, test=False):
"Main function."
args = parse_cli_arguments()
toppra.setup_logging('INFO')
robot = load_rave_robot(args)
basemanip = orpy.interfaces.BaseManipulation(robot)
pc_torque = toppra.create_rave_torque_path_constraint(
robot,
discretization_scheme=toppra.constraint.DiscretizationType.
Interpolation)
it = 0
while True or (test and it > 5):
qrand = generate_random_configuration(robot)
traj_original = basemanip.MoveActiveJoints(qrand,
execute=False,
outputtrajobj=True)
if traj_original is None:
continue
traj_retimed, trajra = toppra.retime_active_joints_kinematics(
traj_original,
robot,
output_interpolator=True,
N=args['Ngrid'],
additional_constraints=[pc_torque],
solver_wrapper='seidel')
print("Original duration: {:.3f}. Retimed duration: {:3f}.".format(
traj_original.GetDuration(), traj_retimed.GetDuration()))
robot.GetController().SetPath(traj_retimed)
if args['verbose']:
plot_trajectories(traj_original, traj_retimed, robot)
robot.WaitForController(0)
it += 1
def run(self, offset=0.01):
"""Run the demo.
For each object, the robot first move to a pose that is
directly on top of it. Afterward, it moves down. Next, it
grabs the object to be transported. Finally, it moves to
another configuration.
Note: planning is done using rrt, not constrained rrt. The
later is far too inefficient to be of any use. An even more
critical issue with constrained rrt is that its path is too
jerky, failing most smoothing attempts.
Args:
offset: (float, optional) Approach distance.
"""
fail = False
# Start planning/control loop:
# Every pick cycle follows the same procedure:
# 1. APPROACH: visit a configuration that is directly on top of the object to pick
# 2. REACH: move down to make contact w the object
# 3. APPROACH: visit the same configuration as 1.
# 4. TRANSPORT: visit the goal configuration.
for obj_dict in self._scenario['objects']:
t0 = self.get_time()
# Basic setup
manip_name = obj_dict["object_attach_to"]
manip = self.get_robot().SetActiveManipulator(manip_name)
Tstart = np.array(obj_dict['T_start']) # object's transform
Tgoal = np.array(obj_dict['T_goal'])
T_ee_start = np.dot(
Tstart, self.get_object(
obj_dict['name']).get_T_object_link())
self.verify_transform(manip, T_ee_start)
T_ee_top = np.array(T_ee_start)
try:
T_ee_top[:3, 3] -= obj_dict['offset'] * T_ee_top[:3, 2]
except BaseException:
T_ee_top[:3, 3] -= offset * T_ee_top[:3, 2]
q_nominal = np.r_[-0.3, 0.9, 0.9, 0, 0, 0]
t1 = self.get_time()
# 1. APPROACH
logger.info("Plan path to APPROACH")
traj0 = plan_to_manip_transform(
self._robot, T_ee_top, q_nominal, max_ppiters=200, max_iters=100)
t1a = self.get_time()
if self.mode == "PRACTICE":
self.check_continue()
fail = not self.execute_trajectory(traj0)
self.get_robot().WaitForController(0)
# 2. REACH: Move a "short" trajectory to reach the object
logger.info("Plan path to REACH")
t2 = self.get_time()
traj0b = plan_to_manip_transform(
self._robot,
T_ee_start,
q_nominal,
max_ppiters=200,
max_iters=100)
t2a = self.get_time()
if self.mode == "PRACTICE":
self.check_continue()
fail = not self.execute_trajectory(traj0b)
self.get_robot().WaitForController(0)
with self._env:
self._robot.Grab(self.get_env().GetKinBody(obj_dict['name']))
logger.info("Grabbing the object. Continue moving in 1 sec.")
# time.sleep(1)
# 3+4: APPROACH+TRANSPORT: Plan two trajectories, one
# trajectory to reach the REACH position, another
# trajectory to reach the GOAL position. Merge them, then
# execute.
t3 = self.get_time()
logger.info("Plan path to GOAL")
# 1st trajectory
traj0c = plan_to_manip_transform(
self._robot, T_ee_top, q_nominal, max_ppiters=1, max_iters=100)
traj0c_waypoints, traj0c_ss = self.extract_waypoints(traj0c)
T_ee_goal = np.dot(
Tgoal, self.get_object(
obj_dict['name']).get_T_object_link())
self.verify_transform(manip, T_ee_goal)
q_nominal = np.r_[0.3, 0.9, 0.9, 0, 0, 0]
# 2nd trajectory
self._robot.SetActiveDOFValues(traj0c_waypoints[-1])
traj1_transport = plan_to_manip_transform(
self._robot, T_ee_goal, q_nominal, max_ppiters=1, max_iters=100)
self._robot.SetActiveDOFValues(traj0c_waypoints[0])
traj1_transport_waypoints, traj1_transport_ss = self.extract_waypoints(
traj1_transport)
# concatenate the two trajectories
traj2_waypoints = np.vstack(
(traj0c_waypoints, traj1_transport_waypoints[1:]))
# retime
traj2_ss = np.hstack(
(traj0c_ss, traj0c_ss[-1] + traj1_transport_ss[1:]))
traj2_ss[:] = traj2_ss / traj2_ss[-1]
traj2_rave = orpy.RaveCreateTrajectory(self._env, "")
spec = self._robot.GetActiveConfigurationSpecification()
traj2_rave.Init(spec)
for p in traj2_waypoints:
traj2_rave.Insert(traj2_rave.GetNumWaypoints(), p)
t3a = self.get_time()
planner = orpy.RaveCreatePlanner(self._env, "ParabolicSmoother")
params = orpy.Planner.PlannerParameters()
params.SetRobotActiveJoints(self._robot)
params.SetMaxIterations(100)
params.SetPostProcessing('', '')
success = planner.InitPlan(self._robot, params)
status = planner.PlanPath(traj2_rave)
if not success or not status:
logger.fatal("[Plan Transport Path] Init status: {1:}, Plan status: {0:}. "
"Use traj2_rave directly.".format(status, success))
t3b = self.get_time()
# Retime the transport trajectory and execute it
logger.debug(
"Original traj nb waypoints: {:d}".format(
traj1_transport.GetNumWaypoints()))
logger.debug("Retime using toppra.")
t4 = self.get_time()
contact = self.get_object(obj_dict['name']).get_contact()
contact_constraint = create_object_transporation_constraint(
contact, self.get_object(obj_dict['name']))
contact_constraint.set_discretization_type(1)
traj2_retimed = toppra.retime_active_joints_kinematics(
traj2_rave,
self._robot,
additional_constraints=[contact_constraint],
solver_wrapper=SOLVER,
vmult=0.999,
amult=0.999)
if traj2_retimed is None:
logger.error(
"Transport trajectory retime fails! Try again without contact constraints.")
traj2_retimed = toppra.retime_active_joints_kinematics(
traj2_rave, self.get_robot(), additional_constraints=[])
t4a = self.get_time()
if self.mode == "PRACTICE":
self.check_continue()
fail = not self.execute_trajectory(traj2_retimed)
self.get_robot().WaitForController(0)
# release the object
logger.info("RELEASE object")
self.check_continue() # release object now
self._robot.Release(self.get_env().GetKinBody(obj_dict['name']))
t4b = self.get_time()
logger.info("Time report"
"\n - setup :{0:f} secs"
"\n - APPROACH plan :{1:f} secs"
"\n - APPROACH duration :{6:f} secs"
"\n - REACH plan :{2:f} secs"
"\n - REACH duration :{7:f} secs"
"\n - MOVE plan :{3:f} secs"
"\n - MOVE shortcut :{4:f} secs"
"\n - MOVE retime :{5:f} secs"
"\n - MOVE duration :{8:f} secs"
"\n - TOTAL duration :{9:f} secs".format(
t1 - t0, t1a - t1, t2a - t2, t3a - t3, t3b - t3a, t4a - t4,
traj0.GetDuration(), traj0b.GetDuration(),
traj2_retimed.GetDuration(), t4b - t0))
# remove objects from environment
T_cur = self.get_env().GetKinBody(obj_dict['name']).GetTransform()
T_cur[2, 3] -= 3e-3 # simulate 5mm drop
self.get_env().GetKinBody(obj_dict['name']).SetTransform(T_cur)
# Move the robot back to HOME
trajHOME = plan_to_joint_configuration(self._robot, self._qHOME)
fail = not self.execute_trajectory(trajHOME)
return not fail
