diff --git a/include/franky/robot_pose.hpp b/include/franky/robot_pose.hpp index b4d97362..c01fb482 100644 --- a/include/franky/robot_pose.hpp +++ b/include/franky/robot_pose.hpp @@ -30,11 +30,11 @@ class RobotPose { [[nodiscard]] franka::CartesianPose as_franka_pose() const; - [[nodiscard]] inline RobotPose left_transform(const Affine &transform) const { + [[nodiscard]] inline RobotPose leftTransform(const Affine &transform) const { return {transform * end_effector_pose_, elbow_position_}; } - [[nodiscard]] inline RobotPose right_transform(const Affine &transform) const { + [[nodiscard]] inline RobotPose rightTransform(const Affine &transform) const { return {end_effector_pose_ * transform, elbow_position_}; } @@ -42,7 +42,7 @@ class RobotPose { return {end_effector_pose_ * transform, elbow_position_}; } - [[nodiscard]] inline RobotPose with_elbow_position(const std::optional elbow_position) const { + [[nodiscard]] inline RobotPose withElbowPosition(const std::optional elbow_position) const { return {end_effector_pose_, elbow_position}; } @@ -60,11 +60,11 @@ class RobotPose { }; inline RobotPose operator*(const RobotPose &robot_pose, const Affine &right_transform) { - return robot_pose.right_transform(right_transform); + return robot_pose.rightTransform(right_transform); } inline RobotPose operator*(const Affine &left_transform, const RobotPose &robot_pose) { - return robot_pose.left_transform(left_transform); + return robot_pose.leftTransform(left_transform); } } // namespace franky diff --git a/python/python.cpp b/python/python.cpp index 485acb00..8037c39f 100644 --- a/python/python.cpp +++ b/python/python.cpp @@ -509,7 +509,7 @@ PYBIND11_MODULE(_franky, m) { "end_effector_pose"_a, "elbow_position"_a = std::nullopt) .def(py::init()) // Copy constructor - .def("with_elbow_position", &RobotPose::with_elbow_position, "elbow_position"_a) + .def("with_elbow_position", &RobotPose::withElbowPosition, "elbow_position"_a) .def_property_readonly("end_effector_pose", &RobotPose::end_effector_pose) .def_property_readonly("elbow_position", &RobotPose::elbow_position) .def("__mul__", diff --git a/src/motion/cartesian_waypoint_motion.cpp b/src/motion/cartesian_waypoint_motion.cpp index 59a6adfd..87d3b7c3 100644 --- a/src/motion/cartesian_waypoint_motion.cpp +++ b/src/motion/cartesian_waypoint_motion.cpp @@ -84,7 +84,7 @@ void CartesianWaypointMotion::setNewWaypoint( auto prev_target_robot_pose = target_state_.pose(); if (!waypoint_pose.elbow_position().has_value()) { - prev_target_robot_pose = prev_target_robot_pose.with_elbow_position(robot_state.elbow[0]); + prev_target_robot_pose = prev_target_robot_pose.withElbowPosition(robot_state.elbow[0]); } std::optional new_elbow; @@ -97,7 +97,7 @@ void CartesianWaypointMotion::setNewWaypoint( } else { new_elbow = waypoint_pose.elbow_position(); } - CartesianState new_target(waypoint_pose.with_elbow_position(new_elbow), new_waypoint.target.velocity()); + CartesianState new_target(waypoint_pose.withElbowPosition(new_elbow), new_waypoint.target.velocity()); if (new_waypoint.reference_type == ReferenceType::Relative) { new_target = prev_target_robot_pose.end_effector_pose() * new_target; } diff --git a/src/path/aggregated_path.cpp b/src/path/aggregated_path.cpp index a4a6b02e..fab6b061 100644 --- a/src/path/aggregated_path.cpp +++ b/src/path/aggregated_path.cpp @@ -15,7 +15,7 @@ AggregatedPath<7> mk_path_from_waypoints( std::vector>> line_segments; RobotPose prev_robot_pose = waypoints[0].robot_pose; if (!prev_robot_pose.elbow_position().has_value()) - prev_robot_pose = prev_robot_pose.with_elbow_position(default_initial_elbow_pos); + prev_robot_pose = prev_robot_pose.withElbowPosition(default_initial_elbow_pos); for (size_t i = 1; i < waypoints.size(); i += 1) { auto segment = std::make_shared>(