search

tenfoldpaper / multipanda_ros2

Apache License 2.0
12 stars 3 forks source link

Instructions on how to move the eef to a desired pose? #3

Closed Ekanshh closed 1 month ago

Ekanshh commented 1 month ago

Issue

Environment Setup:

Description

I am trying the dual_multimode_sim.launch.py. I also loaded the dual_cartesian_impedance_controller by modifying the example script. I am curious to know how I can send a desired goal command like reaching the sphere object. ?

image

Logs

ros2 launch franka_bringup dual_multimode_sim.launch.py 
[INFO] [launch]: Default logging verbosity is set to INFO
[INFO] [robot_state_publisher-1]: process started with pid [2669801]
[INFO] [joint_state_publisher-2]: process started with pid [2669803]
[INFO] [franka_control2_node-3]: process started with pid [2669805]
[INFO] [spawner-4]: process started with pid [2669807]
[INFO] [spawner-5]: process started with pid [2669809]
[INFO] [spawner-6]: process started with pid [2669811]
[INFO] [spawner-7]: process started with pid [2669813]
[INFO] [spawner-8]: process started with pid [2669815]
[INFO] [spawner-9]: process started with pid [2669817]
[INFO] [panda_poly_c2_joint_motion_generator_node-10]: process started with pid [2669819]
[INFO] [panda_poly_c2_joint_motion_generator_node-11]: process started with pid [2669821]
[robot_state_publisher-1] [INFO] [1719652001.142701349] [robot_state_publisher]: got segment base_link
[robot_state_publisher-1] [INFO] [1719652001.142772046] [robot_state_publisher]: got segment mj_left_hand
[robot_state_publisher-1] [INFO] [1719652001.142777273] [robot_state_publisher]: got segment mj_left_hand_tcp
[robot_state_publisher-1] [INFO] [1719652001.142780432] [robot_state_publisher]: got segment mj_left_leftfinger
[robot_state_publisher-1] [INFO] [1719652001.142783161] [robot_state_publisher]: got segment mj_left_link0
[robot_state_publisher-1] [INFO] [1719652001.142785728] [robot_state_publisher]: got segment mj_left_link1
[robot_state_publisher-1] [INFO] [1719652001.142788004] [robot_state_publisher]: got segment mj_left_link2
[robot_state_publisher-1] [INFO] [1719652001.142790478] [robot_state_publisher]: got segment mj_left_link3
[robot_state_publisher-1] [INFO] [1719652001.142792993] [robot_state_publisher]: got segment mj_left_link4
[robot_state_publisher-1] [INFO] [1719652001.142795317] [robot_state_publisher]: got segment mj_left_link5
[robot_state_publisher-1] [INFO] [1719652001.142797731] [robot_state_publisher]: got segment mj_left_link6
[robot_state_publisher-1] [INFO] [1719652001.142800098] [robot_state_publisher]: got segment mj_left_link7
[robot_state_publisher-1] [INFO] [1719652001.142802410] [robot_state_publisher]: got segment mj_left_link8
[robot_state_publisher-1] [INFO] [1719652001.142804684] [robot_state_publisher]: got segment mj_left_rightfinger
[robot_state_publisher-1] [INFO] [1719652001.142807158] [robot_state_publisher]: got segment mj_right_hand
[robot_state_publisher-1] [INFO] [1719652001.142809455] [robot_state_publisher]: got segment mj_right_hand_tcp
[robot_state_publisher-1] [INFO] [1719652001.142811900] [robot_state_publisher]: got segment mj_right_leftfinger
[robot_state_publisher-1] [INFO] [1719652001.142814215] [robot_state_publisher]: got segment mj_right_link0
[robot_state_publisher-1] [INFO] [1719652001.142816696] [robot_state_publisher]: got segment mj_right_link1
[robot_state_publisher-1] [INFO] [1719652001.142819013] [robot_state_publisher]: got segment mj_right_link2
[robot_state_publisher-1] [INFO] [1719652001.142821387] [robot_state_publisher]: got segment mj_right_link3
[robot_state_publisher-1] [INFO] [1719652001.142823667] [robot_state_publisher]: got segment mj_right_link4
[robot_state_publisher-1] [INFO] [1719652001.142826000] [robot_state_publisher]: got segment mj_right_link5
[robot_state_publisher-1] [INFO] [1719652001.142828290] [robot_state_publisher]: got segment mj_right_link6
[robot_state_publisher-1] [INFO] [1719652001.142830551] [robot_state_publisher]: got segment mj_right_link7
[robot_state_publisher-1] [INFO] [1719652001.142832820] [robot_state_publisher]: got segment mj_right_link8
[robot_state_publisher-1] [INFO] [1719652001.142835096] [robot_state_publisher]: got segment mj_right_rightfinger
[franka_control2_node-3] [WARN] [1719652001.146816801] [controller_manager]: [Deprecated] Passing the robot description parameter directly to the control_manager node is deprecated. Use '~/robot_description' topic from 'robot_state_publisher' instead.
[franka_control2_node-3] [INFO] [1719652001.147114528] [resource_manager]: Loading hardware 'FrankaMujocoMultiHardwareInterface' 
[franka_control2_node-3] [INFO] [1719652001.152265034] [resource_manager]: Initialize hardware 'FrankaMujocoMultiHardwareInterface' 
[franka_control2_node-3] [INFO] [1719652001.152317528] [FrankaMujocoMultiHardwareInterface]: Initial checks all passed
[joint_state_publisher-2] [INFO] [1719652001.601561321] [joint_state_publisher]: Waiting for robot_description to be published on the robot_description topic...
[panda_poly_c2_joint_motion_generator_node-10] [INFO] [1719652001.659202863] [mj_left_joint_via_motion_node]: service not available, waiting again...
[panda_poly_c2_joint_motion_generator_node-11] [INFO] [1719652001.671240306] [mj_right_joint_via_motion_node]: service not available, waiting again...
[franka_control2_node-3] [INFO] [1719652001.872677089] [mj_left_gripper_sim_node]: Started gripper sim
[franka_control2_node-3] [INFO] [1719652001.898836006] [mj_right_gripper_sim_node]: Started gripper sim
[franka_control2_node-3] Successfully populated obj_box_01
[franka_control2_node-3] Successfully populated sphere_01
[franka_control2_node-3] Successfully populated mj_left_link0
[franka_control2_node-3] Successfully populated mj_right_link0
[franka_control2_node-3] [INFO] [1719652001.923117859] [mujoco_pose_service_server]: mujoco_pose_service_server started
[franka_control2_node-3] [INFO] [1719652001.924009240] [resource_manager]: Successful initialization of hardware 'FrankaMujocoMultiHardwareInterface'
[franka_control2_node-3] [INFO] [1719652001.924365119] [resource_manager]: 'configure' hardware 'FrankaMujocoMultiHardwareInterface' 
[franka_control2_node-3] [INFO] [1719652001.924396636] [resource_manager]: Successful 'configure' of hardware 'FrankaMujocoMultiHardwareInterface'
[franka_control2_node-3] [INFO] [1719652001.924448227] [resource_manager]: 'activate' hardware 'FrankaMujocoMultiHardwareInterface' 
[franka_control2_node-3] [INFO] [1719652001.924464824] [FrankaMujocoMultiHardwareInterface]: Started
[franka_control2_node-3] [INFO] [1719652001.924482377] [resource_manager]: Successful 'activate' of hardware 'FrankaMujocoMultiHardwareInterface'
[franka_control2_node-3] [INFO] [1719652001.929321193] [controller_manager]: franka_control2_node
[franka_control2_node-3] [INFO] [1719652001.929359248] [controller_manager]: update rate is 1000 Hz
[franka_control2_node-3] [INFO] [1719652001.929454456] [controller_manager]: RT kernel is recommended for better performance
[franka_control2_node-3] [INFO] [1719652001.941690900] [controller_manager]: Loading controller 'mj_left_model_broadcaster'
[spawner-8] [INFO] [1719652001.947113676] [spawner_mj_left_model_broadcaster]: Loaded mj_left_model_broadcaster
[franka_control2_node-3] [INFO] [1719652001.947809822] [controller_manager]: Configuring controller 'mj_left_model_broadcaster'
[franka_control2_node-3] [INFO] [1719652001.947895928] [franka_model_semantic_component]: Initialized FrankaRobotModel with params mj_left, mj_left/robot_model
[franka_control2_node-3] [INFO] [1719652001.948473306] [mj_left_model_broadcaster]: mj_left franka model broadcaster configuration successful
[franka_control2_node-3] [INFO] [1719652001.950739482] [mj_left_model_broadcaster]: on_activate: mj_left/robot_model
[franka_control2_node-3] [INFO] [1719652001.950758565] [mj_left_model_broadcaster]: on_activate: mj_left/robot_state
[spawner-8] [INFO] [1719652001.952337683] [spawner_mj_left_model_broadcaster]: Configured and activated mj_left_model_broadcaster
[franka_control2_node-3] [INFO] [1719652001.996066110] [controller_manager]: Loading controller 'mj_left_state_broadcaster'
[spawner-6] [INFO] [1719652002.001217950] [spawner_mj_left_state_broadcaster]: Loaded mj_left_state_broadcaster
[franka_control2_node-3] [INFO] [1719652002.001788487] [controller_manager]: Configuring controller 'mj_left_state_broadcaster'
[franka_control2_node-3] [INFO] [1719652002.002550688] [mj_left_state_broadcaster]: mj_left franka state broadcaster configuration successful
[spawner-6] [INFO] [1719652002.007463301] [spawner_mj_left_state_broadcaster]: Configured and activated mj_left_state_broadcaster
[franka_control2_node-3] [INFO] [1719652002.025563352] [controller_manager]: Loading controller 'mj_right_state_broadcaster'
[spawner-7] [INFO] [1719652002.031256181] [spawner_mj_right_state_broadcaster]: Loaded mj_right_state_broadcaster
[franka_control2_node-3] [INFO] [1719652002.031890477] [controller_manager]: Configuring controller 'mj_right_state_broadcaster'
[franka_control2_node-3] [INFO] [1719652002.032223298] [mj_right_state_broadcaster]: mj_right franka state broadcaster configuration successful
[spawner-7] [INFO] [1719652002.036167293] [spawner_mj_right_state_broadcaster]: Configured and activated mj_right_state_broadcaster
[franka_control2_node-3] [INFO] [1719652002.063124175] [controller_manager]: Loading controller 'sim_multi_mode_controller'
[franka_control2_node-3] [INFO] [1719652002.077412814] [sim_multi_mode_controller]: Finished initializing multi joint impedance example controller for 2 arms
[spawner-5] [INFO] [1719652002.079070603] [spawner_sim_multi_mode_controller]: Loaded sim_multi_mode_controller
[panda_poly_c2_joint_motion_generator_node-10] [INFO] [1719652002.079086219] [mj_left_joint_via_motion_node]: Initialization of MGRosInterface done with name mj_left_joint_via_motion
[panda_poly_c2_joint_motion_generator_node-11] [INFO] [1719652002.080595668] [mj_right_joint_via_motion_node]: Initialization of MGRosInterface done with name mj_right_joint_via_motion
[franka_control2_node-3] [INFO] [1719652002.080838261] [controller_manager]: Loading controller 'joint_state_broadcaster'
[INFO] [spawner-8]: process has finished cleanly [pid 2669815]
[franka_control2_node-3] [INFO] [1719652002.089031654] [controller_manager]: Configuring controller 'sim_multi_mode_controller'
[franka_control2_node-3] [INFO] [1719652002.089133880] [franka_model_semantic_component]: Initialized FrankaRobotModel with params mj_left, mj_left/robot_model
[franka_control2_node-3] [INFO] [1719652002.089147097] [sim_multi_mode_controller]: Arm set for mj_left
[franka_control2_node-3] [INFO] [1719652002.089164877] [franka_model_semantic_component]: Initialized FrankaRobotModel with params mj_right, mj_right/robot_model
[franka_control2_node-3] [INFO] [1719652002.089169060] [sim_multi_mode_controller]: Arm set for mj_right
[franka_control2_node-3] [INFO] [1719652002.089191551] [sim_multi_mode_controller]: MultiPandaMultiModeController: Creating controller panda_joint_impedance_controller for resource mj_left.
[franka_control2_node-3] [INFO] [1719652002.089235986] [sim_multi_mode_controller]: MultiPandaMultiModeController: Creating controller panda_joint_impedance_controller for resource mj_right.
[franka_control2_node-3] [INFO] [1719652002.089273610] [sim_multi_mode_controller]: MultiPandaMultiModeController: Creating controller panda_joint_velocity_controller for resource mj_left.
[franka_control2_node-3] [INFO] [1719652002.089280944] [sim_multi_mode_controller]: MultiPandaMultiModeController: Creating controller panda_joint_velocity_controller for resource mj_right.
[franka_control2_node-3] [INFO] [1719652002.089287654] [sim_multi_mode_controller]: MultiPandaMultiModeController: Creating controller comless_coupled_dual_cartesian_impedance_controller for resource mj_left&mj_right.
[franka_control2_node-3] [INFO] [1719652002.089310787] [sim_multi_mode_controller]: MultiPandaMultiModeController: Creating controller dual_cartesian_impedance_controller for resource mj_left&mj_right.
[franka_control2_node-3] [INFO] [1719652002.089320808] [sim_multi_mode_controller]: MultiPandaMultiModeController: Creating controller des_coupled_dual_cartesian_impedance_controller for resource mj_left&mj_right.
[spawner-4] [INFO] [1719652002.089428729] [spawner_joint_state_broadcaster]: Loaded joint_state_broadcaster
[franka_control2_node-3] [INFO] [1719652002.091791189] [sim_multi_mode_controller]: MultiPandaMultiModeController: Initializing controller comless_coupled_dual_cartesian_impedance_controller for resource mj_left&mj_right.
[franka_control2_node-3] [INFO] [1719652002.091858790] [sim_multi_mode_controller]: MultiPandaMultiModeController: Initializing controller des_coupled_dual_cartesian_impedance_controller for resource mj_left&mj_right.
[franka_control2_node-3] [INFO] [1719652002.091904704] [sim_multi_mode_controller]: Params set
[franka_control2_node-3] [INFO] [1719652002.091921627] [sim_multi_mode_controller]: MultiPandaMultiModeController: Initializing controller dual_cartesian_impedance_controller for resource mj_left&mj_right.
[franka_control2_node-3] [INFO] [1719652002.091953372] [sim_multi_mode_controller]: MultiPandaMultiModeController: Initializing controller panda_joint_impedance_controller for resource mj_left.
[franka_control2_node-3] [INFO] [1719652002.091985024] [sim_multi_mode_controller]: MultiPandaMultiModeController: Initializing controller panda_joint_impedance_controller for resource mj_right.
[franka_control2_node-3] [INFO] [1719652002.092015876] [sim_multi_mode_controller]: MultiPandaMultiModeController: Initializing controller panda_joint_velocity_controller for resource mj_left.
[franka_control2_node-3] [INFO] [1719652002.092052436] [sim_multi_mode_controller]: MultiPandaMultiModeController: Initializing controller panda_joint_velocity_controller for resource mj_right.
[franka_control2_node-3] [INFO] [1719652002.092089022] [sim_multi_mode_controller]: MultiPandaMultiModeController: New active control set:
[franka_control2_node-3] dual_cartesian_impedance_controller: mj_left&mj_right
[franka_control2_node-3] [INFO] [1719652002.094008063] [controller_manager]: Configuring controller 'joint_state_broadcaster'
[franka_control2_node-3] [INFO] [1719652002.094073408] [joint_state_broadcaster]: 'joints' or 'interfaces' parameter is empty. All available state interfaces will be published
[franka_control2_node-3] [INFO] [1719652002.095724633] [FrankaMujocoMultiHardwareInterface]: Preparing command mode switch
[franka_control2_node-3] [INFO] [1719652002.096642749] [FrankaMujocoMultiHardwareInterface]: Performing command mode switch
[franka_control2_node-3] [INFO] [1719652002.096667286] [FrankaMujocoMultiHardwareInterface]: Arm mj_left current mode: joint_torque
[franka_control2_node-3] [INFO] [1719652002.096673134] [FrankaMujocoMultiHardwareInterface]: Arm mj_right current mode: joint_torque
[franka_control2_node-3] mj_left/robot_state
[franka_control2_node-3] mj_left/robot_model
[franka_control2_node-3] mj_right/robot_state
[franka_control2_node-3] mj_right/robot_model
[spawner-5] [INFO] [1719652002.098636491] [spawner_sim_multi_mode_controller]: Configured and activated sim_multi_mode_controller
[spawner-4] [INFO] [1719652002.099594537] [spawner_joint_state_broadcaster]: Configured and activated joint_state_broadcaster
[franka_control2_node-3] [INFO] [1719652002.102366088] [controller_manager]: Loading controller 'mj_right_model_broadcaster'
[spawner-9] [INFO] [1719652002.109300269] [spawner_mj_right_model_broadcaster]: Loaded mj_right_model_broadcaster
[franka_control2_node-3] [INFO] [1719652002.110502117] [controller_manager]: Configuring controller 'mj_right_model_broadcaster'
[franka_control2_node-3] [INFO] [1719652002.110551072] [franka_model_semantic_component]: Initialized FrankaRobotModel with params mj_right, mj_right/robot_model
[franka_control2_node-3] [INFO] [1719652002.111249775] [mj_right_model_broadcaster]: mj_right franka model broadcaster configuration successful
[franka_control2_node-3] [INFO] [1719652002.112998734] [mj_right_model_broadcaster]: on_activate: mj_right/robot_model
[franka_control2_node-3] [INFO] [1719652002.113025406] [mj_right_model_broadcaster]: on_activate: mj_right/robot_state
[spawner-9] [INFO] [1719652002.114359127] [spawner_mj_right_model_broadcaster]: Configured and activated mj_right_model_broadcaster
[INFO] [spawner-6]: process has finished cleanly [pid 2669811]
[INFO] [spawner-7]: process has finished cleanly [pid 2669813]
[INFO] [spawner-5]: process has finished cleanly [pid 2669809]
[INFO] [spawner-4]: process has finished cleanly [pid 2669807]
[INFO] [spawner-9]: process has finished cleanly [pid 2669817]
[franka_control2_node-3] [ERROR] [1719652167.554653618] [sim_multi_mode_controller]: MultiPandaMultiModeController: Requested Controller dual_cartesian_impedance_controller is not available for resource left&right.
[franka_control2_node-3] [ERROR] [1719652327.525474331] [sim_multi_mode_controller]: MultiPandaMultiModeController: Requested Controller dual_cartesian_impedance_controller is not available for resource mj_left.
[franka_control2_node-3] [INFO] [1719652492.053131282] [sim_multi_mode_controller]: MultiPandaMultiModeController: New active control set:
[franka_control2_node-3] dual_cartesian_impedance_controller: mj_left&mj_right
[franka_control2_node-3] [INFO] [1719652572.324118608] [sim_multi_mode_controller]: MultiPandaMultiModeController: New active control set:
[franka_control2_node-3] dual_cartesian_impedance_controller: mj_left&mj_right
[franka_control2_node-3] [INFO] [1719653572.231157262] [sim_multi_mode_controller]: MultiPandaMultiModeController: New active control set:
[franka_control2_node-3] dual_cartesian_impedance_controller: mj_left&mj_right
Ekanshh commented 1 month ago

Quick update! I am able to interface the simulation with moveit such that I can control the panda arm "move group" using plan and execute function. But, I am unable to perform plan and execute on the gripper. It creates a plan, but fails to execute:

image

Ekanshh commented 1 month ago

Closing this. Found a fix referred in #4.