@@ -2316,26 +2316,26 @@ TEST_F(Joints, IntegratePositionsStateIndependent)
23162316 constexpr double dt = 0.25 ;
23172317 constexpr double tol = 1e-12 ;
23182318
2319- auto verify = [&](Joint& joint,
2320- const Eigen::VectorXd& q0,
2321- const Eigen::VectorXd& v) {
2322- const Eigen::VectorXd positionsBefore = joint.getPositions ();
2323- const Eigen::VectorXd velocitiesBefore = joint.getVelocities ();
2319+ auto verify
2320+ = [&](Joint& joint, const Eigen::VectorXd& q0, const Eigen::VectorXd& v) {
2321+ const Eigen::VectorXd positionsBefore = joint.getPositions ();
2322+ const Eigen::VectorXd velocitiesBefore = joint.getVelocities ();
23242323
2325- const Eigen::VectorXd qNextStateless = joint.integratePositions (q0, v, dt);
2326- EXPECT_VECTOR_NEAR (positionsBefore, joint.getPositions (), 0.0 );
2327- EXPECT_VECTOR_NEAR (velocitiesBefore, joint.getVelocities (), 0.0 );
2324+ const Eigen::VectorXd qNextStateless
2325+ = joint.integratePositions (q0, v, dt);
2326+ EXPECT_VECTOR_NEAR (positionsBefore, joint.getPositions (), 0.0 );
2327+ EXPECT_VECTOR_NEAR (velocitiesBefore, joint.getVelocities (), 0.0 );
23282328
2329- joint.setPositions (q0);
2330- joint.setVelocities (v);
2331- joint.integratePositions (dt);
2332- const Eigen::VectorXd qNextStateful = joint.getPositions ();
2329+ joint.setPositions (q0);
2330+ joint.setVelocities (v);
2331+ joint.integratePositions (dt);
2332+ const Eigen::VectorXd qNextStateful = joint.getPositions ();
23332333
2334- EXPECT_VECTOR_NEAR (qNextStateful, qNextStateless, tol);
2334+ EXPECT_VECTOR_NEAR (qNextStateful, qNextStateless, tol);
23352335
2336- joint.setPositions (positionsBefore);
2337- joint.setVelocities (velocitiesBefore);
2338- };
2336+ joint.setPositions (positionsBefore);
2337+ joint.setVelocities (velocitiesBefore);
2338+ };
23392339
23402340 {
23412341 SkeletonPtr skel = Skeleton::create (" integrate_revolute" );
@@ -2359,8 +2359,7 @@ TEST_F(Joints, IntegratePositionsStateIndependent)
23592359 joint->setPositions (Eigen::Vector3d (0.2 , -0.1 , 0.3 ));
23602360 joint->setVelocities (Eigen::Vector3d (0.01 , 0.02 , -0.03 ));
23612361
2362- const Eigen::VectorXd q0
2363- = (Eigen::Vector3d (0.5 , -0.4 , 0.1 )).eval ();
2362+ const Eigen::VectorXd q0 = (Eigen::Vector3d (0.5 , -0.4 , 0.1 )).eval ();
23642363 const Eigen::VectorXd v = (Eigen::Vector3d (0.07 , -0.02 , 0.04 )).eval ();
23652364
23662365 verify (*joint, q0, v);
0 commit comments