def build_follow_trajectory(self, traj):
# Build 'Follow Joint Trajectory' goal from trajectory (includes tolerances for error)
traj_goal = FollowJointTrajectoryGoal()
traj_goal.trajectory = traj
tolerance = JointTolerance()
tolerance.position = 0.05
tolerance.velocity = 0.1
traj_goal.path_tolerance = [tolerance for i in range(len(traj.points))]
traj_goal.goal_tolerance = [tolerance for i in range(len(traj.points))]
traj_goal.goal_time_tolerance = rospy.Duration(3)
return traj_goal
def build_follow_trajectory(self, traj):
# Build 'Follow Joint Trajectory' goal from trajectory (includes tolerances for error)
traj_goal = FollowJointTrajectoryGoal()
traj_goal.trajectory = traj
tolerance = JointTolerance()
tolerance.position = 0.05
tolerance.velocity = 0.1
traj_goal.path_tolerance = [tolerance for i in range(len(traj.points))]
traj_goal.goal_tolerance = [tolerance for i in range(len(traj.points))]
traj_goal.goal_time_tolerance = rospy.Duration(3)
return traj_goal
def receiverLoop(self):
while True:
try:
input_msg = pickle.load(open("/dev/shm/right_arm_goal.p",
"rb"))
os.remove("/dev/shm/right_arm_goal.p")
except IOError:
print "file not there..."
rospy.sleep(0.1)
continue
print input_msg
goal = FollowJointTrajectoryGoal()
rospy.loginfo("input_msg.goal.goal_time_tolerance: \n" +
str(input_msg.goal.goal_time_tolerance))
goal.goal_time_tolerance = rospy.Duration(secs=0, nsecs=0)
rospy.loginfo("input_msg.goal.goal_tolerance: \n" +
str(input_msg.goal.goal_tolerance))
goal.goal_tolerance = input_msg.goal.goal_tolerance
rospy.loginfo("input_msg.goal.path_tolerance: \n" +
str(input_msg.goal.path_tolerance))
goal.path_tolerance = input_msg.goal.path_tolerance
rospy.loginfo("input_msg.goal.trajectory.header: \n" +
str(input_msg.goal.trajectory.header))
goal.trajectory.header = input_msg.goal.trajectory.header
goal.trajectory.header.frame_id = '/base_link'
goal.trajectory.joint_names = input_msg.goal.trajectory.joint_names
input_times = pickle.load(
open("/dev/shm/right_arm_goal_times.p", "rb"))
rospy.loginfo("input_times is:\n" + str(input_times))
for input_point, time in zip(input_msg.goal.trajectory.points,
input_times):
p = JointTrajectoryPoint()
p.accelerations = input_point.accelerations
p.positions = input_point.positions
p.velocities = input_point.velocities
# rospy.loginfo("???????????????????????????????????????????????????????????????")
# rospy.loginfo("Time from start is:\n" + str(input_point.time_from_start))
# rospy.loginfo("time is:\n" + str(time))
# rospy.loginfo("???????????????????????????????????????????????????????????????")
p.time_from_start = time
goal.trajectory.points.append(p)
os.remove("/dev/shm/right_arm_goal_times.p")
#print "Goal will be:"
rospy.loginfo("!!!!!!!!!!!Goal:\n" + str(goal))
self.right_arm_as.send_goal(goal)
rospy.loginfo("Waiting for result")
self.right_arm_as.wait_for_result()
rospy.loginfo("Goal done")
def receiverLoop(self):
while True:
try:
input_msg = pickle.load( open( "/dev/shm/right_arm_goal.p", "rb" ) )
os.remove("/dev/shm/right_arm_goal.p")
except IOError:
print "file not there..."
rospy.sleep(0.1)
continue
print input_msg
goal = FollowJointTrajectoryGoal()
rospy.loginfo("input_msg.goal.goal_time_tolerance: \n" + str(input_msg.goal.goal_time_tolerance ) )
goal.goal_time_tolerance = rospy.Duration(secs=0, nsecs=0)
rospy.loginfo("input_msg.goal.goal_tolerance: \n" + str(input_msg.goal.goal_tolerance ) )
goal.goal_tolerance = input_msg.goal.goal_tolerance
rospy.loginfo("input_msg.goal.path_tolerance: \n" + str(input_msg.goal.path_tolerance ) )
goal.path_tolerance = input_msg.goal.path_tolerance
rospy.loginfo("input_msg.goal.trajectory.header: \n" + str(input_msg.goal.trajectory.header ) )
goal.trajectory.header = input_msg.goal.trajectory.header
goal.trajectory.header.frame_id = '/base_link'
goal.trajectory.joint_names = input_msg.goal.trajectory.joint_names
input_times = pickle.load( open( "/dev/shm/right_arm_goal_times.p", "rb" ) )
rospy.loginfo("input_times is:\n" + str(input_times))
for input_point, time in zip(input_msg.goal.trajectory.points, input_times):
p = JointTrajectoryPoint()
p.accelerations = input_point.accelerations
p.positions = input_point.positions
p.velocities = input_point.velocities
# rospy.loginfo("???????????????????????????????????????????????????????????????")
# rospy.loginfo("Time from start is:\n" + str(input_point.time_from_start))
# rospy.loginfo("time is:\n" + str(time))
# rospy.loginfo("???????????????????????????????????????????????????????????????")
p.time_from_start = time
goal.trajectory.points.append(p)
os.remove("/dev/shm/right_arm_goal_times.p")
#print "Goal will be:"
rospy.loginfo("!!!!!!!!!!!Goal:\n" +str(goal))
self.right_arm_as.send_goal(goal)
rospy.loginfo("Waiting for result")
self.right_arm_as.wait_for_result()
rospy.loginfo("Goal done")
def receiverLoop(self):
while True:
print "self.conn.recv()"
input_msg = self.conn.recv()
print input_msg
goal = FollowJointTrajectoryGoal()
goal.goal_time_tolerance = input_msg.goal_time_tolerance
goal.goal_tolerance = input_msg.goal_tolerance
goal.path_tolerance = input_msg.path_tolerance
goal.trajectory.header = input_msg.trajectory.header
goal.trajectory.joint_names = input_msg.joint_names
for input_point in input_msg.trajectory.points:
p = JointTrajectoryPoint()
p.accelerations = input_point.accelerations
p.positions = input_point.positions
p.velocities = input_point.velocities
p.time_from_start = input_point.time_from_start
goal.trajectory.points.append(p)
print "Goal will be:"
print goal
#self.right_arm_as.send_goal(goal)
rospy.loginfo("Waiting for result")
self.right_arm_as.wait_for_result()
rospy.loginfo("Goal done")
def receiverLoop(self):
while True:
print "self.conn.recv()"
input_msg = self.conn.recv()
print input_msg
goal = FollowJointTrajectoryGoal()
goal.goal_time_tolerance = input_msg.goal_time_tolerance
goal.goal_tolerance = input_msg.goal_tolerance
goal.path_tolerance = input_msg.path_tolerance
goal.trajectory.header = input_msg.trajectory.header
goal.trajectory.joint_names = input_msg.joint_names
for input_point in input_msg.trajectory.points:
p = JointTrajectoryPoint()
p.accelerations = input_point.accelerations
p.positions = input_point.positions
p.velocities = input_point.velocities
p.time_from_start = input_point.time_from_start
goal.trajectory.points.append(p)
print "Goal will be:"
print goal
#self.right_arm_as.send_goal(goal)
rospy.loginfo("Waiting for result")
self.right_arm_as.wait_for_result()
rospy.loginfo("Goal done")
def wipe(self, start, finish):
dist = int(round(200*self.calc_dist(start, finish))) # 1 point per cm of separation
print "Steps: %s" %dist
x_step = finish.pose.position.x - start.pose.position.x
y_step = finish.pose.position.y - start.pose.position.y
z_step = finish.pose.position.z - start.pose.position.z
#print "Increments: %s,%s,%s" %(x_step, y_step, z_step)
qs = [start.pose.orientation.x, start.pose.orientation.y, start.pose.orientation.z, start.pose.orientation.w]
qf = [finish.pose.orientation.x, finish.pose.orientation.y, finish.pose.orientation.z, finish.pose.orientation.w]
steps = []
#print "Start: %s" %start
#print "Finish: %s" %finish
for i in range(dist):
frac = float(i)/float(dist)
steps.append(PoseStamped())
steps[i].header.stamp = rospy.Time.now()
steps[i].header.frame_id = start.header.frame_id
steps[i].pose.position.x = start.pose.position.x + x_step*frac
steps[i].pose.position.y = start.pose.position.y + y_step*frac
steps[i].pose.position.z = start.pose.position.z + z_step*frac
new_q = transformations.quaternion_slerp(qs,qf,frac)
steps[i].pose.orientation.x = new_q[0]
steps[i].pose.orientation.y = new_q[1]
steps[i].pose.orientation.z = new_q[2]
steps[i].pose.orientation.w = new_q[3]
steps.append(finish)
#print "Steps:"
#print steps
#raw_input("Press Enter to continue")
rospy.loginfo("Planning straight-line path, please wait")
self.wt_log_out.publish(data="Planning straight-line path, please wait")
rospy.loginfo("Initiating wipe action")
self.blind_move(finish)
self.r_arm_traj_client.wait_for_result(rospy.Duration(20))
rospy.loginfo("At beginning of path")
pts = []
for i, p in enumerate(steps):
frac = float(i)/float(len(steps))
request = self.form_ik_request(p)
if not i == 0:
request.ik_request.ik_seed_state.joint_state.position = seed
ik_goal = self.ik_pose_proxy(request)
pts.append(ik_goal.solution.joint_state.position)
seed = pts[i]
points = []
for i in range(len(pts)-1):
angs1 = pts[i]
angs2 = pts[i+1]
increm = np.subtract(angs2, angs1)
for j in range(10):
points.append(np.add(angs1, np.multiply(0.1*j, increm)))
#points = np.unwrap(points,1)
p1= points
traj = JointTrajectory()
traj.header.frame_id = steps[0].header.frame_id
traj.joint_names = self.ik_info.kinematic_solver_info.joint_names
times = []
for i in range(len(points)):
frac = float(i)/float(len(points))
traj.points.append(JointTrajectoryPoint())
traj.points[i].positions = points[i]
traj.points[i].velocities = [0]*7
times.append(rospy.Duration(frac*dist*0.2))
traj_goal = FollowJointTrajectoryGoal()
traj_goal.trajectory = traj
tolerance = JointTolerance()
tolerance.position = 0.05
tolerance.velocity = 0.1
traj_goal.path_tolerance = [tolerance for i in range(len(traj.points))]
traj_goal.goal_tolerance = [tolerance for i in range(len(traj.points))]
traj_goal.goal_time_tolerance = rospy.Duration(3)
#print "Steps: %s" %steps
count = 0
while count < 6:
traj_goal.trajectory.points.reverse()
for i in range(len(times)):
traj_goal.trajectory.points[i].time_from_start = times[i]
if count == 0:
print traj_goal.trajectory.points
raw_input("Review Trajectory Goal")
traj_goal.trajectory.header.stamp = rospy.Time.now()
self.r_arm_follow_traj_client.send_goal(traj_goal)
self.r_arm_follow_traj_client.wait_for_result(rospy.Duration(20))
rospy.sleep(0.5)
count += 1
#traj_goal = JointTrajectoryGoal()
#traj_goal.trajectory = traj
#print "Steps: %s" %steps
#count = 0
#
#while count < 6:
#traj_goal.trajectory.points.reverse()
#for i in range(len(times)):
#traj_goal.trajectory.points[i].time_from_start = times[i]
#print traj_goal
#raw_input("Review Trajectory Goal")
##print "Traj goal start:"
##print traj_goal.trajectory.points[0].positions
##print "Traj goal end:"
##print traj_goal.trajectory.points[-1].positions
#traj_goal.trajectory.header.stamp = rospy.Time.now()
#self.r_arm_traj_client.send_goal(traj_goal)
#self.r_arm_traj_client.wait_for_result(rospy.Duration(20))
#rospy.sleep(0.5)
#count += 1
rospy.loginfo("Done Wiping")
self.wt_log_out.publish(data="Done Wiping")
self.linear_move(Float32(-0.15))
pt3.name = 'joint_3'
pt3.position = 10.1
pt3.velocity = 10.1
pt3.acceleration = 10.1
pt4 = JointTolerance()
pt4.name = 'joint_4'
pt4.position = 10.1
pt4.velocity = 10.1
pt4.acceleration = 10.1
pt5 = JointTolerance()
pt5.name = 'joint_5'
pt5.position = 10.1
pt5.velocity = 10.1
pt5.acceleration = 10.1
pt6 = JointTolerance()
pt6.name = 'joint_6'
pt6.position = 10.1
pt6.velocity = 10.1
pt6.acceleration = 0.1
goal.path_tolerance = [pt1, pt2, pt3, pt4, pt5, pt6]
goal.goal_tolerance = [pt1, pt2, pt3, pt4, pt5, pt6]
goal.goal_time_tolerance = rospy.Duration.from_sec(10)
client.send_goal(goal)
client.wait_for_result(rospy.Duration.from_sec(100.0))
print(goal)
rospy.spin()
def wipe(self, start, finish):
dist = int(round(
200 *
self.calc_dist(start, finish))) # 1 point per cm of separation
print "Steps: %s" % dist
x_step = finish.pose.position.x - start.pose.position.x
y_step = finish.pose.position.y - start.pose.position.y
z_step = finish.pose.position.z - start.pose.position.z
#print "Increments: %s,%s,%s" %(x_step, y_step, z_step)
qs = [
start.pose.orientation.x, start.pose.orientation.y,
start.pose.orientation.z, start.pose.orientation.w
]
qf = [
finish.pose.orientation.x, finish.pose.orientation.y,
finish.pose.orientation.z, finish.pose.orientation.w
]
steps = []
#print "Start: %s" %start
#print "Finish: %s" %finish
for i in range(dist):
frac = float(i) / float(dist)
steps.append(PoseStamped())
steps[i].header.stamp = rospy.Time.now()
steps[i].header.frame_id = start.header.frame_id
steps[i].pose.position.x = start.pose.position.x + x_step * frac
steps[i].pose.position.y = start.pose.position.y + y_step * frac
steps[i].pose.position.z = start.pose.position.z + z_step * frac
new_q = transformations.quaternion_slerp(qs, qf, frac)
steps[i].pose.orientation.x = new_q[0]
steps[i].pose.orientation.y = new_q[1]
steps[i].pose.orientation.z = new_q[2]
steps[i].pose.orientation.w = new_q[3]
steps.append(finish)
#print "Steps:"
#print steps
#raw_input("Press Enter to continue")
rospy.loginfo("Planning straight-line path, please wait")
self.wt_log_out.publish(
data="Planning straight-line path, please wait")
rospy.loginfo("Initiating wipe action")
self.blind_move(finish)
self.r_arm_traj_client.wait_for_result(rospy.Duration(20))
rospy.loginfo("At beginning of path")
pts = []
for i, p in enumerate(steps):
frac = float(i) / float(len(steps))
request = self.form_ik_request(p)
if not i == 0:
request.ik_request.ik_seed_state.joint_state.position = seed
ik_goal = self.ik_pose_proxy(request)
pts.append(ik_goal.solution.joint_state.position)
seed = pts[i]
points = []
for i in range(len(pts) - 1):
angs1 = pts[i]
angs2 = pts[i + 1]
increm = np.subtract(angs2, angs1)
for j in range(10):
points.append(np.add(angs1, np.multiply(0.1 * j, increm)))
#points = np.unwrap(points,1)
p1 = points
traj = JointTrajectory()
traj.header.frame_id = steps[0].header.frame_id
traj.joint_names = self.ik_info.kinematic_solver_info.joint_names
times = []
for i in range(len(points)):
frac = float(i) / float(len(points))
traj.points.append(JointTrajectoryPoint())
traj.points[i].positions = points[i]
traj.points[i].velocities = [0] * 7
times.append(rospy.Duration(frac * dist * 0.2))
traj_goal = FollowJointTrajectoryGoal()
traj_goal.trajectory = traj
tolerance = JointTolerance()
tolerance.position = 0.05
tolerance.velocity = 0.1
traj_goal.path_tolerance = [tolerance for i in range(len(traj.points))]
traj_goal.goal_tolerance = [tolerance for i in range(len(traj.points))]
traj_goal.goal_time_tolerance = rospy.Duration(3)
#print "Steps: %s" %steps
count = 0
while count < 6:
traj_goal.trajectory.points.reverse()
for i in range(len(times)):
traj_goal.trajectory.points[i].time_from_start = times[i]
if count == 0:
print traj_goal.trajectory.points
raw_input("Review Trajectory Goal")
traj_goal.trajectory.header.stamp = rospy.Time.now()
self.r_arm_follow_traj_client.send_goal(traj_goal)
self.r_arm_follow_traj_client.wait_for_result(rospy.Duration(20))
rospy.sleep(0.5)
count += 1
#traj_goal = JointTrajectoryGoal()
#traj_goal.trajectory = traj
#print "Steps: %s" %steps
#count = 0
#
#while count < 6:
#traj_goal.trajectory.points.reverse()
#for i in range(len(times)):
#traj_goal.trajectory.points[i].time_from_start = times[i]
#print traj_goal
#raw_input("Review Trajectory Goal")
##print "Traj goal start:"
##print traj_goal.trajectory.points[0].positions
##print "Traj goal end:"
##print traj_goal.trajectory.points[-1].positions
#traj_goal.trajectory.header.stamp = rospy.Time.now()
#self.r_arm_traj_client.send_goal(traj_goal)
#self.r_arm_traj_client.wait_for_result(rospy.Duration(20))
#rospy.sleep(0.5)
#count += 1
rospy.loginfo("Done Wiping")
self.wt_log_out.publish(data="Done Wiping")
self.linear_move(Float32(-0.15))
def on_enter(self, userdata):
self.return_code = None
self.current_action_topic = self.given_action_topic
self.goal_names = None
self.goal_values = None
try:
if self.current_action_topic is None:
#Logger.logwarn(" %s - using the action topic to %s from %s for client!"%(self.name, userdata.trajectory_action_topic, self.current_action_topic) )
self.current_action_topic = userdata.trajectory_action_topic
if self.action_client is None:
self.action_client = ProxyActionClient(
{self.current_action_topic: FollowJointTrajectoryAction},
self.wait_duration)
Logger.loginfo('%s - created ProxyActionClient for %s!' %
(self.name, self.current_action_topic))
if not self.action_client.is_available(self.current_action_topic):
self.action_client.setupClient(self.current_action_topic,
FollowJointTrajectoryAction,
self.wait_duration)
if not self.action_client.is_available(
self.current_action_topic):
self.status_text = '%s - no connection to %s available!' % (
self.name, self.current_action_topic)
self.return_code = 'param_error'
Logger.loginfo(self.status_text)
return
# Action Initialization
action_goal = FollowJointTrajectoryGoal()
if isinstance(userdata.joint_trajectory, RobotTrajectory):
Logger.loginfo('%s - using RobotTrajectory ' % self.name)
action_goal.trajectory = userdata.joint_trajectory.joint_trajectory
elif isinstance(userdata.joint_trajectory, JointTrajectory):
Logger.loginfo('%s - using JointTrajectory ' % self.name)
action_goal.trajectory = userdata.joint_trajectory
else:
self.status_text = '%s - user data does not have valid trajectory message!\n Must be RobotTrajectory or JointTrajectory type!' % (
self.name)
self.return_code = 'param_error'
Logger.logerr(self.status_text)
return
action_goal.path_tolerance = userdata.joint_path_tolerances
action_goal.goal_tolerance = userdata.joint_goal_tolerances
action_goal.goal_time_tolerance = self.goal_time_tolerance
# Validate that the trajectory and constraints are consistent
num_joints = len(action_goal.trajectory.points[0].positions)
num_path = len(action_goal.path_tolerance)
num_goal = len(action_goal.goal_tolerance)
if num_joints != num_path:
self.status_text = '%s - Invalid number of path constraints %d vs. %d for %s' % (
self.name, num_path, num_joints, self.current_action_topic)
self.return_code = 'param_error'
if num_joints != num_goal:
self.status_text = '%s - Invalid number of goal constraints %d vs. %d for %s' % (
self.name, num_path, num_joints, self.current_action_topic)
self.return_code = 'param_error'
for c, name in enumerate(action_goal.trajectory.joint_names):
if action_goal.path_tolerance[c].name != name:
self.status_text = '%s - %s name mismatch %s vs. %s for path constraints' % (
self.name, self.current_action_topic,
action_goal.path_tolerance[c].name, name)
self.return_code = 'param_error'
if action_goal.goal_tolerance[c].name != name:
self.status_text = '%s - %s name mismatch %s vs. %s for goal constraints' % (
self.name, self.current_action_topic,
action_goal.goal_tolerance[c].name, name)
self.return_code = 'param_error'
if self.return_code is not None:
# Something went wrong
Logger.logerr(self.status_text)
return
# Check that our trajectory is still fresh
elapsed = rospy.Time.now() - action_goal.trajectory.header.stamp
if self.max_delay is not None and elapsed > self.max_delay:
self.status_text = '%s - Stale trajectory - %f > %f - do not send action for %s' % (
self.name, elapsed.to_sec(), self.max_delay.to_sec(),
self.current_action_topic)
self.return_code = 'param_error'
Logger.logerr(self.status_text)
return
elif elapsed.to_sec() > 2.0:
Logger.logwarn(
"%s - trajectory older than %f - send anyway!" %
(self.name, elapsed.to_sec()))
if (self.timeout_duration > rospy.Duration(0.0)):
self.timeout_target = rospy.Time.now(
) + action_goal.trajectory.points[
-1].time_from_start + self.timeout_duration
else:
self.timeout_target = None
# Good to go for it!
action_goal.trajectory.header.stamp = rospy.Time(
0.0
) # Start from the beginning of the trajectory with this node
self.action_client.send_goal(self.current_action_topic,
action_goal)
self.goal_names = action_goal.trajectory.joint_names
self.goal_values = action_goal.trajectory.points[-1].positions
except Exception as e:
self.status_text = '%s - Failed to send action for %s\n%s' % (
self.name, self.current_action_topic, str(e))
self.return_code = 'param_error'
Logger.logerr(self.status_text)
return
