def joint_state_cb(self, msg):
k = chr(cv.WaitKey(1) & 0xff)
if k == 'r':
lpos, rpos = self.get_joint_states(msg)
self.left_arm.recorded.append(lpos)
self.right_arm.recorded.append(rpos)
rospy.loginfo('Recorded \nr: %s \nl: %s' % (str(rpos), str(lpos)))
elif k == chr(27):
self.exit = True
elif k == 'p':
#Construct points
lpos, rpos = self.get_joint_states(msg)
rospy.loginfo('playing back')
tstep = 2.0
l = list(self.right_arm.recorded)
l.append(self.right_arm.recorded[0])
l.insert(0, rpos)
points = self.construct_points(l, tstep)
g = pr2m.JointTrajectoryGoal()
g.trajectory.joint_names = self.right_arm.joint_names
g.trajectory.points = points
#g.trajectory.header.stamp = rospy.get_rostime() + rospy.Duration(len(l) * tstep)
g.trajectory.header.stamp = rospy.get_rostime() + rospy.Duration(0)
self.right_arm.client.send_goal(g)
def set_poses_monitored(self, pos_mat, times, vel_mat=None, block=True, time_look_ahead=.050):
joint_traj = Joint._create_trajectory(self, pos_mat, times, vel_mat)
#Create goal msg
joint_traj.header.stamp = rospy.get_rostime() + rospy.Duration(1.)
g = pm.JointTrajectoryGoal()
g.trajectory = joint_traj
self.client.send_goal(g)
if block:
return self.client.wait_for_result()
return self.client.get_state()
def set_poses(self, pos_mat, times, vel_mat=None, block=True):
p = self.pose()
for i in range(pos_mat.shape[1]):
pos_mat[4, i] = unwrap2(p[4, 0], pos_mat[4, i])
pos_mat[6, i] = unwrap2(p[6, 0], pos_mat[6, i])
p = pos_mat[:, i]
joint_traj = Joint._create_trajectory(self, pos_mat, times, vel_mat)
#Create goal msg
joint_traj.header.stamp = rospy.get_rostime() + rospy.Duration(1.)
g = pm.JointTrajectoryGoal()
g.trajectory = joint_traj
self.client.send_goal(g)
if block:
return self.client.wait_for_result()
return self.client.get_state()
def execute_joint_trajectory(self, joint_traj, block=True, speed=None):
"""
:param joint_traj: list of joint waypoints
:param block: if True, waits until trajectory is completed
:param speed: execution speed in meters/sec
:return if not blocking, return expected execution time
"""
for joints in joint_traj:
assert joints is not None
assert len(joints) == len(self.current_joints)
speed = float(speed) if speed is not None else self.default_speed
goal = pcm.JointTrajectoryGoal()
#joint_trajectory = tm.JointTrajectory()
goal.trajectory.joint_names = self.joint_names
goal.trajectory.header.stamp = rospy.Time.now()
curr_joints = self.current_joints
time_from_start = 0.
for next_joints in joint_traj:
next_position = self.fk(next_joints).position
curr_position = self.fk(curr_joints).position
duration = ((next_position - curr_position).norm)/speed
time_from_start += duration
waypoint = tm.JointTrajectoryPoint()
waypoint.positions = next_joints
waypoint.time_from_start = rospy.Duration(time_from_start)
goal.trajectory.points.append(waypoint)
curr_joints = next_joints
self.joint_command_client.send_goal(goal)
#self.joint_command_pub.publish(goal.trajectory)
if block:
rospy.sleep(time_from_start)
else:
return time_from_start
def process_service(self, req):
if req.play_backward:
rospy.loginfo('TrajPlayback: Playing Reverse (%s)' % self.name)
else:
rospy.loginfo('TrajPlayback: Playing Forward (%s)' % self.name)
if req.play_backward:
tq = self.q[:, ::-1].copy()
tvel = -1 * self.vel[:, ::-1].copy()
tvel[:, -1] = 0
tt = self.t.copy()
tt[0] = 0
tt[1:] += 0.1
else:
tq = self.q.copy()
tvel = self.vel.copy()
tt = self.t.copy()
if self.arm == 'right':
joint_traj = self.pr2.right.create_trajectory(tq, tt, tvel)
else:
joint_traj = self.pr2.left.create_trajectory(tq, tt, tvel)
dur = rospy.Duration.from_sec(tt[-1] + 5)
# result = self.filter_traj( trajectory = joint_traj,
# allowed_time = dur)
# ft = result.trajectory
# ft.header.stamp = rospy.get_rostime() + rospy.Duration( 1.0 )
g = pm.JointTrajectoryGoal()
g.trajectory = joint_traj # why not ft...?
if self.arm == 'right':
self.pr2.right.client.send_goal(g)
self.pr2.right.client.wait_for_result()
else:
self.pr2.left.client.send_goal(g)
self.pr2.left.client.wait_for_result()
return True
def set_poses(self,
pos_mat,
times,
vel_mat=None,
block=True,
allow_spins=False):
#p = self.pose()
#for i in range(pos_mat.shape[1]):
# pos_mat[4,i] = unwrap2(p[4,0], pos_mat[4,i])
# pos_mat[6,i] = unwrap2(p[6,0], pos_mat[6,i])
# p = pos_mat[:,i]
cur_pose = self.pose()
#import pdb
#pdb.set_trace()
pos_mat[4, :] = np.matrix(
angle_consistency_check(cur_pose[4, 0], pos_mat[4, :].A1,
allow_spins))
pos_mat[6, :] = np.matrix(
angle_consistency_check(cur_pose[6, 0], pos_mat[6, :].A1,
allow_spins))
pos_mat = np.column_stack([cur_pose, pos_mat])
#print 'SETPOSES', times, times.__class__
times = np.concatenate(([0], times))
times = times + .1
#print "SET POSES", pos_mat.shape, len(times)
joint_traj = Joint._create_trajectory(self, pos_mat, times, vel_mat)
#Create goal msg
#joint_traj.header.stamp = rospy.get_rostime() + rospy.Duration(5.)
g = pm.JointTrajectoryGoal()
g.trajectory = joint_traj
#g.trajectory.header.stamp = rospy.get_rostime() + rospy.Duration(1.)
self.client.send_goal(g)
if block:
return self.client.wait_for_result()
return self.client.get_state()
#print t
pr2 = PR2()
rospy.wait_for_service('trajectory_filter/filter_trajectory')
filter_traj = rospy.ServiceProxy('trajectory_filter/filter_trajectory',
FilterJointTrajectory)
#print q.shape
#vel = vel[:,0:trim_val]
#t = t[0:trim_val]
if not UNTUCK:
q = q[:, ::-1]
vel = -vel[:, ::-1]
vel[:, -1] = 0
#vel[:,0] = np.mat([[0.1] * 7]).T
#t[-1] = t[-1] - 2
t[0] = 0
t[1:] += 0.1
#print q
#print vel
joint_traj = pr2.right._create_trajectory(q, t, vel)
result = filter_traj(trajectory=joint_traj,
allowed_time=rospy.Duration.from_sec(20))
filtered_traj = result.trajectory
filtered_traj.header.stamp = rospy.get_rostime() + rospy.Duration(1.)
g = pm.JointTrajectoryGoal()
g.trajectory = joint_traj
pr2.right.client.send_goal(g)
#pr2.right.set_poses(q, t, vel)
#def set_poses(self, pos_mat, times, vel_mat=None, block=True):