Try to make the robot moving but "Unable to construct goal representation"

[emarca96]

This is the code i created to move the franka fr3 Robot. Launching moveit.launch.py the log message is the following, and the robot doesn't move:

#!/usr/bin/env python3
import rclpy
from rclpy.node import Node
from geometry_msgs.msg import PoseStamped
from moveit_msgs.action import MoveGroup
from moveit_msgs.msg import Constraints, PositionConstraint, OrientationConstraint
from shape_msgs.msg import SolidPrimitive
from rclpy.action import ActionClient
import tf_transformations


class MoveFR3(Node):
    def __init__(self):
        super().__init__('move_fr3_node')

        # Action client per MoveGroup
        self.action_client = ActionClient(self, MoveGroup, '/move_action')

    def send_goal(self):
        self.get_logger().info('Waiting for move_group action server...')
        self.action_client.wait_for_server()

        # Creazione del messaggio di goal
        goal_msg = MoveGroup.Goal()
        goal_msg.request.group_name = 'fr3_manipulator'
        goal_msg.request.allowed_planning_time = 5.0  # Timeout per il planner

        # Configurazione della posa target
        target_pose = PoseStamped()
        target_pose.header.frame_id = 'fr3_link0'  # Frame di riferimento
        target_pose.pose.position.x = 0.3
        target_pose.pose.position.y = -0.3
        target_pose.pose.position.z = 0.5

        # Calcola il quaternione per l'orientazione
        quaternion = tf_transformations.quaternion_from_euler(0.0, 0.0, 0.0)
        target_pose.pose.orientation.x = quaternion[0]
        target_pose.pose.orientation.y = quaternion[1]
        target_pose.pose.orientation.z = quaternion[2]
        target_pose.pose.orientation.w = quaternion[3]

        self.get_logger().info(f"Target Pose: {target_pose.pose}")

        # Vincoli di posizione
        position_constraint = PositionConstraint()
        position_constraint.header.frame_id = 'fr3_link0'  # Base del robot
        position_constraint.link_name = 'fr3_hand_tcp'  # Link dell'end-effector
        position_constraint.constraint_region.primitives.append(
            SolidPrimitive(type=SolidPrimitive.BOX, dimensions=[0.01, 0.01, 0.01])
        )
        position_constraint.constraint_region.primitive_poses.append(target_pose.pose)
        position_constraint.weight = 1.0

        # Vincoli di orientamento
        orientation_constraint = OrientationConstraint()
        orientation_constraint.header.frame_id = 'fr3_link0'
        orientation_constraint.link_name = 'fr3_hand_tcp'
        orientation_constraint.orientation = target_pose.pose.orientation
        orientation_constraint.absolute_x_axis_tolerance = 0.1
        orientation_constraint.absolute_y_axis_tolerance = 0.1
        orientation_constraint.absolute_z_axis_tolerance = 0.1
        orientation_constraint.weight = 1.0

        # Aggiunta dei vincoli al goal
        constraints = Constraints()
        constraints.position_constraints.append(position_constraint)
        constraints.orientation_constraints.append(orientation_constraint)

        goal_msg.request.goal_constraints.append(constraints)

        # Invio del goal
        self.get_logger().info('Sending goal to move_group...')
        send_goal_future = self.action_client.send_goal_async(goal_msg)
        send_goal_future.add_done_callback(self.goal_response_callback)

    def goal_response_callback(self, future):
        try:
            goal_handle = future.result()
            if not goal_handle.accepted:
                self.get_logger().error('Goal rejected by move_group.')
                return
            self.get_logger().info('Goal accepted by move_group. Waiting for result...')
            goal_handle.get_result_async().add_done_callback(self.result_callback)
        except Exception as e:
            self.get_logger().error(f"Goal response error: {str(e)}")

    def result_callback(self, future):
        try:
            result = future.result().result
            if result.error_code.val == 1:  # 1 indica SUCCESS
                self.get_logger().info('Goal reached successfully!')
            else:
                self.get_logger().error(f'MoveGroup failed with error code: {result.error_code.val}')
        except Exception as e:
            self.get_logger().error(f"Result callback error: {str(e)}")


def main(args=None):
    rclpy.init(args=args)
    node = MoveFR3()
    node.send_goal()

    while rclpy.ok():
        rclpy.spin_once(node)
        break

    node.get_logger().info("Shutting down node.")
    node.destroy_node()
    rclpy.shutdown()


if __name__ == '__main__':
    main()

Terminal log:

[move_group-3] [INFO] [1734462460.405319649] [moveit_move_group_default_capabilities.move_action_capability]: executing..
[move_group-3] [INFO] [1734462460.412628365] [moveit_move_group_default_capabilities.move_action_capability]: Combined planning and execution request received for MoveGroup action. Forwarding to planning and execution pipeline.
[move_group-3] [WARN] [1734462460.412677124] [moveit_move_group_capabilities_base.move_group_capability]: Execution of motions should always start at the robot's current state. Ignoring the state supplied as start state in the motion planning request
[move_group-3] [WARN] [1734462460.412693185] [moveit_move_group_capabilities_base.move_group_capability]: Execution of motions should always start at the robot's current state. Ignoring the state supplied as difference in the planning scene diff
[move_group-3] [INFO] [1734462460.412744857] [moveit_ros.plan_execution]: Planning attempt 1 of at most 1
[move_group-3] [ERROR] [1734462460.413129042] [moveit.ompl_planning.model_based_planning_context]: Unable to construct goal representation
[move_group-3] [INFO] [1734462460.413169724] [moveit_move_group_default_capabilities.move_action_capability]: Catastrophic failure

Any Help to perform the movement using the group "fr3_manipulator" ? Thanks a lot