def _generate_feedback_msg(self, goal):
feedback = GripperCommandFeedback()
feedback.position = self._stat.position
feedback.effort = 0
feedback.stalled = False
feedback.reached_goal = abs(self._stat.position - goal) < self._grip_threshold
return feedback
def _feedback_msg(self, position_target, position_actual, effort_actual,
stalled):
feedback = GripperCommandFeedback()
feedback.position = self.position_to_joint_conv(position_actual)
feedback.effort = self.force_to_effort_conv(effort_actual)
feedback.stalled = stalled
feedback.reached_goal = False
return feedback
def _generate_feedback_msg(self, goal):
feedback = GripperCommandFeedback()
feedback.position = self._stat.position
feedback.effort = 0
feedback.stalled = False
feedback.reached_goal = abs(self._stat.position -
goal) < GRIP_THRESHOLD
return feedback
def execute(self, goal):
"""Action server callback.
Gripper has only two commands: fully open, fully close.
Args:
goal (GripperCommandGoal): action goal
"""
try:
# gripper has only two commands: fully open, fully close
close = goal.command.position > 1e-3
# we can choose from two effort modes
low_force_mode = goal.command.max_effort < 1e-3
if close:
self._gripper.close(low_force_mode, wait=False)
else:
self._gripper.open(low_force_mode, wait=False)
rate = rospy.Rate(self._action_monitor_rate)
while self._server.is_active():
rate.sleep()
width = (1.0 - self._gripper.opening) * self._limit.upper
force = self._limit.effort * (0.125 if low_force_mode else 1.0)
if self._gripper.is_ready:
reached_goal = abs(goal.command.position - width) < 0.001
stalled = not reached_goal
result = GripperCommandResult(
position=width,
effort=force,
stalled=stalled,
reached_goal=reached_goal,
)
self._server.set_succeeded(result)
return
feedback = GripperCommandFeedback(
position=width,
effort=force,
stalled=False,
reached_goal=False,
)
self._server.publish_feedback(feedback)
except Exception as ex:
rospy.logerr('Griper commmand failed: %s', ex)
self._server.set_aborted(text=str(ex))
def __init__(self, gripper, scale_pos, reconfig_server):
self._scale_pos = scale_pos
self._dyn = reconfig_server
self._ee = gripper + '_gripper'
self._ns = 'robot/end_effector/' + self._ee + '/gripper_action'
self._gripper = baxter_interface.Gripper(gripper, CHECK_VERSION)
# Store Gripper Type
self._type = self._gripper.type()
if self._type == 'custom':
msg = ("Stopping %s action server - %s gripper not capable of "
"gripper actions" % (self._gripper.name, self._type))
rospy.logerr(msg)
return
# Verify Grippers Have No Errors and are Calibrated
if self._gripper.error():
self._gripper.reset()
if self._gripper.error():
msg = ("Stopping %s action server - Unable to clear error" %
self._gripper.name)
rospy.logerr(msg)
return
if not self._gripper.calibrated():
self._gripper.calibrate()
if not self._gripper.calibrated():
msg = ("Stopping %s action server - Unable to calibrate" %
self._gripper.name)
rospy.logerr(msg)
return
# Action Server
self._server = actionlib.SimpleActionServer(
self._ns,
GripperCommandAction,
execute_cb=self._on_gripper_action,
auto_start=False)
self._action_name = rospy.get_name()
self._server.start()
# Action Feedback/Result
self._fdbk = GripperCommandFeedback()
self._result = GripperCommandResult()
# Initialize Parameters
self._prm = self._gripper.parameters()
self._timeout = 5.0
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)
def __init__(self):
self._ns = "/robot/gripper_action_server"
self._gripper = intera_interface.Gripper()
# Action Server
self._server = actionlib.SimpleActionServer(
self._ns,
GripperCommandAction,
execute_cb=self._on_gripper_action,
auto_start=False)
self._action_name = rospy.get_name()
self._server.start()
self._gripper.set_dead_zone("0.021")
# Action Feedback/Result
self.feedback_msg = GripperCommandFeedback()
self.result_msg = GripperCommandResult()
# Initialize Parameters
self._timeout = 5.0
def __init__(self, controller_namespace, controllers):
self.update_rate = 1000
self.controller_namespace = controller_namespace
self.joint_names = [ctrl.joint_name for ctrl in controllers]
# Get joint to controller dict
self.joint_to_controller = {}
for ctrl in controllers:
self.joint_to_controller[ctrl.joint_name] = ctrl
# Get port to joints dicts
self.port_to_joints = {}
for ctrl in controllers:
if ctrl.port_namespace not in self.port_to_joints:
self.port_to_joints[ctrl.port_namespace] = []
self.port_to_joints[ctrl.port_namespace].append(ctrl.joint_name)
# Get get port to io (DynamixelIO)
self.port_to_io = {}
for ctrl in controllers:
if ctrl.port_namespace in self.port_to_io:
continue
self.port_to_io[ctrl.port_namespace] = ctrl.dxl_io
self.joint_states = dict(
zip(self.joint_names, [ctrl.joint_state for ctrl in controllers]))
self.num_joints = len(self.joint_names)
self.joint_to_idx = dict(zip(self.joint_names, range(self.num_joints)))
# Base msgs
# Feedback and result base msg
self.feedback = GripperCommandFeedback()
self.result = GripperCommandResult()
# Joint state base msg
self.joint_state = JointState()
self.joint_state.name = self.joint_names
self.joint_state.position = [0.0] * self.num_joints
self.joint_state.velocity = [0.0] * self.num_joints
self.joint_state.effort = [0.0] * self.num_joints
# Current max effort
self.current_goal = 0.0
self.current_effort = 0.5
# Soft sensor
self.left_side_pressure = UInt16()
self.right_side_pressure = UInt16()
self.sensor_effort = 0.0
def __init__(self):
self._ns = 'franka_gripper/gripper_action'
self._gripper = franka_interface.GripperInterface(
gripper_joint_names=['panda_finger_joint1', 'panda_finger_joint2'])
# Action Server
self._server = actionlib.SimpleActionServer(
self._ns,
GripperCommandAction,
execute_cb=self._on_gripper_action,
auto_start=False)
self._action_name = "GripperActionServer"
self._server.start()
# Action Feedback/Result
self._fdbk = GripperCommandFeedback()
self._result = GripperCommandResult()
# Initialize Parameters
self._timeout = 5.0
def __init__(self, reconfig_server):
self._dyn = reconfig_server
self._ee = 'gripper'
self._ns = 'crustcrawler/end_effector/' + self._ee + '/gripper_action'
self._gripper = crustcrawler_interface.Gripper()
# Verify Grippers Have No Errors and are Calibrated
if self._gripper.error():
self._gripper.reset()
if self._gripper.error():
msg = ("Stopping %s action server - Unable to clear error" %
self._gripper.name)
rospy.logerr(msg)
return
if not self._gripper.calibrated():
self._gripper.calibrate()
if not self._gripper.calibrated():
msg = ("Stopping %s action server - Unable to calibrate" %
self._gripper.name)
rospy.logerr(msg)
return
# Action Server
self._server = actionlib.SimpleActionServer(
self._ns,
GripperCommandAction,
execute_cb=self._on_gripper_action,
auto_start=False)
self._action_name = rospy.get_name()
self._server.start()
# Action Feedback/Result
self._fdbk = GripperCommandFeedback()
self._result = GripperCommandResult()
# Initialize Parameters
#self._prm = self._gripper.parameters()
self._timeout = 5.0
def _state_update(self, state):
if not self._as.is_active():
return
rospy.logdebug('Got servo state update:\n{}'.format(state))
self._last_state = state
feedback = GripperCommandFeedback()
self._state_to_feedback(state, feedback)
self._as.publish_feedback(feedback)
if feedback.reached_goal:
rospy.loginfo('Determined from state update that gripper goal has '
'been reached')
if self._timer:
self._timer.shutdown()
self._timer = None
result = GripperCommandResult()
self._as.set_succeeded(result=self._state_to_feedback(
state, result),
text='Reached goal')
elif feedback.stalled:
rospy.loginfo('Determined from state update that gripper has '
'stalled')
if self._timer:
self._timer.shutdown()
self._timer = None
result = GripperCommandResult()
self._as.set_aborted(result=self._state_to_feedback(state, result),
text='Stalled')
elif abs(state.load) > self._overload:
rospy.loginfo('Determined from state update that gripper is '
'blocked (overload protection)')
if self._timer:
self._timer.shutdown()
self._timer = None
self._command_pub.publish(state.current_pos)
result = GripperCommandResult()
self._as.set_aborted(result=self._state_to_feedback(state, result),
text='Blocked')
class ARMRosConn():
_feedback = FollowJointTrajectoryActionFeedback()
_result = FollowJointTrajectoryActionResult()
_gripper_feedback = GripperCommandFeedback()
_gripper_result = GripperCommandActionResult()
def __init__(self):
rospy.init_node('arm')
self.lSpeeds = [220] * 6 # default pwm speed for all 6 motors
self.lAngles = [0] * 6 # current angle for all 6 motors, needed to calculate current error
# switch forward/reverse direction on motors
arm.switch(0)
arm.switch(2)
# these motors do not have anquadrature encoder
arm.set_hall_mode(3, 0)
arm.set_hall_mode(5, 0)
# lower position tolerance of these motors
arm.set_tolerance(3, 0)
arm.set_tolerance(5, 0)
self._as_arm = actionlib.SimpleActionServer("arm_controller/follow_joint_trajectory", FollowJointTrajectoryAction, execute_cb=self.execute_joint_trajectory, auto_start = False)
self._as_arm.start()
self._as_gripper = actionlib.SimpleActionServer("gripper_controller/gripper_action", GripperCommandAction, execute_cb=self.execute_gripper_action, auto_start = False)
self._as_gripper.start()
self.pub_joint_states = rospy.Publisher("joint_states", JointState, queue_size=16)
self.dyn_conf = Server(ArmConfig, self.execute_dyn_reconf)
self.run()
def run(self):
rate = rospy.Rate(20)
while not rospy.is_shutdown():
self.publish_joint_states()
rate.sleep()
def execute_dyn_reconf(self, config, level):
self.lSpeeds = [config["speed_1"], config["speed_2"], config["speed_3"], config["speed_4"], config["speed_5"], config["speed_6"]]
return config
def publish_joint_states(self):
joint_state = JointState()
joint_state.header.stamp = rospy.Time.now()
joint_state.name = lJointNames
# position of last motor (gripper) needs to be adjusted by 0.35, also value is expected two times: For right and left gripper joint.
self.lAngles = [arm.get_angle(0), arm.get_angle(1), arm.get_angle(2), arm.get_angle(3), arm.get_angle(4), 0.35-arm.get_angle(5)]
joint_state.position = self.lAngles[:-1] + [self.lAngles[-1], self.lAngles[-1]]
self.pub_joint_states.publish(joint_state)
def execute_joint_trajectory(self, goal):
self._result.status = FollowJointTrajectoryResult.SUCCESSFUL
for point in goal.trajectory.points:
print goal.trajectory.joint_names
print point.positions
# get new desired joint values in order from link1_joint to link5_joint
lGoalPosOrdered = [
point.positions[goal.trajectory.joint_names.index(lJointNames[0])],
point.positions[goal.trajectory.joint_names.index(lJointNames[1])],
point.positions[goal.trajectory.joint_names.index(lJointNames[2])],
point.positions[goal.trajectory.joint_names.index(lJointNames[3])],
point.positions[goal.trajectory.joint_names.index(lJointNames[4])],
]
try:
# commit new values to hardware
arm.to_angle(0, self.lSpeeds[0], lGoalPosOrdered[0])
arm.to_angle(1, self.lSpeeds[1], lGoalPosOrdered[1])
arm.to_angle(2, self.lSpeeds[2], lGoalPosOrdered[2])
arm.to_angle(3, self.lSpeeds[3], lGoalPosOrdered[3])
arm.to_angle(4, self.lSpeeds[4], lGoalPosOrdered[4])
except arm.RangeError as e:
# reject if position is impossible for the hardware to reach
print >> sys.stderr, e.message
self._feedback.status = GoalStatus.REJECTED
self._as_arm.publish_feedback(self._feedback.feedback)
self._result.status = FollowJointTrajectoryResult.INVALID_GOAL
break
# publish current position error while driving to position
error = 0
while True:
error = np.array(lGoalPosOrdered) - np.array(self.lAngles[:-1]) # error = desired - current
print "Error", error
# Position reached, done
if all(abs(f) < 0.15 for f in error): # allow a tiny tolerance and continue with next goal
print "Position reached"
self._feedback.status = GoalStatus.SUCCEEDED
break
# Cancel if requested
if self._as_arm.is_preempt_requested():
self._feedback.status = GoalStatus.PREEMPTING
self._as_arm.set_preempted()
break
sleep(0.001)
# Give feedback to either canceled or current position reached
self._feedback.feedback.joint_names = lJointNames[:-1]
self._feedback.feedback.desired.positions = lGoalPosOrdered
self._feedback.feedback.actual.positions = self.lAngles[:-1]
self._feedback.feedback.error.positions = error
self._as_arm.publish_feedback(self._feedback.feedback)
self._as_arm.set_succeeded(self._result.result)
def execute_gripper_action(self, goal):
# adjust position and commit to hardware
arm.to_angle(5, self.lSpeeds[5], 0.35-goal.command.position)
while True:
error = goal.command.position - self.lAngles[-1] # error = desired - current
if abs(error) < 0.02: # tolerance for position
break
# Give feedback
self._gripper_feedback.position = self.lAngles[-1]
self._gripper_feedback.stalled = False
self._gripper_feedback.reached_goal = False
# Cancel if requested
if self._as_gripper.is_preempt_requested():
self._as_gripper.set_preempted()
break
sleep(0.001)
# Done, either canceled or position reached
self._gripper_result.status = GoalStatus.SUCCEEDED
self._gripper_result.result.position = goal.command.position
self._gripper_result.result.stalled = False
self._gripper_result.result.reached_goal = True
self._as_gripper.set_succeeded(self._gripper_result.result)
def __init__(self, prefix="", gripper="", interface='eth0', jaco_ip="10.66.171.15", dof="", rate=100.0):
self._alive = False
self.init_success = True
self._action_name = rospy.get_name()
self._prefix = prefix
# Action Feedback/Result
if ("kg2" == gripper) or ("rq85" == gripper):
self.gripper_stall_force = 20.0
self.gripper_dead_zone = 0.01
elif("kg3" == gripper):
self.gripper_stall_force = 30.0
self.gripper_dead_zone = 0.01
self._last_gripper_pos = 0.165
self._gripper_stall_to = 0.7
self._gripper_pos_stall = False
self._last_movement_time = rospy.get_time()
self.dof = dof
self._planner_homing = False
"""
Define the joint names
"""
if ("6dof" == dof):
self._joint_names = [self._prefix+'_shoulder_pan_joint',
self._prefix+'_shoulder_lift_joint',
self._prefix+'_elbow_joint',
self._prefix+'_wrist_1_joint',
self._prefix+'_wrist_2_joint',
self._prefix+'_wrist_3_joint']
self._body_joints = ["right_elbow_joint",
"right_shoulder_lift_joint",
"right_shoulder_pan_joint",
"right_wrist_1_joint",
"right_wrist_2_joint",
"right_wrist_3_joint",
"left_elbow_joint",
"left_shoulder_lift_joint",
"left_shoulder_pan_joint",
"left_wrist_1_joint",
"left_wrist_2_joint",
"left_wrist_3_joint",
"linear_joint",
"pan_joint",
"tilt_joint"]
self._homed = [-2.135, -0.227, -1.478, -2.083, 1.445, 1.321, 2.135, 0.227, 1.478, 2.083, -1.445, -1.321, 0.25, 0.0, 0.0]
elif ("7dof" == dof):
self._joint_names = [self._prefix + '_shoulder_pan_joint',
self._prefix + '_shoulder_lift_joint',
self._prefix + '_arm_half_joint',
self._prefix + '_elbow_joint',
self._prefix + '_wrist_spherical_1_joint',
self._prefix + '_wrist_spherical_2_joint',
self._prefix + '_wrist_3_joint']
self._body_joints = ["right_shoulder_pan_joint",
"right_shoulder_lift_joint",
"right_arm_half_joint",
"right_elbow_joint",
"right_wrist_spherical_1_joint",
"right_wrist_spherical_2_joint",
"right_wrist_3_joint",
"left_shoulder_pan_joint",
"left_shoulder_lift_joint",
"left_arm_half_joint",
"left_elbow_joint",
"left_wrist_spherical_1_joint",
"left_wrist_spherical_2_joint",
"left_wrist_3_joint",
"linear_joint",
"pan_joint",
"tilt_joint"]
self._homed = [-1.5, -0.2, -0.15, -2.0, 2.0, -1.24, -1.1, 1.5, 0.2, 0.15, 2.0, -2.0, 1.24, 1.1, 0.25, 0, 0]
else:
rospy.logerr("DoF needs to be set 6 or 7, cannot start MovoArmJTAS")
return
"""
Controller parameters from arguments, messages, and dynamic
reconfigure
"""
self._trajectory_control_rate = rate # Hz
self._goal_time = 0.0
self._stopped_velocity = 0.0
self._goal_error = dict()
self._path_thresh = dict()
self._traj_smoother = TrajectorySmoother(rospy.get_name(),self._prefix)
self._ctl = SIArmController(self._prefix,gripper,interface,jaco_ip, dof)
self._ctl.Pause()
self._estop_delay = 0
self.home_arm_sub = rospy.Subscriber('/movo/home_arms', Bool, self._home_arms)
self.home_arm_pub = rospy.Publisher('/movo/arms_are_homed', Bool, queue_size=1)
self._arms_homing = False
if not self._ctl.init_success:
rospy.logerr("Failed to initialize controller, make sure the serial number exists")
self.clean_shutdown()
self.init_success = False
return
self.estop = False
self._fdbk = FollowJointTrajectoryFeedback()
self._result = FollowJointTrajectoryResult()
#self._dyn = reconfig_server
self._ns = '/movo/%s_arm_controller'%self._prefix
self._fjt_ns = self._ns + '/follow_joint_trajectory'
self._server = actionlib.SimpleActionServer(
self._fjt_ns,
FollowJointTrajectoryAction,
execute_cb=self._on_trajectory_action,
auto_start=False)
self._alive = True
self._movo_status_sub = rospy.Subscriber("/movo/feedback/status",Status,self._update_movo_status)
self._server.start()
# Action Server
self._gripper_server = actionlib.SimpleActionServer(
'/movo/%s_gripper_controller/gripper_cmd'%self._prefix,
GripperCommandAction,
execute_cb=self._on_gripper_action,
auto_start=False)
self._gripper_server.start()
self._gripper_action_name = '/movo/%s_gripper_controller/gripper_cmd'%self._prefix
# Action Feedback/Result
self._gripper_fdbk = GripperCommandFeedback()
self._gripper_result = GripperCommandResult()
self._gripper_timeout = 6.0
self._ctl.api.InitFingers()
class JointController():
_trajectory_feedback = FollowJointTrajectoryFeedback()
_trajectory_result = FollowJointTrajectoryResult()
_gripper_feedback = GripperCommandFeedback()
_gripper_result = GripperCommandActionResult()
def __init__(self):
self.name = rospy.get_name()
# store angles to send to servos
self.robot_joints = {}
# publishers
self.pub_servos = rospy.Publisher('motors', UInt16MultiArray, queue_size=10) # xsi
self.publisher = rospy.Publisher('joint_states', JointState, queue_size=10)
# Set up action FollowJointTrajectoryAction
self.action_server_trajectory = actionlib.SimpleActionServer('%s/follow_joint_trajectory'%self.name,
FollowJointTrajectoryAction,
self.do_action_trajectory_callback, False)
self.action_server_trajectory.start()
# Set up action GripperCommandAction
self.action_server_gripper = actionlib.SimpleActionServer("/gripper_controller/gripper_action",
GripperCommandAction,
self.do_action_gripper_callback, False)
self.action_server_gripper.start()
self.rate = rospy.Rate(10) # 10hz
self.joint_state = JointState()
self.joint_state.header = Header()
self.joint_state.name = ['joint_1', 'joint_2', 'joint_3', 'joint_6']
self.joint_state.position = [0, 0, 0, 0] # will be updated in home() before publication in the loop
self.joint_state.velocity = []
self.joint_state.effort = []
# define ignore (0) direction (-/+) and multiplication for each joint received on message
self.actuated_joints = [-2, -1, 1] # default for ebamk2
# Initialise
self.home()
def deg(self, rad):
rad = rad + 1.578
return rad * 57.2958
def do_action_trajectory_callback(self, goal):
print ('Received trajectory goal %d'%len(goal.trajectory.points[0].positions))
timePassed = rospy.Duration(0)
self.joint_state.name = goal.trajectory.joint_names
# we do not have joint_6 in the trajectory message so check and add
if not "joint_6" in self.joint_state.name:
self.joint_state.name.append("joint_6")
for point in goal.trajectory.points:
self.joint_state.header.stamp = rospy.Time.now()
self.move_arm(goal.trajectory.joint_names, point.positions)
self.publisher.publish(self.joint_state)
rospy.sleep(point.time_from_start - timePassed)
timePassed = point.time_from_start
#print('trajectory step')
self._trajectory_result.error_code = 0
self.action_server_trajectory.set_succeeded(self._trajectory_result)
print('trajectory move complete')
def do_action_gripper_callback(self, goal):
print ('Received gripper goal %.2f'%goal.command.position)
# adjust position and commit to hardware
self.c(goal.command.position)
self.publish_servos_state(self.robot_joints)
# code for action feedback and cancel
#while True:
# error = goal.command.position - self.lAngles[-1] # error = desired - current
# if abs(error) < 0.02: # tolerance for position
# break
# Give feedback
# self._gripper_feedback.position = self.lAngles[-1]
# self._gripper_feedback.stalled = False
# self._gripper_feedback.reached_goal = False
# Cancel if requested
# if self._as_gripper.is_preempt_requested():
# self._as_gripper.set_preempted()
# break
# sleep(0.001)
# Action done, either canceled or position reached
#self._gripper_result.status = GoalStatus.SUCCEEDED
self._gripper_result.status = 3 # 3 = GoalStatus.SUCCEEDED
self._gripper_result.result.position = goal.command.position
self._gripper_result.result.stalled = False
self._gripper_result.result.reached_goal = True
self.action_server_gripper.set_succeeded(self._gripper_result.result)
print('gripper move complete')
def get_joint_goal(self, joint_name, joint_list, goal_list):
if joint_name in joint_list:
index = joint_list.index(joint_name)
return goal_list[index]
print 'ERROR!! Could not find %s goal'% joint_name
def set_joint_state(self, joint_name, angle):
index = self.joint_state.name.index(joint_name)
print '%s at %d is %0.5f (%0.2f)' % (joint_name, index, angle, math.degrees(angle))
self.joint_state.position[index] = angle
def move_arm(self, names, goal):
joint_1 = self.get_joint_goal('joint_1', names, goal)
joint_2 = self.get_joint_goal('joint_2', names, goal)
joint_3 = self.get_joint_goal('joint_3', names, goal)
self.setJointAngles(joint_1, joint_2, joint_3)
def check_min_max(self, minVal, maxVal, test):
if (test < minVal):
test = minVal
print "min %d" % (minVal)
if (test > maxVal):
test = maxVal
print "max %d" % (maxVal)
return test
# Helper function to make setting a servo pulse width simpler.
def set_servo_angle(self, channel, angle):
self.robot_joints[channel] = int(self.deg(angle))
def b(self, angle):
angle = self.check_min_max(cal.MIN_LINK_1, cal.MAX_LINK_1, angle)
self.set_joint_state('joint_1', angle)
# prepare the angle sent to mcu (in degrees)
angle = angle * self.actuated_joints[0] # joint_1 index
self.set_servo_angle('joint_1', angle)
def s(self, angle):
angle = self.check_min_max(cal.MIN_LINK_2, cal.MAX_LINK_2, angle)
self.set_joint_state('joint_2', angle)
# prepare the angle sent to mcu (in degrees)
angle = angle * self.actuated_joints[1] # joint_2 index
self.set_servo_angle('joint_2', angle)
def e(self, angle):
angle = self.check_min_max(cal.MIN_LINK_3, cal.MAX_LINK_3, angle)
self.set_joint_state('joint_3', angle)
# prepare the angle sent to mcu (in degrees)
angle = angle * self.actuated_joints[2] # joint_3 index
self.set_servo_angle('joint_3', angle)
def c(self, angle):
angle = self.check_min_max(cal.MIN_LINK_6, cal.MAX_LINK_6, angle)
self.set_joint_state('joint_6', angle)
self.set_servo_angle('joint_6', angle)
def setJointAngles(self, joint_1, joint_2, joint_3):
self.b(joint_1)
self.s(joint_2)
self.e(joint_3)
self.publish_servos_state(self.robot_joints)
def home(self):
self.b(cal.HOME_LINK_1)
self.s(cal.HOME_LINK_2)
self.e(cal.HOME_LINK_3)
self.c(cal.HOME_LINK_6)
self.publish_servos_state(self.robot_joints)
def off(self):
self.b(cal.HOME_LINK_1)
self.s(cal.HOME_LINK_2)
self.e(cal.HOME_LINK_3)
self.c(cal.HOME_LINK_6)
self.publish_servos_state(self.robot_joints)
def publish_servos_state(self, state):
array_msg = UInt16MultiArray()
array_msg.data = [state["joint_1"], state["joint_2"], state["joint_3"], state["joint_6"]]
self.pub_servos.publish(array_msg)
def loop(self):
# publish states regularly though this is also done on commands
while not rospy.is_shutdown():
self.joint_state.header.stamp = rospy.Time.now()
self.publisher.publish(self.joint_state)
self.rate.sleep()
self.off()
def actionCb(self, goal):
result = GripperCommandResult()
feedback = GripperCommandFeedback()
""" Take an input command of width to open gripper. """
rospy.loginfo('Gripper Controller action goal received: position %f, max_effort %f',
goal.command.position, goal.command.max_effort)
# send command to gripper
if not self.model.setCommand(goal.command):
self.server.set_aborted()
rospy.loginfo('Gripper Controller: Aborted.')
return
# register progress so we can guess if the gripper is stalled; our buffer
# must contain up to: stalled_time / joint_states period position values
if len(self.state_cb_times) == self.state_cb_times.maxlen:
T = sum(self.state_cb_times[i] - self.state_cb_times[i-1] \
for i in xrange(1, len(self.state_cb_times) - 1)) \
/(len(self.state_cb_times) - 1)
progress = collections.deque(maxlen=round(self.stalled_time/T))
else:
self.server.set_aborted()
rospy.logerr('Gripper Controller: no messages from joint_states topic received')
return
diff_at_start = round(abs(goal.command.position - self.current_position), 3)
# keep watching for gripper position...
while True:
if self.server.is_preempt_requested():
self.server.set_preempted()
rospy.loginfo('Gripper Controller: Preempted.')
return
# synchronize with the joints state callbacks;
self.state_cb_event.wait()
self.state_cb_event.clear()
# break when we have reached the goal position...
diff = abs(goal.command.position - self.current_position)
if diff < self._EPSILON_OPENING_DIFF_:
result.reached_goal = True
break
# ...or when the gripper is exerting beyond max effort
if goal.command.max_effort and self.current_effort >= goal.command.max_effort:
# stop moving to prevent damaging the gripper
goal.command.position = self.current_position
self.model.setCommand(goal.command)
rospy.logerr('Gripper Controller: max effort reached at position %f (%f >= %f)',
self.current_position, self.current_effort, goal.command.max_effort)
result.stalled = True
break
# ...or when progress stagnates, probably signaling that the gripper is exerting max effort and not moving
progress.append(round(diff, 3)) # round to millimeter to neglect tiny motions of the stalled gripper
if len(progress) == progress.maxlen and progress.count(progress[0]) == len(progress):
if progress[0] == diff_at_start:
# we didn't move at all! is the gripper connected?
self.server.set_aborted()
rospy.logerr('Gripper Controller: gripper not moving; is the servo connected?')
return
# buffer full with all-equal positions -> gripper stalled
goal.command.position = self.current_position
self.model.setCommand(goal.command)
rospy.logerr('Gripper Controller: gripper stalled at position %f', self.current_position)
result.stalled = True
break
# publish feedback
feedback.position = self.current_position
feedback.effort = self.current_effort
self.server.publish_feedback(feedback)
# publish one last feedback and the result (identical)
feedback.position = self.current_position
feedback.effort = self.current_effort
feedback.reached_goal = result.reached_goal
feedback.stalled = result.stalled
self.server.publish_feedback(feedback)
result.position = self.current_position
result.effort = self.current_effort
self.server.set_succeeded(result)
rospy.loginfo('Gripper Controller: Succeeded (%s)', 'goal reached' if result.reached_goal else 'stalled')