diff --git a/franka_example_controllers/src/cartesian_pose_example_controller.cpp b/franka_example_controllers/src/cartesian_pose_example_controller.cpp index bd00e6ae7b09d7bd2953611b77bc0d0be8b4ae38..441119d4703d41786c9217ae220b0f7d348917db 100644 --- a/franka_example_controllers/src/cartesian_pose_example_controller.cpp +++ b/franka_example_controllers/src/cartesian_pose_example_controller.cpp @@ -47,7 +47,7 @@ bool CartesianPoseExampleController::init(hardware_interface::RobotHW* robot_har } void CartesianPoseExampleController::starting(const ros::Time& /*time*/) { - initial_pose_ = cartesian_pose_handle_->getRobotState().O_T_EE; + initial_pose_ = cartesian_pose_handle_->getRobotState().O_T_EE_d; } void CartesianPoseExampleController::update(const ros::Time& /*time*/, diff --git a/franka_example_controllers/src/joint_impedance_example_controller.cpp b/franka_example_controllers/src/joint_impedance_example_controller.cpp index 7e090daaa18a252e54551600154c3a6077c56504..dfc81a847d52be3f04e9d037f191e5435e66e035 100644 --- a/franka_example_controllers/src/joint_impedance_example_controller.cpp +++ b/franka_example_controllers/src/joint_impedance_example_controller.cpp @@ -140,7 +140,7 @@ bool JointImpedanceExampleController::init(hardware_interface::RobotHW* robot_hw } void JointImpedanceExampleController::starting(const ros::Time& /*time*/) { - initial_pose_ = cartesian_pose_handle_->getRobotState().O_T_EE; + initial_pose_ = cartesian_pose_handle_->getRobotState().O_T_EE_d; } void JointImpedanceExampleController::update(const ros::Time& /*time*/,