def __init__(self, reconfig_server, rate=100.0, interpolation='minjerk'):
self._dyn = reconfig_server
self._fjt_ns = "position_joint_trajectory_controller/follow_joint_trajectory"
self._server = actionlib.SimpleActionServer(
self._fjt_ns,
FollowJointTrajectoryAction,
execute_cb=self._on_trajectory_action,
auto_start=False)
self._arm = franka_interface.ArmInterface(synchronous_pub=True)
self._enable = franka_interface.RobotEnable()
self._interpolation = interpolation
# Verify joint control mode
self._server.start()
self._alive = True
# Action Feedback/Result
self._fdbk = FollowJointTrajectoryFeedback()
self._result = FollowJointTrajectoryResult()
# Controller parameters from arguments, messages, and dynamic
# reconfigure
self._control_rate = rate # Hz
self._control_joints = []
self._pid_gains = {'kp': dict(), 'ki': dict(), 'kd': dict()}
self._goal_time = 0.0
self._stopped_velocity = 0.0001
self._goal_error = dict()
self._path_thresh = dict()
# Create our PID controllers
self._pid = dict()
for joint in self._arm.joint_names():
self._pid[joint] = pid.PID()
# Create our spline coefficients
self._coeff = [None] * len(self._arm.joint_names())
def main():
rospy.init_node('panda_robot_enable')
parser = argparse.ArgumentParser()
parser.add_argument('-s',
'--state',
const='state',
dest='actions',
action='append_const',
help='Print current robot state')
parser.add_argument('-e',
'--enable',
const='enable',
dest='actions',
action='append_const',
help='Enable the robot')
parser.add_argument('-d',
'--disable',
const='disable',
dest='actions',
action='append_const',
help='Disable the robot')
parser.add_argument('-r',
'--reset',
const='reset',
dest='actions',
action='append_const',
help='Reset the robot')
parser.add_argument('-S',
'--stop',
const='stop',
dest='actions',
action='append_const',
help='Stop the robot')
args = parser.parse_args(rospy.myargv()[1:])
if args.actions == None:
parser.print_usage()
parser.exit(0, "No action defined\n")
rs = franka_interface.RobotEnable()
try:
for act in args.actions:
if act == 'state':
print(rs.state())
elif act == 'enable':
rs.enable()
elif act == 'disable':
rs.disable()
elif act == 'reset':
rs.reset()
elif act == 'stop':
rs.stop()
except Exception as e:
rospy.logerr(e)
return 0
def __init__(self, on_state_callback=None, reset_frames=True):
"""
Constructor class. Functions from `franka_interface.ArmInterface <https://justagist.github.io/franka_ros_interface/DOC.html#arminterface>`_
:param on_state_callback: optional callback function to run on each state update
:param reset_frames: if True, EE frame is reset using :py:class:`franka_interface.ArmInteface`
(using `franka_interface.ArmInterface <https://justagist.github.io/franka_ros_interface/DOC.html#arminterface>`_ and `franka_tools.FrankaFramesInterface <https://justagist.github.io/franka_ros_interface/DOC.html#frankaframesinterface>`_)
"""
self._logger = logging.getLogger(__name__)
self._arm_configured = False # ----- don't update robot state value in this class until robot is fully configured
# Parent constructor
franka_interface.ArmInterface.__init__(self)
self._jnt_limits = [{
'lower': self.get_joint_limits().position_lower[i],
'upper': self.get_joint_limits().position_upper[i]
} for i in range(len(self.joint_names()))]
# number of joints
self._nq = len(self._jnt_limits)
# number of control commands
self._nu = len(self._jnt_limits)
self._configure(on_state_callback)
self._tuck = [self._neutral_pose_joints[j] for j in self._joint_names]
self._untuck = self._tuck
self._q_mean = np.array([
0.5 * (limit['lower'] + limit['upper'])
for limit in self._jnt_limits
])
self._franka_robot_enable_interface = franka_interface.RobotEnable(
self._params)
if not self._franka_robot_enable_interface.is_enabled():
self._franka_robot_enable_interface.enable()
# this will be useful to compute ee_velocity using finite differences
self._ee_pos_old, self._ee_ori_old = self.ee_pose()
self._time_now_old = time_in_seconds()
if reset_frames:
self.set_EE_frame_to_link(
'panda_hand' if self.has_gripper else 'panda_link8')