void urdf_traverser::scaleTranslation(LinkPtr& link, double scale_factor)
{
for (std::vector<VisualPtr >::iterator vit = link->visual_array.begin();
vit != link->visual_array.end(); ++vit)
{
VisualPtr visual = *vit;
EigenTransform vTrans = getTransform(visual->origin);
scaleTranslation(vTrans, scale_factor);
setTransform(vTrans, visual->origin);
}
for (std::vector<CollisionPtr >::iterator cit = link->collision_array.begin();
cit != link->collision_array.end(); ++cit)
{
CollisionPtr coll = *cit;
EigenTransform vTrans = getTransform(coll->origin);
scaleTranslation(vTrans, scale_factor);
setTransform(vTrans, coll->origin);
}
if (!link->inertial)
{
// ROS_WARN("Link %s has no inertial",link->name.c_str());
return;
}
EigenTransform vTrans = getTransform(link->inertial->origin);
scaleTranslation(vTrans, scale_factor);
setTransform(vTrans, link->inertial->origin);
}
static void test_invert(skiatest::Reporter* reporter) {
SkMatrix44 inverse(SkMatrix44::kUninitialized_Constructor);
double inverseData[16];
SkMatrix44 identity(SkMatrix44::kIdentity_Constructor);
identity.invert(&inverse);
inverse.asRowMajord(inverseData);
assert16<double>(reporter, inverseData,
1, 0, 0, 0,
0, 1, 0, 0,
0, 0, 1, 0,
0, 0, 0, 1);
SkMatrix44 translation(SkMatrix44::kUninitialized_Constructor);
translation.setTranslate(2, 3, 4);
translation.invert(&inverse);
inverse.asRowMajord(inverseData);
assert16<double>(reporter, inverseData,
1, 0, 0, -2,
0, 1, 0, -3,
0, 0, 1, -4,
0, 0, 0, 1);
SkMatrix44 scale(SkMatrix44::kUninitialized_Constructor);
scale.setScale(2, 4, 8);
scale.invert(&inverse);
inverse.asRowMajord(inverseData);
assert16<double>(reporter, inverseData,
0.5, 0, 0, 0,
0, 0.25, 0, 0,
0, 0, 0.125, 0,
0, 0, 0, 1);
SkMatrix44 scaleTranslation(SkMatrix44::kUninitialized_Constructor);
scaleTranslation.setScale(32, 128, 1024);
scaleTranslation.preTranslate(2, 3, 4);
scaleTranslation.invert(&inverse);
inverse.asRowMajord(inverseData);
assert16<double>(reporter, inverseData,
0.03125, 0, 0, -2,
0, 0.0078125, 0, -3,
0, 0, 0.0009765625, -4,
0, 0, 0, 1);
SkMatrix44 rotation(SkMatrix44::kUninitialized_Constructor);
rotation.setRotateDegreesAbout(0, 0, 1, 90);
rotation.invert(&inverse);
SkMatrix44 expected(SkMatrix44::kUninitialized_Constructor);
double expectedInverseRotation[16] =
{ 0, 1, 0, 0,
-1, 0, 0, 0,
0, 0, 1, 0,
0, 0, 0, 1
};
expected.setRowMajord(expectedInverseRotation);
REPORTER_ASSERT(reporter, nearly_equal(expected, inverse));
SkMatrix44 affine(SkMatrix44::kUninitialized_Constructor);
affine.setRotateDegreesAbout(0, 0, 1, 90);
affine.preScale(10, 20, 100);
affine.preTranslate(2, 3, 4);
affine.invert(&inverse);
double expectedInverseAffine[16] =
{ 0, 0.1, 0, -2,
-0.05, 0, 0, -3,
0, 0, 0.01, -4,
0, 0, 0, 1
};
expected.setRowMajord(expectedInverseAffine);
REPORTER_ASSERT(reporter, nearly_equal(expected, inverse));
SkMatrix44 perspective(SkMatrix44::kIdentity_Constructor);
perspective.setDouble(3, 2, 1.0);
perspective.invert(&inverse);
double expectedInversePerspective[16] =
{ 1, 0, 0, 0,
0, 1, 0, 0,
0, 0, 1, 0,
0, 0, -1, 1
};
expected.setRowMajord(expectedInversePerspective);
REPORTER_ASSERT(reporter, nearly_equal(expected, inverse));
SkMatrix44 affineAndPerspective(SkMatrix44::kIdentity_Constructor);
affineAndPerspective.setDouble(3, 2, 1.0);
affineAndPerspective.preScale(10, 20, 100);
affineAndPerspective.preTranslate(2, 3, 4);
affineAndPerspective.invert(&inverse);
double expectedInverseAffineAndPerspective[16] =
{ 0.1, 0, 2, -2,
0, 0.05, 3, -3,
0, 0, 4.01, -4,
0, 0, -1, 1
};
expected.setRowMajord(expectedInverseAffineAndPerspective);
REPORTER_ASSERT(reporter, nearly_equal(expected, inverse));
}
bool urdf_traverser::scaleTranslation(JointPtr& joint, double scale_factor)
{
EigenTransform vTrans = getTransform(joint);
scaleTranslation(vTrans, scale_factor);
setTransform(vTrans, joint);
}
