/**
@return false if the restored state is same as the current state
*/
bool BodyItem::restoreKinematicState(const BodyState& state)
{
bool modified = false;
BodyState currentState;
storeKinematicState(currentState);
state.getZMP(impl->zmp);
state.restorePositions(*impl->body);
//cout << "(currentState == state):" << (currentState == state) << endl;
//return (currentState == state);
return true;
}
inline void applyAngVelToState(TrackingSystem const &sys, BodyState &state,
BodyProcessModel &processModel,
CannedIMUMeasurement const &meas) {
Eigen::Vector3d angVel;
meas.restoreAngVel(angVel);
Eigen::Vector3d var;
meas.restoreAngVelVariance(var);
#if 0
static const double dt = 0.02;
/// Rotate it into camera space - it's bTb and we want cTc
/// @todo do this without rotating into camera space?
Eigen::Quaterniond cTb = state.getQuaternion();
// Eigen::Matrix3d bTc(state.getQuaternion().conjugate());
/// Turn it into an incremental quat to do the space transformation
Eigen::Quaterniond incrementalQuat =
cTb * angVelVecToIncRot(angVel, dt) * cTb.conjugate();
/// Then turn it back into an angular velocity vector
angVel = incRotToAngVelVec(incrementalQuat, dt);
// angVel = (getRotationMatrixToCameraSpace(sys) * angVel).eval();
/// @todo transform variance?
kalman::AngularVelocityMeasurement<BodyState> kalmanMeas{angVel, var};
kalman::correct(state, processModel, kalmanMeas);
#else
kalman::IMUAngVelMeasurement kalmanMeas{angVel, var};
auto correctionInProgress =
kalman::beginUnscentedCorrection(state, kalmanMeas);
auto outputMeas = [&] {
std::cout << "state: " << state.angularVelocity().transpose()
<< " and measurement: " << angVel.transpose();
};
if (!correctionInProgress.stateCorrectionFinite) {
std::cout
<< "Non-finite state correction in applying angular velocity: ";
outputMeas();
std::cout << "\n";
return;
}
if (!correctionInProgress.finishCorrection(true)) {
std::cout << "Non-finite error covariance after applying angular "
"velocity: ";
outputMeas();
std::cout << "\n";
}
#endif
}
void applyIMUToState(TrackingSystem const &sys,
util::time::TimeValue const &initialTime,
BodyState &state, BodyProcessModel &processModel,
util::time::TimeValue const &newTime,
CannedIMUMeasurement const &meas) {
if (newTime != initialTime) {
auto dt = osvrTimeValueDurationSeconds(&newTime, &initialTime);
kalman::predict(state, processModel, dt);
state.externalizeRotation();
}
if (meas.orientationValid()) {
applyOriToState(sys, state, processModel, meas);
} else if (meas.angVelValid()) {
applyAngVelToState(sys, state, processModel, meas);
} else {
// unusually, the measurement is totally invalid. Just normalize and
// go on.
state.postCorrect();
}
}
void TFCallBack(tf2_msgs::TFMessage msg){
for(int i=0;i<msg.transforms.size();i++){
if(!(tracking<1)){
for(int i2=0;i2<15;i2++){
std::ostringstream checking;
checking<<"/tracker/user_"<<tracking<<"/"<<bodyParts[i2];
if(msg.transforms[i].child_frame_id==checking.str()){
stateTracker.updateSkeletonData(i2, msg.transforms[i].transform.translation.x, msg.transforms[i].transform.translation.y, msg.transforms[i].transform.translation.z);
//std::cout<<"Recieved good data "<<i2<<std::endl;
}
}
}
more=(tracking==-1?false:true);
if(!more)
justChanged=true;
}
}
inline void applyAngVelToState(TrackingSystem const &sys, BodyState &state,
BodyProcessModel &processModel,
CannedIMUMeasurement const &meas) {
Eigen::Vector3d angVel;
meas.restoreAngVel(angVel);
Eigen::Vector3d var;
meas.restoreAngVelVariance(var);
/// Rotate it into camera space - it's bTb and we want cTc
/// @todo do this without rotating into camera space?
Eigen::Quaterniond cTb = state.getQuaternion();
//Eigen::Matrix3d bTc(state.getQuaternion().conjugate());
Eigen::Quaterniond incrementalQuat = cTb * util::quat_exp_map(angVel).exp() *
cTb.conjugate();
angVel = util::quat_exp_map(incrementalQuat).ln();
// angVel = (getRotationMatrixToCameraSpace(sys) * angVel).eval();
/// @todo transform variance?
kalman::AngularVelocityMeasurement<BodyState> kalmanMeas{angVel, var};
kalman::correct(state, processModel, kalmanMeas);
}
int main() {
// Logging:
// --------
const int nbFile=7;
std::vector<std::string> name_vec(nbFile);
name_vec[0]="CoM_X";
name_vec[1]="CoM_Y";
name_vec[2]="CoM_Yaw";
name_vec[3]="CoP_X";
name_vec[4]="CoP_Y";
name_vec[5]="BAS_X";
name_vec[6]="BAS_Y";
std::vector<std::ofstream*> data_vec(nbFile);
std::vector<std::ifstream*> ref_vec(nbFile);
for (int i = 0; i < nbFile; ++i) {
data_vec[i] = new std::ofstream((name_vec[i]+".data").c_str());
ref_vec[i] = new std::ifstream((name_vec[i]+".ref").c_str());
}
// Create and initialize generator:
// -------------------------------
WalkgenAbstract * walk = createWalkgen(CURRENT_QPSOLVERTYPE);
MPCData mpcData;
mpcData.weighting.basePositionInt[0]=0;
mpcData.weighting.basePosition[0]=10;
walk->init(mpcData);
EnvData envData;
Eigen::VectorXd obstacleRadius(3);
Eigen::VectorXd obstaclePositionX(3);
Eigen::VectorXd obstaclePositionY(3);
obstacleRadius(0) = 1;
obstaclePositionX(0) = 2;
obstaclePositionY(0) = -0.2;
obstacleRadius(1) = 0.5;
obstaclePositionX(1) = 4;
obstaclePositionY(1) = 0.1;
obstacleRadius(2) = 0.5;
obstaclePositionX(2) = 5;
obstaclePositionY(2) = -0.3;
envData.obstacleRadius = obstacleRadius;
envData.obstaclePositionX = obstaclePositionX;
envData.obstaclePositionY = obstaclePositionY;
envData.nbObstacle = 3;
// Run:
// ----
BodyState state = walk->bodyState(COM);
state.x(1) += 0;
state.x(2) += 0;
walk->bodyState(COM, state);
double velocity = 0.5;
walk->velReferenceInGlobalFrame(velocity, 0, 0);
walk->posReferenceInGlobalFrame(0, 0, 0);
walk->online(0.0);
double t = 0;
walk->online(t);
MPCSolution result;
Eigen::VectorXd position(10);
Eigen::VectorXd zero(10);
Eigen::VectorXd basePos(20);
basePos.fill(0);
zero.fill(0);
for (t += 0.001; t < 45; t += 0.001){
for(int i=0; i<10; ++i)
{
position(i)=0.16*(i+1)*velocity+result.state_vec[1].baseTrajX_(0);
}
walk->posReferenceInGlobalFrame(position, zero, zero);
result = walk->online(t);
Eigen::VectorXd basePos;
walk->QPBasePosition(basePos);
envData.obstacleLinearizationPointX = basePos.segment(0, 10);
envData.obstacleLinearizationPointY = basePos.segment(10, 10);
walk->envData(envData);
dumpTrajectory(result, data_vec);
}
// Reopen the files:
// -----------------
std::vector<std::ifstream*> check_vec(nbFile);
for(int i = 0;i < nbFile; ++i){
check_vec[i] = new std::ifstream((name_vec[i]+".data").c_str());
}
bool success = ((fabs(result.state_vec[2].baseTrajX_(0)-velocity)<1e-4)
&&(fabs(result.state_vec[2].baseTrajY_(0)-0)<1e-4)
&&(fabs(result.state_vec[2].CoMTrajYaw_(0)-0)<1e-4));
for(unsigned i = 0; i < check_vec.size();++i){
// if the reference file exists, compare with the previous version.
if (*ref_vec[i]){
if (!checkFiles(*check_vec[i],*ref_vec[i])){
//success = false;
}
}
// otherwise, create it
else{
copyFile((name_vec[i]+".data"), (name_vec[i]+".ref"));
}
check_vec[i]->close();
ref_vec[i]->close();
}
makeScilabFile("data", t);
makeScilabFile("ref", t);
for (int i=0; i < nbFile; ++i) {
delete check_vec[i];
delete ref_vec[i];
delete data_vec[i];
}
delete walk;
return (success)?0:1;
}
void BodyItem::storeKinematicState(BodyState& state)
{
state.storePositions(*impl->body);
state.setZMP(impl->zmp);
}
int main(int argc, char **argv)
{
//So, for some odd reason involving the initialization of static non-copyable data, i can not get GUI to hold it's own RenderWindow
//So i'm going to bite the bullet and just do it here in this class... :P
gui.init();
//Create my BodyState tracker
stateTracker=BodyState();
//Set up some ROS stuff
ros::init(argc, argv, "mirror");
ros::NodeHandle n;
//First my subscribers
ros::Subscriber myoIMU_listener = n.subscribe("/myo/imu",1000, myoIMUCallBack);
ros::Subscriber myoGesture_listener = n.subscribe("/myo/gesture",1000, myoGestureCallBack);
ros::Subscriber myoOnboardGesture_listener = n.subscribe("/myo/onboardGesture",1000, myoOnboardGestureCallBack);
ros::Subscriber left_eye_watcher = n.subscribe("/multisense/left/image_rect/compressed",1000, leftEyeCallBack);
ros::Subscriber right_eye_watcher = n.subscribe("/multisense/right/image_rect/compressed",1000, rightEyeCallBack);
ros::Subscriber joint_listener = n.subscribe("/ihmc_ros/valkyrie/output/joint_states", 1000, jointStateCallBack);
ros::Subscriber transform_listener = n.subscribe("/tf", 1000, TFCallBack);
ros::Subscriber people_listener = n.subscribe("/people", 1000, peopleCallBack);
//Now my publishers
ros::Publisher arm_controller = n.advertise<ihmc_msgs::ArmTrajectoryRosMessage>("/ihmc_ros/valkyrie/control/arm_trajectory", 1000);
ros::Publisher neck_controller = n.advertise<ihmc_msgs::NeckTrajectoryRosMessage>("/ihmc_ros/valkyrie/control/neck_trajectory", 1000);
ros::Publisher pelvis_height_controller = n.advertise<ihmc_msgs::PelvisHeightTrajectoryRosMessage>("/ihmc_ros/valkyrie/control/pelvis_height_trajectory", 1000);
ros::Publisher chest_controller = n.advertise<ihmc_msgs::ChestTrajectoryRosMessage>("/ihmc_ros/valkyrie/control/chest_trajectory", 1000);
ros::Publisher go_home_pub = n.advertise<ihmc_msgs::GoHomeRosMessage>("/ihmc_ros/valkyrie/control/go_home",1000);
//Some SFML stufsf::RenderWindow windowf
sf::RenderWindow window;
if(gui.isOcculusOn){
sf::RenderWindow window(sf::VideoMode(2248, 544), "SFML works!");
sf::CircleShape shape(100.f);
shape.setFillColor(sf::Color::Green);
}
//Now the primary loop
ros::Rate loop_rate(6);
ihmc_msgs::GoHomeRosMessage goHomeMsg;
goHomeMsg.trajectory_time=3;
goHomeMsg.unique_id=14;
long int end, start;
while (ros::ok()){
struct timeval tp;
gettimeofday(&tp, NULL);
long int start = tp.tv_sec * 1000 + tp.tv_usec / 1000;
//std::cout<<tracking<<td::endl;
//Some more SFML stuff
if(GUIwindow.isOpen()){
//TODO:add in some human data gathering and print out human position and robot position data
//Also, add a table for Myo stuff
pollEventsGUI();
updateGUI();
}
//std::cout<<lastLeftEye.height<<" "<<lastLeftEye.width<<" "<<lastLeftEye.encoding<<" "<<lastLeftEye.step<<" "<<lastLeftEye.data.size()<<" "<<1024*544*3<<std::endl;
if(gui.isOcculusOn && window.isOpen() && lastLeftEye.data.size()!=0 && lastRightEye.data.size()!=0){
sf::Event event;
while (window.pollEvent(event))
{
if (event.type == sf::Event::Closed)
window.close();
}
sf::Uint8* pixels = new sf::Uint8[2248 * 544 * 4];
sf::Image image,imageL,imageR;
sf::Uint8* Ldata=new sf::Uint8[(size_t)lastLeftEye.data.size()];
sf::Uint8* Rdata=new sf::Uint8[(size_t)lastRightEye.data.size()];
for(int i=0;i<(size_t)lastLeftEye.data.size();i++){
Ldata[i]=lastLeftEye.data[i];
}
for(int i=0;i<(size_t)lastRightEye.data.size();i++){
Rdata[i]=lastRightEye.data[i];
}
imageL.loadFromMemory(Ldata,(size_t)lastLeftEye.data.size());
imageR.loadFromMemory(Rdata,(size_t)lastRightEye.data.size());
delete Ldata;
delete Rdata;
if(imageL.getSize().x!=imageR.getSize().x || imageL.getSize().y!=imageR.getSize().y)
std::cout<<"imageL:("<<imageL.getSize().x<<","<<imageL.getSize().y<<") imageR:("<<imageR.getSize().x<<","<<imageR.getSize().y<<")"<<std::endl;
sf::Texture texture;
int max_x=imageL.getSize().x;
int max_y=imageL.getSize().y;
for(int y = 0; y <max_y; y++)
{
for(int x = 0; x < max_x; x++)
{
pixels[(y*(2*max_x+200)+x)*4] = imageL.getPixelsPtr()[((max_y-y)*max_x+x)*4]; // R?
pixels[(y*(2*max_x+200)+x)*4 + 1] = imageL.getPixelsPtr()[((max_y-y)*max_x+x)*4+1]; // G?
pixels[(y*(2*max_x+200)+x)*4 + 2] = imageL.getPixelsPtr()[((max_y-y)*max_x+x)*4+2]; // B?
pixels[(y*(2*max_x+200)+x)*4 + 3] = 255; // A?
}
for(int x=1024;x<1224;x++){
pixels[(y*(2*max_x+200)+x)*4] = 0; // R?
pixels[(y*(2*max_x+200)+x)*4 + 1] = 0; // G?
pixels[(y*(2*max_x+200)+x)*4 + 2] = 0; // B?
pixels[(y*(2*max_x+200)+x)*4 + 3] = 255; // A?
}
for(int x = (max_x+200); x < (2*max_x+200); x++)
{
pixels[(y*(2*max_x+200)+x)*4] = imageR.getPixelsPtr()[((max_y-y)*max_x+x-(max_x+200))*4]; // R?
pixels[(y*(2*max_x+200)+x)*4 + 1] = imageR.getPixelsPtr()[((max_y-y)*max_x+x-(max_x+200))*4+1]; // G?
pixels[(y*(2*max_x+200)+x)*4 + 2] = imageR.getPixelsPtr()[((max_y-y)*max_x+x-(max_x+200))*4+2]; // B?
pixels[(y*(2*max_x+200)+x)*4 + 3] = 255; // A?
}
}
window.clear();
image.create(2248, 544, pixels);
texture.create(2248, 544);
texture.update(image);
sf::Sprite sprite;
sprite.setTexture(texture);
window.draw(sprite);
window.display();
delete pixels;
}
if(gui.goHomeCount==12){
goHomeMsg.body_part=0;
goHomeMsg.robot_side=0;
go_home_pub.publish(goHomeMsg);
std::cout<<"just published goHome LEFT_ARM"<<std::endl;
for(int i=0;i<6;i++){
ros::spinOnce();
loop_rate.sleep();
}
goHomeMsg.robot_side=1;
go_home_pub.publish(goHomeMsg);
std::cout<<"just published goHome RIGHT_ARM"<<std::endl;
for(int i=0;i<6;i++){
ros::spinOnce();
loop_rate.sleep();
}
goHomeMsg.body_part=1;
go_home_pub.publish(goHomeMsg);
std::cout<<"just published goHome TORSO"<<std::endl;
for(int i=0;i<6;i++){
ros::spinOnce();
loop_rate.sleep();
}
goHomeMsg.body_part=2;
go_home_pub.publish(goHomeMsg);
std::cout<<"just published goHome PELVIS"<<std::endl;
gui.startGoingHome=false;
gui.goHomeCount--;
ros::spinOnce();
loop_rate.sleep();
continue;
/*
moveSpeedOverRide=3;
arm_controller.publish(moveArm(JRIGHT,stateTracker.getArmHomeAngles()));
arm_controller.publish(moveArm(JLEFT,stateTracker.getArmHomeAngles()));
chest_controller.publish(moveChest(stateTracker.getChestHomeAngles()));
neck_controller.publish(moveHead(stateTracker.getNeckHomeAngles()));
pelvis_height_controller.publish(movePelvisHeight(stateTracker.getPelvisHeightHomeAngles()));
moveSpeedOverRide=-1;*/
}
if(!more || justChanged || (gui.goHomeCount>0 && gui.goHomeCount<12)){
//std::cout<<gui.goHomeCount<<" "<<more<<" "<<justChanged<<std::endl;
if(justChanged && more)
justChanged=false;
/*
ihmc_msgs::GoHomeRosMessage msg;
msg.trajectory_time=3;
msg.unique_id=5;
go_home_pub.publish(msg);
msg.robot_side=1;
go_home_pub.publish(msg);*/
if(gui.goHomeCount>0)
gui.goHomeCount--;
ros::spinOnce();
loop_rate.sleep();
continue;
}
std::vector<double> out;
out.push_back((tp.tv_sec * 1000 + tp.tv_usec / 1000)-end);//oh wow, this is terrible programming practice... dang man
if(end!=0)
gui.record("*: ",out);
//gui.gesture==0 acts as an unlock feature
if(gui.isRightArmOn && gui.gesture==1)
arm_controller.publish(moveArm(JRIGHT,stateTracker.getRightArmAngles()));
else if(gui.isRightArmOn && gui.gesture!=1 && false){
moveSpeedOverRide=3;
arm_controller.publish(moveArm(JRIGHT,stateTracker.getArmHomeAngles()));
moveSpeedOverRide=-1;
moveArm(JRIGHT,stateTracker.getRightArmAngles(),false);
}
if(gui.isLeftArmOn && gui.gesture==1)
arm_controller.publish(moveArm(JLEFT,stateTracker.getLeftArmAngles()));
else if(gui.isLeftArmOn && gui.gesture!=1 && false){
moveSpeedOverRide=3;
arm_controller.publish(moveArm(JLEFT,stateTracker.getArmHomeAngles()));
moveSpeedOverRide=-1;
//moveArm(JLEFT,stateTracker.getLeftArmAngles(),false);
}
if(gui.isChestOn && gui.gesture==1)
chest_controller.publish(moveChest(stateTracker.getChestAngles()));
else if(gui.isChestOn && gui.gesture!=1 && false){
moveSpeedOverRide=3;
chest_controller.publish(moveChest(stateTracker.getChestHomeAngles()));
moveSpeedOverRide=-1;
//moveChest(stateTracker.getChestAngles(),false);
}
if(gui.isHeadOn && gui.gesture==1)
neck_controller.publish(moveHead(stateTracker.getNeckAngles()));
else if(gui.isHeadOn && gui.gesture!=1 && false){
moveSpeedOverRide=3;
neck_controller.publish(moveHead(stateTracker.getNeckHomeAngles()));
moveSpeedOverRide=-1;
//moveHead(stateTracker.getNeckAngles(),false);
}
if(gui.isPelvisOn && gui.gesture==1)
pelvis_height_controller.publish(movePelvisHeight(stateTracker.getStandingHeight()));
else if(gui.isPelvisOn && gui.gesture!=1 && false){
moveSpeedOverRide=3;
pelvis_height_controller.publish(movePelvisHeight(stateTracker.getPelvisHeightHomeAngles()));
moveSpeedOverRide=-1;
//movePelvisHeight(stateTracker.getStandingHeight(),false);
}
ros::spinOnce();
gettimeofday(&tp, NULL);
long int start2 = tp.tv_sec * 1000 + tp.tv_usec / 1000;
loop_rate.sleep();
gettimeofday(&tp, NULL);
end = tp.tv_sec * 1000 + tp.tv_usec / 1000;
std::cout<<end-start2<<std::endl;
}
go_home_pub.publish(goHomeMsg);
goHomeMsg.robot_side=1;
go_home_pub.publish(goHomeMsg);
return 0;
}
