Skip to content

Commit

Permalink
Fixed function naming inconsistencies
Browse files Browse the repository at this point in the history
  • Loading branch information
TimSchneider42 committed May 27, 2024
1 parent 101fc6d commit 5052375
Show file tree
Hide file tree
Showing 4 changed files with 9 additions and 9 deletions.
10 changes: 5 additions & 5 deletions include/franky/robot_pose.hpp
Original file line number Diff line number Diff line change
Expand Up @@ -30,19 +30,19 @@ 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_};
}

[[nodiscard]] inline RobotPose changeEndEffectorFrame(const Affine &transform) const {
return {end_effector_pose_ * transform, elbow_position_};
}

[[nodiscard]] inline RobotPose with_elbow_position(const std::optional<double> elbow_position) const {
[[nodiscard]] inline RobotPose withElbowPosition(const std::optional<double> elbow_position) const {
return {end_effector_pose_, elbow_position};
}

Expand All @@ -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
2 changes: 1 addition & 1 deletion python/python.cpp
Original file line number Diff line number Diff line change
Expand Up @@ -509,7 +509,7 @@ PYBIND11_MODULE(_franky, m) {
"end_effector_pose"_a,
"elbow_position"_a = std::nullopt)
.def(py::init<const RobotPose &>()) // 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__",
Expand Down
4 changes: 2 additions & 2 deletions src/motion/cartesian_waypoint_motion.cpp
Original file line number Diff line number Diff line change
Expand Up @@ -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<double> new_elbow;
Expand All @@ -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;
}
Expand Down
2 changes: 1 addition & 1 deletion src/path/aggregated_path.cpp
Original file line number Diff line number Diff line change
Expand Up @@ -15,7 +15,7 @@ AggregatedPath<7> mk_path_from_waypoints(
std::vector<std::shared_ptr<LinearPath<7>>> 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<LinearPath<7>>(
Expand Down

0 comments on commit 5052375

Please sign in to comment.