Skip to content

Commit 6a30720

Browse files
ReobosReobos
committed
motion header fix
1 parent 6b5cdbd commit 6a30720

2 files changed

Lines changed: 92 additions & 5 deletions

File tree

‎fsb-core/include/fsb_motion.h‎

Lines changed: 16 additions & 5 deletions
Original file line numberDiff line numberDiff line change
@@ -98,6 +98,17 @@ Vec3 vector_subtract(const Vec3& v_a, const Vec3& v_b);
9898
*/
9999
Vec3 vector_scale(real_t scalar, const Vec3& vec);
100100

101+
/**
102+
* @brief Element-wise vector multiplication
103+
*
104+
* \f$ \mathbf{v}_a \cdot \mathbf{v}_b \f$
105+
*
106+
* @param v_a Vector a
107+
* @param v_b Vector b
108+
* @return Element-wise multiplication of a and b
109+
*/
110+
Vec3 vector_multiply_elem(const Vec3& v_a, const Vec3& v_b);
111+
101112
/**
102113
* @brief Vector cross product
103114
*
@@ -196,21 +207,21 @@ Vec3 coord_transform_position(const Transform& transf, const Vec3& pos);
196207
/**
197208
* @brief Apply motion vector offset to transform in space frame
198209
*
210+
* @param transf_initial Initial transform
199211
* @param offset Offset motion vector in space frame
200-
* @param transf Coordinate transform applied after motion offset
201212
* @return Final transform including offset applied in space frame
202213
*/
203-
Transform coord_transform_apply_space_offset(const MotionVector& offset, const Transform& transf);
214+
Transform coord_transform_apply_error(const Transform& transf_initial, const MotionVector& offset);
204215

205216
/**
206217
* @brief Get motion vector offset between two coordinate transforms in space frame
207218
*
208-
* @param transf_post Coordinate transform applied after motion offset
209-
* @param transf Final transform including offset applied in space frame
219+
* @param transf_initial Coordinate transform applied after motion offset
220+
* @param transf_final Final transform including offset applied in space frame
210221
* @return Offset motion vector in space frame
211222
*/
212223
MotionVector
213-
coord_transform_get_space_offset(const Transform& transf_post, const Transform& transf);
224+
coord_transform_get_error(const Transform& transf_initial, const Transform& transf_final);
214225

215226
/**
216227
* @brief Apply motion vector offset to transform in mobile frame

‎fsb-core/test/fsb_motion_test.cpp‎

Lines changed: 76 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -475,6 +475,82 @@ TEST_CASE("Get body offset" * doctest::description("[fsb_motion][fsb::coord_tran
475475
REQUIRE(tr_b_actual.translation.y == FsbApprox(tr_b.translation.y));
476476
REQUIRE(tr_b_actual.translation.z == FsbApprox(tr_b.translation.z));
477477
}
478+
//
479+
// TEST_CASE("Apply space offset" * doctest::description("[fsb_motion][fsb::coord_transform_apply_space_offset]"))
480+
// {
481+
// // Inputs
482+
// const fsb::MotionVector offset = {
483+
// {0.075, -0.12, 0.56},
484+
// {-0.982, 1.234, -0.9987}
485+
// };
486+
// const fsb::Transform tr_a = {
487+
// {0.311126983722081, 0.0565685424949238, -0.848528137423857, 0.424264068711929},
488+
// {0.1, -0.22, 3.12}
489+
// };
490+
// // Expected
491+
// const fsb::Transform tr_expected = {
492+
// {0.12879363305026925, 0.27492701003833253, -0.831866064238503, 0.46457097456194846},
493+
// {-1.070731152097746, 0.7774957237582711, 2.0958970058184354}
494+
// };
495+
// // Process
496+
// const fsb::Transform tr_actual = fsb::coord_transform_apply_space_offset(tr_a, offset);
497+
// const fsb::MotionVector offset_actual = fsb::coord_transform_get_space_offset(tr_a, tr_actual);
498+
//
499+
// REQUIRE(fsb::quat_norm(tr_actual.rotation) == FsbApprox(1.0));
500+
// REQUIRE(tr_actual.rotation.qw == FsbApprox(tr_expected.rotation.qw));
501+
// REQUIRE(tr_actual.rotation.qx == FsbApprox(tr_expected.rotation.qx));
502+
// REQUIRE(tr_actual.rotation.qy == FsbApprox(tr_expected.rotation.qy));
503+
// REQUIRE(tr_actual.rotation.qz == FsbApprox(tr_expected.rotation.qz));
504+
//
505+
// REQUIRE(tr_actual.translation.x == FsbApprox(tr_expected.translation.x));
506+
// REQUIRE(tr_actual.translation.y == FsbApprox(tr_expected.translation.y));
507+
// REQUIRE(tr_actual.translation.z == FsbApprox(tr_expected.translation.z));
508+
//
509+
// REQUIRE(offset_actual.angular.x == FsbApprox(offset.angular.x));
510+
// REQUIRE(offset_actual.angular.y == FsbApprox(offset.angular.y));
511+
// REQUIRE(offset_actual.angular.z == FsbApprox(offset.angular.z));
512+
// REQUIRE(offset_actual.linear.x == FsbApprox(offset.linear.x));
513+
// REQUIRE(offset_actual.linear.y == FsbApprox(offset.linear.y));
514+
// REQUIRE(offset_actual.linear.z == FsbApprox(offset.linear.z));
515+
// }
516+
//
517+
// TEST_CASE("Get space offset" * doctest::description("[fsb_motion][fsb::coord_transform_apply_space_offset]"))
518+
// {
519+
// // Inputs
520+
// const fsb::Transform tr_a = {
521+
// {0.311126983722081, 0.0565685424949238, -0.848528137423857, 0.424264068711929},
522+
// {0.1, -0.22, 3.12}
523+
// };
524+
// const fsb::Transform tr_b = {
525+
// {0.12879363305026925, 0.27492701003833253, -0.831866064238503, 0.46457097456194846},
526+
// {-1.070731152097746, 0.7774957237582711, 2.0958970058184354}
527+
// };
528+
// // Expected
529+
// const fsb::MotionVector offset_expected = {
530+
// {0.075, -0.12, 0.56},
531+
// {-0.982, 1.234, -0.9987}
532+
// };
533+
// // Process
534+
// const fsb::MotionVector offset_actual = fsb::coord_transform_get_space_offset(tr_a, tr_b);
535+
// const fsb::Transform tr_b_actual = fsb::coord_transform_apply_space_offset(tr_a, offset_actual);
536+
//
537+
// REQUIRE(offset_actual.angular.x == FsbApprox(offset_expected.angular.x));
538+
// REQUIRE(offset_actual.angular.y == FsbApprox(offset_expected.angular.y));
539+
// REQUIRE(offset_actual.angular.z == FsbApprox(offset_expected.angular.z));
540+
// REQUIRE(offset_actual.linear.x == FsbApprox(offset_expected.linear.x));
541+
// REQUIRE(offset_actual.linear.y == FsbApprox(offset_expected.linear.y));
542+
// REQUIRE(offset_actual.linear.z == FsbApprox(offset_expected.linear.z));
543+
//
544+
// REQUIRE(fsb::quat_norm(tr_b_actual.rotation) == FsbApprox(1.0));
545+
// REQUIRE(tr_b_actual.rotation.qw == FsbApprox(tr_b.rotation.qw));
546+
// REQUIRE(tr_b_actual.rotation.qx == FsbApprox(tr_b.rotation.qx));
547+
// REQUIRE(tr_b_actual.rotation.qy == FsbApprox(tr_b.rotation.qy));
548+
// REQUIRE(tr_b_actual.rotation.qz == FsbApprox(tr_b.rotation.qz));
549+
//
550+
// REQUIRE(tr_b_actual.translation.x == FsbApprox(tr_b.translation.x));
551+
// REQUIRE(tr_b_actual.translation.y == FsbApprox(tr_b.translation.y));
552+
// REQUIRE(tr_b_actual.translation.z == FsbApprox(tr_b.translation.z));
553+
// }
478554

479555
// TEST_CASE("Motion body-fixed transform velocity" * doctest::description("[fsb_motion][fsb::motion_transform_body_velocity]"))
480556
// {

0 commit comments

Comments
 (0)