From 8ea5150c7051fbcd36159efaca44a5cab0526aa9 Mon Sep 17 00:00:00 2001 From: yswi Date: Mon, 7 Mar 2022 19:41:27 -0500 Subject: [PATCH] Panda controllers setup and impedance interface. --- arm_robots/src/arm_robots/panda.py | 63 +++++++++++++++++++++++++++++- 1 file changed, 61 insertions(+), 2 deletions(-) diff --git a/arm_robots/src/arm_robots/panda.py b/arm_robots/src/arm_robots/panda.py index 2ecb8a8..eb6e753 100755 --- a/arm_robots/src/arm_robots/panda.py +++ b/arm_robots/src/arm_robots/panda.py @@ -1,9 +1,15 @@ #! /usr/bin/env python +import rospy from rosgraph.names import ns_join from typing import List, Tuple from arm_robots.robot import MoveitEnabledRobot from trajectory_msgs.msg import JointTrajectoryPoint +from franka_msgs.srv import SetJointImpedance, SetLoad +from controller_manager_msgs.srv import LoadController, SwitchController + +DEFAULT_JOINT_IMPEDANCE = [3000.0, 3000.0, 3000.0, 2500.0, 2500.0, 2000.0, 2000.0] +POSITION_JOINT_TRAJECTORY_CONTROLLER_NAME = 'position_joint_trajectory_controller' class Panda(MoveitEnabledRobot): @@ -12,14 +18,67 @@ def __init__(self, robot_namespace: str = 'combined_panda', force_trigger: float MoveitEnabledRobot.__init__(self, robot_namespace=robot_namespace, robot_description=ns_join(robot_namespace, 'robot_description'), - arms_controller_name='effort_joint_trajectory_controller', + arms_controller_name='position_joint_trajectory_controller', force_trigger=force_trigger, **kwargs) + # Panda HW Services - for setting internal controller parameters. + self.joint_impedance_srv = rospy.ServiceProxy(self.ns('franka_control/set_joint_impedance'), SetJointImpedance) + self.set_load_srv = rospy.ServiceProxy(self.ns('franka_control/set_load'), SetLoad) + + # Controller Manager Services - for loading/unloading/switching controllers. + self.load_controller_srv = rospy.ServiceProxy(self.ns('controller_manager/load_controller'), LoadController) + self.switch_controller_srv = rospy.ServiceProxy(self.ns('controller_manager/switch_controller'), + SwitchController) + + # Load default position joint trajectory controller. + self.load_controller(POSITION_JOINT_TRAJECTORY_CONTROLLER_NAME) + self.switch_controller(start_controllers=[POSITION_JOINT_TRAJECTORY_CONTROLLER_NAME]) + self.active_controller_name = POSITION_JOINT_TRAJECTORY_CONTROLLER_NAME + def send_joint_command(self, joint_names: List[str], trajectory_point: JointTrajectoryPoint) -> Tuple[bool, str]: # TODO: Fill in to send set point to controller. pass # TODO: Add gripper helpers. - # TODO: Add control mode setter/getter. + def switch_controller(self, start_controllers=None, stop_controllers=None) -> bool: + if stop_controllers is None: + stop_controllers = [] + if start_controllers is None: + start_controllers = [] + + try: + switch_controller_resp = self.switch_controller_srv(start_controllers=start_controllers, + stop_controllers=stop_controllers, + strictness=2) + except rospy.ServiceException as e: + raise Exception("Service call failed: %s" % e) + + return switch_controller_resp.ok + + def load_controller(self, controller_name: str) -> bool: + try: + load_controller_resp = self.load_controller_srv(controller_name) + except rospy.ServiceException as e: + raise Exception("Service call failed: %s" % e) + + return load_controller_resp.ok + + def set_joint_impedance(self, joint_impedance: List[float]) -> bool: + # Start by switching off the current controller, if active. + if self.active_controller_name is not None: + self.switch_controller(stop_controllers=[self.active_controller_name]) + + # Change joint impedance. + try: + set_joint_imped_resp = self.joint_impedance_srv(joint_impedance) + except rospy.ServiceException as e: + raise Exception("Failed to set impedance: %s" % e) + + if not set_joint_imped_resp.success: + raise Exception("Failed to set impedance: %s" % set_joint_imped_resp.error) + + # Switch to joint position controller. + self.switch_controller(start_controllers=[POSITION_JOINT_TRAJECTORY_CONTROLLER_NAME]) + self.active_controller_name = POSITION_JOINT_TRAJECTORY_CONTROLLER_NAME