diff --git a/arm_robots/scripts/tutorial/panda_basic_motion.py b/arm_robots/scripts/tutorial/panda_basic_motion.py index d26f3bc..6778b84 100755 --- a/arm_robots/scripts/tutorial/panda_basic_motion.py +++ b/arm_robots/scripts/tutorial/panda_basic_motion.py @@ -1,11 +1,63 @@ #! /usr/bin/env python +import pdb import rospy +import numpy as np from arm_robots.panda import Panda +from moveit_msgs.msg import RobotTrajectory +import tf + + +def execute_plan(panda, group_name, joint_trajectory): + move_group = panda.get_move_group_commander(group_name) + plan_msg = RobotTrajectory() + plan_msg.joint_trajectory = joint_trajectory + move_group.execute(plan_msg) + if __name__ == '__main__': rospy.init_node('panda_motion') panda = Panda() panda.connect() + print("Panda connected! Planning now...") + # pdb.set_trace() + panda.set_execute(False) # this will stop the robot from actually executing a path, good for testing + if not panda.execute: + print("Real execution disabled.") + panda_1_action_1_start = [0.25, 0.25, 1.16, np.radians(180), 0, np.radians(-90)] + panda_2_action_1_start = [0.25, 0.15, 1.16, np.radians(-90), np.radians(-90), 0] + panda_1_action_1_start_plan = panda.plan_to_pose(panda.panda_1, panda.panda_1_EE, panda_1_action_1_start, frame_id="world") + panda_2_action_1_start_plan = panda.plan_to_pose(panda.panda_2, panda.panda_2_EE, panda_2_action_1_start, frame_id="world") + + pdb.set_trace() + + execute_plan(panda, panda.panda_1, panda_1_action_1_start_plan.planning_result.plan.joint_trajectory) + execute_plan(panda, panda.panda_2, panda_2_action_1_start_plan.planning_result.plan.joint_trajectory) + panda.set_execute(False) + + pdb.set_trace() + + panda_1_action_2_start = [0.25, 0.25, 1.4, np.radians(90), np.radians(-90), 0] + panda_2_action_2_start = [0.25, 0.15, 1.36, np.radians(-90), np.radians(-90), 0] + panda_1_action_2_start_plan = panda.plan_to_pose(panda.panda_1, panda.panda_1_EE, panda_1_action_2_start, frame_id="world") + panda_2_action_2_start_plan = panda.plan_to_pose(panda.panda_2, panda.panda_2_EE, panda_2_action_2_start, frame_id="world") + + pdb.set_trace() + + execute_plan(panda, panda.panda_1, panda_1_action_2_start_plan.planning_result.plan.joint_trajectory) + execute_plan(panda, panda.panda_2, panda_2_action_2_start_plan.planning_result.plan.joint_trajectory) + panda.set_execute(False) + + pdb.set_trace() + + # panda_2_action_1_end = [0.25, 0.20, 1.155, np.radians(-90), np.radians(-90), 0] + # pt = [[np.array(panda_2_action_1_end[:3])]] + # ori = [tf.transformations.quaternion_from_euler(panda_2_action_1_end[3], panda_2_action_1_end[4], panda_2_action_1_end[5])] + # panda_2_action_1_end_plan = panda.follow_jacobian_to_position(panda.panda_2, ['panda_2_link_planning_EE'], pt, ori, 0.01) + # + # pdb.set_trace() + # + # execute_plan(panda, panda.panda_2, panda_2_action_1_end_plan.planning_result.plan.joint_trajectory) + # + # pdb.set_trace() - # TODO: Reference med_motion.py for examples. \ No newline at end of file diff --git a/arm_robots/src/arm_robots/panda.py b/arm_robots/src/arm_robots/panda.py index 2ecb8a8..7fc7e75 100755 --- a/arm_robots/src/arm_robots/panda.py +++ b/arm_robots/src/arm_robots/panda.py @@ -1,25 +1,153 @@ #! /usr/bin/env python -from rosgraph.names import ns_join -from typing import List, Tuple +import pdb +from typing import Optional, Callable, List, Tuple + +import actionlib +import rospy +import numpy as np +from geometry_msgs.msg import WrenchStamped +from moveit_msgs.msg import RobotTrajectory +from trajectory_msgs.msg import JointTrajectory, JointTrajectoryPoint from arm_robots.robot import MoveitEnabledRobot -from trajectory_msgs.msg import JointTrajectoryPoint +from franka_gripper.msg import GraspAction, GraspGoal, MoveAction, MoveGoal, HomingAction, HomingGoal, StopAction, \ + StopGoal +from netft_rdt_driver.srv import Zero +from control_msgs.msg import GripperCommandActionGoal, GripperCommand, GripperCommandGoal +from franka_msgs.msg import ErrorRecoveryAction, ErrorRecoveryGoal, FrankaState +from rosgraph.names import ns_join +from sensor_msgs.msg import JointState +from arm_robots.robot_utils import ExecutionResult class Panda(MoveitEnabledRobot): - def __init__(self, robot_namespace: str = 'combined_panda', force_trigger: float = -0.0, **kwargs): + def __init__(self, robot_namespace: str = '', force_trigger: float = -0.0, **kwargs): 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=None, force_trigger=force_trigger, **kwargs) + self.panda_1 = 'panda_1' + self.panda_2 = 'panda_2' + self.panda_1_command_pub = rospy.Publisher(ns_join(self.panda_1, 'panda_1_position_joint_trajectory_controller/command'), JointTrajectory, queue_size=10) + self.panda_2_command_pub = rospy.Publisher(ns_join(self.panda_2, 'panda_2_position_joint_trajectory_controller/command'), JointTrajectory, queue_size=10) + self.display_goals = False + self.panda_1_gripper = PandaGripper(self.robot_namespace, self.panda_1) + self.panda_2_gripper = PandaGripper(self.robot_namespace, self.panda_2) + self.panda_1_netft = PandaNetft(self.robot_namespace, self.panda_1) + self.panda_2_netft = PandaNetft(self.robot_namespace, self.panda_2) + + self.panda_1_client = self.setup_joint_trajectory_controller_client(ns_join(self.panda_1, f'{self.panda_1}_position_joint_trajectory_controller')) + self.panda_2_client = self.setup_joint_trajectory_controller_client(ns_join(self.panda_2, f'{self.panda_2}_position_joint_trajectory_controller')) + + def follow_arms_joint_trajectory(self, trajectory: JointTrajectory, stop_condition: Optional[Callable] = None, + group_name: Optional[str] = None): + if group_name == self.panda_1: + return self.follow_joint_trajectory(trajectory, self.panda_1_client, stop_condition=stop_condition) + elif group_name == self.panda_2: + return self.follow_joint_trajectory(trajectory, self.panda_2_client, stop_condition=stop_condition) + + def send_joint_command(self, joint_names: List[str], trajectory_point: JointTrajectoryPoint, group_name: Optional[str] = None): + robot_command = JointTrajectory() + robot_command.joint_names = joint_names + robot_command.points.append(trajectory_point) + pdb.set_trace() + if group_name == self.panda_1: + self.panda_1_command_pub.publish(robot_command) + elif group_name == self.panda_2: + self.panda_2_command_pub.publish(robot_command) + + + +class PandaNetft: + def __init__(self, robot_ns, arm_id, stop_force=3.0, stop_torque=0.3): + self.netft_ns = ns_join(robot_ns, f'{arm_id}_netft') + self.netft_zero = rospy.ServiceProxy(ns_join(self.netft_ns, 'zero'), Zero) + self.netft_data = None + self.netft_data_sub = rospy.Subscriber(ns_join(self.netft_ns, 'netft_data'), WrenchStamped, self.netft_data_cb, queue_size=None) + self.stop_force = stop_force + self.stop_torque = stop_torque + + def zero_netft(self): + self.netft_zero() + + def netft_data_cb(self, wrench_msg): + self.netft_data = np.array((wrench_msg.wrench.force.x, wrench_msg.wrench.force.y, wrench_msg.wrench.force.z, wrench_msg.wrench.torque.x, wrench_msg.wrench.torque.y, wrench_msg.wrench.torque.z)) + + def stop_condition(self, feedback): + force_magnitude = np.linalg.norm(self.netft_data[:3]) + return force_magnitude > self.stop_force + + +class PandaGripper: + def __init__(self, robot_ns, arm_id): + self.gripper_ns = ns_join(robot_ns, f'{arm_id}/franka_gripper') + self.grasp_client = actionlib.SimpleActionClient(ns_join(self.gripper_ns, 'grasp'), GraspAction) + self.move_client = actionlib.SimpleActionClient(ns_join(self.gripper_ns, 'move'), MoveAction) + self.homing_client = actionlib.SimpleActionClient(ns_join(self.gripper_ns, 'homing'), HomingAction) + self.stop_client = actionlib.SimpleActionClient(ns_join(self.gripper_ns, 'stop'), StopAction) + self.grasp_client.wait_for_server() + self.move_client.wait_for_server() + self.homing_client.wait_for_server() + self.stop_client.wait_for_server() + self.gripper_width = None + rospy.Subscriber(ns_join(self.gripper_ns, 'joint_states'), JointState, self.gripper_cb) + self.MIN_FORCE = 0.05 + self.MAX_FORCE = 50 # documentation says up to 70N is possible as continuous force + self.MIN_WIDTH = 0.0 + self.MAX_WIDTH = 0.08 + self.DEFAULT_EPSILON = 0.005 + self.DEFAULT_SPEED = 0.02 + self.DEFAULT_FORCE = 10 + + def gripper_cb(self, data): + self.gripper_width = data.position[0] + data.position[1] + + def grasp(self, width, speed=None, epsilon_outer=None, epsilon_inner=None, force=None, wait_for_result=False): + if width > self.gripper_width: + self.move(self.MAX_WIDTH, wait_for_result=True) + goal = GraspGoal() + goal.width = width + goal.epsilon.outer = self.DEFAULT_EPSILON if not epsilon_outer else epsilon_outer + goal.epsilon.inner = self.DEFAULT_EPSILON if not epsilon_inner else epsilon_inner + goal.speed = self.DEFAULT_SPEED if not speed else speed + goal.force = self.DEFAULT_FORCE if not force else force + self.grasp_client.send_goal(goal) + if wait_for_result: + result = self.grasp_client.wait_for_result(rospy.Duration(10)) + return result + return True + + def move(self, width, speed=None, wait_for_result=False): + goal = MoveGoal() + goal.width = width + goal.speed = self.DEFAULT_SPEED if not speed else speed + self.move_client.send_goal(goal) + if wait_for_result: + result = self.move_client.wait_for_result(rospy.Duration(10)) + return result + return True + + def homing(self, wait_for_result=True): + goal = HomingGoal() + self.homing_client.send_goal(goal) + if wait_for_result: + result = self.homing_client.wait_for_result(rospy.Duration(10)) + return result + return True - 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 + def stop(self, wait_for_result=False): + goal = StopGoal() + self.stop_client.send_goal(goal) + if wait_for_result: + result = self.stop_client.wait_for_result(rospy.Duration(10)) + return result + return True - # TODO: Add gripper helpers. + def open(self): + self.move(self.MAX_WIDTH) - # TODO: Add control mode setter/getter. + def close(self): + self.move(self.MIN_WIDTH) diff --git a/arm_robots/src/arm_robots/robot.py b/arm_robots/src/arm_robots/robot.py index 3f8c4b5..e83bd9f 100644 --- a/arm_robots/src/arm_robots/robot.py +++ b/arm_robots/src/arm_robots/robot.py @@ -1,8 +1,8 @@ #! /usr/bin/env python +import pdb from typing import List, Union, Tuple, Callable, Optional, Dict import numpy as np -import pyjacobian_follower from matplotlib import colors import moveit_commander @@ -55,17 +55,16 @@ def __init__(self, block: bool = True, raise_on_failure: bool = False, display_goals: bool = True, - force_trigger: float = 9.0, - jacobian_follower: Optional[pyjacobian_follower.JacobianFollower] = None): + force_trigger: float = 9.0): super().__init__(robot_namespace, robot_description) self._max_velocity_scale_factor = 0.1 + self._max_acceleration_scale_factor = 0.1 self.stored_tool_orientations = None self.raise_on_failure = raise_on_failure self.execute = execute self.block = block self.display_goals = display_goals self.force_trigger = force_trigger - self.jacobian_follower = jacobian_follower self.arms_controller_name = arms_controller_name @@ -73,17 +72,10 @@ def __init__(self, self.display_goal_position_pub = rospy.Publisher('goal_position', Marker, queue_size=10) self.display_robot_state_pubs = {} self.display_robot_traj_pubs = {} + self.scene = None self.arms_client = None - if jacobian_follower is None: - self.jacobian_follower = pyjacobian_follower.JacobianFollower(robot_namespace=self.robot_namespace, - robot_description=robot_description, - translation_step_size=0.005, - minimize_rotation=True, - collision_check=True, - visualize=True) - self.feedback_callbacks = [] self._move_groups = {} @@ -96,23 +88,25 @@ def connect(self, preload_move_groups=True): """ super().connect() - self.jacobian_follower.connect_to_psm() - # TODO: bad api? raii? this class isn't fully usable by the time it's constructor finishes, that's bad. self.arms_client = self.setup_joint_trajectory_controller_client(self.arms_controller_name) if preload_move_groups: for group_name in self.robot_commander.get_group_names(): self.get_move_group_commander(group_name) + self.scene = moveit_commander.PlanningSceneInterface() + def setup_joint_trajectory_controller_client(self, controller_name): - action_name = ns_join(self.robot_namespace, ns_join(controller_name, "follow_joint_trajectory")) - client = SimpleActionClient(action_name, FollowJointTrajectoryAction) - resolved_action_name = rospy.resolve_name(action_name) - wait_msg = f"Waiting for joint trajectory follower server {resolved_action_name}..." - rospy.loginfo(wait_msg) - client.wait_for_server() - rospy.loginfo(f"Connected.") - return client + if controller_name is not None: + action_name = ns_join(self.robot_namespace, ns_join(controller_name, "follow_joint_trajectory")) + client = SimpleActionClient(action_name, FollowJointTrajectoryAction) + resolved_action_name = rospy.resolve_name(action_name) + wait_msg = f"Waiting for joint trajectory follower server {resolved_action_name}..." + rospy.loginfo(wait_msg) + client.wait_for_server() + rospy.loginfo(f"Connected.") + return client + return None def set_execute(self, execute: bool): self.execute = execute @@ -133,6 +127,7 @@ def get_move_group_commander(self, group_name: str) -> moveit_commander.MoveGrou # faster than the kuka controller's max velocity, otherwise the kuka controllers will error out. safety_factor = 0.9 move_group.set_max_velocity_scaling_factor(self._max_velocity_scale_factor * safety_factor) + move_group.set_max_acceleration_scaling_factor(self._max_acceleration_scale_factor * safety_factor) return move_group def plan_to_position(self, @@ -148,7 +143,8 @@ def plan_to_position(self, if self.raise_on_failure and not planning_result.success: raise RobotPlanningError(f"Plan to position failed {planning_result.planning_error_code}") - execution_result = self.follow_arms_joint_trajectory(planning_result.plan.joint_trajectory, stop_condition) + execution_result = self.follow_arms_joint_trajectory(planning_result.plan.joint_trajectory, stop_condition, + group_name) return PlanningAndExecutionResult(planning_result, execution_result) def plan_to_position_cartesian(self, @@ -176,28 +172,31 @@ def plan_to_position_cartesian(self, if self.raise_on_failure and not planning_result.success: raise RobotPlanningError(f"Cartesian path is only {fraction * 100}% complete") - execution_result = self.follow_arms_joint_trajectory(plan.joint_trajectory, stop_condition) + execution_result = self.follow_arms_joint_trajectory(planning_result.plan.joint_trajectory, stop_condition, + group_name) return PlanningAndExecutionResult(planning_result, execution_result) def plan_to_pose(self, group_name, ee_link_name, target_pose, frame_id: str = 'robot_root', - stop_condition: Optional[Callable] = None): + stop_condition: Optional[Callable] = None, position_tol: float = 0.0005, + orientation_tol: float = 0.005, timeout: float = 5.0): self.check_inputs(group_name, ee_link_name) move_group = self.get_move_group_commander(group_name) + move_group.set_planning_time(timeout) move_group.set_end_effector_link(ee_link_name) target_pose_stamped = convert_to_pose_msg(target_pose) target_pose_stamped.header.frame_id = frame_id move_group.set_pose_target(target_pose_stamped) - move_group.set_goal_position_tolerance(0.002) - move_group.set_goal_orientation_tolerance(0.02) + move_group.set_goal_position_tolerance(position_tol) + move_group.set_goal_orientation_tolerance(orientation_tol) if self.display_goals: - self.display_goal_pose(target_pose_stamped.pose) + self.display_goal_pose(target_pose_stamped.pose, frame_id, ee_link_name) planning_result = PlanningResult(move_group.plan()) if self.raise_on_failure and not planning_result.success: raise RobotPlanningError(f"Plan to pose failed {planning_result.planning_error_code}") - - execution_result = self.follow_arms_joint_trajectory(planning_result.plan.joint_trajectory, stop_condition) + execution_result = self.follow_arms_joint_trajectory(planning_result.plan.joint_trajectory, stop_condition, + group_name) return PlanningAndExecutionResult(planning_result, execution_result) def get_link_pose(self, link_name: str): @@ -234,6 +233,8 @@ def plan_to_joint_config(self, group_name: str, joint_config: Union[List, Dict, joint_config = {name: val for name, val in zip(move_group.get_active_joints(), joint_config)} move_group.set_joint_value_target(joint_config) + # move_group.set_goal_position_tolerance(0.0005) + # move_group.set_goal_orientation_tolerance(0.005) if self.display_goals: robot_state = RobotState(joint_state=JointState(name=list(joint_config.keys()), @@ -243,7 +244,8 @@ def plan_to_joint_config(self, group_name: str, joint_config: Union[List, Dict, planning_result = PlanningResult(move_group.plan()) if self.raise_on_failure and not planning_result.success: raise RobotPlanningError(f"Plan to position failed {planning_result.planning_error_code}") - execution_result = self.follow_arms_joint_trajectory(planning_result.plan.joint_trajectory, stop_condition) + execution_result = self.follow_arms_joint_trajectory(planning_result.plan.joint_trajectory, stop_condition, + group_name) return PlanningAndExecutionResult(planning_result, execution_result) def make_follow_joint_trajectory_goal(self, trajectory) -> FollowJointTrajectoryGoal: @@ -275,6 +277,9 @@ def _feedback_cb(feedback: FollowJointTrajectoryFeedback): feedback_callback(client, goal, feedback) if stop_condition is not None and stop_condition(feedback): client.cancel_all_goals() + info_msg = "Stop condition met! Canceled all goals." + rospy.loginfo(info_msg) + # NOTE: this is where execution is actually requested in the form of a joint trajectory client.send_goal(goal, feedback_cb=_feedback_cb) @@ -297,7 +302,8 @@ def follow_joint_config(self, joint_names: List[str], joint_positions, client: S trajectory.points.append(point) return self.follow_joint_trajectory(trajectory, client) - def follow_arms_joint_trajectory(self, trajectory: JointTrajectory, stop_condition: Optional[Callable] = None): + def follow_arms_joint_trajectory(self, trajectory: JointTrajectory, stop_condition: Optional[Callable] = None, + group_name: Optional[str] = None): return self.follow_joint_trajectory(trajectory, self.arms_client, stop_condition=stop_condition) def distance(self, ee_link_name: str, target_position): @@ -305,126 +311,6 @@ def distance(self, ee_link_name: str, target_position): error = np.linalg.norm(ros_numpy.numpify(current_pose.position) - target_position) return error - def get_orientation(self, name: str): - link = self.robot_commander.get_link(name) - return ros_numpy.numpify(link.pose().pose.orientation) - - def store_tool_orientations(self, preferred_tool_orientations: Optional[Dict]): - """ dict values can be Pose, Quaternion, or just a numpy array/list """ - self.stored_tool_orientations = {} - for k, v in preferred_tool_orientations.items(): - if isinstance(v, Pose): - q = ros_numpy.numpify(v.orientation) - self.stored_tool_orientations[k] = q - elif isinstance(v, Quaternion): - q = ros_numpy.numpify(v) - self.stored_tool_orientations[k] = q - else: - self.stored_tool_orientations[k] = v - - def store_current_tool_orientations(self, tool_names: Optional[List]): - self.stored_tool_orientations = {n: self.get_orientation(n) for n in tool_names} - - def merge_tool_orientations_with_defaults(self, tool_names: List[str], - preferred_tool_orientations: Optional[List] = STORED_ORIENTATION): - # If preferred_tool_orientations is None, we use the stored ones as a fallback - if preferred_tool_orientations == STORED_ORIENTATION: - preferred_tool_orientations = [] - if self.stored_tool_orientations is None: - logfatal(store_error_msg) - for k in tool_names: - if k not in self.stored_tool_orientations: - rospy.logerr(f"tool {k} has no stored orientation. aborting.") - return [] - preferred_tool_orientations.append(self.stored_tool_orientations[k]) - return preferred_tool_orientations - - def follow_jacobian_to_position_from_scene_and_state(self, - scene_msg: PlanningScene, - joint_state: JointState, - group_name: str, - tool_names: List[str], - points: List[List], - preferred_tool_orientations: Optional = STORED_ORIENTATION, - vel_scaling=0.1, - stop_condition: Optional[Callable] = None, - ): - if isinstance(tool_names, str): - err_msg = "you need to pass in a list of strings, not a single string." - rospy.logerr(err_msg) - raise ValueError(err_msg) - - preferred_tool_orientations = self.merge_tool_orientations_with_defaults(tool_names, - preferred_tool_orientations) - if len(preferred_tool_orientations) == 0: - return ExecutionResult(trajectory=None, - execution_result=None, - action_client_state=self.arms_client.get_state(), - success=False) - - robot_trajectory_msg: RobotTrajectory - reached: bool - scene_msg, robot_state = merge_joint_state_and_scene_msg(scene_msg, joint_state) - robot_trajectory_msg, reached = self.jacobian_follower.plan( - group_name=group_name, - tool_names=tool_names, - preferred_tool_orientations=preferred_tool_orientations, - start_state=robot_state, - scene=scene_msg, - grippers=points, - max_velocity_scaling_factor=vel_scaling, - max_acceleration_scaling_factor=0.1, - ) - - planning_success = reached - planning_result = PlanningResult(success=planning_success, plan=robot_trajectory_msg) - if self.raise_on_failure and not planning_success: - raise RobotPlanningError(f"Tried to execute a jacobian action which could not be reached") - - execution_result = self.follow_arms_joint_trajectory(robot_trajectory_msg.joint_trajectory, - stop_condition=stop_condition) - return PlanningAndExecutionResult(planning_result, execution_result) - - def follow_jacobian_to_position(self, - group_name: str, - tool_names: List[str], - points: List[List], - preferred_tool_orientations: Optional[List] = STORED_ORIENTATION, - vel_scaling=0.1, - stop_condition: Optional[Callable] = None, - ): - if isinstance(tool_names, str): - err_msg = "you need to pass in a list of strings, not a single string." - rospy.logerr(err_msg) - raise ValueError(err_msg) - - preferred_tool_orientations = self.merge_tool_orientations_with_defaults(tool_names, - preferred_tool_orientations) - if len(preferred_tool_orientations) == 0: - return ExecutionResult(trajectory=None, - execution_result=None, - action_client_state=self.arms_client.get_state(), - success=False) - - robot_trajectory_msg: RobotTrajectory - reached: bool - robot_trajectory_msg, reached = self.jacobian_follower.plan( - group_name=group_name, - tool_names=tool_names, - preferred_tool_orientations=preferred_tool_orientations, - grippers=points, - max_velocity_scaling_factor=vel_scaling, - max_acceleration_scaling_factor=0.1, - ) - planning_success = reached - planning_result = PlanningResult(success=planning_success, plan=robot_trajectory_msg) - if self.raise_on_failure and not planning_success: - raise RobotPlanningError(f"Jacobian planning failed") - - execution_result = self.follow_arms_joint_trajectory(robot_trajectory_msg.joint_trajectory, - stop_condition=stop_condition) - return PlanningAndExecutionResult(planning_result, execution_result) - def get_joint_names(self, group_name: Optional[str] = None): # NOTE: Getting a list of joint names should not require anything besides a lookup on the ros parameter server. # However, MoveIt does not have this implemented that way, it requires connecting to the move group node. @@ -515,8 +401,8 @@ def display_goal_position(self, point: Point): m.pose.orientation.w = 1 self.display_goal_position_pub.publish(m) - def display_goal_pose(self, pose: Pose): - self.tf_wrapper.send_transform_from_pose_msg(pose, 'robot_root', 'arm_robots_goal') + def display_goal_pose(self, pose: Pose, root='robot_root', goal='arm_robots_goal'): + self.tf_wrapper.send_transform_from_pose_msg(pose, root, goal) def get_state(self, group_name: str = None): robot_state = RobotState()