// TODO: Use link lengths in expectations explicitly
TEST(FORWARD_KINEMATICS, TWO_ROLLS) {
// Create the world
const double link1 = 1.5, link2 = 1.0;
SkeletonDynamics* robot = createTwoLinkRobot(Vector3d(0.3, 0.3, link1), DOF_ROLL, Vector3d(0.3, 0.3, link2),
DOF_ROLL);
// Set the test cases with the joint values and the expected end-effector positions
const size_t numTests = 2;
Vector2d joints [numTests] = {Vector2d(0.0, M_PI/2.0), Vector2d(3*M_PI/4.0, -M_PI/4.0)};
Vector3d expectedPos [numTests] = {Vector3d(0.0, -1.0, 1.5), Vector3d(0.0, -2.06, -1.06)};
// Check each case by setting the joint values and obtaining the end-effector position
for(size_t i = 0; i < numTests; i++) {
robot->setConfig(twoLinkIndices, joints[i]);
Vector3d actual = robot->getNode("ee")->getWorldTransform().topRightCorner(3,1);
bool equality = equals(actual, expectedPos[i], 1e-3);
EXPECT_TRUE(equality);
if(!equality) {
cout << "Joint values: " << joints[i].transpose() << endl;
cout << "Actual pos: " << actual.transpose() << endl;
cout << "Expected pos: " << expectedPos[i].transpose() << endl;
}
}
}
/* ******************************************************************************************** */
void computeOffset (double imu, double waist, const somatic_motor_t& lwa, const Vector6d& raw,
SkeletonDynamics& robot, Vector6d& offset, bool left) {
// Get the point transform wrench due to moving the affected position from com to sensor origin
// The transform is an identity with the bottom left a skew symmetric of the point translation
Matrix6d pTcom_sensor = MatrixXd::Identity(6,6);
pTcom_sensor.bottomLeftCorner<3,3>() << 0.0, -s2com(2), s2com(1), s2com(2), 0.0, -s2com(0),
-s2com(1), s2com(0), 0.0;
// Get the rotation between the world frame and the sensor frame.
robot.setConfig(imuWaist_ids, Vector2d(-imu + M_PI_2, waist));
robot.setConfig(left ? left_arm_ids : right_arm_ids, Map <Vector7d> (lwa.pos));
const char* nodeName = left ? "lGripper" : "rGripper";
Matrix3d R = robot.getNode(nodeName)->getWorldTransform().topLeftCorner<3,3>().transpose();
cout << "Transform : "<< endl << R << endl;
vector <int> dofs;
for(size_t i = 0; i < 25; i++) dofs.push_back(i);
cout << "\nq in computeExternal: " << robot.getConfig(dofs).transpose() << endl;
// Create the wrench with computed rotation to change the frame from the bracket to the sensor
Matrix6d pSsensor_bracket = MatrixXd::Identity(6,6);
pSsensor_bracket.topLeftCorner<3,3>() = R;
pSsensor_bracket.bottomRightCorner<3,3>() = R;
// Get the weight vector (note that we use the bracket frame for gravity so towards -y)
Vector6d weightVector_in_bracket;
weightVector_in_bracket << 0.0, 0.0, -eeMass * 9.81, 0.0, 0.0, 0.0;
// Compute what the force and torque should be without any external values by multiplying the
// position and rotation transforms with the expected effect of the gravity
Vector6d expectedFT = pTcom_sensor * pSsensor_bracket * weightVector_in_bracket;
pv(raw);
pv(expectedFT);
// Compute the difference between the actual and expected f/t values
offset = expectedFT - raw;
pv(offset);
}
