Commit 9c1f0dcf authored by pre-commit-ci[bot]'s avatar pre-commit-ci[bot]
Browse files

[pre-commit.ci] auto fixes from pre-commit.com hooks

for more information, see https://pre-commit.ci
parent 3abd7b3a
Pipeline #19157 failed with stage
in 57 seconds
......@@ -451,71 +451,69 @@ BOOST_AUTO_TEST_CASE(equality) {
joint_b2
*/
DevicePtr_t createRobot ()
{
std::string urdf ("<robot name='test'>"
DevicePtr_t createRobot() {
std::string urdf(
"<robot name='test'>"
"<link name='base_link'/>"
"<link name='link_test_x'/>"
"<joint name='test_x' type='prismatic'>"
"<parent link='base_link'/>"
"<child link='link_test_x'/>"
"<limit effort='30' velocity='1.0' lower='-4' upper='4'/>"
"<parent link='base_link'/>"
"<child link='link_test_x'/>"
"<limit effort='30' velocity='1.0' lower='-4' upper='4'/>"
"</joint>"
"<link name='link_a0'/>"
"<link name='link_a1'/>"
"<joint name='joint_a0' type='prismatic'>"
"<parent link='link_test_x'/>"
"<child link='link_a0'/>"
"<limit effort='30' velocity='1.0' lower='-4' upper='4'/>"
"<parent link='link_test_x'/>"
"<child link='link_a0'/>"
"<limit effort='30' velocity='1.0' lower='-4' upper='4'/>"
"</joint>"
"<joint name='joint_a1' type='prismatic'>"
"<parent link='link_a0'/>"
"<child link='link_a1'/>"
"<limit effort='30' velocity='1.0' lower='-4' upper='4'/>"
"<parent link='link_a0'/>"
"<child link='link_a1'/>"
"<limit effort='30' velocity='1.0' lower='-4' upper='4'/>"
"</joint>"
"<link name='link_b0'/>"
"<link name='link_b1'/>"
"<link name='link_b2'/>"
"<joint name='joint_b0' type='prismatic'>"
"<parent link='link_test_x'/>"
"<child link='link_b0'/>"
"<limit effort='30' velocity='1.0' lower='-4' upper='4'/>"
"<parent link='link_test_x'/>"
"<child link='link_b0'/>"
"<limit effort='30' velocity='1.0' lower='-4' upper='4'/>"
"</joint>"
"<joint name='joint_b1' type='prismatic'>"
"<parent link='link_b0'/>"
"<child link='link_b1'/>"
"<limit effort='30' velocity='1.0' lower='-4' upper='4'/>"
"<parent link='link_b0'/>"
"<child link='link_b1'/>"
"<limit effort='30' velocity='1.0' lower='-4' upper='4'/>"
"</joint>"
"<joint name='joint_b2' type='floating'>"
"<parent link='link_b1'/>"
"<child link='link_b2'/>"
"<parent link='link_b1'/>"
"<child link='link_b2'/>"
"</joint>"
"</robot>"
);
"</robot>");
DevicePtr_t robot = Device::create ("test");
loadModelFromString (robot, 0, "", "anchor", urdf, "");
DevicePtr_t robot = Device::create("test");
loadModelFromString(robot, 0, "", "anchor", urdf, "");
return robot;
}
BOOST_AUTO_TEST_CASE (dependsOnRelPoseBetween) {
BOOST_AUTO_TEST_CASE(dependsOnRelPoseBetween) {
DevicePtr_t device = createRobot();
BOOST_REQUIRE (device);
BOOST_REQUIRE(device);
JointPtr_t ee1 = device->getJointByName ("joint_a1"),
ee2 = device->getJointByName ("joint_b2");
BOOST_REQUIRE (device);
JointPtr_t ee1 = device->getJointByName("joint_a1"),
ee2 = device->getJointByName("joint_b2");
BOOST_REQUIRE(device);
// ensure that the joint indices are as expected
BOOST_REQUIRE (ee1->index() < ee2->index());
BOOST_REQUIRE(ee1->index() < ee2->index());
device->currentConfiguration (device->neutralConfiguration());
device->computeForwardKinematics ();
Transform3f tf1 (ee1->currentTransformation ());
Transform3f tf2 (ee2->currentTransformation ());
device->currentConfiguration(device->neutralConfiguration());
device->computeForwardKinematics();
Transform3f tf1(ee1->currentTransformation());
Transform3f tf2(ee2->currentTransformation());
DifferentiableFunctionPtr_t function;
std::pair<JointConstPtr_t, JointConstPtr_t> joints;
......@@ -641,14 +639,13 @@ BOOST_AUTO_TEST_CASE (dependsOnRelPoseBetween) {
BOOST_CHECK_EQUAL(jointsConstrained.second->index(), ee1->index());
/// test the locked joint constraint as well
constraint = LockedJoint::create
(ee2, ee2->configurationSpace ()->neutral ());
constraint = LockedJoint::create(ee2, ee2->configurationSpace()->neutral());
std::cout << constraint->functionPtr()->name() << std::endl;
joints = constraint->functionPtr()->dependsOnRelPoseBetween (device);
BOOST_CHECK_EQUAL (joints.first->index(), ee2->parentJoint()->index());
BOOST_CHECK_EQUAL (joints.second->index(), ee2->index());
joints = constraint->functionPtr()->dependsOnRelPoseBetween(device);
BOOST_CHECK_EQUAL(joints.first->index(), ee2->parentJoint()->index());
BOOST_CHECK_EQUAL(joints.second->index(), ee2->index());
// constraint fully locks the joint
jointsConstrained = constraint->doesConstrainRelPoseBetween(device);
BOOST_CHECK_EQUAL (jointsConstrained.second->index(), ee2->index());
BOOST_CHECK_EQUAL(jointsConstrained.second->index(), ee2->index());
}
Supports Markdown
0% or .
You are about to add 0 people to the discussion. Proceed with caution.
Finish editing this message first!
Please register or to comment