q.append(qname)
configs.append(q)
except Exception:
print "Error reading configuration on line", lineno
raise
#shift backward to times[0]
if shiftTime:
times = [t - times[0] for t in times]
#fill in None's with interpolated values
for dof in range(len(configs[0])):
if any(q[dof] == None for q in configs):
if fillPolicy == 'interpolate':
from klampt.trajectory import Trajectory
traj = Trajectory()
traj.times = [t for t, q in zip(times, configs) if q[dof] != None]
traj.milestones = [[q[dof]] for q in configs if q[dof] != None]
for i, t in enumerate(times):
if configs[i][dof] == None:
configs[i][dof] = traj.eval(t, 'clamp')[0]
elif fillPolicy == 'last':
last = None
first = None
for i, t in enumerate(times):
if configs[i][dof] == None:
configs[i][dof] = last
else:
last = configs[i][dof]
if first == None: first = configs[i][dof]
for i, t in enumerate(times):
def advance(self, **inputs):
res = {}
for msg in self.jointTrajectoryRosMsgQueue:
#parse in the message -- are positions, velocities, efforts specified?
self.currentTrajectoryStart = inputs['t']
self.currentTrajectoryNames = msg.joint_names
#read in the start time according to the msg time stamp, as
#specified by the ROS JointTrajectoryActionController
starttime = msg.header.stamp.to_sec()
#read in the relative times
times = [p.time_from_start.to_sec() for p in msg.points]
milestones = [p.positions for p in msg.points]
velocities = [p.velocities for p in msg.points]
accels = [p.accelerations for p in msg.points]
efforts = [p.efforts for p in msg.points]
#TODO: quintic interpolation with accelerations
if any(len(x) != 0 for x in accels):
print(
"RosRobotController: Warning, acceleration trajectories not handled"
)
if all(len(x) != 0 for x in milestones):
if all(len(x) != 0 for x in velocities):
#Hermite interpolation
traj = HermiteTrajectory(times, milestones, velocities)
if self.currentPhaseTrajectory == None:
self.currentPhaseTrajectory = traj
else:
self.currentPhaseTrajectory = self.currentPhaseTrajectory.splice(
traj,
time=splicetime,
relative=True,
jumpPolicy='blend')
else:
#linear interpolation
self.currentPhaseTrajectory = None
traj = Trajectory(times, milestones)
if self.currentPositionTrajectory == None:
self.currentPositionTrajectory = traj
else:
self.currentPositionTrajectory = self.currentPositionTrajectory.splice(
traj,
time=splicetime,
relative=True,
jumpPolicy='blend')
else:
self.currentPositionTrajectory = None
self.currentPhaseTrajectory = None
if all(len(x) != 0 for x in velocities):
#velocity control
traj = Trajectory(times, velocities)
if self.currentVelocityTrajectory == None:
self.currentVelocityTrajectory = traj
else:
self.currentVelocityTrajectory = self.currentVelocityTrajectory.splice(
traj,
time=splicetime,
relative=True,
jumpPolicy='blend')
else:
self.currentVelocityTrajectory = None
if all(len(x) != 0 for x in efforts):
traj = Trajectory(times, efforts)
if self.currentEffortTrajectory == None:
self.currentEffortTrajectory = traj
else:
self.currentEffortTrajectory.splice(traj,
time=splicetime,
relative=True,
jumpPolicy='blend')
else:
self.currentEffortTrajectory = None
#clear the message queue
self.jointTrajectoryRosMsgQueue = []
#evaluate the trajectory and send it to controller's output
t = inputs['t']
if self.currentPhaseTrajectory != None:
#hermite trajectory mode
qdqcmd = self.currentPhaseTrajectory.eval(t, 'halt')
qcmd = qdqcmd[:len(qdqcmd) / 2]
dqcmd = qdqcmd[len(qdqcmd) / 2:]
self.map_output(qcmd, self.currentTrajectoryNames, res, 'qcmd')
self.map_output(dqcmd, self.currentTrajectoryNames, res, 'dqcmd')
elif self.currentPositionTrajectory != None:
#piecewise linear trajectory mode
qcmd = self.currentPositionTrajectory.eval(t, 'halt')
self.map_output(qcmd, self.currentTrajectoryNames, res, 'qcmd')
#automatic differentiation
dqcmd = self.currentPositionTrajectory.deriv(t, 'halt')
self.map_output(dqcmd, self.currentTrajectoryNames, res, 'dqcmd')
elif self.currentVelocityTrajectory != None:
#velocity trajectory mode
dqcmd = self.currentVelocityTrajectory.deriv(t, 'halt')
self.map_output(dqcmd, self.currentTrajectoryNames, res, 'dqcmd')
#TODO: compute actual time of velocity
res['tcmd'] = 1.0
if self.currentEffortTrajectory != None:
torquecmd = self.currentEffortTrajectory.eval(t, 'halt')
self.map_output(torquecmd, self.currentTrajectoryNames, res,
'torquecmd')
#sense the configuration and velocity, possibly the effort
self.state.header.stamp = rospy.get_rostime()
self.state.header.seq += 1
if 'q' in inputs:
self.state.position = inputs['q']
if 'dq' in inputs:
self.state.velocity = inputs['dq']
if 'torque' in inputs:
self.state.effort = inputs['torque']
self.pub.publish(self.state)
#print("ROS control result is",res)
return res
def driverToConfigTrajectory(robot,traj):
qtraj = Trajectory()
qtraj.times = traj.times[:]
for q in traj.milestones:
qtraj.milestones.append(driversToConfig(robot,q))
return qtraj
