void
shiftHistogram (const Eigen::VectorXf &hist, Eigen::VectorXf &hist_shifted, bool direction)
{
int bins = hist.rows();
hist_shifted = Eigen::VectorXf::Zero(bins);
if(direction){ //shift right
hist_shifted.tail(bins - 1) = hist.head(bins-1);
hist_shifted(bins-1) += hist(bins-1);
}
else { // shift left
hist_shifted.head(bins - 1) = hist.tail(bins-1);
hist_shifted(0) += hist(0);
}
}
void SimDynamicsWindow::updateJointInfo()
{
//std::stringstream info;
std::string info;
int n = UI.comboBoxRobotNode->currentIndex();
QString qMin("0");
QString qMax("0");
QString qName("Name: <not set>");
QString qJV("Joint value: 0");
QString qTarget("Joint target: 0");
QString qVel("Joint velocity: 0");
QString qVelTarget("Joint velocity target: 0");
QString qGP("GlobalPose (simox): 0/0/0");
QString qVisu("VISU (simox): 0/0/0");
QString qCom("COM (bullet): 0/0/0");
QString tmp;
RobotNodeRevolutePtr rn;
if (n >= 0 && n < (int)robotNodes.size())
{
rn = robotNodes[n];
}
SimDynamics::DynamicsObjectPtr dynRN = dynamicsRobot->getDynamicsRobotNode(rn);
SimDynamics::BulletObjectPtr bulletRN = boost::dynamic_pointer_cast<SimDynamics::BulletObject>(dynRN);
if (bulletRN)
{
// cout << "FORCE: " << bulletRN->getRigidBody()->getTotalForce()[0] << ", " << bulletRN->getRigidBody()->getTotalForce()[1] << ", " << bulletRN->getRigidBody()->getTotalForce()[2] << endl;
// cout << "TORQUE: " << bulletRN->getRigidBody()->getTotalTorque()[0] << ", " << bulletRN->getRigidBody()->getTotalTorque()[1] << ", " << bulletRN->getRigidBody()->getTotalTorque()[2] << endl;
// cout << "getLinearVelocity: " << bulletRN->getRigidBody()->getLinearVelocity()[0] << ", " << bulletRN->getRigidBody()->getLinearVelocity()[1] << ", " << bulletRN->getRigidBody()->getLinearVelocity()[2] << endl;
// cout << "getAngularVelocity: " << bulletRN->getRigidBody()->getAngularVelocity()[0] << ", " << bulletRN->getRigidBody()->getAngularVelocity()[1] << ", " << bulletRN->getRigidBody()->getAngularVelocity()[2] << endl;
}
BulletRobotPtr bulletRobot = boost::dynamic_pointer_cast<SimDynamics::BulletRobot>(dynamicsRobot);
if (rn && bulletRobot && bulletRobot->hasLink(rn))
{
BulletRobot::LinkInfo linkInfo = bulletRobot->getLink(rn);
linkInfo.nodeA->showCoordinateSystem(true);
linkInfo.nodeB->showCoordinateSystem(true);
bulletRobot->enableForceTorqueFeedback(linkInfo);
Eigen::VectorXf ftA = bulletRobot->getForceTorqueFeedbackA(linkInfo);
Eigen::VectorXf ftB = bulletRobot->getForceTorqueFeedbackB(linkInfo);
std::stringstream streamFTA;
streamFTA << "ForceTorqueA: " << std::fixed;
streamFTA.precision(1);
for (int i = 0; i < 6; ++i)
{
streamFTA << ftA[i] << ",";
}
UI.label_ForceTorqueA->setText(QString::fromStdString(streamFTA.str()));
std::stringstream streamFTB;
streamFTB << "ForceTorqueB: " << std::fixed;
streamFTB.precision(1);
for (int i = 0; i < 6; ++i)
{
streamFTB << ftB[i] << ",";
}
UI.label_ForceTorqueB->setText(QString::fromStdString(streamFTB.str()));
// cout << "ForceTorqueA:" << endl;
// MathTools::print(ftA);
// cout << "ForceTorqueB:" << endl;
// MathTools::print(ftB);
forceSep->removeAllChildren();
SoUnits* u = new SoUnits();
u->units = SoUnits::MILLIMETERS;
forceSep->addChild(u);
Eigen::Vector3f n = ftA.head(3);
//n = linkInfo.nodeA->toGlobalCoordinateSystemVec(n);
float l = ftA.head(3).norm();
float w = 5.0f;
SoSeparator* forceA = new SoSeparator;
SoSeparator* arrowForceA = CoinVisualizationFactory::CreateArrow(n, l, w, VisualizationFactory::Color::Red());
/*
cout << "FORCE_A: " << linkInfo.dynNode1->getRigidBody()->getTotalForce()[0] << "," << linkInfo.dynNode1->getRigidBody()->getTotalForce()[1] << "," << linkInfo.dynNode1->getRigidBody()->getTotalForce()[2] << endl;
cout << "TORQUEA: " << linkInfo.dynNode1->getRigidBody()->getTotalTorque()[0] << "," << linkInfo.dynNode1->getRigidBody()->getTotalTorque()[1] << "," << linkInfo.dynNode1->getRigidBody()->getTotalTorque()[2] << endl;
cout << "FORCE_B: " << linkInfo.dynNode2->getRigidBody()->getTotalForce()[0] << "," << linkInfo.dynNode2->getRigidBody()->getTotalForce()[1] << "," << linkInfo.dynNode2->getRigidBody()->getTotalForce()[2] << endl;
cout << "TORQUEB: " << linkInfo.dynNode2->getRigidBody()->getTotalTorque()[0] << "," << linkInfo.dynNode2->getRigidBody()->getTotalTorque()[1] << "," << linkInfo.dynNode2->getRigidBody()->getTotalTorque()[2] << endl;
*/
// show as nodeA local coords system
/*
Eigen::Matrix4f comCoord = linkInfo.nodeA->getGlobalPose();
Eigen::Matrix4f comLocal = Eigen::Matrix4f::Identity();
comLocal.block(0,3,3,1) = linkInfo.nodeA->getCoMLocal();
comCoord = comCoord * comLocal;
*/
// show as global coords
Eigen::Matrix4f comGlobal = Eigen::Matrix4f::Identity();
comGlobal.block(0, 3, 3, 1) = linkInfo.nodeA->getCoMGlobal();
SoMatrixTransform* m = CoinVisualizationFactory::getMatrixTransform(comGlobal);
forceA->addChild(m);
forceA->addChild(arrowForceA);
forceSep->addChild(forceA);
Eigen::Vector3f jointGlobal = linkInfo.nodeJoint->getGlobalPose().block(0, 3, 3, 1);
Eigen::Vector3f comBGlobal = linkInfo.nodeB->getCoMGlobal();
// force that is applied on objectA by objectB
Eigen::Vector3f FBGlobal = ftA.head(3);
Eigen::Vector3f TBGlobal = ftB.tail(3) ;
Eigen::VectorXf torqueJointGlobal = bulletRobot->getJointForceTorqueGlobal(linkInfo);//= TBGlobal - (comBGlobal-jointGlobal).cross(FBGlobal) * 0.001;
// cout << "torqueJointGlobal: " << torqueJointGlobal << endl;
std::stringstream streamFTJoint;
streamFTJoint.precision(1);
streamFTJoint << "ForceTorqueJoint: " << std::fixed;
for (int i = 0; i < 6; ++i)
{
streamFTJoint << torqueJointGlobal[i] << ",";
}
UI.label_ForceTorqueJoint->setText(QString::fromStdString(streamFTJoint.str()));
n = ftB.head(3);
//n = linkInfo.nodeB->toGlobalCoordinateSystemVec(n);
l = ftB.head(3).norm();
w = 5.0f;
SoSeparator* forceB = new SoSeparator;
SoSeparator* arrowForceB = CoinVisualizationFactory::CreateArrow(n, l, w, VisualizationFactory::Color::Red());
comGlobal = Eigen::Matrix4f::Identity();
comGlobal.block(0, 3, 3, 1) = linkInfo.nodeB->getCoMGlobal();
m = CoinVisualizationFactory::getMatrixTransform(comGlobal);
forceB->addChild(m);
forceB->addChild(arrowForceB);
forceSep->addChild(forceB);
/*
if (!linkInfo.joint->needsFeedback())
{
linkInfo.joint->enableFeedback(true);
btJointFeedback* feedback = new btJointFeedback;
feedback->m_appliedForceBodyA = btVector3(0, 0, 0);
feedback->m_appliedForceBodyB = btVector3(0, 0, 0);
feedback->m_appliedTorqueBodyA = btVector3(0, 0, 0);
feedback->m_appliedTorqueBodyB = btVector3(0, 0, 0);
linkInfo.joint->setJointFeedback(feedback);
}
else
{
btJointFeedback* feedback = linkInfo.joint->getJointFeedback();
cout << "feedback->m_appliedForceBodyA: " << feedback->m_appliedForceBodyA[0] << "," << feedback->m_appliedForceBodyA[1] << "," << feedback->m_appliedForceBodyA[2] << endl;
cout << "feedback->m_appliedForceBodyB: " << feedback->m_appliedForceBodyB[0] << "," << feedback->m_appliedForceBodyB[1] << "," << feedback->m_appliedForceBodyB[2] << endl;
cout << "feedback->m_appliedTorqueBodyA: " << feedback->m_appliedTorqueBodyA[0] << "," << feedback->m_appliedTorqueBodyA[1] << "," << feedback->m_appliedTorqueBodyA[2] << endl;
cout << "feedback->m_appliedTorqueBodyB: " << feedback->m_appliedTorqueBodyB[0] << "," << feedback->m_appliedTorqueBodyB[1] << "," << feedback->m_appliedTorqueBodyB[2] << endl;
}
*/
}
if (rn)
{
qMin = QString::number(rn->getJointLimitLo(), 'f', 4);
qMax = QString::number(rn->getJointLimitHi(), 'f', 4);
qName = QString("Name: ");
qName += QString(rn->getName().c_str());
qJV = QString("Joint value: ");
tmp = QString::number(rn->getJointValue(), 'f', 3);
qJV += tmp;
info += "jv rn:";
std::string a1 = (const char*)tmp.toAscii();
info += a1;
qJV += QString(" / ");
if (dynamicsRobot->isNodeActuated(rn))
{
tmp = QString::number(dynamicsRobot->getJointAngle(rn), 'f', 3);
}
else
{
tmp = QString("-");
}
qJV += tmp;
info += ",\tjv bul:";
a1 = (const char*)tmp.toAscii();
info += a1;
qTarget = QString("Joint target: ");
if (dynamicsRobot->isNodeActuated(rn))
{
tmp = QString::number(dynamicsRobot->getNodeTarget(rn), 'f', 3);
}
else
{
tmp = QString("-");
}
qTarget += tmp;
info += std::string(",targ:");
a1 = (const char*)tmp.toAscii();
info += a1;
qVel = QString("Joint velocity: ");
if (dynamicsRobot->isNodeActuated(rn))
{
tmp = QString::number(dynamicsRobot->getJointSpeed(rn), 'f', 3);
}
else
{
tmp = QString("-");
}
qVel += tmp;
info += ",vel:";
a1 = (const char*)tmp.toAscii();
info += a1;
qVelTarget = QString("Joint velocity target: ");
if (dynamicsRobot->isNodeActuated(rn))
{
tmp = QString::number(dynamicsRobot->getJointTargetSpeed(rn), 'f', 3);
}
else
{
tmp = QString("-");
}
qVelTarget += tmp;
info += ",velTarget:";
a1 = (const char*)tmp.toAscii();
info += a1;
Eigen::Matrix4f gp = rn->getGlobalPose();
qGP = QString("GlobalPose (simox):");
tmp = QString::number(gp(0, 3), 'f', 2);
qGP += tmp;
info += ",gp:";
info += (const char*)tmp.toAscii();
qGP += QString("/");
tmp = QString::number(gp(1, 3), 'f', 2);
qGP += tmp;
info += "/";
info += (const char*)tmp.toAscii();
qGP += QString("/");
tmp = QString::number(gp(2, 3), 'f', 2);
qGP += tmp;
info += "/";
info += (const char*)tmp.toAscii();
gp = rn->getGlobalPose();
qVisu = QString("VISU (simox):");
qVisu += QString::number(gp(0, 3), 'f', 2);
qVisu += QString("/");
qVisu += QString::number(gp(1, 3), 'f', 2);
qVisu += QString("/");
qVisu += QString::number(gp(2, 3), 'f', 2);
if (dynamicsRobot->hasDynamicsRobotNode(rn))
{
gp = dynamicsRobot->getComGlobal(rn);
}
else
{
gp = Eigen::Matrix4f::Identity();
}
qCom = QString("COM (bullet):");
qCom += QString::number(gp(0, 3), 'f', 2);
qCom += QString("/");
qCom += QString::number(gp(1, 3), 'f', 2);
qCom += QString("/");
qCom += QString::number(gp(2, 3), 'f', 2);
}
UI.label_TargetMin->setText(qMin);
UI.label_TargetMax->setText(qMax);
UI.label_RNName->setText(qName);
UI.label_RNValue->setText(qJV);
UI.label_RNTarget->setText(qTarget);
UI.label_RNVelocity->setText(qVel);
UI.label_RNVelocityTarget->setText(qVelTarget);
UI.label_RNPosGP->setText(qGP);
UI.label_RNPosVisu->setText(qVisu);
UI.label_RNPosCom->setText(qCom);
#if 0
// print some joint info
if (viewer->engineRunning())
{
cout << info << endl;
}
#endif
}