// 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(); }