// 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 computeExternal (const Vector6d& input, SkeletonDynamics& robot, Vector6d& external,
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 by setting the arm values
// and the imu/waist values
const char* nodeName = left ? "lGripper" : "rGripper";
Matrix3d Rsw = robot.getNode(nodeName)->getWorldTransform().topLeftCorner<3,3>().transpose();
// vector <int> dofs;
// for(size_t i = 0; i < 25; i++) dofs.push_back(i);
// if(myDebug) cout << "\nq in computeExternal: " << robot.getConfig(dofs).transpose() << endl;
// Create the wrench with computed rotation to change the frame from the world to the sensor
Matrix6d pSsensor_world = MatrixXd::Identity(6,6);
pSsensor_world.topLeftCorner<3,3>() = Rsw;
pSsensor_world.bottomRightCorner<3,3>() = Rsw;
// Get the weight vector (note that we use the world frame for gravity so towards -y)
// static const double eeMass = 0.169; // kg - ft extension
Vector6d weightVector_in_world;
weightVector_in_world << 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 wrenchWeight = pTcom_sensor * pSsensor_world * weightVector_in_world;
// Remove the effect from the sensor value and convert the wrench into the world frame
external = input - wrenchWeight;
external = pSsensor_world.transpose() * external;
}
void ContactDynamics::initialize() {
// Allocate the Collision Detection class
//mCollisionChecker = new FCLCollisionDetector();
mCollisionChecker = new FCLMESHCollisionDetector();
mBodyIndexToSkelIndex.clear();
// Add all body nodes into mCollisionChecker
for (int i = 0; i < getNumSkels(); i++) {
SkeletonDynamics* skel = mSkels[i];
int nNodes = skel->getNumNodes();
for (int j = 0; j < nNodes; j++) {
mCollisionChecker->addCollisionSkeletonNode(skel->getNode(j));
mBodyIndexToSkelIndex.push_back(i);
}
}
mConstrForces.resize(getNumSkels());
for (int i = 0; i < getNumSkels(); i++){
if (!mSkels[i]->getImmobileState())
mConstrForces[i] = VectorXd::Zero(mSkels[i]->getNumDofs());
}
mIndices.clear();
int sumNDofs = 0;
mIndices.push_back(sumNDofs);
for (int i = 0; i < getNumSkels(); i++) {
if (!mSkels[i]->getImmobileState())
sumNDofs += mSkels[i]->getNumDofs();
mIndices.push_back(sumNDofs);
}
mTauStar = VectorXd::Zero(getNumTotalDofs());
}
void ContactDynamics::initialize() {
// Allocate the Collision Detection class
mCollisionChecker = new SkeletonCollision();
mBodyIndexToSkelIndex.clear();
// Add all body nodes into mCollisionChecker
int rows = 0;
int cols = 0;
for (int i = 0; i < getNumSkels(); i++) {
SkeletonDynamics* skel = mSkels[i];
int nNodes = skel->getNumNodes();
for (int j = 0; j < nNodes; j++) {
kinematics::BodyNode* node = skel->getNode(j);
if (node->getShape()->getShapeType() != kinematics::Shape::P_UNDEFINED)
{
mCollisionChecker->addCollisionSkeletonNode(node);
mBodyIndexToSkelIndex.push_back(i);
}
}
if (!mSkels[i]->getImmobileState()) {
// Immobile objets have mass of infinity
rows += skel->getMassMatrix().rows();
cols += skel->getMassMatrix().cols();
}
}
mConstrForces.resize(getNumSkels());
for (int i = 0; i < getNumSkels(); i++){
if (!mSkels[i]->getImmobileState())
mConstrForces[i] = VectorXd::Zero(mSkels[i]->getNumDofs());
}
mMInv = MatrixXd::Zero(rows, cols);
mTauStar = VectorXd::Zero(rows);
// Initialize the index vector:
// If we have 3 skeletons,
// mIndices[0] = 0
// mIndices[1] = nDof0
// mIndices[2] = nDof0 + nDof1
// mIndices[3] = nDof0 + nDof1 + nDof2
mIndices.clear();
int sumNDofs = 0;
mIndices.push_back(sumNDofs);
for (int i = 0; i < getNumSkels(); i++) {
SkeletonDynamics* skel = mSkels[i];
int nDofs = skel->getNumDofs();
if (mSkels[i]->getImmobileState())
nDofs = 0;
sumNDofs += nDofs;
mIndices.push_back(sumNDofs);
}
}
void ContactDynamics::initialize() {
// Allocate the Collision Detection class
//mCollisionChecker = new FCLCollisionDetector();
mCollisionChecker = new FCLMESHCollisionDetector();
mBodyIndexToSkelIndex.clear();
// Add all body nodes into mCollisionChecker
for (int i = 0; i < getNumSkels(); i++) {
SkeletonDynamics* skel = mSkels[i];
int nNodes = skel->getNumNodes();
for (int j = 0; j < nNodes; j++) {
kinematics::BodyNode* node = skel->getNode(j);
// If the collision shape of the node is NULL, then the node is
// uncollidable object. We don't care uncollidable object in
// ContactDynamics.
if (node->getCollisionShape() == NULL)
continue;
if (node->getCollisionShape()->getShapeType() != kinematics::Shape::P_UNDEFINED)
{
mCollisionChecker->addCollisionSkeletonNode(node);
mBodyIndexToSkelIndex.push_back(i);
}
}
}
mConstrForces.resize(getNumSkels());
for (int i = 0; i < getNumSkels(); i++){
if (!mSkels[i]->getImmobileState())
mConstrForces[i] = VectorXd::Zero(mSkels[i]->getNumDofs());
}
mIndices.clear();
int sumNDofs = 0;
mIndices.push_back(sumNDofs);
for (int i = 0; i < getNumSkels(); i++) {
if (!mSkels[i]->getImmobileState())
sumNDofs += mSkels[i]->getNumDofs();
mIndices.push_back(sumNDofs);
}
mTauStar = VectorXd::Zero(getNumTotalDofs());
}
// Compute the wrench on the wheel as an effect of the wrench acting on the sensor
void computeWheelWrench(const Vector6d& wrenchSensor, SkeletonDynamics& robot, Vector6d& wheelWrench, bool left) {
// Get the position vector of the sensor with respect to the wheels
const char* nodeName = left ? "lGripper" : "rGripper";
Vector3d Tws = robot.getNode(nodeName)->getWorldTransform().topRightCorner<3,1>();
if(0 && myDebug) cout << Tws.transpose() << endl;
// Get the wrench shift operator to move the wrench from sensor origin to the wheel axis
// TODO: This shifting is to the origin of the world frame in dart. It works now because it is not
// being updated by the amc encoders. We need to shift the wrench to a frame having origin at the
// wheel axis.
Tws(2) -= 0.264;
Matrix6d pTsensor_wheel = MatrixXd::Identity(6,6);
pTsensor_wheel.bottomLeftCorner<3,3>() << 0.0, -Tws(2), Tws(1), Tws(2), 0.0, -Tws(0),
-Tws(1), Tws(0), 0.0;
// Shift the wrench from the sensor origin to the wheel axis
wheelWrench = pTsensor_wheel * wrenchSensor;
}
/* ******************************************************************************************** */
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);
}
/* ********************************************************************************************* */
int main(int argc, char* argv[]) {
// Create Left Leg skeleton
SkeletonDynamics LeftLegSkel;
// Pointers to be used during the Skeleton building
Matrix3d inertiaMatrix;
inertiaMatrix << 0, 0, 0, 0, 0, 0, 0, 0, 0;
double mass = 1.0;
// ***** BodyNode 1: Left Hip Yaw (LHY) ***** *
BodyNodeDynamics* node = (BodyNodeDynamics*) LeftLegSkel.createBodyNode("LHY");
Joint* joint = new Joint(NULL, node, "LHY");
add_XyzRpy(joint, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0);
add_DOF(&LeftLegSkel, joint, 0.0, 0.0, M_PI, DOF_YAW);
Shape* shape = new ShapeBox(Vector3d(0.3, 0.3, 1.0));
node->setVisualizationShape(shape);
node->setCollisionShape(shape);
node->setMass(mass);
LeftLegSkel.addNode(node);
// ***** BodyNode 2: Left Hip Roll (LHR) whose parent is: LHY *****
BodyNodeDynamics* parent_node = (BodyNodeDynamics*) LeftLegSkel.getNode("LHY");
node = (BodyNodeDynamics*) LeftLegSkel.createBodyNode("LHR");
joint = new Joint(parent_node, node, "LHR");
add_XyzRpy(joint, 0.0, 0.0, 0.5, 0.0, 0.0, 0.0);
add_DOF(&LeftLegSkel, joint, 0.0, 0.0, M_PI, DOF_ROLL);
shape = new ShapeBox(Vector3d(0.3, 0.3, 1.0));
shape->setOffset(Vector3d(0.0, 0.0, 0.5));
node->setLocalCOM(shape->getOffset());
node->setMass(mass);
node->setVisualizationShape(shape);
node->setCollisionShape(shape);
LeftLegSkel.addNode(node);
// ***** BodyNode 3: Left Hip Pitch (LHP) whose parent is: LHR *****
parent_node = (BodyNodeDynamics*) LeftLegSkel.getNode("LHR");
node = (BodyNodeDynamics*) LeftLegSkel.createBodyNode("LHP");
joint = new Joint(parent_node, node, "LHP");
add_XyzRpy(joint, 0.0, 0.0, 1.0, 0.0, 0.0, 0.0);
add_DOF(&LeftLegSkel, joint, 0.0, 0.0, M_PI, DOF_ROLL);
shape = new ShapeBox(Vector3d(0.3, 0.3, 1.0));
shape->setOffset(Vector3d(0.0, 0.0, 0.5));
node->setLocalCOM(shape->getOffset());
node->setMass(mass);
Shape* shape1 = new ShapeEllipsoid(Vector3d(0.3, 0.3, 1.0));
shape1->setOffset(Vector3d(0.0, 0.0, 0.5));
node->setVisualizationShape(shape1);
node->setCollisionShape(shape);
LeftLegSkel.addNode(node);
// Initialize the skeleton
LeftLegSkel.initSkel();
// Window stuff
MyWindow window(&LeftLegSkel);
glutInit(&argc, argv);
window.initWindow(640, 480, "Skeleton example");
glutMainLoop();
return 0;
}
int main(int argc, char* argv[])
{
////////////////////////////////////////////////////////////////////////////
/// SETUP WORLD
////////////////////////////////////////////////////////////////////////////
// load a robot
DartLoader dart_loader;
string robot_file = VRC_DATA_PATH;
robot_file = argv[1];
World *mWorld = dart_loader.parseWorld(VRC_DATA_PATH ROBOT_URDF);
SkeletonDynamics *robot = mWorld->getSkeleton("atlas");
atlas_state_t state;
state.init(robot);
atlas_kinematics_t kin;
kin.init(robot);
robot->setPose(robot->getPose().setZero());
// load a skeleton file
FileInfoSkel<SkeletonDynamics> model;
model.loadFile(VRC_DATA_PATH"/models/skel/ground1.skel", SKEL);
SkeletonDynamics *ground = dynamic_cast<SkeletonDynamics *>(model.getSkel());
ground->setName("ground");
mWorld->addSkeleton(ground);
// Zero atlas's feet at ground
kinematics::BodyNode* foot = robot->getNode(ROBOT_LEFT_FOOT);
kinematics::Shape* shoe = foot->getCollisionShape();
double body_z = -shoe->getTransform().matrix()(2,3) + shoe->getDim()(2)/2;
body_z += -foot->getWorldTransform()(2,3);
VectorXd robot_dofs = robot->getPose();
cout << "body_z = " << body_z << endl;
state.dofs(2) = body_z;
robot->setPose(state.dart_pose());
cout << robot->getNode(ROBOT_BODY)->getWorldTransform() << endl;
// Zero ground
double ground_z = ground->getNode("ground1_root")->getCollisionShape()->getDim()(2)/2;
ground->getNode("ground1_root")->getVisualizationShape()->setColor(Vector3d(0.5,0.5,0.5));
VectorXd ground_dofs = ground->getPose();
ground_dofs(1) = foot->getWorldTransform()(1,3);
ground_dofs(2) = -ground_z;
cout << "ground z = " << ground_z << endl;
ground->setPose(ground_dofs);
////////////////////////////////////////////////////////////////////////////
/// COM IK
////////////////////////////////////////////////////////////////////////////
Vector3d world_com;
Isometry3d end_effectors[NUM_MANIPULATORS];
IK_Mode mode[NUM_MANIPULATORS];
BodyNode* left_foot = state.robot()->getNode(ROBOT_LEFT_FOOT);
BodyNode* right_foot = state.robot()->getNode(ROBOT_RIGHT_FOOT);
end_effectors[MANIP_L_FOOT] = state.robot()->getNode(ROBOT_LEFT_FOOT)->getWorldTransform();
end_effectors[MANIP_R_FOOT] = state.robot()->getNode(ROBOT_RIGHT_FOOT)->getWorldTransform();
mode[MANIP_L_FOOT] = IK_MODE_SUPPORT;
mode[MANIP_R_FOOT] = IK_MODE_WORLD;
mode[MANIP_L_HAND] = IK_MODE_FIXED;
mode[MANIP_R_HAND] = IK_MODE_FIXED;
world_com = state.robot()->getWorldCOM();
world_com(2) -= 0.1;
Isometry3d body;
state.get_body(body);
body.rotate(AngleAxisd(M_PI/4, Vector3d::UnitY()));
state.set_body(body);
kin.com_ik(world_com, end_effectors, mode, state);
robot->setPose(state.dart_pose());
MyWindow window;
window.setWorld(mWorld);
glutInit(&argc, argv);
window.initWindow(640, 480, "Robot Viz");
glutMainLoop();
}
