#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):
if configs[i][dof] == None:
configs[i][dof] = first
else:
break
else:
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
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)
while motion.robot.arms_mq.moving():
time.sleep(0.1)
print "Starting motion..."
"""
#this is direct interpolation in Python at 50 Hz
t0 = time.time()
while time.time()-t0 < traj.times[-1]/speed:
q = traj.eval((time.time()-t0)/speed)
motion.robot.left_arm.setPosition(q[0:7])
motion.robot.right_arm.setPosition(q[7:14])
time.sleep(0.02)
"""
#this uses the motion queue
t = trim
q = traj.milestones[0]
#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):
if configs[i][dof] == None:
configs[i][dof] = first
else:
break
else:
