def createHandGoal(side, j1, j2, j3):
"""Creates a FollowJointTrajectoryGoal with the values specified in j1, j2 and j3 for the joint positions
with the hand specified in side
@arg side string 'right' or 'left'
@arg j1 float value for joint 'hand_'+side+'_thumb_joint'
@arg j2 float value for joint 'hand_'+side+'_middle_joint'
@arg j3 float value for joint 'hand_'+side+'_index_joint'
@return FollowJointTrajectoryGoal with the specified goal"""
fjtg = FollowJointTrajectoryGoal()
fjtg.trajectory.joint_names.append('hand_'+side+'_thumb_joint')
fjtg.trajectory.joint_names.append('hand_'+side+'_middle_joint')
fjtg.trajectory.joint_names.append('hand_'+side+'_index_joint')
point = JointTrajectoryPoint()
point.positions.append(j1)
point.positions.append(j2)
point.positions.append(j3)
point.velocities.append(0.0)
point.velocities.append(0.0)
point.velocities.append(0.0)
point.time_from_start = rospy.Duration(4.0)
fjtg.trajectory.points.append(point)
for joint in fjtg.trajectory.joint_names: # Specifying high tolerances for the hand as they are slow compared to other hardware
goal_tol = JointTolerance()
goal_tol.name = joint
goal_tol.position = 5.0
goal_tol.velocity = 5.0
goal_tol.acceleration = 5.0
fjtg.goal_tolerance.append(goal_tol)
fjtg.goal_time_tolerance = rospy.Duration(3)
fjtg.trajectory.header.stamp = rospy.Time.now()
return fjtg
def createHandGoal(side, j1, j2, j3):
"""Creates a FollowJointTrajectoryGoal with the values specified in j1, j2 and j3 for the joint positions
with the hand specified in side
@arg side string 'right' or 'left'
@arg j1 float value for joint 'hand_'+side+'_thumb_joint'
@arg j2 float value for joint 'hand_'+side+'_middle_joint'
@arg j3 float value for joint 'hand_'+side+'_index_joint'
@return FollowJointTrajectoryGoal with the specified goal"""
fjtg = FollowJointTrajectoryGoal()
fjtg.trajectory.joint_names.append('hand_' + side + '_thumb_joint')
fjtg.trajectory.joint_names.append('hand_' + side + '_middle_joint')
fjtg.trajectory.joint_names.append('hand_' + side + '_index_joint')
point = JointTrajectoryPoint()
point.positions.append(j1)
point.positions.append(j2)
point.positions.append(j3)
point.velocities.append(0.0)
point.velocities.append(0.0)
point.velocities.append(0.0)
point.time_from_start = rospy.Duration(4.0)
fjtg.trajectory.points.append(point)
for joint in fjtg.trajectory.joint_names: # Specifying high tolerances for the hand as they are slow compared to other hardware
goal_tol = JointTolerance()
goal_tol.name = joint
goal_tol.position = 5.0
goal_tol.velocity = 5.0
goal_tol.acceleration = 5.0
fjtg.goal_tolerance.append(goal_tol)
fjtg.goal_time_tolerance = rospy.Duration(3)
fjtg.trajectory.header.stamp = rospy.Time.now()
return fjtg
def createHeadGoal(j1, j2):
"""Creates a FollowJointTrajectoryGoal with the values specified in j1 and j2 for the joint positions
@arg j1 float value for head_1_joint
@arg j2 float value for head_2_joint
@returns FollowJointTrajectoryGoal with the specified goal"""
fjtg = FollowJointTrajectoryGoal()
fjtg.trajectory.joint_names.append('head_1_joint')
fjtg.trajectory.joint_names.append('head_2_joint')
point = JointTrajectoryPoint()
point.positions.append(j1)
point.positions.append(j2)
point.velocities.append(0.0)
point.velocities.append(0.0)
point.time_from_start = rospy.Duration(4.0)
for joint in fjtg.trajectory.joint_names: # Specifying high tolerances for the hand as they are slow compared to other hardware
goal_tol = JointTolerance()
goal_tol.name = joint
goal_tol.position = 5.0
goal_tol.velocity = 5.0
goal_tol.acceleration = 5.0
fjtg.goal_tolerance.append(goal_tol)
fjtg.goal_time_tolerance = rospy.Duration(3)
fjtg.trajectory.points.append(point)
fjtg.trajectory.header.stamp = rospy.Time.now()
return fjtg
def make_joint_tolerance(pos, name):
j = JointTolerance()
j.position = pos
j.velocity = 1
j.acceleration = 100 # this is high because if we bump the arm, the controller will abort unnecessarily
j.name = name
return j
def execute(self, userdata):
rospy.loginfo('In create_move_group_joints_goal')
_move_joint_goal = FollowJointTrajectoryGoal()
_move_joint_goal.trajectory.joint_names = userdata.move_joint_list
jointTrajectoryPoint = JointTrajectoryPoint()
jointTrajectoryPoint.positions = userdata.move_joint_poses
for x in range(0, len(userdata.move_joint_poses)):
jointTrajectoryPoint.velocities.append(0.0)
jointTrajectoryPoint.time_from_start = rospy.Duration(2.0)
_move_joint_goal.trajectory.points.append(jointTrajectoryPoint)
for joint in _move_joint_goal.trajectory.joint_names:
goal_tol = JointTolerance()
goal_tol.name = joint
goal_tol.position = 5.0
goal_tol.velocity = 5.0
goal_tol.acceleration = 5.0
_move_joint_goal.goal_tolerance.append(goal_tol)
_move_joint_goal.goal_time_tolerance = rospy.Duration(2.0)
_move_joint_goal.trajectory.header.stamp = rospy.Time.now()
rospy.loginfo('Move_Joint_GOAL: ' + str(_move_joint_goal))
userdata.move_joint_goal = _move_joint_goal
return 'succeeded'
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 create_position_goal(self, absolute, position_setpoint, velocity):
current_position = self.message.position
# set up variables
trajectory = JointTrajectory()
point = JointTrajectoryPoint()
# Final position
if (absolute == ActuatorsState.ABS):
# absolute move
point.positions = [position_setpoint]
else:
# Relative move
point.positions = [current_position + position_setpoint]
point.velocities = [0] # cubic spline in position, smooth moves
# point.accelerations = [0] # cubic spline in position, smooth moves
trajectory.points.append(point)
# How long to get there
if (velocity != 0):
time_for_move = abs(current_position -
position_setpoint) / velocity
else:
time_for_move = 0 # porbably should handle different
if (time_for_move <= 0.1):
time_for_move = 0.1 # probably asserting joint.
trajectory.points[0].time_from_start = rospy.Duration(time_for_move)
# specify which joint to move
trajectory.joint_names.append(self.message.name)
follow_trajectory_goal = FollowJointTrajectoryGoal()
follow_trajectory_goal.trajectory = trajectory
# TOLERANCES FOR MOVE
# For some reason the defaults are not being loaded from control.yaml
tolerance = JointTolerance()
tolerance.name = self.message.name # joint name
tolerance.velocity = -1 # no fault for velocity
tolerance.acceleration = -1 # no fault for acceleration
# Tracking fault
tolerance.position = -1
follow_trajectory_goal.path_tolerance.append(tolerance)
# End position fault
tolerance.position = -1 # p.constraints.goal_pos
follow_trajectory_goal.goal_tolerance.append(tolerance)
# Time to reach goal precision
follow_trajectory_goal.goal_time_tolerance = rospy.Duration(10.0)
return follow_trajectory_goal
def execute(self,traj,speed=DEFAULT_SPEED,joint_tolerance=DEFAULT_JOINT_TOLERANCE,speed_tolerance=DEFAULT_SPEED_TOLERANCE,timeout=DEFAULT_TIMEOUT,path_tolerance=0):
traj = deepcopy(traj)
self.applyTrajectorySpeed(traj,speed)
rospy.logdebug("Performing trajectory on %s at speed %f tolerance %f speed_tolerance %f timeout %f"%(self.side,speed,joint_tolerance,speed_tolerance,timeout))
joint_names=traj.joint_trajectory.joint_names
self.waitForStopped()
with self.moving_lock:
if self.is_moving!=0:
rospy.logerr("ERROR: trajectory already running")
return False
self.is_moving=1
goal=FollowJointTrajectoryActionGoal()
self.current_goal_id+=1
goal.goal_id.id=self.side+"_"+str(self.current_goal_id)
goal.goal.trajectory=traj.joint_trajectory
if path_tolerance<=0: path_tolerance=DEFAULT_PATH_TOLERANCE*speed
for name in joint_names:
t=JointTolerance()
t.name=name
t.position=joint_tolerance
t.velocity=speed_tolerance
t.acceleration=0
goal.goal.goal_tolerance.append(t)
pt=JointTolerance()
pt.name=name
if name.find("w2") is False:
pt.position=path_tolerance
else:
pt.position=0.0
goal.goal.path_tolerance.append(pt)
goal.goal.goal_time_tolerance=rospy.Duration(timeout)
# Send goal and wait for trajectory finished
self.pub_traj_goal.publish(goal)
self.waitForStopped()
return not self.result_error
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))
p2 = JointTrajectoryPoint()
p2.positions = [0.5, 0.5, 0, 0, 0, 0]
p2.velocities = [0, 0, 0, 0, 0, 0]
p2.time_from_start = rospy.Duration.from_sec(5.0)
p3 = JointTrajectoryPoint()
p3.positions = [0, 0, 0, 0, 0, 0]
p3.velocities = [0, 0, 0, 0, 0, 0]
p3.time_from_start = rospy.Duration.from_sec(7.0)
goal.trajectory.points = [p1, p2, p3]
pt1 = JointTolerance()
pt1.name = 'joint_1'
pt1.position = 10.1
pt1.velocity = 10.1
pt1.acceleration = 10.1
pt2 = JointTolerance()
pt2.name = 'joint_2'
pt2.position = 10.1
pt2.velocity = 10.1
pt2.acceleration = 10.1
pt3 = JointTolerance()
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
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):
# Clear data
userdata.joint_tolerances = None
self.return_code = None
try:
num_names = len(userdata.joint_names)
num_posn = len(self.position_constraints)
num_vel = len(self.velocity_constraints)
num_acc = len(self.acceleration_constraints)
if (num_posn > 1 and num_posn != num_names):
Logger.logwarn(
'Mis-match in joint names and position constraints -%d vs. %d'
% (num_names, num_posn))
self.return_code = 'param_error'
if (num_vel > 1 and num_vel != num_names):
Logger.logwarn(
'Mis-match in joint names and position constraints - %d vs. %d'
% (num_names, num_vel))
self.return_code = 'param_error'
if (num_acc > 1 and num_acc != num_names):
Logger.logwarn(
'Mis-match in joint names and position constraints - %d vs. %d'
% (num_names, num_acc))
self.return_code = 'param_error'
if self.return_code is not None:
return self.return_code
tolerances = []
for ndx, name in enumerate(userdata.joint_names):
jt = JointTolerance()
jt.name = name
if (num_posn > 1):
jt.position = self.position_constraints[ndx]
else:
jt.position = self.position_constraints[0]
if (num_vel > 1):
jt.velocity = self.velocity_constraints[ndx]
else:
jt.velocity = self.velocity_constraints[0]
if (num_acc > 1):
jt.acceleration = self.acceleration_constraints[ndx]
else:
jt.acceleration = self.acceleration_constraints[0]
tolerances.append(jt)
# Successfully set the joint tolerances and added to user data
userdata.joint_tolerances = tolerances
self.return_code = 'configured'
except Exception as e:
Logger.logwarn('Unable to configure joint tolerances for %s \n%s' %
(self.name, str(e)))
self.return_code = 'param_error'
return