Skip to content

Can you give sample launch file and config file for fr3 robot with moveit. #3

@rikhsitlladeveloper

Description

@rikhsitlladeveloper

Hello @mrceki .

I am still having trouble.
I changed robot version to 5.6 and libfranka 0.13.3 and franka_ros2 v0.1.15.

I have fr3 robot without gripper.
When I launch it with your given config file I had issue with "Exceptions: libfranka: Move command aborted: motion aborted by reflex! ["communication_constraints_violation"]"
error then from launch file I make false to hand.

Then I got such long errors

[moveit_ros.current_state_monitor]: State monitor received invalid joint state (number of joint names does not match number of positions)
[robot_state_publisher-1] [ERROR] [1752211477.861645140] [robot_state_publisher]: Robot state publisher ignored an invalid JointState message
[robot_state_publisher-1] [ERROR] [1752211477.868217244] [robot_state_publisher]: Robot state publisher ignored an invalid JointState message

...

But in ros control list

ros2 control list_controllers
franka_robot_state_broadcaster franka_robot_state_broadcaster/FrankaRobotStateBroadcaster   active
joint_state_broadcaster        joint_state_broadcaster/JointStateBroadcaster                active
cartesian_impedance_controller cartesian_impedance_controller/CartesianImpedanceController  active

can you suggest any solution .
here is config file:

controller_manager:
  ros__parameters:
    update_rate: 950  # Hz

    fr3_arm_controller:
      type: joint_trajectory_controller/JointTrajectoryController

    joint_state_broadcaster:
      type: joint_state_broadcaster/JointStateBroadcaster

    franka_robot_state_broadcaster:
      type: franka_robot_state_broadcaster/FrankaRobotStateBroadcaster
    
    cartesian_impedance_controller:
      type: cartesian_impedance_controller/CartesianImpedanceController
 
franka_robot_state_broadcaster:
  ros__parameters:
    arm_id: fr3

fr3_arm_controller:
  ros__parameters:
    command_interfaces:
      - effort
    state_interfaces:
      - position
      - velocity
    joints:
      - fr3_joint1
      - fr3_joint2
      - fr3_joint3
      - fr3_joint4
      - fr3_joint5
      - fr3_joint6
      - fr3_joint7
    gains:
      fr3_joint1: { p: 600., d: 30., i: 0., i_clamp: 1. }
      fr3_joint2: { p: 600., d: 30., i: 0., i_clamp: 1. }
      fr3_joint3: { p: 600., d: 30., i: 0., i_clamp: 1. }
      fr3_joint4: { p: 600., d: 30., i: 0., i_clamp: 1. }
      fr3_joint5: { p: 250., d: 10., i: 0., i_clamp: 1. }
      fr3_joint6: { p: 150., d: 10., i: 0., i_clamp: 1. }
      fr3_joint7: { p: 50., d: 5., i: 0., i_clamp: 1. }

cartesian_impedance_controller:
  ros__parameters:
    joints:
      - fr3_joint1
      - fr3_joint2
      - fr3_joint3
      - fr3_joint4
      - fr3_joint5
      - fr3_joint6
      - fr3_joint7
    end_effector: fr3_link7
    update_frequency: 500.0
    handle_trajectories: true
    robot_description: "robot_description"
    wrench_ee_frame: fr3_link7
    delta_tau_max: 1.0
  
    damping:
      translation:
        x: 1.0
        y: 1.0
        z: 1.0
      rotation:
        x: 1.0
        y: 1.0
        z: 1.0
      nullspace_damping: 1.0
      update_damping_factors: false
  
    stiffness:
      translation:
        x: 200.0
        y: 200.0
        z: 200.0
      rotation:
        x: 20.0
        y: 20.0
        z: 20.0
      nullspace_stiffness: 0.0
      update_stiffness: false
  
    wrench:
      apply_wrench: false
      force_x: 0.0
      force_y: 0.0
      force_z: 0.0
      torque_x: 0.0
      torque_y: 0.0
      torque_z: 0.0
  
    filtering:
      nullspace_config: 0.1
      pose: 0.1
      stiffness: 0.1
      wrench: 0.1
  
    verbosity:
      verbose_print: false
      state_msgs: true
      tf_frames: false

Metadata

Metadata

Assignees

No one assigned

    Labels

    No labels
    No labels

    Projects

    No projects

    Milestone

    No milestone

    Relationships

    None yet

    Development

    No branches or pull requests

    Issue actions