def __init__(self, reconfig_server, rate=100.0):
self._dyn = reconfig_server
self.continuous = self._dyn.config['continuous']
self._fjt = '/follow_joint_trajectory'
self._server = actionlib.SimpleActionServer(
self._fjt,
FollowJointTrajectoryAction,
execute_cb=self._on_trajectory_action,
auto_start=False)
self._action_name = rospy.get_name()
# Current joint states
self._positions = {}
self._velocities = {}
# Actual robot control
self.states = EdoStates()
rospy.Subscriber("joint_states", JointState, self._js_callback)
# Action Feedback/Result
self._fdbk = FollowJointTrajectoryFeedback()
self._result = FollowJointTrajectoryResult()
# Controller parameters from arguments, messages, and dynamic reconfigure
self._control_rate = rate # Hz
self._update_rate_spinner = rospy.Rate(self._control_rate)
self._control_joints = []
self._goal_time = 0.0
self._stopped_velocity = 0.0
self._goal_error = dict()
self._path_thresh = dict()
# Start the action server
rospy.sleep(0.5)
while True:
if self.states.edo_current_state in [
self.states.CS_CALIBRATED, self.states.CS_BRAKED
]:
self._server.start()
self._alive = True
rospy.loginfo("Trayectory action server started")
break
else:
rospy.logerr(
"Joint Trajectory Action Server cannot be started when robot is in state %s"
% self.states.get_current_code_string())
rospy.logerr(
"Make sure your robot is started and calibrated properly with calibrate.launch"
)
rospy.logerr("Trying again in 1 second... ")
rospy.sleep(1)
def __init__(self):
self._name_space = 'grip/gripper_action'
# Start Action Server.
self._server = actionlib.SimpleActionServer(
self._name_space,
GripperCommandAction,
execute_cb=self._on_gripper_action,
auto_start=False)
self._action_name = rospy.get_name()
self._server.start()
# initialize a edo states in the action server
self.states = EdoStates()
# Action Feedback/Result
self._feedback = GripperCommandFeedback()
self._result = GripperCommandResult()
self._timeout = 5.0
self._action_name = rospy.get_name()
self._update_rate_spinner = rospy.Rate(20)
class GripperActionServer(object):
def __init__(self):
self._name_space = '/gripper_action'
# Start Action Server.
self._server = actionlib.SimpleActionServer(
self._name_space,
GripperCommandAction,
execute_cb=self._on_gripper_action,
auto_start=False
)
self._action_name = rospy.get_name()
self._server.start()
# initialize a edo states in the action server
self.states = EdoStates()
# Action Feedback/Result
self._feedback = GripperCommandFeedback()
self._result = GripperCommandResult()
self._timeout = 5.0
self._action_name = rospy.get_name()
self._update_rate_spinner = rospy.Rate(20)
def spin(self):
while not rospy.is_shutdown():
self.states.update()
self._update_rate_spinner.sleep()
def _check_state(self, position):
return fabs(self.states.gripper_position - position) < 5
def _command_gripper(self, position):
if position >= 100.0:
self.states.change_gripper_state(True)
else:
self.states.change_gripper_state(False)
def _update_feedback(self, position):
self._feedback.position = self.states.gripper_position
# True when the grip reach 5mm to its destination.
self._feedback.reached_goal = (fabs(self.states.gripper_position - position) < 5)
self._result = self._feedback
self._server.publish_feedback(self._feedback)
def _on_gripper_action(self, goal):
position = goal.command.position
control_rate = rospy.Rate(20.0)
self._update_feedback(position)
start_time = rospy.get_time()
def now_from_start(start):
return rospy.get_time() - start
while ((now_from_start(start_time) < self._timeout or self._timeout < 0.0 and not rospy.is_shutdown())):
if self._check_state(position):
self._server.set_suceeded(self._result)
return
self._command_gripper(position)
control_rate.sleep()
if not rospy.is_shutdown():
rospy.logerr("%s: Gripper Command Not Achieved in Allotted Time" %
(self._action_name,))
self._update_feedback(position)
self._server.set_aborted(self._result)
class JointTrajectoryActionServer(object):
def __init__(self, reconfig_server, rate=100.0):
self._dyn = reconfig_server
self.continuous = self._dyn.config['continuous']
self._fjt = '/follow_joint_trajectory'
self._server = actionlib.SimpleActionServer(
self._fjt,
FollowJointTrajectoryAction,
execute_cb=self._on_trajectory_action,
auto_start=False)
self._action_name = rospy.get_name()
# Current joint states
self._positions = {}
self._velocities = {}
# Actual robot control
self.states = EdoStates()
rospy.Subscriber("joint_states", JointState, self._js_callback)
# Action Feedback/Result
self._fdbk = FollowJointTrajectoryFeedback()
self._result = FollowJointTrajectoryResult()
# Controller parameters from arguments, messages, and dynamic reconfigure
self._control_rate = rate # Hz
self._update_rate_spinner = rospy.Rate(self._control_rate)
self._control_joints = []
self._goal_time = 0.0
self._stopped_velocity = 0.0
self._goal_error = dict()
self._path_thresh = dict()
# Start the action server
rospy.sleep(0.5)
while True:
if self.states.edo_current_state in [
self.states.CS_CALIBRATED, self.states.CS_BRAKED
]:
self._server.start()
self._alive = True
rospy.loginfo("Trayectory action server started")
break
else:
rospy.logerr(
"Joint Trajectory Action Server cannot be started when robot is in state %s"
% self.states.get_current_code_string())
rospy.logerr(
"Make sure your robot is started and calibrated properly with calibrate.launch"
)
rospy.logerr("Trying again in 1 second... ")
rospy.sleep(1)
def spin(self):
while not rospy.is_shutdown():
self.states.update()
#self.states.joint_control_pub.publish(self.states.msg_jca)
self._update_rate_spinner.sleep()
def robot_is_enabled(self):
return True # TODO read e-stop
def clean_shutdown(self):
self._alive = False
def _js_callback(self, js):
self._positions.update(dict(zip(js.name, js.position)))
self._velocities.update(dict(zip(js.name, js.velocity)))
def _get_trajectory_parameters(self, joint_names, goal):
# For each input trajectory, if path, goal, or goal_time tolerances
# provided, we will use these as opposed to reading from the
# parameter server/dynamic reconfigure
# Goal time tolerance - time buffer allowing goal constraints to be met
if goal.goal_time_tolerance:
self._goal_time = goal.goal_time_tolerance.to_sec()
else:
self._goal_time = self._dyn.config['goal_time']
# Stopped velocity tolerance - max velocity at end of execution
self._stopped_velocity = self._dyn.config['stopped_velocity_tolerance']
# Path execution and goal tolerances per joint
for jnt in joint_names:
if jnt not in ordered_joint_names:
rospy.logerr(
"%s: Trajectory Aborted - Provided Invalid Joint Name %s" %
(
self._action_name,
jnt,
))
self._result.error_code = self._result.INVALID_JOINTS
self._server.set_aborted(self._result)
return
# Path execution tolerance
path_error = self._dyn.config[jnt + '_trajectory']
if goal.path_tolerance:
for tolerance in goal.path_tolerance:
if jnt == tolerance.name:
if tolerance.position != 0.0:
self._path_thresh[jnt] = tolerance.position
else:
self._path_thresh[jnt] = path_error
else:
self._path_thresh[jnt] = path_error
# Goal error tolerance
goal_error = self._dyn.config[jnt + '_goal']
if goal.goal_tolerance:
for tolerance in goal.goal_tolerance:
if jnt == tolerance.name:
if tolerance.position != 0.0:
self._goal_error[jnt] = tolerance.position
else:
self._goal_error[jnt] = goal_error
else:
self._goal_error[jnt] = goal_error
def _get_current_position(self, joint_names):
return [self._positions[joint] for joint in joint_names]
def _get_current_velocities(self, joint_names):
return [self._velocities[joint] for joint in joint_names]
def _get_current_error(self, joint_names, set_point):
current = self._get_current_position(joint_names)
error = map(operator.sub, set_point, current)
return zip(joint_names, error)
def _update_feedback(self, cmd_point, jnt_names, cur_time):
self._fdbk.header.stamp = rospy.Duration.from_sec(rospy.get_time())
self._fdbk.joint_names = jnt_names
self._fdbk.desired = cmd_point
self._fdbk.desired.time_from_start = rospy.Duration.from_sec(cur_time)
self._fdbk.actual.positions = self._get_current_position(jnt_names)
self._fdbk.actual.time_from_start = rospy.Duration.from_sec(cur_time)
self._fdbk.error.positions = map(operator.sub,
self._fdbk.desired.positions,
self._fdbk.actual.positions)
self._fdbk.error.time_from_start = rospy.Duration.from_sec(cur_time)
self._server.publish_feedback(self._fdbk)
def _command_joints(self, joint_names, point):
if self._server.is_preempt_requested() or not self.robot_is_enabled():
rospy.loginfo("%s: Trajectory Preempted" % (self._action_name, ))
self._server.set_preempted()
return False
self.states.joint_control_pub.publish(
self.states.create_joint_command_message(joint_names, point))
return True
def _determine_dimensions(self, trajectory_points):
# Determine dimensions supplied
position_flag = True
velocity_flag = (len(trajectory_points[0].velocities) != 0
and len(trajectory_points[-1].velocities) != 0)
acceleration_flag = (len(trajectory_points[0].accelerations) != 0
and len(trajectory_points[-1].accelerations) != 0)
return {
'positions': position_flag,
'velocities': velocity_flag,
'accelerations': acceleration_flag
}
def _compute_bezier_coeff(self, joint_names, trajectory_points,
dimensions_dict):
# Compute Full Bezier Curve
num_joints = len(joint_names)
num_traj_pts = len(trajectory_points)
num_traj_dim = sum(dimensions_dict.values())
num_b_values = len(['b0', 'b1', 'b2', 'b3'])
b_matrix = np.zeros(shape=(num_joints, num_traj_dim, num_traj_pts - 1,
num_b_values))
for jnt in xrange(num_joints):
traj_array = np.zeros(shape=(len(trajectory_points), num_traj_dim))
for idx, point in enumerate(trajectory_points):
current_point = list()
current_point.append(point.positions[jnt])
if dimensions_dict['velocities']:
current_point.append(point.velocities[jnt])
if dimensions_dict['accelerations']:
current_point.append(point.accelerations[jnt])
traj_array[idx, :] = current_point
d_pts = bezier.de_boor_control_pts(traj_array)
b_matrix[jnt, :, :, :] = bezier.bezier_coefficients(
traj_array, d_pts)
return b_matrix
def _get_bezier_point(self, b_matrix, idx, t, cmd_time, dimensions_dict):
pnt = JointTrajectoryPoint()
pnt.time_from_start = rospy.Duration(cmd_time)
num_joints = b_matrix.shape[0]
pnt.positions = [0.0] * num_joints
if dimensions_dict['velocities']:
pnt.velocities = [0.0] * num_joints
if dimensions_dict['accelerations']:
pnt.accelerations = [0.0] * num_joints
for jnt in range(num_joints):
b_point = bezier.bezier_point(b_matrix[jnt, :, :, :], idx, t)
# Positions at specified time
pnt.positions[jnt] = b_point[0]
# Velocities at specified time
if dimensions_dict['velocities']:
pnt.velocities[jnt] = b_point[1]
# Accelerations at specified time
if dimensions_dict['accelerations']:
pnt.accelerations[jnt] = b_point[-1]
return pnt
def _on_trajectory_action(self, goal):
joint_names = goal.trajectory.joint_names
trajectory_points = goal.trajectory.points
dimensions_dict = self._determine_dimensions(trajectory_points)
# Load parameters for trajectory
self._get_trajectory_parameters(joint_names, goal)
# Create a new discretized joint trajectory
num_points = len(trajectory_points)
if num_points == 0:
rospy.logerr("%s: Empty Trajectory" % (self._action_name, ))
self._server.set_aborted()
return
rospy.logwarn(
"{}: Executing requested joint trajectory {:0.1f} sec".format(
self._action_name,
trajectory_points[-1].time_from_start.to_sec()))
control_rate = rospy.Rate(self._control_rate)
if num_points == 1:
# Add current position as trajectory point
first_trajectory_point = JointTrajectoryPoint()
first_trajectory_point.positions = self._get_current_position(
joint_names)
# To preserve desired velocities and accelerations, copy them to the first
# trajectory point if the trajectory is only 1 point.
if dimensions_dict['velocities']:
first_trajectory_point.velocities = deepcopy(
trajectory_points[0].velocities)
if dimensions_dict['accelerations']:
first_trajectory_point.accelerations = deepcopy(
trajectory_points[0].accelerations)
first_trajectory_point.velocities = deepcopy(
trajectory_points[0].velocities)
first_trajectory_point.accelerations = deepcopy(
trajectory_points[0].accelerations)
first_trajectory_point.time_from_start = rospy.Duration(0)
trajectory_points.insert(0, first_trajectory_point)
num_points = len(trajectory_points)
if not self.continuous:
if dimensions_dict['velocities']:
trajectory_points[-1].velocities = [0.0] * len(joint_names)
if dimensions_dict['accelerations']:
trajectory_points[-1].accelerations = [0.0] * len(joint_names)
# Compute Full Bezier Curve Coefficients for all 7 joints
pnt_times = [pnt.time_from_start.to_sec() for pnt in trajectory_points]
try:
b_matrix = self._compute_bezier_coeff(joint_names,
trajectory_points,
dimensions_dict)
except Exception as ex:
rospy.logerr("Failed to compute a Bezier trajectory: {}".format(
repr(o)))
self._server.set_aborted()
return
# Wait for the specified execution time, if not provided use now
start_time = goal.trajectory.header.stamp.to_sec()
if start_time == 0.0:
start_time = rospy.get_time()
wait_for(lambda: rospy.get_time() >= start_time, timeout=float('inf'))
# Loop until end of trajectory time. Provide a single time step
# of the control rate past the end to ensure we get to the end.
# Keep track of current indices for spline segment generation
now_from_start = rospy.get_time() - start_time
end_time = trajectory_points[-1].time_from_start.to_sec()
while (now_from_start < end_time and not rospy.is_shutdown()
and self.robot_is_enabled()):
#Acquire Mutex
now = rospy.get_time()
now_from_start = now - start_time
idx = bisect.bisect(pnt_times, now_from_start)
#Calculate percentage of time passed in this interval
if idx >= num_points:
cmd_time = now_from_start - pnt_times[-1]
t = 1.0
elif idx >= 0:
cmd_time = (now_from_start - pnt_times[idx - 1])
t = cmd_time / (pnt_times[idx] - pnt_times[idx - 1])
else:
cmd_time = 0
t = 0
point = self._get_bezier_point(b_matrix, idx, t, cmd_time,
dimensions_dict)
# Command Joint Position, Velocity, Acceleration
command_executed = self._command_joints(joint_names, point)
self._update_feedback(deepcopy(point), joint_names, now_from_start)
if not command_executed:
return
control_rate.sleep()
# Keep trying to meet goal until goal_time constraint expired
last = trajectory_points[-1]
last_time = trajectory_points[-1].time_from_start.to_sec()
while (now_from_start < (last_time + self._goal_time)
and not rospy.is_shutdown() and self.robot_is_enabled()):
if not self._command_joints(joint_names, last):
self._server.set_aborted(self._result)
return
now_from_start = rospy.get_time() - start_time
self._update_feedback(deepcopy(last), joint_names, now_from_start)
control_rate.sleep()
now_from_start = rospy.get_time() - start_time
self._update_feedback(deepcopy(last), joint_names, now_from_start)
# Verify goal constraint
def check_goal_state():
for error in self._get_current_error(joint_names, last.positions):
if (self._goal_error[error[0]] > 0
and self._goal_error[error[0]] < math.fabs(error[1])):
return error[0]
if (self._stopped_velocity > 0.0 and max([
abs(cur_vel)
for cur_vel in self._get_current_velocities(joint_names)
]) > self._stopped_velocity):
return False
else:
return True
result = check_goal_state()
if result is True:
rospy.loginfo("%s: Joint Trajectory Action Succeeded" %
(self._action_name, ))
self._result.error_code = self._result.SUCCESSFUL
self._server.set_succeeded(self._result)
elif result is False:
rospy.logerr("%s: Exceeded Max Goal Velocity Threshold" %
(self._action_name, ))
self._result.error_code = self._result.GOAL_TOLERANCE_VIOLATED
self._server.set_aborted(self._result)
#!/usr/bin/env python
import rospy
import traceback
from edo.states import EdoStates
if __name__ == '__main__':
rospy.init_node('edo_calibrate', anonymous=True)
states = EdoStates(enable_algorithm_node=True)
calibrated = False
rospy.logwarn("Starting robot calibration procedure...")
try:
calibrated = states.calibration()
except:
traceback.print_exc()
rospy.logerr("Robot calibration did not finish")
else:
if calibrated:
rospy.logwarn("Robot is calibrated!")
else:
rospy.logerr("Robot calibration did not finish successfully")