feat: add flange in sim

Author: juelg

src/sim/SimRobot.cpp

@@ -117,16 +117,16 @@ SimRobotState* SimRobot::get_state() {
 }
 
 common::Pose SimRobot::get_cartesian_position() {
+  return this->get_cartesian_flange_position() * cfg.tcp_offset;
+}
+
+common::Pose SimRobot::get_cartesian_flange_position() {
   Eigen::Matrix<double, 3, 3, Eigen::RowMajor> rotation(
       this->sim->d->site_xmat + 9 * this->ids.attachment_site);
   Eigen::Vector3d translation(this->sim->d->site_xpos +
                               3 * this->ids.attachment_site);
   common::Pose attachment_site(Eigen::Matrix3d(rotation), translation);
-  return this->to_pose_in_robot_coordinates(attachment_site) * cfg.tcp_offset;
-}
-
-common::Pose SimRobot::get_cartesian_flange_position() {
-  return this->get_cartesian_position();
+  return this->to_pose_in_robot_coordinates(attachment_site);
 }
 
 void SimRobot::set_joint_position(const common::VectorXd& q) {