def _update(self, event):
#update robot state
now = rospy.get_rostime()
msg = JointState()
msg.header.stamp = now
msg.header.frame_id = "From arduino webserver for braccio"
msg.name = joint_names
if self.traj and self.goal_handle:
#rospy.logwarn("UPDATE ************************")
#rospy.logwarn(self.traj)
if self.index == self.lenpoints:
msg_success = 'Trajectory execution successfully completed'
rospy.logwarn(msg_success)
res = FollowJointTrajectoryResult()
res.error_code = FollowJointTrajectoryResult.SUCCESSFUL
self.goal_handle.set_succeeded(result=res, text=msg_success)
self.goal_handle = None
self.index = 0
self.lenpoints = None
self.traj = None
else:
position = self.traj.points[self.index].positions
self.index += 1
self.current_positions = position
msg.position = self.current_positions
#msg.effort = [0] * 5
self.pub_joint_states.publish(msg)
def execute_cb(self, goal):
"""
:type goal: FollowJointTrajectoryGoal
"""
rospy.loginfo("Got a goal!")
# Just resend the goal to the real as
# while checking for cancel goals
# Create goal
g = FollowJointTrajectoryGoal()
# We ignore path_tolerance and goal_tolerance... does not make sense in
# a gripper
g.goal_time_tolerance = goal.goal_time_tolerance
jt = self.substitute_trajectory(goal.trajectory)
g.trajectory = jt
if not self._ac.wait_for_server(rospy.Duration(5.0)):
rospy.logerr("Action server for gripper not found!")
self._as.set_aborted(FollowJointTrajectoryResult())
# Send goal subscribing to feedback to republish it
self._ac.send_goal(g, feedback_cb=self._feedback_cb)
# wait for goal to finish while checking if cancel is requested
while not self._ac.wait_for_result(rospy.Duration(0.1)):
rospy.loginfo("Waiting for goal to finish...")
# Deal with goal cancelled
if self._as.is_preempt_requested():
self._ac.cancel_all_goals()
self._as.set_preempted()
return
result = self._ac.get_result()
res = FollowJointTrajectoryResult()
if result:
res.error_code = FollowJointTrajectoryResult.SUCCESSFUL
self._as.set_succeeded(res)
else:
res.error_code = result
self._as.set_aborted(res)
def execute_cb(self, goal):
"""
:type goal: FollowJointTrajectoryGoal
"""
rospy.loginfo("Got a goal!")
# Just resend the goal to the real as
# while checking for cancel goals
# Create goal
g = FollowJointTrajectoryGoal()
# We ignore path_tolerance and goal_tolerance... does not make sense in
# a gripper
g.goal_time_tolerance = goal.goal_time_tolerance
jt = self.substitute_trajectory(goal.trajectory)
g.trajectory = jt
if not self._ac.wait_for_server(rospy.Duration(5.0)):
rospy.logerr("Action server for gripper not found!")
self._as.set_aborted(FollowJointTrajectoryResult())
# Send goal subscribing to feedback to republish it
self._ac.send_goal(g, feedback_cb=self._feedback_cb)
# wait for goal to finish while checking if cancel is requested
while not self._ac.wait_for_result(rospy.Duration(0.1)):
rospy.loginfo("Waiting for goal to finish...")
# Deal with goal cancelled
if self._as.is_preempt_requested():
self._ac.cancel_all_goals()
self._as.set_preempted()
return
result = self._ac.get_result()
res = FollowJointTrajectoryResult()
if result:
res.error_code = FollowJointTrajectoryResult.SUCCESSFUL
self._as.set_succeeded(res)
else:
res.error_code = result
self._as.set_aborted(res)
def execute_cb(self, goal):
z_only = False
if len(goal.trajectory.joint_names) == 1:
goal.trajectory.joint_names.append("virtual_lifter_x_joint")
z_only = True
if not self.equalJoints(goal.trajectory.joint_names):
rospy.logerr("inaccurate joint names received")
return
print "execute" + rospy.get_name()
print "joint_names"
print goal.trajectory.joint_names
print goal.trajectory.points[-1].positions[0]
try:
srv = rospy.ServiceProxy("aero_torso_controller",
AeroTorsoController)
if z_only:
x = 0
else:
x = goal.trajectory.points[-1].positions[
goal.trajectory.joint_names.index(
"virtual_lifter_x_joint")] * 1000
z = goal.trajectory.points[-1].positions[
goal.trajectory.joint_names.index(
"virtual_lifter_z_joint")] * 1000
res = srv(x, z, "world")
result = FollowJointTrajectoryResult()
if res.status == "success":
time.sleep(res.time_sec)
result.error_code = 0
self._as.set_succeeded()
rospy.loginfo('%s: Succeeded' % self._action_name)
return
#error syori wo kaku
self._as.set_aborted()
except rospy.ServiceException, e:
print "Service call failed: %s" % e
self._as.set_aborted()
def execute_position_cb(self, goal):
is_timed_out = False
is_over_effort = False
over_effort_counter = 0
start = rospy.Time.now()
extra_time = rospy.Duration(5.0)
# print goal
for i in range(len(goal.trajectory.points)):
joint_positions = brics_actuator.msg.JointPositions()
conf = np.array(goal.trajectory.points[i].positions)
# transform from ROS to BRICS message
for j in range(len(self.joint_names)):
joint_value = brics_actuator.msg.JointValue()
joint_value.joint_uri = self.joint_names[j]
joint_value.value = self.limit_joint(self.joint_names[j], conf[j])
conf[j] = joint_value.value
joint_value.unit = self.unit
joint_positions.positions.append(joint_value)
if self.is_over_effort():
over_effort_counter += 1
rospy.logerr("%d, over effort", over_effort_counter)
if over_effort_counter > LIMIT_OVER_EFFORT:
is_over_effort = True
break
self.position_pub.publish(joint_positions)
# wait to reach the goal position
while not rospy.is_shutdown():
if self.is_over_effort():
over_effort_counter += 1
rospy.logerr("%d, over effort", over_effort_counter)
if over_effort_counter > LIMIT_OVER_EFFORT:
is_over_effort = True
break
if self.is_goal_reached(conf, self.configuration):
rospy.sleep(0.01)
break
if (rospy.Time.now() - start) > (goal.trajectory.points[i].time_from_start + extra_time):
is_timed_out = True
break
rospy.sleep(0.01)
if is_timed_out or is_over_effort:
break
result = FollowJointTrajectoryResult()
if is_over_effort:
result.error_code = FollowJointTrajectoryResult.PATH_TOLERANCE_VIOLATED
rospy.logerr("arm over-effort")
self.arm_up_recover()
self.position_action_server.set_preempted(result)
elif is_timed_out:
result.error_code = FollowJointTrajectoryResult.PATH_TOLERANCE_VIOLATED
rospy.logerr("arm timed_out")
self.arm_up_recover()
self.position_action_server.set_aborted(result)
else:
result.error_code = FollowJointTrajectoryResult.SUCCESSFUL
self.position_action_server.set_succeeded(result)
def follow_trajectory_goal(self,
traj_msg: FollowJointTrajectoryGoal,
feedback_cb: Optional[Callable] = None,
stop_cb: Optional[Callable] = None):
result = FollowJointTrajectoryResult()
# Interpolate the trajectory to a fine resolution
# if you set max_step_size to be large and position tolerance to be small, then things will be jerky
if len(traj_msg.trajectory.points) == 0:
rospy.loginfo("Ignoring empty trajectory")
result.error_code = FollowJointTrajectoryResult.SUCCESSFUL
result.error_string = "empty trajectory"
return result
# construct a list of the tolerances in order of the joint names
trajectory_joint_names = traj_msg.trajectory.joint_names
tolerance = get_ordered_tolerance_list(trajectory_joint_names, traj_msg.path_tolerance)
goal_tolerance = get_ordered_tolerance_list(trajectory_joint_names, traj_msg.goal_tolerance, is_goal=True)
interpolated_points = interpolate_joint_trajectory_points(traj_msg.trajectory.points, max_step_size=0.1)
if len(interpolated_points) == 0:
rospy.loginfo("Trajectory was empty after interpolation")
result.error_code = FollowJointTrajectoryResult.SUCCESSFUL
result.error_string = "empty trajectory"
return result
rospy.logdebug(interpolated_points)
trajectory_point_idx = 0
t0 = rospy.Time.now()
while True:
# tiny sleep lets the listeners process messages better, results in smoother following
rospy.sleep(1e-6)
desired_point = interpolated_points[trajectory_point_idx]
command_failed, command_failed_msg = self.robot.send_joint_command(trajectory_joint_names, desired_point)
# get feedback
actual_point = self.get_actual_trajectory_point(trajectory_joint_names)
# let the caller stop
if stop_cb is not None:
stop, stop_msg = stop_cb(actual_point)
else:
stop = None
stop_msg = ""
dt = rospy.Time.now() - t0
error = waypoint_error(actual_point, desired_point)
rospy.logdebug_throttle(1, f"{error} {desired_point.time_from_start.to_sec()} {dt.to_sec()}")
if desired_point.time_from_start.to_sec() > 0 and dt > desired_point.time_from_start * 5.0:
stop = True
if trajectory_point_idx == len(interpolated_points) - 1:
stop_msg = f"timeout. expected t={desired_point.time_from_start.to_sec()} but t={dt.to_sec()}." \
+ f" error to waypoint is {error}, tolerance is {goal_tolerance}"
else:
stop_msg = f"timeout. expected t={desired_point.time_from_start.to_sec()} but t={dt.to_sec()}." \
+ f" error to waypoint is {error}, tolerance is {tolerance}"
if command_failed or stop:
# command the current configuration
self.robot.send_joint_command(trajectory_joint_names, actual_point)
rospy.loginfo("Preempt requested, aborting.")
if command_failed_msg:
rospy.loginfo(command_failed_msg)
if stop_msg:
rospy.logwarn(stop_msg)
result.error_code = -10
result.error_string = stop_msg
break
# If we're close enough, advance
if trajectory_point_idx >= len(interpolated_points) - 1:
if waypoint_reached(desired_point, actual_point, goal_tolerance):
# we're done!
result.error_code = FollowJointTrajectoryResult.SUCCESSFUL
break
else:
if waypoint_reached(desired_point, actual_point, tolerance):
trajectory_point_idx += 1
if feedback_cb is not None:
feedback = FollowJointTrajectoryFeedback()
feedback.header.stamp = rospy.Time.now()
feedback.joint_names = trajectory_joint_names
feedback.desired = desired_point
feedback.actual = actual_point
feedback_cb(feedback)
return result
def process_trajectory(self, traj):
num_points = len(traj.points)
# make sure the joints in the goal match the joints of the controller
if set(self.joint_names) != set(traj.joint_names):
res = FollowJointTrajectoryResult()
res.error_code = FollowJointTrajectoryResult.INVALID_JOINTS
msg = 'Incoming trajectory joints do not match the joints of the controller'
rospy.logerr(msg)
rospy.logerr(' self.joint_names={%s}' % (set(self.joint_names)))
rospy.logerr(' traj.joint_names={%s}' % (set(traj.joint_names)))
self.action_server.set_aborted(result=res, text=msg)
return
# make sure trajectory is not empty
if num_points == 0:
msg = 'Incoming trajectory is empty'
rospy.logerr(msg)
self.action_server.set_aborted(text=msg)
return
# correlate the joints we're commanding to the joints in the message
# map from an index of joint in the controller to an index in the trajectory
lookup = [traj.joint_names.index(joint) for joint in self.joint_names]
durations = [0.0] * num_points
# find out the duration of each segment in the trajectory
durations[0] = traj.points[0].time_from_start.to_sec()
for i in range(1, num_points):
durations[i] = (traj.points[i].time_from_start -
traj.points[i - 1].time_from_start).to_sec()
if not traj.points[0].positions:
res = FollowJointTrajectoryResult()
res.error_code = FollowJointTrajectoryResult.INVALID_GOAL
msg = 'First point of trajectory has no positions'
rospy.logerr(msg)
self.action_server.set_aborted(result=res, text=msg)
return
trajectory = []
time = rospy.Time.now() + rospy.Duration(0.01)
for i in range(num_points):
seg = Segment(self.num_joints)
if traj.header.stamp == rospy.Time(0.0):
seg.start_time = (time + traj.points[i].time_from_start
).to_sec() - durations[i]
else:
seg.start_time = (
traj.header.stamp +
traj.points[i].time_from_start).to_sec() - durations[i]
seg.duration = durations[i]
# Checks that the incoming segment has the right number of elements.
if traj.points[i].velocities and len(
traj.points[i].velocities) != self.num_joints:
res = FollowJointTrajectoryResult()
res.error_code = FollowJointTrajectoryResult.INVALID_GOAL
msg = 'Command point %d has %d elements for the velocities' % (
i, len(traj.points[i].velocities))
rospy.logerr(msg)
self.action_server.set_aborted(result=res, text=msg)
return
if len(traj.points[i].positions) != self.num_joints:
res = FollowJointTrajectoryResult()
res.error_code = FollowJointTrajectoryResult.INVALID_GOAL
msg = 'Command point %d has %d elements for the positions' % (
i, len(traj.points[i].positions))
rospy.logerr(msg)
self.action_server.set_aborted(result=res, text=msg)
return
for j in range(self.num_joints):
if traj.points[i].velocities:
seg.velocities[j] = traj.points[i].velocities[lookup[j]]
if traj.points[i].positions:
seg.positions[j] = traj.points[i].positions[lookup[j]]
trajectory.append(seg)
rospy.loginfo('Trajectory start requested at %.3lf, waiting...',
traj.header.stamp.to_sec())
rate = rospy.Rate(self.update_rate)
while traj.header.stamp > time:
time = rospy.Time.now()
rate.sleep()
end_time = traj.header.stamp + rospy.Duration(sum(durations))
seg_end_times = [
rospy.Time.from_sec(trajectory[seg].start_time + durations[seg])
for seg in range(len(trajectory))
]
rospy.loginfo(
'Trajectory start time is %.3lf, end time is %.3lf, total duration is %.3lf',
time.to_sec(), end_time.to_sec(), sum(durations))
self.trajectory = trajectory
traj_start_time = rospy.Time.now()
for seg in range(len(trajectory)):
rospy.logdebug(
'current segment is %d time left %f cur time %f' %
(seg, durations[seg] -
(time.to_sec() - trajectory[seg].start_time), time.to_sec()))
rospy.logdebug('goal positions are: %s' %
str(trajectory[seg].positions))
# first point in trajectories calculated by OMPL is current position with duration of 0 seconds, skip it
if durations[seg] == 0:
rospy.logdebug(
'skipping segment %d with duration of 0 seconds' % seg)
continue
multi_packet = {}
for port, joints in self.port_to_joints.items():
vals = []
for joint in joints:
j = self.joint_names.index(joint)
start_position = self.joint_states[joint].current_pos
if seg != 0:
start_position = trajectory[seg - 1].positions[j]
desired_position = trajectory[seg].positions[j]
desired_velocity = max(
self.min_velocity,
abs(desired_position - start_position) /
durations[seg])
self.msg.desired.positions[j] = desired_position
self.msg.desired.velocities[j] = desired_velocity
# probably need a more elegant way of figuring out if we are dealing with a dual controller
if hasattr(self.joint_to_controller[joint], "master_id"):
master_id = self.joint_to_controller[joint].master_id
slave_id = self.joint_to_controller[joint].slave_id
master_pos, slave_pos = self.joint_to_controller[
joint].pos_rad_to_raw(desired_position)
spd = self.joint_to_controller[joint].spd_rad_to_raw(
desired_velocity)
vals.append((master_id, master_pos, spd))
vals.append((slave_id, slave_pos, spd))
else:
motor_id = self.joint_to_controller[joint].motor_id
pos = self.joint_to_controller[joint].pos_rad_to_raw(
desired_position)
spd = self.joint_to_controller[joint].spd_rad_to_raw(
desired_velocity)
vals.append((motor_id, pos, spd))
##ADD COMMAND FOR PAIRED MOTOR
#if the motor_id is the one need to syncronize, append the value for B motor
#also the rotational direction of B must be opposite with A
if (motor_id == self.pairA_motor_id):
vals.append(
(self.pairB_motor_id, self.pairB_motor_init +
self.pairA_motor_init - pos, spd))
multi_packet[port] = vals
print('valie to command the motors : ', vals)
for port, vals in multi_packet.items():
self.port_to_io[port].set_multi_position_and_speed(vals)
while time < seg_end_times[seg]:
# check if new trajectory was received, if so abort current trajectory execution
# by setting the goal to the current position
if self.action_server.is_preempt_requested():
msg = 'New trajectory received. Aborting old trajectory.'
multi_packet = {}
for port, joints in self.port_to_joints.items():
vals = []
for joint in joints:
cur_pos = self.joint_states[joint].current_pos
motor_id = self.joint_to_controller[joint].motor_id
pos = self.joint_to_controller[
joint].pos_rad_to_raw(cur_pos)
vals.append((motor_id, pos))
##ADD COMMAND FOR PAIRED MOTOR
#if the motor_id is the one need to syncronize, append the value for B motor
#also the rotational direction of B must be opposite with A
if (motor_id == self.pairA_motor_id):
vals.append((self.pairB_motor_id,
self.pairB_motor_init +
self.pairA_motor_init - pos))
multi_packet[port] = vals
for port, vals in multi_packet.items():
self.port_to_io[port].set_multi_position(vals)
self.action_server.set_preempted(text=msg)
rospy.logwarn(msg)
return
rate.sleep()
time = rospy.Time.now()
# Verifies trajectory constraints
for j, joint in enumerate(self.joint_names):
if self.trajectory_constraints[
j] > 0 and self.msg.error.positions[
j] > self.trajectory_constraints[j]:
res = FollowJointTrajectoryResult()
res.error_code = FollowJointTrajectoryResult.PATH_TOLERANCE_VIOLATED
msg = 'Unsatisfied position constraint for %s, trajectory point %d, %f is larger than %f' % \
(joint, seg, self.msg.error.positions[j], self.trajectory_constraints[j])
rospy.logwarn(msg)
self.action_server.set_aborted(result=res, text=msg)
return
# let motors roll for specified amount of time
rospy.sleep(self.goal_time_constraint)
for i, j in enumerate(self.joint_names):
rospy.logdebug(
'desired pos was %f, actual pos is %f, error is %f' %
(trajectory[-1].positions[i], self.joint_states[j].current_pos,
self.joint_states[j].current_pos -
trajectory[-1].positions[i]))
# Checks that we have ended inside the goal constraints
for (joint, pos_error, pos_constraint) in zip(self.joint_names,
self.msg.error.positions,
self.goal_constraints):
if pos_constraint > 0 and abs(pos_error) > pos_constraint:
res = FollowJointTrajectoryResult()
res.error_code = FollowJointTrajectoryResult.GOAL_TOLERANCE_VIOLATED
msg = 'Aborting because %s joint wound up outside the goal constraints, %f is larger than %f' % \
(joint, pos_error, pos_constraint)
rospy.logwarn(msg)
self.action_server.set_aborted(result=res, text=msg)
break
else:
msg = 'Trajectory execution successfully completed'
rospy.loginfo(msg)
res = FollowJointTrajectoryResult()
res.error_code = FollowJointTrajectoryResult.SUCCESSFUL
self.action_server.set_succeeded(result=res, text=msg)
def process_trajectory(self, traj):
# Optionally convert from URDF convention to Dynamixel Convention
if self.has_converter:
traj = self.converter.convert_trajectory(traj)
num_points = len(traj.points)
# make sure the joints in the goal match the joints of the controller
if set(self.joint_names) != set(traj.joint_names):
res = FollowJointTrajectoryResult()
res.error_code = FollowJointTrajectoryResult.INVALID_JOINTS
msg = 'Incoming trajectory joints do not match the joints of the controller:\n'
msg = msg + "Expected: " + str(self.joint_names) + "\nReceived: "+ str(traj.joint_names)
rospy.logerr(msg)
self.action_server.set_aborted(result=res, text=msg)
return
# make sure trajectory is not empty
if num_points == 0:
msg = 'Incoming trajectory is empty'
rospy.logerr(msg)
self.action_server.set_aborted(text=msg)
return
# correlate the joints we're commanding to the joints in the message
# map from an index of joint in the controller to an index in the trajectory
lookup = [traj.joint_names.index(joint) for joint in self.joint_names]
durations = [0.0] * num_points
# find out the duration of each segment in the trajectory
durations[0] = traj.points[0].time_from_start.to_sec()
for i in range(1, num_points):
durations[i] = (traj.points[i].time_from_start - traj.points[i - 1].time_from_start).to_sec()
if not traj.points[0].positions:
res = FollowJointTrajectoryResult()
res.error_code = FollowJointTrajectoryResult.INVALID_GOAL
msg = 'First point of trajectory has no positions'
rospy.logerr(msg)
self.action_server.set_aborted(result=res, text=msg)
return
trajectory = []
time = rospy.Time.now() + rospy.Duration(0.01)
for i in range(num_points):
seg = Segment(self.num_joints)
if traj.header.stamp == rospy.Time(0.0):
seg.start_time = (time + traj.points[i].time_from_start).to_sec() - durations[i]
else:
seg.start_time = (traj.header.stamp + traj.points[i].time_from_start).to_sec() - durations[i]
seg.duration = durations[i]
# Checks that the incoming segment has the right number of elements.
if traj.points[i].velocities and len(traj.points[i].velocities) != self.num_joints:
res = FollowJointTrajectoryResult()
res.error_code = FollowJointTrajectoryResult.INVALID_GOAL
msg = 'Command point %d has %d elements for the velocities' % (i, len(traj.points[i].velocities))
rospy.logerr(msg)
self.action_server.set_aborted(result=res, text=msg)
return
if len(traj.points[i].positions) != self.num_joints:
res = FollowJointTrajectoryResult()
res.error_code = FollowJointTrajectoryResult.INVALID_GOAL
msg = 'Command point %d has %d elements for the positions' % (i, len(traj.points[i].positions))
rospy.logerr(msg)
self.action_server.set_aborted(result=res, text=msg)
return
for j in range(self.num_joints):
if traj.points[i].velocities:
seg.velocities[j] = traj.points[i].velocities[lookup[j]]
if traj.points[i].positions:
seg.positions[j] = traj.points[i].positions[lookup[j]]
trajectory.append(seg)
rospy.loginfo('Trajectory start requested at %.3lf, waiting...', traj.header.stamp.to_sec())
rate = rospy.Rate(self.update_rate)
while traj.header.stamp > time:
time = rospy.Time.now()
rate.sleep()
end_time = traj.header.stamp + rospy.Duration(sum(durations))
seg_end_times = [rospy.Time.from_sec(trajectory[seg].start_time + durations[seg]) for seg in range(len(trajectory))]
rospy.loginfo('Trajectory start time is %.3lf, end time is %.3lf, total duration is %.3lf', time.to_sec(), end_time.to_sec(), sum(durations))
self.trajectory = trajectory
traj_start_time = rospy.Time.now()
for seg in range(len(trajectory)):
rospy.logdebug('current segment is %d time left %f cur time %f' % (seg, durations[seg] - (time.to_sec() - trajectory[seg].start_time), time.to_sec()))
rospy.logdebug('goal positions are: %s' % str(trajectory[seg].positions))
# first point in trajectories calculated by OMPL is current position with duration of 0 seconds, skip it
if durations[seg] == 0:
rospy.logdebug('skipping segment %d with duration of 0 seconds' % seg)
continue
multi_packet = {}
for port, joints in self.port_to_joints.items():
vals = []
for joint in joints:
j = self.joint_names.index(joint)
start_position = self.joint_states[joint].current_pos
if seg != 0: start_position = trajectory[seg - 1].positions[j]
desired_position = trajectory[seg].positions[j]
desired_velocity = max(self.min_velocity, abs(desired_position - start_position) / durations[seg])
self.msg.desired.positions[j] = desired_position
self.msg.desired.velocities[j] = desired_velocity
# probably need a more elegant way of figuring out if we are dealing with a dual controller
if hasattr(self.joint_to_controller[joint], "master_id"):
master_id = self.joint_to_controller[joint].master_id
slave_id = self.joint_to_controller[joint].slave_id
master_pos, slave_pos = self.joint_to_controller[joint].pos_rad_to_raw(desired_position)
spd = self.joint_to_controller[joint].spd_rad_to_raw(desired_velocity)
vals.append((master_id, master_pos, spd))
vals.append((slave_id, slave_pos, spd))
else:
motor_id = self.joint_to_controller[joint].motor_id
pos = self.joint_to_controller[joint].pos_rad_to_raw(desired_position)
spd = self.joint_to_controller[joint].spd_rad_to_raw(desired_velocity)
vals.append((motor_id, pos, spd))
multi_packet[port] = vals
for port, vals in multi_packet.items():
self.port_to_io[port].set_multi_position_and_speed(vals)
while time < seg_end_times[seg]:
# check if new trajectory was received, if so abort current trajectory execution
# by setting the goal to the current position
if self.action_server.is_preempt_requested():
msg = 'New trajectory received. Aborting old trajectory.'
multi_packet = {}
for port, joints in self.port_to_joints.items():
vals = []
for joint in joints:
cur_pos = self.joint_states[joint].current_pos
motor_id = self.joint_to_controller[joint].motor_id
pos = self.joint_to_controller[joint].pos_rad_to_raw(cur_pos)
vals.append((motor_id, pos))
multi_packet[port] = vals
for port, vals in multi_packet.items():
self.port_to_io[port].set_multi_position(vals)
self.action_server.set_preempted(text=msg)
rospy.logwarn(msg)
return
rate.sleep()
time = rospy.Time.now()
# Verifies trajectory constraints
for j, joint in enumerate(self.joint_names):
if self.trajectory_constraints[j] > 0 and self.msg.error.positions[j] > self.trajectory_constraints[j]:
res = FollowJointTrajectoryResult()
res.error_code = FollowJointTrajectoryResult.PATH_TOLERANCE_VIOLATED
msg = 'Unsatisfied position constraint for %s, trajectory point %d, %f is larger than %f' % \
(joint, seg, self.msg.error.positions[j], self.trajectory_constraints[j])
rospy.logwarn(msg)
self.action_server.set_aborted(result=res, text=msg)
return
# let motors roll for specified amount of time
rospy.sleep(self.goal_time_constraint)
for i, j in enumerate(self.joint_names):
rospy.logdebug('desired pos was %f, actual pos is %f, error is %f' % (trajectory[-1].positions[i], self.joint_states[j].current_pos, self.joint_states[j].current_pos - trajectory[-1].positions[i]))
# Checks that we have ended inside the goal constraints
for (joint, pos_error, pos_constraint) in zip(self.joint_names, self.msg.error.positions, self.goal_constraints):
if pos_constraint > 0 and abs(pos_error) > pos_constraint:
res = FollowJointTrajectoryResult()
res.error_code = FollowJointTrajectoryResult.GOAL_TOLERANCE_VIOLATED
msg = 'Aborting because %s joint wound up outside the goal constraints, %f is larger than %f' % \
(joint, pos_error, pos_constraint)
rospy.logwarn(msg)
self.action_server.set_aborted(result=res, text=msg)
break
else:
res = FollowJointTrajectoryResult()
res.error_code = FollowJointTrajectoryResult.SUCCESSFUL
msg = 'Trajectory execution successfully completed'
rospy.loginfo(msg)
self.action_server.set_succeeded(result=res, text=msg)
def process_trajectory(self, goal, goal_id):
while not self.joint_states_rdy and not rospy.is_shutdown():
rospy.logwarn("Waiting for joint_states...")
if not self.running.has_key(goal_id):
return # it was cancelled...
rospy.sleep(0.1)
traj = goal.get_goal().trajectory
self.current_goal = goal
num_points = len(traj.points)
# Make sure the joints in the goal (trajectory_msgs/JointTrajectory) match
# the joints of the controller
if set(self.joint_names) != set(traj.joint_names):
# A Python set is always created only with unique items, therefore a set
# eliminates duplicates, but it also sort them.
res = FollowJointTrajectoryResult()
res.error_code=FollowJointTrajectoryResult.INVALID_JOINTS
msg = 'Incoming trajectory joints do not match the joints of the controller'
rospy.logerr(msg)
rospy.logerr(' self.joint_names={%s}' % (set(self.joint_names)))
rospy.logerr(' traj.joint_names={%s}' % (set(traj.joint_names)))
goal.set_aborted(result=res, text=msg)
self.running[goal_id] = False
return
# make sure trajectory is not empty
if num_points == 0:
msg = 'Incoming trajectory is empty'
rospy.logerr(msg)
goal.set_aborted(text=msg)
self.running[goal_id] = False
return
rospy.logdebug('Number of points:{0}'.format(num_points))
# correlate the joints we're commanding to the joints in the message
# map from an index of joint in the controller to an index in the trajectory
lookup = [traj.joint_names.index(joint) for joint in self.joint_names]
durations = [0.0] * num_points
# find out the duration of each segment in the trajectory
durations[0] = traj.points[0].time_from_start.to_sec()
for i in range(1, num_points):
durations[i] = ((traj.points[i].time_from_start - traj.points[i - 1].time_from_start).to_sec())/self.speed_factor
# Here the durations are calculated in relation to the previous point.
# Nevertheless, the index 0 will have the trajectory's starting time.
if not traj.points[0].positions:
res = FollowJointTrajectoryResult()
res.error_code = FollowJointTrajectoryResult.INVALID_GOAL
msg = 'First point of trajectory has no positions'
rospy.logerr(msg)
goal.set_aborted(result=res, text=msg)
self.running[goal_id] = False
return
trajectory = []
time = rospy.Time.now() #+ rospy.Duration(0.01) # Why 0.01???
for i in range(num_points): # Processing all the JointTrajectoryPoints
seg = Segment(self.num_joints)
# self.start_time = 0.0 # trajectory segment start time
# self.duration = 0.0 # trajectory segment duration
# self.positions = [0.0] * num_joints
# self.velocities = [0.0] * num_joints
if traj.header.stamp == rospy.Time(0.0):
seg.start_time = (time + traj.points[i].time_from_start).to_sec() - durations[i]
else:
seg.start_time = (traj.header.stamp + traj.points[i].time_from_start).to_sec() - durations[i]
seg.duration = durations[i]
# Checks that the incoming segment has the right number of elements.
if traj.points[i].velocities and len(traj.points[i].velocities) != self.num_joints:
res = FollowJointTrajectoryResult()
res.error_code=FollowJointTrajectoryResult.INVALID_GOAL
msg = 'Command point %d has %d elements for the velocities' % (i, len(traj.points[i].velocities))
rospy.logerr(msg)
goal.set_aborted(result=res, text=msg)
self.running[goal_id] = False
return
if len(traj.points[i].positions) != self.num_joints:
res = FollowJointTrajectoryResult()
res.error_code=FollowJointTrajectoryResult.INVALID_GOAL
msg = 'Command point %d has %d elements for the positions' % (i, len(traj.points[i].positions))
rospy.logerr(msg)
goal.set_aborted(result=res, text=msg)
self.running[goal_id] = False
return
for j in range(self.num_joints):
if traj.points[i].velocities:
seg.velocities[j] = traj.points[i].velocities[lookup[j]]
if traj.points[i].positions:
seg.positions[j] = traj.points[i].positions[lookup[j]]
trajectory.append(seg)
# Clearly, according to the lines below, future goals can be received and they will
# wait here until it's their time to start (according to self.update_rate).
rospy.loginfo('Trajectory start requested at %.3lf, waiting...', traj.header.stamp.to_sec())
rate = rospy.Rate(self.update_rate)
while traj.header.stamp > time:
time = rospy.Time.now()
rate.sleep()
end_time = traj.header.stamp + rospy.Duration(sum(durations))
seg_end_times = [rospy.Time.from_sec(trajectory[seg].start_time + durations[seg]) for seg in range(len(trajectory))]
rospy.loginfo('Trajectory start time is %.3lf, end time is %.3lf, total duration is %.3lf', time.to_sec(), time.to_sec()+end_time.to_sec(), sum(durations))
self.trajectory = trajectory
traj_start_time = rospy.Time.now() # According to this, the trajectory will start being executed here!
# And the trajectory execution is sliced by segments.
start_joint_positions = {}
for seg in range(len(trajectory)):
if not self.running.has_key(goal_id):
# stop the trajectory execution
break
rospy.logdebug('Current segment is %d, duration %f, time passed %f, cur time %f' % (seg, durations[seg], durations[seg] - (time.to_sec() - trajectory[seg].start_time), time.to_sec()))
rospy.logdebug('Goal positions are: %s' % str(trajectory[seg].positions))
# first point in trajectories calculated by OMPL is current position with duration of 0 seconds, skip it
if durations[seg] == 0:
rospy.logdebug('Skipping segment %d with duration of 0 seconds' % seg)
continue
joint_positions = {}
joint_velocities = {}
for j,joint in enumerate(self.joint_names):
desired_position = trajectory[seg].positions[j]
joint_positions[joint] = desired_position
start_joint_positions[joint] = desired_position
if seg != 0:
start_joint_positions[joint] = trajectory[seg - 1].positions[j]
desired_velocities = trajectory[seg].velocities[j]
joint_velocities[joint] = desired_velocities
#
# The commands sent to the robot using sendCommand2Gummi
# will always obey the joint name order.
if durations[seg]>=1/self.update_command_rate or seg==(len(trajectory)-1) or self.always_smooth:
self.sendCommand2Gummi(seg, start_joint_positions, joint_positions, joint_velocities, durations[seg])
#
#
# Verifies trajectory constraints
for j, joint in enumerate(self.joint_names):
if self.trajectory_constraints[j] > 0 and self.feedback.error.positions[j] > self.trajectory_constraints[j]:
res = FollowJointTrajectoryResult()
res.error_code=FollowJointTrajectoryResult.PATH_TOLERANCE_VIOLATED
msg = 'Unsatisfied position constraint for %s, trajectory point %d, %f is larger than %f' % \
(joint, seg, self.feedback.error.positions[j], self.trajectory_constraints[j])
rospy.logwarn(msg)
goal.set_aborted(result=res, text=msg)
self.running[goal_id] = False
return
# let motors roll for specified amount of time
rospy.sleep(self.goal_time_constraint)
for i, joint in enumerate(self.joint_names):
rospy.logdebug('Desired pos was %f, actual pos is %f, error is %f' % (trajectory[-1].positions[i], self.joint_positions[joint], self.joint_positions[joint] - trajectory[-1].positions[i]))
# Checks that we have ended inside the goal constraints
for (joint, pos_error, pos_constraint) in zip(self.joint_names, self.feedback.error.positions, self.goal_constraints):
if pos_constraint > 0 and abs(pos_error) > pos_constraint:
res = FollowJointTrajectoryResult()
res.error_code=FollowJointTrajectoryResult.GOAL_TOLERANCE_VIOLATED
msg = 'Aborting because %s joint wound up outside the goal constraints, %f is larger than %f' % \
(joint, pos_error, pos_constraint)
rospy.logwarn(msg)
goal.set_aborted(result=res, text=msg)
self.running[goal_id] = False
return
if self.running.has_key(goal_id):
_ = self.running.pop(goal_id)
msg = 'Trajectory execution successfully completed (goalID:{0})!'.format(goal_id)
rospy.loginfo(msg)
res = FollowJointTrajectoryResult()
res.error_code=FollowJointTrajectoryResult.SUCCESSFUL
goal.set_succeeded(result=res, text=msg) # this must be called only once.
else:
msg = 'Trajectory execution cancelled/aborted... (goalID:{0})!'.format(goal.get_goal_id().id)
rospy.logwarn(msg)
goal.set_canceled(text=msg)
def execute_joint_trajectory_cb(self, goal):
rospy.loginfo("Trajectory received with %d points",
len(goal.trajectory.points))
feedback = FollowJointTrajectoryFeedback()
result = FollowJointTrajectoryResult()
current_status = self._driver.get_current_gripper_status()
# Check trajectory joint names
joint_names = goal.trajectory.joint_names
if len(joint_names
) != 1 and joint_names[0] != self._driver._joint_name:
msg = "Joint trajectory joints do not match gripper joint"
rospy.logerr(msg)
result.error_code = result.INVALID_JOINTS
result.error_string = msg
self._joint_trajectory_action_server.set_aborted(result)
return
# Check trajectory points
if len(goal.trajectory.points) == 0:
msg = "Ignoring empty trajectory "
rospy.logerr(msg)
result.error_code = result.INVALID_GOAL
result.error_string = msg
self._joint_trajectory_action_server.set_aborted(result)
return
# Process goal trajectory
self._processing_goal = True
self._is_stalled = False
goal_command = CommandRobotiqGripperGoal()
feedback.joint_names = goal.trajectory.joint_names
watchdog = rospy.Timer(rospy.Duration(
goal.trajectory.points[-1].time_from_start.to_sec() + 0.5),
self._execution_timeout,
oneshot=True)
# Follow trajectory points
goal_trajectory_point = goal.trajectory.points[-1]
# Validate trajectory point
if len(goal_trajectory_point.positions) != 1:
result.error_code = result.INVALID_GOAL
result.error_string = "Invalid joint position on trajectory point "
self._joint_trajectory_action_server.set_aborted(result)
return
target_speed = 0.1
#target_speed = goal.trajectory.points[int(len(goal.trajectory.points)/2)].velocities[0] if goal.trajectory.points[int(len(goal.trajectory.points)/2)].velocities > 0 else 0.1
#print("Target gripper speed: " + target_speed)
#target_speed = goal_trajectory_point.velocities[0] if len(goal_trajectory_point.velocities) > 0 else 0.01
target_force = goal_trajectory_point.effort[0] if len(
goal_trajectory_point.effort) > 0 else 0.1
goal_command.position = self._driver.from_radians_to_distance(
goal_trajectory_point.positions[0])
goal_command.speed = abs(target_speed) # To-Do: Convert to rad/s
goal_command.force = target_force
# Send incoming command to gripper driver
self._driver.update_gripper_command(goal_command)
# Set feedback desired value
feedback.desired.positions = [goal_trajectory_point.positions[0]]
while not rospy.is_shutdown(
) and self._processing_goal and not self._is_stalled:
current_status = self._driver.get_current_gripper_status()
feedback.actual.positions = [
self._driver.get_current_joint_position()
]
error = abs(feedback.actual.positions[0] -
feedback.desired.positions[0])
rospy.logdebug("Error : %.3f -- Actual: %.3f -- Desired: %.3f",
error, self._driver.get_current_joint_position(),
feedback.desired.positions[0])
feedback.error.positions = [error]
self._joint_trajectory_action_server.publish_feedback(feedback)
# Check for errors
if current_status.fault_status != 0 and not self._is_stalled:
self._is_stalled = True
self._processing_goal = False
rospy.logerr(msg)
result.error_code = -6
result.error_string = "Gripper fault status (gFLT): " + current_status.fault_status
self._joint_trajectory_action_server.set_aborted(result)
return
# Check if object was detected
if current_status.obj_detected:
watchdog.shutdown() # Stop timeout watchdog.
self._processing_goal = False
self._is_stalled = False
result.error_code = result.SUCCESSFUL
result.error_string = "Object detected/grasped"
self._joint_trajectory_action_server.set_succeeded(result)
return
# Check if current trajectory point was reached
if error < GOAL_DETECTION_THRESHOLD:
break
# Entire trajectory was followed/reached
watchdog.shutdown()
rospy.logdebug(self._action_name + ": goal reached")
result.error_code = result.SUCCESSFUL
result.error_string = "Goal reached"
self._joint_trajectory_action_server.set_succeeded(result)
self._processing_goal = False
self._is_stalled = False
def execute_position_cb(self, goal):
is_timed_out = False
is_over_effort = False
over_effort_counter = 0
start = rospy.Time.now()
extra_time = rospy.Duration(5.0)
# print goal
for i in range(len(goal.trajectory.points)):
joint_positions = brics_actuator.msg.JointPositions()
conf = np.array(goal.trajectory.points[i].positions)
# transform from ROS to BRICS message
for j in range(len(self.joint_names)):
joint_value = brics_actuator.msg.JointValue()
joint_value.joint_uri = self.joint_names[j]
joint_value.value = self.limit_joint(self.joint_names[j],
conf[j])
conf[j] = joint_value.value
joint_value.unit = self.unit
joint_positions.positions.append(joint_value)
if self.is_over_effort():
over_effort_counter += 1
rospy.logerr("%d, over effort", over_effort_counter)
if over_effort_counter > LIMIT_OVER_EFFORT:
is_over_effort = True
break
self.position_pub.publish(joint_positions)
# wait to reach the goal position
while not rospy.is_shutdown():
if self.is_over_effort():
over_effort_counter += 1
rospy.logerr("%d, over effort", over_effort_counter)
if over_effort_counter > LIMIT_OVER_EFFORT:
is_over_effort = True
break
if self.is_goal_reached(conf, self.configuration):
rospy.sleep(0.01)
break
if (rospy.Time.now() -
start) > (goal.trajectory.points[i].time_from_start +
extra_time):
is_timed_out = True
break
rospy.sleep(0.01)
if is_timed_out or is_over_effort:
break
result = FollowJointTrajectoryResult()
if is_over_effort:
result.error_code = FollowJointTrajectoryResult.PATH_TOLERANCE_VIOLATED
rospy.logerr("arm over-effort")
self.arm_up_recover()
self.position_action_server.set_preempted(result)
elif is_timed_out:
result.error_code = FollowJointTrajectoryResult.PATH_TOLERANCE_VIOLATED
rospy.logerr("arm timed_out")
self.arm_up_recover()
self.position_action_server.set_aborted(result)
else:
result.error_code = FollowJointTrajectoryResult.SUCCESSFUL
self.position_action_server.set_succeeded(result)