#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: