def retime_traj(robot,
inds,
traj,
max_cart_vel=.02,
max_finger_vel=.02,
upsample_time=.1):
"""retime a trajectory so that it executes slowly enough for the simulation"""
cart_traj = np.empty((len(traj), 6))
finger_traj = np.empty((len(traj), 2))
leftarm, rightarm = robot.GetManipulator("leftarm"), robot.GetManipulator(
"rightarm")
with robot:
for i in range(len(traj)):
robot.SetDOFValues(traj[i], inds)
cart_traj[i, :3] = leftarm.GetTransform()[:3, 3]
cart_traj[i, 3:] = rightarm.GetTransform()[:3, 3]
finger_traj[i, :1] = robot.GetDOFValues(
leftarm.GetGripperIndices())
finger_traj[i,
1:] = robot.GetDOFValues(rightarm.GetGripperIndices())
times = retiming.retime_with_vel_limits(
np.c_[cart_traj, finger_traj], np.r_[np.repeat(max_cart_vel, 6),
np.repeat(max_finger_vel, 2)])
times_up = np.linspace(
0, times[-1], times[-1] /
upsample_time) if times[-1] > upsample_time else times
traj_up = math_utils.interp2d(times_up, times, traj)
return traj_up
def follow_joint_trajectory(self, traj):
traj = np.r_[np.atleast_2d(self.get_joint_positions()), traj]
for i in [2, 4, 6]:
traj[:, i] = np.unwrap(traj[:, i])
times = retiming.retime_with_vel_limits(traj, self.vel_limits)
times_up = np.arange(0, times[-1], .1)
traj_up = mu.interp2d(times_up, times, traj)
vels = resampling.get_velocities(traj_up, times_up, .001)
self.follow_timed_joint_trajectory(traj_up, vels, times_up)
def follow_joint_trajectory(self, traj):
traj = np.r_[np.atleast_2d(self.get_joint_positions()), traj]
for i in [2,4,6]:
traj[:,i] = np.unwrap(traj[:,i])
times = retiming.retime_with_vel_limits(traj, self.vel_limits)
times_up = np.arange(0,times[-1],.1)
traj_up = mu.interp2d(times_up, times, traj)
vels = resampling.get_velocities(traj_up, times_up, .001)
self.follow_timed_joint_trajectory(traj_up, vels, times_up)
def retime_traj(robot, inds, traj, max_cart_vel=.02, max_finger_vel=.02, upsample_time=.1):
"""retime a trajectory so that it executes slowly enough for the simulation"""
cart_traj = np.empty((len(traj), 6))
finger_traj = np.empty((len(traj),2))
leftarm, rightarm = robot.GetManipulator("leftarm"), robot.GetManipulator("rightarm")
with robot:
for i in range(len(traj)):
robot.SetDOFValues(traj[i], inds)
cart_traj[i,:3] = leftarm.GetTransform()[:3,3]
cart_traj[i,3:] = rightarm.GetTransform()[:3,3]
finger_traj[i,:1] = robot.GetDOFValues(leftarm.GetGripperIndices())
finger_traj[i,1:] = robot.GetDOFValues(rightarm.GetGripperIndices())
times = retiming.retime_with_vel_limits(np.c_[cart_traj, finger_traj], np.r_[np.repeat(max_cart_vel, 6),np.repeat(max_finger_vel,2)])
times_up = np.linspace(0, times[-1], times[-1]/upsample_time) if times[-1] > upsample_time else times
traj_up = math_utils.interp2d(times_up, times, traj)
return traj_up
def follow_body_traj(pr2,
bodypart2traj,
wait=True,
base_frame="/base_footprint",
speed_factor=1):
LOG.info("following trajectory with bodyparts %s",
" ".join(bodypart2traj.keys()))
name2part = {
"lgrip": pr2.lgrip,
"rgrip": pr2.rgrip,
"larm": pr2.larm,
"rarm": pr2.rarm,
"base": pr2.base
}
for partname in bodypart2traj:
if partname not in name2part:
raise Exception("invalid part name %s" % partname)
#### Go to initial positions #######
for (name, part) in name2part.items():
if name in bodypart2traj:
part_traj = bodypart2traj[name]
if name == "lgrip" or name == "rgrip":
part.set_angle(np.squeeze(part_traj)[0])
elif name == "larm" or name == "rarm":
part.goto_joint_positions(part_traj[0])
elif name == "base":
part.goto_pose(part_traj[0], base_frame)
pr2.join_all()
#### Construct total trajectory so we can retime it #######
n_dof = 0
trajectories = []
vel_limits = []
acc_limits = []
bodypart2inds = {}
for (name, part) in name2part.items():
if name in bodypart2traj:
traj = bodypart2traj[name]
if traj.ndim == 1: traj = traj.reshape(-1, 1)
trajectories.append(traj)
vel_limits.extend(part.vel_limits)
acc_limits.extend(part.acc_limits)
bodypart2inds[name] = range(n_dof, n_dof + part.n_joints)
n_dof += part.n_joints
trajectories = np.concatenate(trajectories, 1)
vel_limits = np.array(vel_limits) * speed_factor
times = retiming.retime_with_vel_limits(trajectories, vel_limits)
times_up = np.linspace(0, times[-1], int(np.ceil(times[-1] / .1)))
traj_up = mu.interp2d(times_up, times, trajectories)
#### Send all part trajectories ###########
for (name, part) in name2part.items():
if name in bodypart2traj:
part_traj = traj_up[:, bodypart2inds[name]]
if name == "lgrip" or name == "rgrip":
part.follow_timed_trajectory(times_up, part_traj.flatten())
elif name == "larm" or name == "rarm":
vels = resampling.get_velocities(part_traj, times_up, .001)
part.follow_timed_joint_trajectory(part_traj, vels, times_up)
elif name == "base":
part.follow_timed_trajectory(times_up, part_traj, base_frame)
if wait: pr2.join_all()
return True
def follow_body_traj(pr2, bodypart2traj, wait=True, base_frame = "/base_footprint", speed_factor=1):
LOG.info("following trajectory with bodyparts %s", " ".join(bodypart2traj.keys()))
name2part = {"lgrip":pr2.lgrip,
"rgrip":pr2.rgrip,
"larm":pr2.larm,
"rarm":pr2.rarm,
"base":pr2.base}
for partname in bodypart2traj:
if partname not in name2part:
raise Exception("invalid part name %s"%partname)
#### Go to initial positions #######
for (name, part) in name2part.items():
if name in bodypart2traj:
part_traj = bodypart2traj[name]
if name == "lgrip" or name == "rgrip":
part.set_angle(np.squeeze(part_traj)[0])
elif name == "larm" or name == "rarm":
part.goto_joint_positions(part_traj[0])
elif name == "base":
part.goto_pose(part_traj[0], base_frame)
pr2.join_all()
#### Construct total trajectory so we can retime it #######
n_dof = 0
trajectories = []
vel_limits = []
acc_limits = []
bodypart2inds = {}
for (name, part) in name2part.items():
if name in bodypart2traj:
traj = bodypart2traj[name]
if traj.ndim == 1: traj = traj.reshape(-1,1)
trajectories.append(traj)
vel_limits.extend(part.vel_limits)
acc_limits.extend(part.acc_limits)
bodypart2inds[name] = range(n_dof, n_dof+part.n_joints)
n_dof += part.n_joints
trajectories = np.concatenate(trajectories, 1)
vel_limits = np.array(vel_limits)*speed_factor
times = retiming.retime_with_vel_limits(trajectories, vel_limits)
times_up = np.linspace(0, times[-1], int(np.ceil(times[-1]/.1)))
traj_up = mu.interp2d(times_up, times, trajectories)
#### Send all part trajectories ###########
for (name, part) in name2part.items():
if name in bodypart2traj:
part_traj = traj_up[:,bodypart2inds[name]]
if name == "lgrip" or name == "rgrip":
part.follow_timed_trajectory(times_up, part_traj.flatten())
elif name == "larm" or name == "rarm":
vels = resampling.get_velocities(part_traj, times_up, .001)
part.follow_timed_joint_trajectory(part_traj, vels, times_up)
elif name == "base":
part.follow_timed_trajectory(times_up, part_traj, base_frame)
if wait: pr2.join_all()
return True
