int main(int argc, char* argv[])
{
// load a skeleton file
FileInfoSkel<SkeletonDynamics> model;
model.loadFile(DART_DATA_PATH"/skel/Chain.skel", SKEL);
// create and initialize the world
World *myWorld = new World();
Vector3d gravity(0.0, -9.81, 0.0);
myWorld->setGravity(gravity);
myWorld->setTimeStep(1.0/2000);
myWorld->addSkeleton((SkeletonDynamics*)model.getSkel());
int nDof = myWorld->getSkeleton(0)->getNumDofs();
VectorXd initPose(nDof);
for (int i = 0; i < nDof; i++)
initPose[i] = random(-0.5, 0.5);
myWorld->getSkeleton(0)->setPose(initPose);
// create a window and link it to the world
MyWindow window;
window.setWorld(myWorld);
glutInit(&argc, argv);
window.initWindow(640, 480, "Forward Simulation");
glutMainLoop();
return 0;
}
int main(int argc, char* argv[])
{
FileInfoSkel<SkeletonDynamics> model;
model.loadFile(DART_DATA_PATH"/skel/mobilManipulator.skel", SKEL);
MyWindow window((SkeletonDynamics*)model.getSkel(), NULL);
glutInit(&argc, argv);
window.initWindow(640, 480, "Hybrid");
glutMainLoop();
return 0;
}
int main(int argc, char* argv[])
{
FileInfoSkel<SkeletonDynamics> model;
model.loadFile(RTQL8_DATA_PATH"/skel/Chain.skel", SKEL);
MyWindow window((SkeletonDynamics*)model.getSkel());
glutInit(&argc, argv);
window.initWindow(640, 480, "Forward Simulation");
glutMainLoop();
return 0;
}
/* ********************************************************************************************* */
TEST(KINEMATICS, VSK_LOADER) {
using namespace Eigen;
using namespace kinematics;
FileInfoSkel<Skeleton> modelFile;
modelFile.loadFile(DART_DATA_PATH"skel/Yuting.vsk");
Skeleton* skel = modelFile.getSkel();
EXPECT_TRUE(skel != NULL);
EXPECT_EQ(skel->getNumDofs(), 66);
EXPECT_EQ(skel->getNumNodes(), 30);
EXPECT_EQ(skel->getNumMarkers(), 53);
}
//############################################################
//### PTHREAD START ROUTINE
//############################################################
void* teleop_gui_t::start_routine(void *args)
{
teleop_gui_t *window = ((teleop_gui_t *) args);
// 1. Load a local copy of Atlas
DartLoader dart_loader;
simulation::World *mWorld = dart_loader.parseWorld(DRC_DATA_PATH ROBOT_URDF);
robotSkel = mWorld->getSkeleton(ROBOT_NAME);
VectorXd dofs = robotSkel->getPose();
robotSkel->setPose(dofs);
// 2. Load the ground
FileInfoSkel<SkeletonDynamics> model;
model.loadFile(DRC_DATA_PATH"/models/skel/ground1.skel", SKEL);
ground = dynamic_cast<SkeletonDynamics *>(model.getSkel());
ground->setName("ground");
mWorld->addSkeleton(ground);
dofs = ground->getPose();
ground->setPose(dofs);
// 3. Zero atlas's feet on ground
kinematics::BodyNode* left_foot = robotSkel->getNode(ROBOT_LEFT_FOOT);
kinematics::Shape* left_shoe = left_foot->getCollisionShape();
// height from ankle to base of shoe
foot2ground = left_shoe->getDim()(2)/2 - left_shoe->getTransform().matrix()(2,3);
// height from ground center to surface
double ground_z = ground->getNode("ground1_root")->getCollisionShape()->getDim()(2)/2;
// invert to get height below left ankle
foot2ground *= -1;
// set into ground
dofs(2) = -ground_z;
ground->setPose(dofs);
// 4. Initialize GL stuff
QUAD_OBJ_INIT;
// 4. Finish init and
window->setWorld(mWorld);
// fake argc, argv
char *argv[1];
int argc = 1;
argv[0] = "robot-viz";
// gl stuff
glutInit(&argc, argv);
window->initWindow(640, 480, "Robot Viz");
glutMainLoop();
return args;
}
///////////////////////////////////////////////////////////////////////////////
// MAIN
///////////////////////////////////////////////////////////////////////////////
int main( int argc, char** argv ) {
// Command line inputs
bool show_gui = false;
bool use_ros = false;
walktype walk_type = walk_canned;
double walk_circle_radius = 5.0;
double walk_dist = 20;
double footsep_y = 0.085; // half of horizontal separation distance between feet
double foot_liftoff_z = 0.05; // foot liftoff height
double step_length = 0.15;
bool walk_sideways = false;
double com_height = 0.48; // height of COM above ANKLE
double com_ik_ascl = 0;
double zmpoff_y = 0; // lateral displacement between zmp and ankle
double zmpoff_x = 0;
double lookahead_time = 2.5;
double startup_time = 1.0;
double shutdown_time = 1.0;
double double_support_time = 0.05;
double single_support_time = 0.70;
size_t max_step_count = 4;
double zmp_jerk_penalty = 1e-8; // jerk penalty on ZMP controller
ik_error_sensitivity ik_sense = ik_strict;
// Parse command line inputs
const struct option long_options[] = {
{ "show-gui", no_argument, 0, 'g' },
{ "use-ros", no_argument, 0, 'R' },
{ "ik-errors", required_argument, 0, 'I' },
{ "walk-type", required_argument, 0, 'w' },
{ "walk-distance", required_argument, 0, 'D' },
{ "walk-circle-radius", required_argument, 0, 'r' },
{ "max-step-count", required_argument, 0, 'c' },
{ "foot-separation-y", required_argument, 0, 'y' },
{ "foot-liftoff-z", required_argument, 0, 'z' },
{ "step-length", required_argument, 0, 'l' },
{ "walk-sideways", no_argument, 0, 'S' },
{ "com-height", required_argument, 0, 'h' },
{ "comik-angle-weight", required_argument, 0, 'a' },
{ "zmp-offset-y", required_argument, 0, 'Y' },
{ "zmp-offset-x", required_argument, 0, 'X' },
{ "lookahead-time", required_argument, 0, 'T' },
{ "startup-time", required_argument, 0, 'p' },
{ "shutdown-time", required_argument, 0, 'n' },
{ "double-support-time", required_argument, 0, 'd' },
{ "single-support-time", required_argument, 0, 's' },
{ "zmp-jerk-penalty", required_argument, 0, 'P' },
{ "help", no_argument, 0, 'H' },
{ 0, 0, 0, 0 },
};
const char* short_options = "gRI:w:D:r:c:y:z:l:Sh:a:Y:X:T:p:n:d:s:P:H";
int opt, option_index;
while ( (opt = getopt_long(argc, argv, short_options, long_options, &option_index)) != -1 ) {
switch (opt) {
case 'g': show_gui = true; break;
case 'R': use_ros = true; break;
case 'I': ik_sense = getiksense(optarg); break;
case 'w': walk_type = getwalktype(optarg); break;
case 'D': walk_dist = getdouble(optarg); break;
case 'r': walk_circle_radius = getdouble(optarg); break;
case 'c': max_step_count = getlong(optarg); break;
case 'y': footsep_y = getdouble(optarg); break;
case 'z': foot_liftoff_z = getdouble(optarg); break;
case 'l': step_length = getdouble(optarg); break;
case 'S': walk_sideways = true; break;
case 'h': com_height = getdouble(optarg); break;
case 'a': com_ik_ascl = getdouble(optarg); break;
case 'Y': zmpoff_y = getdouble(optarg); break;
case 'X': zmpoff_x = getdouble(optarg); break;
case 'T': lookahead_time = getdouble(optarg); break;
case 'p': startup_time = getdouble(optarg); break;
case 'n': shutdown_time = getdouble(optarg); break;
case 'd': double_support_time = getdouble(optarg); break;
case 's': single_support_time = getdouble(optarg); break;
case 'P': zmp_jerk_penalty = getdouble(optarg); break;
case 'H': usage(std::cout); exit(0); break;
default: usage(std::cerr); exit(1); break;
}
}
/* Initialize ROS */
double frequency = 200;
//FIXME: ROS dependent
if(use_ros) {
ros::init( argc, argv, "publish_and_readonce" );
rosnode = new ros::NodeHandle();
loop_rate = new ros::Rate(frequency);
ros::Time last_ros_time_;
// Wait until sim is active (play)
bool wait = true;
while( wait ) {
last_ros_time_ = ros::Time::now();
if( last_ros_time_.toSec() > 0 ) {
wait = false;
}
}
// init ros joints
RosJointInit();
// ros topic subscribtions
ros::SubscribeOptions jointStatesSo =
ros::SubscribeOptions::create<sensor_msgs::JointState>(
"/atlas/joint_states", 1, GetJointStates,
ros::VoidPtr(), rosnode->getCallbackQueue());
jointStatesSo.transport_hints = ros::TransportHints().unreliable();
ros::Subscriber subJointStates = rosnode->subscribe(jointStatesSo);
pub_joint_commands_ =
rosnode->advertise<osrf_msgs::JointCommands>(
"/atlas/joint_commands", 1, true);
}
/* Initialize AK */
if(!_ak) {
DartLoader dart_loader;
World *mWorld = dart_loader.parseWorld(ATLAS_DATA_PATH "atlas/atlas_world.urdf");
_atlas = mWorld->getSkeleton("atlas");
_ak = new AtlasKinematics();
_ak->init(_atlas);
}
_atlas->setPose(_atlas->getPose().setZero(), true);
AtlasKinematics *AK = _ak;
/* Begin generating trajectories */
/* Trajectory that stores dof ticks */
vector<VectorXd> joint_traj;
/* Setup dofs initial conditions */
/* Relax */
VectorXd dofs = _atlas->getPose().setZero();
_atlas->setPose(dofs, true);
const int relax_ticks = 1000;
Relax(AK, _atlas, dofs, joint_traj, relax_ticks);
/* Walking variables */
IK_Mode mode[NUM_MANIPULATORS];
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;
Vector3d comDelta = Vector3d::Zero();
Vector3d leftDelta = Vector3d::Zero();
Vector3d rightDelta = Vector3d::Zero();
int N = 0;
Matrix4d Twm[NUM_MANIPULATORS];
Twm[MANIP_L_FOOT] = AK->getLimbTransB(_atlas, MANIP_L_FOOT);
Twm[MANIP_R_FOOT] = AK->getLimbTransB(_atlas, MANIP_R_FOOT);
Matrix4d Twb;
Twb.setIdentity();
VectorXd dofs_save;
/* Move COM down */
comDelta << 0, 0, -0.04;
leftDelta.setZero();
rightDelta.setZero();
const int com_ticks = 1000;
genCOMIKTraj(AK, _atlas, Twb, Twm, dofs, comDelta, leftDelta, rightDelta, joint_traj, com_ticks);
/* ZMP Walking */
/* ZMP parameters */
// number of steps to walk
int numSteps = 12;
// lenght of a half step
double stepLength = 0.15;
// half foot seperation
dofs_save = _atlas->getPose();
cout << "********************************************" << endl;
cout << "Start ZMP walking" << endl;
cout << "*************************************" << endl;
cout << "POS ERROR: " << (dofs_save - dofs).norm() << endl;
cout << "*************************************" << endl;
_atlas->setPose(dofs);
double footSeparation = (AK->getLimbTransW(_atlas, Twb, MANIP_L_FOOT)(1,3) -
AK->getLimbTransW(_atlas, Twb, MANIP_R_FOOT)(1,3) ) / 2;
cout << "Half foot seperation: " << footSeparation << endl;
// one step time
double stepDuration = 1.0;
// move ZMP time
double slopeTime = 0.15;
// keep ZMP time
double levelTime = 0.85;
// command sending period
double dt = 1/frequency;
// height of COM
double zg = AK->getCOMW(_atlas, Twb)(2) - AK->getLimbTransW(_atlas, Twb, MANIP_L_FOOT)(2,3);
cout << "zg " << zg << endl;
// preview how many steps
int numPreviewSteps = 2;
// double Qe = 1;
// double R = 1e-6;
double Qe = 1e7;
double R = 10;
/****************************************
* Generate joint trajectory
***************************************/
gZU.setParameters( dt, 9.81, dofs );
gZU.generateZmpPositions( numSteps, true,
stepLength, footSeparation,
stepDuration,
slopeTime,
levelTime );
gZU.getControllerGains( Qe, R, zg, numPreviewSteps );
gZU.generateCOMPositions();
gZU.getJointTrajectories();
gZU.print( "jointsWholeBody.txt", gZU.mWholeBody );
gZU.mDartDofs;
joint_traj.insert(joint_traj.end(), gZU.mDartDofs.begin(), gZU.mDartDofs.end());
// Bake me some GUI viz
FileInfoSkel<SkeletonDynamics> model;
model.loadFile(ATLAS_DATA_PATH"/skel/ground1.skel", SKEL);
SkeletonDynamics *ground = dynamic_cast<SkeletonDynamics *>(model.getSkel());
ground->setName("ground");
vector<SkeletonDynamics *> skels;
skels.push_back(ground);
skels.push_back(_atlas);
ZmpGUI gui(skels);
gui.bake(joint_traj);
glutInit(&argc, argv);
gui.initWindow(640, 480, "Atlas ZMP Walking");
glutMainLoop();
/**************************************
* Publish joint trajectory
****************************************/
//FIXME: ROS Dependent
//MoveJointTractory(AK, _atlas, dofs, gZU.mWholeBody, 20, 20, 20, 1.2, 1.2, 1.2);
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();
}
/* ********************************************************************************************* */
TEST(KINEMATICS, TRANS_AND_DERIV) {
using namespace Eigen;
using namespace kinematics;
FileInfoSkel<Skeleton> modelFile;
bool loadModelResult = modelFile.loadFile(DART_DATA_PATH"skel/SehoonVSK3.vsk");
ASSERT_TRUE(loadModelResult);
Skeleton* skel = modelFile.getSkel();
EXPECT_TRUE(skel != NULL);
/* LOG(INFO) << "# Dofs = " << skel->getNumDofs(); */
/* LOG(INFO) << "# Nodes = " << skel->getNumNodes(); */
FileInfoDof dofFile(skel);
bool loadDofResult = dofFile.loadFile(DART_DATA_PATH"dof/init_Tpose.dof");
ASSERT_TRUE(loadDofResult);
/* LOG(INFO) << "# frames = " << dofFile.getNumFrames(); */
VectorXd pose = dofFile.getPoseAtFrame(0);
skel->setPose(pose, true, true);
const int nodeIndex = 1;
BodyNode* node = skel->getNode(nodeIndex);
EXPECT_TRUE(node != NULL);
const double TOLERANCE = 0.000001;
// Check the one global transform matrix
const Matrix4d& W = skel->getNode(20)->getWorldTransform();
Matrix4d W_truth;
W_truth << -0.110662, 0.991044, 0.074734, 0.642841
, -0.987564, -0.118099, 0.103775, 0.327496
, 0.111672, -0.0623207, 0.991789, -0.12855
, 0, 0, 0, 1;
for (int i = 0; i < 4; i++) {
for (int j = 0; j < 4; j++) {
EXPECT_NEAR(W(i, j), W_truth(i, j), TOLERANCE);
}
}
// Check the derivative of one global transform matrix
// const Matrix4d& Wq = skel->getNode(10)->mWq.at(4);
const Matrix4d& Wq = skel->getNode(10)->getDerivWorldTransform(4);
Matrix4d Wq_truth;
Wq_truth << 0.121382, 0.413015, 0.902378, 0.161838
,-0.0175714, 0.00698451, 0.0149899, 0.00571836
,-0.992336, 0.0556535, 0.107702, 0.175059
,0, 0, 0, 0;
for (int i = 0; i < 4; i++) {
for (int j = 0; j < 4; j++) {
EXPECT_NEAR(Wq(i, j), Wq_truth(i, j), TOLERANCE);
}
}
// Check Jacobians
BodyNode *nodecheck = skel->getNode(23);
// Linear Jacobian
const MatrixXd Jv = nodecheck->getJacobianLinear();
MatrixXd Jv_truth = MatrixXd::Zero(Jv.rows(), Jv.cols());
Jv_truth<<1, 0, 0, 0.013694, -0.0194371, -0.422612, 0.0157281, -0.00961473, -0.421925, 0.0026645, -0.0048767, -0.179914, -0.0013539, -0.00132969, -0.00885535,
0, 1, 0, 0.0321939, 0.00226492, -0.135736, 0.0330525, 0.00462613, -0.135448, 0.0181491, 0.00758086, -0.122187, 0.00532694, 0.0049221, -0.128917,
0, 0, 1, 0.423948, 0.130694, 0.0166546, 0.42642, 0.118176, 0.0221309, 0.180296, 0.120584, 0.0105059, 0.0838259, 0.081304, 0.00719314;
for (int i = 0; i < Jv.rows(); i++) {
for (int j = 0; j < Jv.cols(); j++) {
EXPECT_NEAR(Jv(i, j), Jv_truth(i, j), TOLERANCE);
}
}
// Angular Jacobian
const MatrixXd Jwbody = nodecheck->getWorldTransform().topLeftCorner<3,3>().transpose()
* nodecheck->getJacobianAngular();
MatrixXd Jwbody_truth = MatrixXd::Zero(Jwbody.rows(), Jwbody.cols());
Jwbody_truth << 0, 0, 0, 0.0818662, 0.996527, 0.000504051, 0.110263, 0.993552, 0.0249018, 0.0772274, 0.995683, 0.0423836, 0.648386, 0.628838, 0.0338389,
0, 0, 0, -0.995079, 0.082001, -0.0518665, -0.992054, 0.111479, -0.0554717, -0.996033, 0.078601, -0.0313861, -0.629201, 0.649145, -0.00994729,
0, 0, 0, -0.0518265, 0.00391001, 0.998406, -0.0579139, -0.0187244, 0.998021, -0.0343359, -0.0398718, 0.998564, -0.0262454, -0.0235626, 0.999159;
for (int i = 0; i < Jwbody.rows(); i++) {
for (int j = 0; j < Jwbody.cols(); j++) {
EXPECT_NEAR(Jwbody(i, j), Jwbody_truth(i, j), TOLERANCE);
}
}
}