def output(self,**inputs):
res = {}
if self.currentJointTrajectoryMsg != None:
#parse in the message -- are positions, velocities, efforts specified?
self.currentTrajectoryStart = inputs['t']
self.currentTrajectoryNames = self.currentJointTrajectoryMsg.joint_names
times = [p.time_from_start for p in self.currentJointTrajectoryMsg.points]
milestones = [p.positions for p in self.currentJointTrajectoryMsg.points]
velocities = [p.velocities for p in self.currentJointTrajectoryMsg.points]
efforts = [p.velocities for p in self.currentJointTrajectoryMsg.points]
if all(len(x) != 0 for x in milestones):
self.currentPositionTrajectory = Trajectory(times,milestones)
else:
self.currentPositionTrajectory = None
if all(len(x) != 0 for x in velocities):
self.currentVelocityTrajectory = Trajectory(times,velocities)
else:
self.currentVelocityTrajectory = None
if all(len(x) != 0 for x in efforts):
self.currentEffortTrajectory = Trajectory(times,efforts)
else:
self.currentEffortTrajectory = None
self.currentJointTrajectoryMsg = None
#evaluate the trajectory and send it to controller's output
t = inputs['t']-self.currentTrajectoryStart
if self.currentPositionTrajectory != None:
qcmd = self.currentPositionTrajectory.eval(t,'halt')
self.map_output(qcmd,self.currentTrajectoryNames,res,'qcmd')
if self.currentVelocityTrajectory != None:
dqcmd = self.currentVelocityTrajectory.deriv(t,'halt')
self.map_output(dqcmd,self.currentTrajectoryNames,res,'dqcmd')
elif self.currentPositionTrajectory != None:
#automatic differentiation
dqcmd = self.currentPositionTrajectory.deriv(t,'halt')
self.map_output(dqcmd,self.currentTrajectoryNames,res,'dqcmd')
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()
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)
return res
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 configToDriverTrajectory(robot,traj):
qtraj = Trajectory()
qtraj.times = traj.times[:]
for q in traj.milestones:
qtraj.milestones.append(configToDrivers(robot,q))
return qtraj
for q in traj.milestones:
qtraj.milestones.append(configToDrivers(robot,q))
return qtraj
if __name__=="__main__":
import sys
if len(sys.argv) != 4:
print "Usage: config_to_driver_trajectory robot config_traj driver_traj"
exit(0)
robotfn = sys.argv[1]
trajfn = sys.argv[2]
outfn = sys.argv[3]
#load the robot file
world = klampt.WorldModel()
#this makes it a little faster to load the robot -- you don't need the geometry
world.enableGeometryLoading(False)
robot = world.loadRobot(robotfn)
if robot.getName()=='':
print "Unable to load robot file",robotfn
exit(1)
traj = Trajectory()
try:
traj.load(trajfn)
except IOError:
print "Unable to load trajectory file",trajfn
exit(1)
outtraj = configToDriverTrajectory(robot,traj)
print "Saving to",outfn,"..."
outtraj.save(outfn)
speed = 100
trim = 0
print "Plays back an arm motion"
print "Usage: arm_motion_playback.py FILE [SPEED] [TRIM]"
if len(sys.argv) <= 1:
print "Please specify a motion file"
exit(0)
if len(sys.argv) > 2:
speed = float(sys.argv[2])
if len(sys.argv) > 3:
trim = float(sys.argv[3])
fn = sys.argv[1]
print "Loading motion from",fn
traj = Trajectory()
traj.load(fn)
if len(traj.milestones[0]) != 14:
print "Error loading arms trajectory, size is not 14"
#simple motion setup
config.setup(parse_sys=False)
motion.robot.startup()
try:
if trim == 0:
print "Moving to start, 20%% speed..."
else:
print "Moving to config at time %f, 20%% speed..."%(trim,)
motion.robot.arms_mq.setRamp(traj.eval(trim),speed=0.2)
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
class RosRobotController(controller.BaseController):
"""A controller that reads JointTrajectory from the ROS topic
'/[robot_name]/joint_trajectory' and writes JointState to the ROS topic
'/[robot_name]/joint_state'.
Uses PID control with feedforward torques.
Supports partial commands as well, e.g., only controlling part of the
robot, ignoring feedforward torques, etc. Note that if you start sending
some command and then stop (like velocities or feedforward torques) this
controller will NOT zero them out.
Currently does not support:
* Setting PID gain constants,
* Setting PID integral term bounds.
"""
def __init__(self,klampt_robot_model):
self.robot = klampt_robot_model
self.state = JointState()
n = self.robot.numLinks()
self.state.name = [self.robot.getLink(i).getName() for i in range(n)]
self.state.position = []
self.state.velocity = []
self.state.effort = []
# fast indexing structure for partial commands
self.nameToIndex = dict(zip(self.state.name,range(i)))
# Setup publisher of robot states
robot_name = self.robot.getName()
self.pub = rospy.Publisher("/%s/joint_states"%(robot_name,), JointState)
# set up the command subscriber
self.firstJointTrajectory = False
self.currentJointTrajectoryMsg = None
rospy.Subscriber('/%s/joint_trajectory'%(robot_name), JointTrajectory,self.jointTrajectoryCallback)
# these are parsed in from the trajectory message
self.currentTrajectoryStart = 0
self.currentTrajectoryNames = []
self.currentPositionTrajectory = None
self.currentVelocityTrajectory = None
self.currentEffortTrajectory = None
return
def jointTrajectoryCallback(self,msg):
self.currentJointTrajectoryMsg = msg
return
def output(self,**inputs):
res = {}
if self.currentJointTrajectoryMsg != None:
#parse in the message -- are positions, velocities, efforts specified?
self.currentTrajectoryStart = inputs['t']
self.currentTrajectoryNames = self.currentJointTrajectoryMsg.joint_names
times = [p.time_from_start for p in self.currentJointTrajectoryMsg.points]
milestones = [p.positions for p in self.currentJointTrajectoryMsg.points]
velocities = [p.velocities for p in self.currentJointTrajectoryMsg.points]
efforts = [p.velocities for p in self.currentJointTrajectoryMsg.points]
if all(len(x) != 0 for x in milestones):
self.currentPositionTrajectory = Trajectory(times,milestones)
else:
self.currentPositionTrajectory = None
if all(len(x) != 0 for x in velocities):
self.currentVelocityTrajectory = Trajectory(times,velocities)
else:
self.currentVelocityTrajectory = None
if all(len(x) != 0 for x in efforts):
self.currentEffortTrajectory = Trajectory(times,efforts)
else:
self.currentEffortTrajectory = None
self.currentJointTrajectoryMsg = None
#evaluate the trajectory and send it to controller's output
t = inputs['t']-self.currentTrajectoryStart
if self.currentPositionTrajectory != None:
qcmd = self.currentPositionTrajectory.eval(t,'halt')
self.map_output(qcmd,self.currentTrajectoryNames,res,'qcmd')
if self.currentVelocityTrajectory != None:
dqcmd = self.currentVelocityTrajectory.deriv(t,'halt')
self.map_output(dqcmd,self.currentTrajectoryNames,res,'dqcmd')
elif self.currentPositionTrajectory != None:
#automatic differentiation
dqcmd = self.currentPositionTrajectory.deriv(t,'halt')
self.map_output(dqcmd,self.currentTrajectoryNames,res,'dqcmd')
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()
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)
return res
def map_output(self,vector,names,output_map,output_name):
"""Maps a partial vector to output_map[output_name].
If output_name exists in output_map, then only the named values
are overwritten. Otherwise, the missing values are set to zero.
"""
val = []
if output_name in output_map:
val = output_map[output_name]
else:
val = [0.0]*len(self.robot.numLinks())
for n,v in zip(names,vector):
val[self.nameToIndex[n]] = v
output_map[output_name] = val
return
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 driverToConfigTrajectory(robot,traj):
qtraj = Trajectory()
qtraj.times = traj.times[:]
for q in traj.milestones:
qtraj.milestones.append(driversToConfig(robot,q))
return qtraj
import sys
if len(sys.argv) != 4:
print "Usage: driver_to_config_trajectory robot driver_traj config_traj"
exit(0)
robotfn = sys.argv[1]
trajfn = sys.argv[2]
outfn = sys.argv[3]
#load the robot file
world = klampt.WorldModel()
#this makes it a little faster to load the robot -- you don't need the geometry
world.enableGeometryLoading(False)
robot = world.loadRobot(robotfn)
if robot.getName()=='':
print "Unable to load robot file",robotfn
exit(1)
traj = Trajectory()
try:
traj.load(trajfn)
except IOError:
print "Unable to load trajectory file",trajfn
exit(1)
outtraj = driverToConfigTrajectory(robot,traj)
qmin,qmax = robot.getJointLimits()
for i in range(len(outtraj.milestones[0])):
qmini = min(q[i] for q in outtraj.milestones)
qmaxi = max(q[i] for q in outtraj.milestones)
if qmini < qmin[i] or qmaxi > qmax[i]:
print "Warning, item %d out of bounds: [%f,%f] not in [%f,%f]"%(i,qmini,qmaxi,qmin[i],qmax[i])
else:
print "Range on item %d: [%f,%f]"%(i,qmini,qmaxi)
