def __init__(self):
self.name = 'gripper'
self._cmd_sender = rospy.get_name() + '_%s'
self._cmd_sequence = 0
ns = 'crustcrawler/end_effector/' + self.name + "/"
self._state = None
self.on_gripping_changed = crustcrawler_dataflow.Signal()
self.on_moving_changed = crustcrawler_dataflow.Signal()
self._parameters = dict()
self._cmd_pub = rospy.Publisher(ns + 'command',
EndEffectorCommand,
queue_size=10)
self._state_pub = rospy.Publisher(ns + 'set_state',
EndEffectorState,
latch=True,
queue_size=10)
self._state_sub = rospy.Subscriber(ns + 'state', EndEffectorState,
self._on_gripper_state)
# Wait for the gripper state message to be populated
crustcrawler_dataflow.wait_for(
lambda: not self._state is None,
timeout=5.0,
timeout_msg=("Failed to get state from %s" % (ns + 'state', )))
def command(self,
cmd,
block=False,
test=lambda: True,
timeout=0.0,
args=None):
"""
Raw command call to directly control gripper.
@type cmd: str
@param cmd: string of known gripper commands
@type block: bool
@param block: command is blocking or non-blocking [False]
@type test: func
@param test: test function for command validation
@type timeout: float
@param timeout: timeout in seconds for command evaluation
@type args: dict({str:float})
@param args: dictionary of parameter:value
"""
ee_cmd = EndEffectorCommand()
ee_cmd.id = self.hardware_id()
ee_cmd.command = cmd
ee_cmd.sender = self._cmd_sender % (cmd, )
ee_cmd.sequence = self._inc_cmd_sequence()
ee_cmd.args = ''
if args != None:
ee_cmd.args = JSONEncoder().encode(args)
seq_test = lambda: (self._state.command_sender == ee_cmd.sender and
(self._state.command_sequence == ee_cmd.sequence or
self._state.command_sequence == 0))
self._cmd_pub.publish(ee_cmd)
if block:
finish_time = rospy.get_time() + timeout
cmd_seq = crustcrawler_dataflow.wait_for(
test=seq_test,
timeout=timeout,
raise_on_error=False,
body=lambda: self._cmd_pub.publish(ee_cmd))
if not cmd_seq:
seq_msg = (("Timed out on gripper command acknowledgement for"
" %s:%s") % (self.name, ee_cmd.command))
rospy.logdebug(seq_msg)
time_remain = max(0.5, finish_time - rospy.get_time())
return crustcrawler_dataflow.wait_for(
test=test,
timeout=time_remain,
raise_on_error=False,
body=lambda: self._cmd_pub.publish(ee_cmd))
else:
return True
def move_to_joint_positions(self,
positions,
timeout=15.0,
threshold=settings.JOINT_ANGLE_TOLERANCE,
test=None):
"""
(Blocking) Commands the crustcrawler limb to the provided positions.
Waits until the reported joint state matches that specified.
This function uses a low-pass filter to smooth the movement.
@type positions: dict({str:float})
@param positions: joint_name:angle command
@type timeout: float
@param timeout: seconds to wait for move to finish [15]
@type threshold: float
@param threshold: position threshold in radians across each joint when
move is considered successful [0.008726646]
@param test: optional function returning True if motion must be aborted
"""
cmd = self.joint_angles()
def filtered_cmd():
# First Order Filter - 0.2 Hz Cutoff
for joint in positions.keys():
cmd[joint] = 0.012488 * positions[joint] + 0.98751 * cmd[joint]
return cmd
def genf(joint, angle):
def joint_diff():
return abs(angle - self._joint_angle[joint])
return joint_diff
diffs = [
genf(j, a) for j, a in positions.items() if j in self._joint_angle
]
self.set_joint_positions(filtered_cmd())
crustcrawler_dataflow.wait_for(
test=lambda: callable(test) and test() == True or \
(all(diff() < threshold for diff in diffs)),
timeout=timeout,
timeout_msg=("%s limb failed to reach commanded joint positions" %
(self.name.capitalize(),)),
rate=100,
raise_on_error=False,
body=lambda: self.set_joint_positions(filtered_cmd())
)
def reset_custom_state(self, timeout=2.0):
"""
Resets default state for custom grippers
@return: True if custom gripper state reset successfully
@rtype: bool
"""
state_true = EndEffectorState.STATE_TRUE
state_unknown = EndEffectorState.STATE_UNKNOWN
# Create default state message
state_msg = EndEffectorState()
for idx, attr in enumerate(state_msg.__slots__):
if 'int' in state_msg._slot_types[idx]:
setattr(state_msg, attr, state_unknown)
setattr(state_msg, 'enabled', state_true)
self._state_pub.publish(state_msg)
# Verify state reset successfully
test = lambda: (self._state.enabled == state_true and self._state.
calibrated == state_unknown and self._state.ready ==
state_unknown and self._state.position == 0.0)
return crustcrawler_dataflow.wait_for(
test=test,
timeout=timeout,
raise_on_error=False,
body=lambda: self._state_pub.publish(state_msg))
def _on_trajectory_action(self, goal):
joint_names = goal.trajectory.joint_names
trajectory_points = goal.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.loginfo("%s: Executing requested joint trajectory" %
(self._action_name, ))
rospy.logdebug("Trajectory Points: {0}".format(trajectory_points))
control_rate = rospy.Rate(self._control_rate)
dimensions_dict = self._determine_dimensions(trajectory_points)
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.time_from_start = rospy.Duration(0)
trajectory_points.insert(0, first_trajectory_point)
num_points = len(trajectory_points)
# Force Velocites/Accelerations to zero at the final timestep
# if they exist in the trajectory
# Remove this behavior if you are stringing together trajectories,
# and want continuous, non-zero velocities/accelerations between
# trajectories
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 6 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(("{0}: Failed to compute a Bezier trajectory for {1}"
" arm with error \"{2}: {3}\"").format(
self._action_name, self._name,
type(ex).__name__, ex))
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()
crustcrawler_dataflow.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()):
#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,
start_time,
dimensions_dict)
self._update_feedback(deepcopy(point), joint_names, now_from_start)
# Release the Mutex
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()
end_angles = dict(zip(joint_names, last.positions))
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
while (now_from_start < (last_time + self._goal_time)
and not rospy.is_shutdown()):
if not self._command_joints(joint_names, last, start_time,
dimensions_dict):
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
result = check_goal_state()
if result is True:
rospy.loginfo("%s: Joint Trajectory Action Succeeded for %s arm" %
(self._action_name, self._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 for %s arm" %
(self._action_name, self._name))
self._result.error_code = self._result.GOAL_TOLERANCE_VIOLATED
self._server.set_aborted(self._result)
else:
rospy.logerr("%s: Exceeded Goal Threshold Error %s for %s arm" %
(self._action_name, result, self._name))
self._result.error_code = self._result.GOAL_TOLERANCE_VIOLATED
self._server.set_aborted(self._result)
self._command_stop(goal.trajectory.joint_names, end_angles, start_time,
dimensions_dict)
def __init__(self):
"""
Constructor.
"""
self.name = 'crustcrawler'
self._joint_angle = dict()
self._joint_velocity = dict()
self._joint_effort = dict()
self._cartesian_pose = dict()
self._cartesian_velocity = dict()
self._cartesian_effort = dict()
self._joint_names = {
'crustcrawler':
['joint_1', 'joint_2', 'joint_3', 'joint_4', 'joint_5', 'joint_6']
}
ns = '/crustcrawler/'
self._command_msg = JointCommand()
self._pub_speed_ratio = rospy.Publisher(ns + 'set_speed_ratio',
Float64,
latch=True,
queue_size=10)
self._pub_joint_cmd = rospy.Publisher(ns + 'joint_command',
JointCommand,
tcp_nodelay=True,
queue_size=1)
self._pub_joint_1_cmd = rospy.Publisher(
ns + 'joint_1_position_controller/command',
Float64,
tcp_nodelay=True,
queue_size=1)
self._pub_joint_2_cmd = rospy.Publisher(
ns + 'joint_2_position_controller/command',
Float64,
tcp_nodelay=True,
queue_size=1)
self._pub_joint_3_cmd = rospy.Publisher(
ns + 'joint_3_position_controller/command',
Float64,
tcp_nodelay=True,
queue_size=1)
self._pub_joint_4_cmd = rospy.Publisher(
ns + 'joint_4_position_controller/command',
Float64,
tcp_nodelay=True,
queue_size=1)
self._pub_joint_5_cmd = rospy.Publisher(
ns + 'joint_5_position_controller/command',
Float64,
tcp_nodelay=True,
queue_size=1)
self._pub_joint_6_cmd = rospy.Publisher(
ns + 'joint_6_position_controller/command',
Float64,
tcp_nodelay=True,
queue_size=1)
self._pub_joint_cmd_timeout = rospy.Publisher(ns +
'joint_command_timeout',
Float64,
latch=True,
queue_size=10)
_cartesian_state_sub = rospy.Subscriber(ns + 'endpoint_state',
EndpointState,
self._on_endpoint_states,
queue_size=1,
tcp_nodelay=True)
joint_state_topic = 'crustcrawler/joint_states'
_joint_state_sub = rospy.Subscriber(joint_state_topic,
JointState,
self._on_joint_states,
queue_size=1,
tcp_nodelay=True)
err_msg = ("%s limb init failed to get current joint_states "
"from %s") % (self.name.capitalize(), joint_state_topic)
crustcrawler_dataflow.wait_for(
lambda: len(self._joint_angle.keys()) > 0, timeout_msg=err_msg)
err_msg = ("%s limb init failed to get current endpoint_state "
"from %s") % (self.name.capitalize(), ns + 'endpoint_state')
#crustcrawler_dataflow.wait_for(lambda: len(self._cartesian_pose.keys()) > 0,
# timeout_msg=err_msg)
print(self._joint_angle)