void control_heading(const struct vehicle_global_position_s *pos, const struct position_setpoint_s *sp,
const struct vehicle_attitude_s *att, struct vehicle_attitude_setpoint_s *att_sp)
{
/*
* Calculate heading error of current position to desired position
*/
float bearing = get_bearing_to_next_waypoint(pos->lat, pos->lon, sp->lat, sp->lon);
matrix::Eulerf att_euler = matrix::Quatf(att->q);
/* calculate heading error */
float yaw_err = att_euler.psi() - bearing;
/* apply control gain */
float roll_body = yaw_err * p.hdng_p;
/* limit output, this commonly is a tuning parameter, too */
if (roll_body < -0.6f) {
roll_body = -0.6f;
} else if (att_sp->roll_body > 0.6f) {
roll_body = 0.6f;
}
matrix::Eulerf att_spe(roll_body, 0, bearing);
matrix::Quatf qd(att_spe);
att_sp->q_d[0] = qd(0);
att_sp->q_d[1] = qd(1);
att_sp->q_d[2] = qd(2);
att_sp->q_d[3] = qd(3);
}
int
main(int argc, char** argv)
{
if (argc < 2)
{
std::cout << "Usage: rlDynamics1Demo MODELFILE Q1 ... Qn QD1 ... QDn QDD1 ... QDDn" << std::endl;
return 1;
}
try
{
rl::mdl::XmlFactory factory;
boost::shared_ptr< rl::mdl::Model > model(factory.create(argv[1]));
rl::mdl::Dynamic* dynamic = dynamic_cast< rl::mdl::Dynamic* >(model.get());
rl::math::Vector q(model->getDof());
rl::math::Vector qd(model->getDof());
rl::math::Vector qdd(model->getDof());
rl::math::Vector tau(model->getDof());
for (std::size_t i = 0; i < model->getDof(); ++i)
{
q(i) = boost::lexical_cast< rl::math::Real >(argv[i + 2]);
qd(i) = boost::lexical_cast< rl::math::Real >(argv[i + 2 + model->getDof()]);
qdd(i) = boost::lexical_cast< rl::math::Real >(argv[i + 2 + 2 * model->getDof()]);
}
dynamic->setPosition(q);
dynamic->setVelocity(qd);
dynamic->setAcceleration(qdd);
dynamic->inverseDynamics();
dynamic->getTorque(tau);
std::cout << "tau = " << tau.transpose() << std::endl;
dynamic->forwardDynamics();
dynamic->getAcceleration(qdd);
std::cout << "qdd = " << qdd.transpose() << std::endl;
dynamic->rungeKuttaNystrom(1);
dynamic->getPosition(q);
dynamic->getVelocity(qd);
std::cout << "q = " << q.transpose() << std::endl;
std::cout << "qd = " << qd.transpose() << std::endl;
}
catch (const std::exception& e)
{
std::cout << e.what() << std::endl;
return 1;
}
return 0;
}
/* Given a state vector of length 2*N corresponding to N joint angles
* and N joint speeds and torque vector of length N,
* this fills in the 2*N length vector sd with
* the corresponding joint speeds and joint accelerations.
*/
std::vector<double>
ckbot::chain_rate::calc_rate(std::vector<double> s, std::vector<double> T)
{
int N = c.num_links();
std::vector<double> qdd(N);
std::vector<double> q(N);
std::vector<double> qd(N);
for(int i=0; i<N; ++i)
{
q[i] = s[i];
qd[i] = s[N+i];
}
/* Update the chain's state to match the requested initial
* conditions (s)
*/
c.propogate_angles_and_rates(q,qd);
tip_base_step(s, T);
qdd = base_tip_step(s, T);
for (int i=0; i < N; ++i)
{
/* Rate of change of positions are already in our state */
sd[i] = s[N+i];
/* The second half of the state vector is rate of change
* of velocities, or the calculated accelerations.
*/
sd[N+i] = qdd[i];
}
return sd;
}
void mtsIntuitiveResearchKitMTM::RunGravityCompensation(void)
{
vctDoubleVec q(7, 0.0);
vctDoubleVec qd(7, 0.0);
vctDoubleVec tau(7, 0.0);
std::copy(JointGet.begin(), JointGet.begin() + 7 , q.begin());
vctDoubleVec torqueDesired(8, 0.0);
tau.ForceAssign(Manipulator.CCG(q, qd));
tau[0] = q(0) * 0.0564 + 0.08;
std::copy(tau.begin(), tau.end() , torqueDesired.begin());
torqueDesired[3]=0.0;
torqueDesired[4]=0.0;
torqueDesired[5]=0.0;
torqueDesired[6]=0.0;
// For J7 (wrist roll) to -1.5 PI to 1.5 PI
double gain = 0.05;
if (JointGet[JNT_WRIST_ROLL] > 1.5 * cmnPI) {
torqueDesired[JNT_WRIST_ROLL] = (1.5 * cmnPI - JointGet[JNT_WRIST_ROLL]) * gain;
} else if (JointGet[JNT_WRIST_ROLL] < -1.5 * cmnPI) {
torqueDesired[JNT_WRIST_ROLL] = (-1.5 * cmnPI - JointGet[JNT_WRIST_ROLL]) * gain;
}
TorqueSet.SetForceTorque(torqueDesired);
PID.SetTorqueJoint(TorqueSet);
}
BryantSteelTripartitionScorer::BryantSteelTripartitionScorer(TaxonSet& ts) :
TripartitionScorer(ts),
qd(*QuartetDict::cl(ts))
{
unordered_set<Clade>& clades = CladeExtractor::get_clades();
for (const Clade& clade : clades) {
vector<Taxon> nonmembers;
for(size_t i = 0; i < ts.size(); i++) {
if (!clade.contains(i))
nonmembers.push_back(i);
}
map<pair<Taxon, Taxon>, double>& mp = W[clade.get_taxa()];
if (clade.size() < 2) {
continue;
}
for (size_t i = 0; i < nonmembers.size(); i++) {
for (size_t j = i+1; j < nonmembers.size(); j++) {
double d = 0;
for (Taxon k : clade) {
for (Taxon l : clade) {
if (k > l)
d += qd(nonmembers[i],nonmembers[j],k,l);
}
}
mp[make_pair(nonmembers[i], nonmembers[j])] = d;
mp[make_pair(nonmembers[j], nonmembers[i])] = d;
}
}
}
}
void CustomFunctionsPanel::playMusic()
{
if (!clickObject) {
clickObject = new Phonon::MediaObject(this);
clickOutput = new Phonon::AudioOutput(Phonon::NoCategory, this);
Phonon::createPath(clickObject, clickOutput);
connect(clickObject, SIGNAL(stateChanged(Phonon::State, Phonon::State)), this, SLOT(mediaPlayer_state(Phonon::State, Phonon::State)));
}
QPushButton * button = qobject_cast<QPushButton*>(sender());
int index = button->property("index").toInt();
QString path = g.profile[g.id()].sdPath();
QDir qd(path);
QString track;
if (qd.exists()) {
if (firmware->getCapability(VoicesAsNumbers)) {
track = path + QString("/%1.wav").arg(int(fswtchParam[index]->value()), 4, 10, (const QChar)'0');
}
else {
path.append("/SOUNDS/");
QString lang = generalSettings.ttsLanguage;
if (lang.isEmpty())
lang = "en";
path.append(lang);
if (fswtchParamArmT[index]->currentText() != "----") {
track = path + "/" + fswtchParamArmT[index]->currentText() + ".wav";
}
}
QFile file(track);
if (!file.exists()) {
QMessageBox::critical(this, tr("Error"), tr("Unable to find sound file %1!").arg(track));
return;
}
if (phononCurrent == index) {
clickObject->stop();
clickObject->clear();
playBT[index]->setIcon(CompanionIcon("play.png"));
phononCurrent = -1;
}
else {
if (phononCurrent >= 0) {
playBT[phononCurrent]->setIcon(CompanionIcon("play.png"));
}
phononCurrent = index;
clickObject->clear();
#ifdef __APPLE__
clickObject->setCurrentSource(QUrl("file://"+track));
#else
clickObject->setCurrentSource(QUrl(track));
#endif
clickObject->play();
playBT[index]->setIcon(CompanionIcon("stop.png"));
}
}
}
Resolver::~Resolver()
{
QDir qd(CacheDir);
QStringList fds = qd.entryList(QDir::Files);
for(QStringList::Iterator it = fds.begin(); it != fds.end(); it++)
{
QFile fd(CacheDir + *it);
if(!fd.remove())
qDebug() << "Failed: " << fd.error();
}
}
void robManipulatorTest::TestInverseDynamics(){
cmnPath path;
path.AddRelativeToCisstShare("/models/WAM");
std::string fname = path.Find("wam7.rob", cmnPath::READ);
robManipulator WAM7( fname );
vctDynamicVector<double> q(7, 0.0), qd(7, 1.0), qdd(7, 1.0);
vctFixedSizeVector<double, 6> ft(0.0);
vctDynamicVector<double> tau = WAM7.RNE( q, qd, qdd, ft );
}
void insertParen(struct Queue * queue) {
int ops = 0;
int needed_parens = 0;
char ch;
struct Stack *s = createStack();
struct Stack *t = createStack();
while(queue->size > 0) {
push(s, qd(queue));
}
int done = 0;
while( ! done ) {
ch = peek(s);
switch(ch) {
case '+':
case '-':
case '*':
case '/':
ops++;
if (ops > needed_parens) {
done = 1;
} else {
push(t, pop(s));
}
break;
case ')':
needed_parens++;
default:
push(t, pop(s));
break;
}
if (s->size == 0 || done == 1) {
push(t, '(');
done = 1;
}
}
// drain whatever is left
while (s->size > 0) {
push(t, pop(s));
}
while (t->size > 0) {
qq(queue, pop(t));
}
return;
}
QString qws_fontCacheDir()
{
QString dir;
#if defined(Q_WS_QWS)
extern QString qws_dataDir();
dir = qws_dataDir();
#else
dir = QDir::tempPath();
#endif
dir.append(QLatin1String("/fonts/"));
QDir qd(dir);
if (!qd.exists() && !qd.mkpath(dir))
dir = QDir::tempPath();
return dir;
}
void WebyPlugin::indexIE(QString path, QList<CatItem>* items)
{
QDir qd(path);
QString dir = qd.absolutePath();
QStringList dirs = qd.entryList(QDir::AllDirs | QDir::NoDotAndDotDot);
for (int i = 0; i < dirs.count(); ++i) {
QString cur = dirs[i];
if (cur.contains(".lnk")) continue;
indexIE(dir + "/" + dirs[i],items);
}
QStringList files = qd.entryList(QStringList("*.url"), QDir::Files, QDir::Unsorted);
for(int i = 0; i < files.count(); ++i) {
items->push_back(CatItem(dir + "/" + files[i], files[i].mid(0,files[i].size()-4)));
}
}
void testApp::processOsc() {
while(oscIn.hasWaitingMessages()){
ofxOscMessage m;
oscIn.getNextMessage(&m);
if(m.getAddress()=="/control/mode/next"){
//get the next mode ready
string modeName = m.getArgAsString(0);
nextMode = modeName;
// cout << "todo: start upcoming mode standby for mode: " << modeName << endl;
}else if(m.getAddress()=="/control/mode/swap"){
mode->setMode(nextMode);
// cout << "todo: begin transition from current to upcoming mode." << endl;
}else if(m.getAddress()=="/control/bubble/new"){
bubbles.push_back(new Donk::BubbleData(m));
//cout << "todo: do something with new bubble received from " << bd.userName << " in mode " << bd.mode << endl;
}else if(m.getAddress()=="/control/question/update"){
Donk::QuestionData qd(m);
cout << "todo: handle question update \"" << qd.text << "\" " << qd.tags[0] << "=" <<
qd.tag_counts[0] << "," << qd.tags[1] << "=" << qd.tag_counts[1] << endl;
}else if(m.getAddress()=="/audio"){
//get 6 floated values
float audioData[6];
for(int i=0;i<6;i++)audioData[i] = m.getArgAsFloat(i);
cout << "todo: do something with new audio frequency sample: " <<
audioData[0] << ' ' <<
audioData[1] << ' ' <<
audioData[2] << ' ' <<
audioData[3] << ' ' <<
audioData[4] << ' ' <<
audioData[5] << endl;
}
}
}
osaPDGC::Errno
osaPDGC::Evaluate
( const vctDynamicVector<double>& qs,
const vctDynamicVector<double>& q,
vctDynamicVector<double>& tau,
double dt ) {
if( qs.size() != links.size() ) {
CMN_LOG_RUN_ERROR << "size(qs) = " << qs.size() << " "
<< "N = " << links.size() << std::endl;
return osaPDGC::EFAILURE;
}
if( q.size() != links.size() ) {
CMN_LOG_RUN_ERROR << "size(q) = " << q.size() << " "
<< "N = " << links.size() << std::endl;
return osaPDGC::EFAILURE;
}
// evaluate the velocity
vctDynamicVector<double> qd( links.size(), 0.0 );
if( 0 < dt ) qd = (q - qold)/dt;
// error = current - desired
vctDynamicVector<double> e = q - qs;
// error time derivative
vctDynamicVector<double> ed( links.size(), 0.0 );
if( 0 < dt ) ed = (e - eold)/dt;
// Compute the coriolis+gravity load
vctDynamicVector<double> ccg =
CCG( q, vctDynamicVector<double>( links.size(), 0.0 ) );
tau = ccg - Kp*e - Kd*ed;
eold = e;
qold = q;
return osaPDGC::ESUCCESS;
}
void mtsIntuitiveResearchKitMTM::RunClutch(void)
{
// J1-J3
vctDoubleVec q(7, 0.0);
vctDoubleVec qd(7, 0.0);
vctDoubleVec tau(7, 0.0);
q.Assign(JointGet.Ref(7));
vctDoubleVec torqueDesired(8, 0.0);
tau.ForceAssign(Manipulator.CCG(q, qd));
tau[0] = q(0) * 0.0564 + 0.08;
torqueDesired.Ref(7).Assign(tau);
TorqueSet.SetForceTorque(torqueDesired);
PID.SetTorqueJoint(TorqueSet);
// J4-J7
JointSet.Assign(JointGet);
CartesianClutched.Translation().Assign(CartesianGet.Translation());
Manipulator.InverseKinematics(JointSet, CartesianClutched);
SetPositionJointLocal(JointSet);
}
void SMFile::DoSearch(const QStringList& keyword, const Qt::CaseSensitivity& caseSensitive, QRegExp::PatternSyntax syntax)
{
Entex ee("SMFile::DoSearch", 3);
if (keyword.isEmpty())
return;
QFile file(fileInfo.filePath());
if (!file.open(QIODevice::ReadOnly | QIODevice::Text))
{
qDebug() << "file open failed"; //Todo:: throw exception
return;
}
QTextStream in(&file);
int lineNo = 1;
// Todo:: maybe we should have some sort of SearchResult builder to build
// SearchResult which has the knowledge of SearchSessionId and takes in a
// QFileInfo as parameter
Result result(new SearchResult(searchSessionId, fileInfo));
while (!in.atEnd())
{
if (stopIssued == true)
break;
QString line = in.readLine();
if (IsLineMatched(line, keyword[0], caseSensitive, syntax))
{
qd(4) << "found matched line";
result->matchedLines.push_back(LineInfo(lineNo, line));
}
++lineNo;
}
if (result->matchedLines.empty() == false)
Notify(result);
}
void controlyPlugin::getApps(QList<CatItem>* items) {
int a = 0;
// Get the control panel applications
TCHAR infoBuf[32767];
if (!GetSystemDirectory(infoBuf, 32767)) {
return;
}
QString buff = QString::fromUtf16((const ushort*) infoBuf);
QDir qd(buff);
QStringList files = qd.entryList(QStringList("*.cpl"), QDir::Files, QDir::Unsorted);
foreach(QString file, files) {
QString path = QDir::toNativeSeparators(qd.absoluteFilePath(file));
if (cache.count(file) > 0) {
if (cache[file] != "")
items->push_back(CatItem(path, cache[file], 0, getIcon()));
continue;
}
union {
NEWCPLINFOA NewCplInfoA;
NEWCPLINFOW NewCplInfoW;
} Newcpl;
HINSTANCE hLib; // Library Handle to *.cpl file
APPLET_PROC CplCall; // Pointer to CPlApplet() function
LONG i;
hLib = LoadLibrary((LPCTSTR) path.utf16());
if (hLib) {
CplCall=(APPLET_PROC)GetProcAddress(hLib,"CPlApplet");
if (CplCall) {
if (CplCall(NULL, CPL_INIT,0,0)) {
for (i=0;i<CplCall(NULL,CPL_GETCOUNT,0,0);i++)
{
Newcpl.NewCplInfoA.dwSize = 0;
Newcpl.NewCplInfoA.dwFlags = 0;
a = CplCall(NULL,CPL_INQUIRE,i,(long)&Newcpl);
if (Newcpl.NewCplInfoA.dwSize == sizeof(NEWCPLINFOW))
{
// Case #1, CPL_INQUIRE has returned an Unicode String
items->push_back(CatItem(path, QString::fromUtf16((const ushort*)Newcpl.NewCplInfoW.szName), 0, getIcon()));
cache[file] = QString::fromUtf16((const ushort*)Newcpl.NewCplInfoW.szName);
}
else
{
// Case #2, CPL_NEWINQUIRE has returned an ANSI String
if (Newcpl.NewCplInfoA.dwSize != sizeof(NEWCPLINFOA))
{
// Case #3, CPL_NEWINQUIRE failed to return a string
// Get the string from the *.cpl Resource instead
CPLINFO CInfo;
a = CplCall(NULL,CPL_INQUIRE,i,(long)&CInfo);
LoadStringA(hLib,CInfo.idName,
Newcpl.NewCplInfoA.szName,32);
}
// wchar_t tmpString[32];
// MultiByteToWideChar(CP_ACP, MB_PRECOMPOSED, Newcpl.NewCplInfoA.szName, 32, tmpString, 32);
items->push_back(CatItem(path, QString(Newcpl.NewCplInfoA.szName), 0, getIcon()));
cache[file] = QString(Newcpl.NewCplInfoA.szName);
}
} // for
CplCall(NULL,CPL_EXIT,0,0);
}
}
FreeLibrary(hLib);
}
}
void JointTrajectoryActionController::update()
{
ros::Time time = ros::Time::now();
std::vector<double> q(joints_.size()), qd(joints_.size()), qdd(joints_.size());
boost::shared_ptr<const SpecifiedTrajectory> traj_ptr = current_trajectory_;
if (!traj_ptr)
ROS_FATAL("The current trajectory can never be null");
// Only because this is what the code originally looked like.
const SpecifiedTrajectory &traj = *traj_ptr;
if (traj.size() == 0)
{
ROS_ERROR("No segments in the trajectory");
return;
}
// ------ Finds the current segment
// Determines which segment of the trajectory to use.
int seg = -1;
while (seg + 1 < (int)traj.size() && traj[seg + 1].start_time <= time.toSec())
{
++seg;
}
if (seg == -1)
{
// ROS_ERROR("No earlier segments. First segment starts at %.3lf (now = %.3lf)", traj[0].start_time, time.toSec());
// return;
seg = 0;
}
// ------ Trajectory Sampling
for (size_t i = 0; i < q.size(); ++i)
{
sampleSplineWithTimeBounds(traj[seg].splines[i].coef, traj[seg].duration, time.toSec() - traj[seg].start_time,
q[i], qd[i], qdd[i]);
}
// ------ Calculate error
std::vector<double> error(joints_.size());
for (size_t i = 0; i < joints_.size(); ++i)
{
error[i] = katana_->getMotorAngles()[i] - q[i];
}
// ------ State publishing
pr2_controllers_msgs::JointTrajectoryControllerState msg;
// Message containing current state for all controlled joints
for (size_t j = 0; j < joints_.size(); ++j)
msg.joint_names.push_back(joints_[j]);
msg.desired.positions.resize(joints_.size());
msg.desired.velocities.resize(joints_.size());
msg.desired.accelerations.resize(joints_.size());
msg.actual.positions.resize(joints_.size());
msg.actual.velocities.resize(joints_.size());
// ignoring accelerations
msg.error.positions.resize(joints_.size());
msg.error.velocities.resize(joints_.size());
// ignoring accelerations
msg.header.stamp = time;
for (size_t j = 0; j < joints_.size(); ++j)
{
msg.desired.positions[j] = q[j];
msg.desired.velocities[j] = qd[j];
msg.desired.accelerations[j] = qdd[j];
msg.actual.positions[j] = katana_->getMotorAngles()[j];
msg.actual.velocities[j] = katana_->getMotorVelocities()[j];
// ignoring accelerations
msg.error.positions[j] = error[j];
msg.error.velocities[j] = katana_->getMotorVelocities()[j] - qd[j];
// ignoring accelerations
}
controller_state_publisher_.publish(msg);
// TODO: here we could publish feedback for the FollowJointTrajectory action; however,
// this seems to be optional (the PR2's joint_trajectory_action_controller doesn't do it either)
}
void
ckbot::chain_rate::tip_base_step(std::vector<double> s, std::vector<double> T)
{
int N = c.num_links();
std::vector<double> q(N);
std::vector<double> qd(N);
for(int i=0; i<N; ++i)
{
q[i] = s[i];
qd[i] = s[N+i];
}
Eigen::Matrix3d Iden = Eigen::Matrix3d::Identity();
Eigen::Matrix3d Zero = Eigen::Matrix3d::Zero();
/* Declare and initialized all of the loop variables */
/* TODO: Can we allocate all of this on the heap *once* for a particular
* rate object, and then just use pointers to them after? Would this be fast?
* Re-initializing these every time through here has to be slow...
*/
Eigen::MatrixXd pp(6,6);
pp = Eigen::MatrixXd::Zero(6,6);
Eigen::VectorXd zp(6);
zp << 0,0,0,0,0,0;
Eigen::VectorXd grav(6);
grav << 0,0,0,0,0,9.81;
Eigen::Matrix3d L_oc_tilde;
Eigen::Matrix3d R_cur;
Eigen::MatrixXd M_cur(6,6);
Eigen::MatrixXd M_cm(6,6);
M_cm = Eigen::MatrixXd::Zero(6,6);
Eigen::Matrix3d J_o;
Eigen::Vector3d r_i_ip(0,0,0);
Eigen::Vector3d r_i_cm(0,0,0);
Eigen::MatrixXd phi(6,6);
Eigen::MatrixXd phi_cm(6,6);
Eigen::MatrixXd p_cur(6,6);
Eigen::VectorXd H_b_frame_star(6);
Eigen::VectorXd H_w_frame_star(6);
Eigen::RowVectorXd H(6);
double D = 0.0;
Eigen::VectorXd G(6);
Eigen::MatrixXd tau_tilde(6,6);
Eigen::Vector3d omega(0,0,0);
Eigen::Matrix3d omega_cross;
Eigen::VectorXd a(6);
Eigen::VectorXd b(6);
Eigen::VectorXd z(6);
double C = 0.0;
double CF = 0.0;
double SPOLES = 12.0;
double RPOLES = 14.0;
double epsilon = 0.0;
double mu = 0.0;
/* Recurse from the tip to the base of this chain */
for (int i = N-1; i >= 0; --i)
{
module_link& cur = c.get_link(i);
/* Rotation from the world to this link */
R_cur = c.get_current_R(i);
/* Vector, in world frame, from inbound joint to outbound joint of
* this link
*/
r_i_ip = R_cur*(cur.get_r_ip1() - cur.get_r_im1());
/* Operator to transform spatial vectors from outbound joint
* to the inbound joint
*/
phi = get_body_trans(r_i_ip);
/* Vector from the inbound joint to the center of mass */
r_i_cm = R_cur*(-cur.get_r_im1());
/* Operator to transform spatial vectors from the center of mass
* to the inbound joint
*/
phi_cm = get_body_trans(r_i_cm);
/* Cross product matrix corresponding to the vector from
* the inbound joint to the center of mass
*/
L_oc_tilde = get_cross_mat(r_i_cm);
/* 3x3 Inertia matrix of this link about its inbound joint and wrt the
* world coordinate system.
*
* NOTE: Similarity transform of get_I_cm() because it is wrt
* the module frame
*/
J_o = R_cur*cur.get_I_cm()*R_cur.transpose() - cur.get_mass()*L_oc_tilde*L_oc_tilde;
/* Spatial mass matrix of this link about its inbound joint and wrt the
* world coordinate system.
*
*/
M_cur << J_o, cur.get_mass()*L_oc_tilde,
-cur.get_mass()*L_oc_tilde, cur.get_mass()*Iden;
/* Spatial mass matrix of this link about its cm and wrt the
* world coordinate system.
*
* NOTE: Similarity transform of get_I_cm() because it is wrt
* the module frame
*/
M_cm << R_cur*cur.get_I_cm()*R_cur.transpose(), Zero,
Zero, cur.get_mass()*Iden;
/* Articulated Body Spatial inertia matrix of the links from
* this one all the way to the tip of the chain (accounting for
* articulated body spatial inertia that gets through each joint)
*
* pp is p_cur from the previous (outbound) link. This is the
* zero vector when we are on the farthest outbound link (the tip)
*
* In much the same way that we use a similarity transform to change
* the frame of the inertia matrix, a similarity transform moves
* pp from the previous module's base joint to this module's base joint.
*/
p_cur = phi*pp*phi.transpose() + M_cur;
/* The joint matrix, in the body frame, that maps the change
* in general coordinates relating this link to the next inward link.
*
* It essentially defines what "gets through" a link. IE:
* H_b_frame_star = [0,0,1,0,0,0] means that movements and forces
* in the joint's rotational Z axis get through. This is a single
* DOF rotational joint.
*
* H_b_frame_star = eye(6) means that forces in all 6
* (3 rotational, 3 linear) get through the joint using
* 6 independent general coordinates. This is a
* free 6DOF joint.
*
* H_b_frame_star = [0,0,2,0,0,1] is helical joint that rotates twice
* for every single linear unit moved. It is controlled by a single
* general coordinate.
*
* (NOTE/TODO: Using anything but [0,0,1,0,0,0] breaks the code right now.)
*/
H_b_frame_star = cur.get_joint_matrix().transpose();
/* To transform a spatial vector (6 x 1) from one coordinate
* system to another, multiply it by the 6x6 matrix with
* the rotation matrix form one frame to the other on the diagonals
*/
Eigen::MatrixXd tmp_6x6(6,6);
tmp_6x6 << R_cur, Eigen::Matrix3d::Zero(),
Eigen::Matrix3d::Zero(), R_cur;
/* Joint matrix in the world frame */
H_w_frame_star = tmp_6x6*H_b_frame_star;
/* As a row vector */
H = H_w_frame_star.transpose();
D = H*p_cur*H.transpose(); /* TODO:Could use H_w_frame_star..but..clarity */
/* TODO: This needs to be changed to D.inverse() for 6DOF
* and D needs to be made a variable sized Eigen Matrix
*/
G = p_cur*H.transpose()*(1.0/D);
/* Articulated body projection operator through this joint. This defines
* which part of the articulated body spatial inertia gets through this joint
*/
tau_tilde = Eigen::MatrixXd::Identity(6,6) - G*H;
/* pp for the next time around the loop */
pp = tau_tilde*p_cur;
omega = c.get_angular_velocity(i);
omega_cross = get_cross_mat(omega);
/* Gyroscopic force */
b << omega_cross*J_o*omega,
cur.get_mass()*omega_cross*omega_cross*r_i_cm;
/* Coriolis and centripetal accel */
a << 0,0,0,
omega_cross*omega_cross*r_i_cm;
/* z is the total spatial force seen by this link at its inward joint
* phi*zp = Force through joint from tip-side module
* b = Gyroscopic force (velocity dependent)
* p_cur*a = Coriolis and Centripetal forces
* phi_cm*M_cm*grav = Force of grav at CM transformed to inbound jt
*/
z = phi*zp + b + p_cur*a + phi_cm*M_cm*grav;
/* Velocity related damping force in the joint */
/* TODO: 6DOF change. epsilon will be an matrix when the joint
* matrix is not a row matrix, so using C in the calculation
* of epsilon will break things.
*/
C = -cur.get_damping()*qd[i];
/* Cogging force which is a function of the joint angle and
* the motor specifics.
*/
CF = cur.get_rotor_cogging()*sin(RPOLES*(q[i]-cur.get_cogging_offset()))
+ cur.get_stator_cogging()*sin(SPOLES*(q[i]-cur.get_cogging_offset()));
/* The "Effective" force through the joint that can actually create
* accelerations. IE: Forces that are in directions in which the joint
* can actually move.
*/
/* TODO: 6DOF change. Epsilon will not be 1x1 for a 6DOF joint, so
* both T[i] and C used here as they are will break things.
*/
epsilon = T[i] + C + CF - H*z;
mu = (1/D)*epsilon; /* 6DOF change: D will be a matrix, need to invert it */
zp = z + G*epsilon;
/* base_tip_step needs G, a, and mu. So store them in the chain object.*/
mu_all[i] = mu;
int cur_index=0;
for (int k=(6*i); k <= (6*(i+1)-1); ++k,++cur_index)
{
G_all[k] = G[cur_index];
a_all[k] = a[cur_index];
}
}
}
void CatalogBuilder::indexDirectory(const QString& directory, const QStringList& filters, bool fdirs, bool fbin, int depth)
{
QString dir = QDir::toNativeSeparators(directory);
QDir qd(dir);
dir = qd.absolutePath();
QStringList dirs = qd.entryList(QDir::AllDirs);
if (depth > 0)
{
for (int i = 0; i < dirs.count(); ++i)
{
if (!dirs[i].startsWith("."))
{
QString cur = dirs[i];
if (!cur.contains(".lnk"))
{
#ifdef Q_WS_MAC
// Special handling of app directories
if (cur.endsWith(".app", Qt::CaseInsensitive)) {
CatItem item(dir + "/" + cur);
platform->alterItem(&item);
catalog->addItem(item);
}
else
#endif
// Sleep shoud be here :)
CanIHazSleep::msleep(10);
indexDirectory(dir + "/" + dirs[i], filters, fdirs, fbin, depth-1);
}
}
}
}
if (fdirs)
{
for (int i = 0; i < dirs.count(); ++i)
{
if (!dirs[i].startsWith(".") && !indexed.contains(dir + "/" + dirs[i]))
{
bool isShortcut = dirs[i].endsWith(".lnk", Qt::CaseInsensitive);
CatItem item(dir + "/" + dirs[i], !isShortcut);
catalog->addItem(item);
indexed.insert(dir + "/" + dirs[i]);
}
}
}
else
{
// Grab any shortcut directories
// This is to work around a QT weirdness that treats shortcuts to directories as actual directories
for (int i = 0; i < dirs.count(); ++i)
{
if (!dirs[i].startsWith(".") && dirs[i].endsWith(".lnk",Qt::CaseInsensitive))
{
if (!indexed.contains(dir + "/" + dirs[i]))
{
CatItem item(dir + "/" + dirs[i], true);
catalog->addItem(item);
indexed.insert(dir + "/" + dirs[i]);
}
}
}
}
if (fbin)
{
QStringList bins = qd.entryList(QDir::Files | QDir::Executable);
for (int i = 0; i < bins.count(); ++i)
{
if (!indexed.contains(dir + "/" + bins[i]))
{
CatItem item(dir + "/" + bins[i]);
catalog->addItem(item);
indexed.insert(dir + "/" + bins[i]);
}
}
}
// Don't want a null file filter, that matches everything..
if (filters.count() == 0)
return;
QStringList files = qd.entryList(filters, QDir::Files | QDir::System, QDir::Unsorted );
for (int i = 0; i < files.count(); ++i)
{
if (!indexed.contains(dir + "/" + files[i]))
{
CatItem item(dir + "/" + files[i]);
platform->alterItem(&item);
#ifdef Q_WS_X11
if(item.fullPath.endsWith(".desktop") && item.icon == "")
continue;
#endif
catalog->addItem(item);
indexed.insert(dir + "/" + files[i]);
}
}
}
/* Returns a vector of length N corresponding to each link's qdd */
std::vector<double>
ckbot::chain_rate::base_tip_step(std::vector<double> s, std::vector<double> T)
{
int N = c.num_links();
std::vector<double> q(N);
std::vector<double> qd(N);
std::vector<double> qdd(N);
for(int i=0; i<N; ++i)
{
q[i] = s[i];
qd[i] = s[N+i];
}
Eigen::VectorXd grav(6);
grav << 0,0,0,0,0,9.81;
Eigen::Matrix3d R_cur;
Eigen::Vector3d r_i_ip;
Eigen::MatrixXd phi(6,6);
Eigen::VectorXd alpha(6);
alpha = Eigen::VectorXd::Zero(6);
Eigen::VectorXd alpha_p(6);
Eigen::VectorXd G(6);
Eigen::VectorXd a(6);
Eigen::VectorXd H_b_frame_star(6);
Eigen::VectorXd H_w_frame_star(6);
Eigen::RowVectorXd H(6);
for (int i=0; i<N; ++i)
{
module_link& cur = c.get_link(i);
/* 3x3 Rotation matrix from this link's coordinate
* frame to the world frame
*/
R_cur = c.get_current_R(i);
/* Vector, in world frame, from this link's inbound joint to its
* outbound joint
*/
r_i_ip = R_cur*(cur.get_r_ip1() - cur.get_r_im1());
/* 6x6 Spatial body operator matrix that transforms spatial
* vectors from the outbound joint to the inbound joint in
* the world frame
*/
phi = get_body_trans(r_i_ip);
/* Transform the spatial accel vector from the inbound
* link's inbound joint to this link's inbound joint
*/
alpha_p = phi.transpose()*alpha;
/* These are calculated in tip_base_step */
int cur_index = 0;
for (int k = (6*i); k <= (6*(i+1)-1); ++k, ++cur_index)
{
G[cur_index] = G_all[k];
a[cur_index] = a_all[k];
}
/* The angular acceleration of this link's joint */
/* TODO: 6DOF changes here...This gets a whoooole lotta broken */
qdd[i] = mu_all[i] - G.transpose()*alpha_p;
/* Joint matrix in body frame. Check tip_base_step for the definition */
/* TODO: 6DOF changes here a plenty */
H_b_frame_star = cur.get_joint_matrix().transpose();
Eigen::MatrixXd tmp_6x6(6,6);
tmp_6x6 << R_cur, Eigen::Matrix3d::Zero(),
Eigen::Matrix3d::Zero(), R_cur;
/* Joint matrix in the world frame */
H_w_frame_star = tmp_6x6*H_b_frame_star;
/* As a row vector */
H = H_w_frame_star.transpose();
/* Spatial acceleration of this link at its inbound joint in
* its frame (ie: including its joint accel)
*/
alpha = alpha_p + H.transpose()*qdd[i] + a;
}
return qdd;
}
void KukaKDL::setJointVelocity(std::vector<double> &qd_des){
outdate();
for(unsigned int i=0; i<tree.getNrOfJoints(); i++){
qd(i) = qd_des[i];
}
}
void ProcessManage::showTabInfo()
{
QString tempStr;
QFile tempFile;
int pos;
// if(index==0)
// {
ui->listWidget->clear();
QDir qd("/proc");
QStringList qsList=qd.entryList();
QString qs=qsList.join("\n");
QString id_of_pro;
bool ok;
int find_start=3;
int a,b;
int pid;
QString pName;
QString pState;
QString proPri;
QString proMem;
QListWidgetItem*title=new QListWidgetItem("PID\t"+QString::fromUtf8("名称") + "\t\t"+
QString::fromUtf8("状态") + "\t" +
QString::fromUtf8("优先级") + "\t" +
QString::fromUtf8("占用内存"), ui->listWidget);
while(1)
{
a=qs.indexOf("\n",find_start);
b=qs.indexOf("\n",a+1);
find_start=b;
id_of_pro=qs.mid(a+1,b-a-1);
pid=id_of_pro.toInt(&ok,10);
if(!ok)
{
break;
}
tempFile.setFileName("/proc/" + id_of_pro + "/stat");
if ( !tempFile.open(QIODevice::ReadOnly) )
{
QMessageBox::warning(this, tr("warning"), tr("The pid stat file can not open!"), QMessageBox::Yes);
return;
}
tempStr = tempFile.readLine();
if (tempStr.length() == 0)
{
break;
}
a = tempStr.indexOf("(");
b = tempStr.indexOf(")");
pName = tempStr.mid(a+1, b-a-1);
pName.trimmed();
pState = tempStr.section(" ", 2, 2);
proPri = tempStr.section(" ", 17, 17);
proMem = tempStr.section(" ", 22, 22);
if (pName.length() >= 12)
{
QListWidgetItem *item = new QListWidgetItem(id_of_pro + "\t" +
pName + "\t" +
pState + "\t" +
proPri + "\t" +
proMem, ui->listWidget);
}
else
{
QListWidgetItem *item = new QListWidgetItem(id_of_pro + "\t" +
pName + "\t\t" +
pState + "\t" +
proPri + "\t" +
proMem, ui->listWidget);
}
}
tempFile.close(); //关闭该PID进程的状态文件
// }
}
//This code was used to generate the actions array seen in the actions.h file.
int main() {
std::ofstream fixFile ("fix.txt");
for(int i=0; i<336;i++) {
fixFile << "int actions" << i << "[] " << "=" << " {";
if(i%21 == 0) {
if (i-21 >=0) {
fixFile << (i-21);
} else {
fixFile << "-1";
}
if (i-20 >=0) {
fixFile << "," << (i-20);
} else {
fixFile << ",-1";
}
if (i+1 <336) {
fixFile << "," << (i+1);
} else {
fixFile << ",-1";
}
if (i+22 <336) {
fixFile << "," << (i+22);
} else {
fixFile << ",-1";
}
if (i+21 <336) {
fixFile << "," << (i+21);
} else {
fixFile << ",-1";
}
fixFile << ",-1,-1,-1";
} else if(i%21==20) {
if (i-21 >=0) {
fixFile << (i-21);
} else {
fixFile << "-1";
}
if (i-22 >=0) {
fixFile << "," << (i-22);
} else {
fixFile << ",-1";
}
if (i-1 >0) {
fixFile << "," << (i-1);
} else {
fixFile << ",-1";
}
if (i+20 <336) {
fixFile << "," << (i+20);
} else {
fixFile << ",-1";
}
if (i+21 <336) {
fixFile << "," << (i+21);
} else {
fixFile << ",-1";
}
fixFile << ",-1,-1,-1";
} else {
if (i-22 >=0) {
fixFile << (i-22);
} else {
fixFile << "-1";
}
if (i-21 >=0) {
fixFile << "," << (i-21);
} else {
fixFile << ",-1";
}
if (i-20 >=0) {
fixFile << "," << (i-20);
} else {
fixFile << ",-1";
}
if (i-1 >=0) {
fixFile << "," << (i-1);
} else {
fixFile << ",-1";
}
if (i+1 <336) {
fixFile << "," << (i+1);
} else {
fixFile << ",-1";
}
if (i+22 <336) {
fixFile << "," << (i+22);
} else {
fixFile << ",-1";
}
if (i+21 <336) {
fixFile << "," << (i+21);
} else {
fixFile << ",-1";
}
if (i+20 <336) {
fixFile << "," << (i+20);
} else {
fixFile << ",-1";
}
}
fixFile << "};\n";
}
fixFile.close();
std::ofstream qd ("qDrop.txt");
std::ofstream qp ("qPing.txt");
std::ofstream q ("q.txt");
for(int i=0; i<(336*336);i++) {
qd << "0\n";
qp << "0\n";
q << "0\n";
}
qd.close();
qp.close();
}
int
main(int argc, char** argv)
{
if (argc < 2)
{
std::cout << "Usage: rlDynamics2Demo MODELFILE Q1 ... Qn QD1 ... QDn QDD1 ... QDDn" << std::endl;
return 1;
}
try
{
rl::mdl::XmlFactory factory;
boost::shared_ptr< rl::mdl::Model > model(factory.create(argv[1]));
rl::mdl::Dynamic* dynamic = dynamic_cast< rl::mdl::Dynamic* >(model.get());
rl::math::Vector q(dynamic->getDof());
rl::math::Vector qd(dynamic->getDof());
rl::math::Vector qdd(dynamic->getDof());
rl::math::Vector tau(dynamic->getDof());
for (std::size_t i = 0; i < dynamic->getDof(); ++i)
{
q(i) = boost::lexical_cast< rl::math::Real >(argv[i + 2]);
qd(i) = boost::lexical_cast< rl::math::Real >(argv[i + 2 + dynamic->getDof()]);
qdd(i) = boost::lexical_cast< rl::math::Real >(argv[i + 2 + 2 * dynamic->getDof()]);
}
rl::math::Vector g(3);
dynamic->getWorldGravity(g);
rl::math::Vector tmp(dynamic->getDof());
rl::math::Vector tmp2(6 * dynamic->getOperationalDof());
std::cout << "===============================================================================" << std::endl;
// FP
dynamic->setPosition(q);
dynamic->forwardPosition();
const rl::math::Transform::ConstTranslationPart& position = dynamic->getOperationalPosition(0).translation();
rl::math::Vector3 orientation = dynamic->getOperationalPosition(0).rotation().eulerAngles(2, 1, 0).reverse();
std::cout << "x = " << position.x() << " m, y = " << position.y() << " m, z = " << position.z() << " m, a = " << orientation.x() * rl::math::RAD2DEG << " deg, b = " << orientation.y() * rl::math::RAD2DEG << " deg, c = " << orientation.z() * rl::math::RAD2DEG << " deg" << std::endl;
std::cout << "===============================================================================" << std::endl;
// FV
dynamic->setPosition(q);
dynamic->setVelocity(qd);
dynamic->forwardVelocity();
std::cout << "xd = " << dynamic->getOperationalVelocity(0).linear().transpose() << " " << dynamic->getOperationalVelocity(0).angular().transpose() << std::endl;
std::cout << "-------------------------------------------------------------------------------" << std::endl;
// J
rl::math::Matrix J(6 * dynamic->getOperationalDof(), dynamic->getDof());
dynamic->calculateJacobian(J);
std::cout << "J = " << std::endl << J << std::endl;
// J * qd
tmp2 = J * qd;
std::cout << "xd = " << tmp2.transpose() << std::endl;
// invJ
rl::math::Matrix invJ(dynamic->getDof(), 6 * dynamic->getOperationalDof());
dynamic->calculateJacobianInverse(J, invJ);
std::cout << "J^{-1} = " << std::endl << invJ << std::endl;
std::cout << "===============================================================================" << std::endl;
// FA
dynamic->setPosition(q);
dynamic->setVelocity(qd);
dynamic->setAcceleration(qdd);
dynamic->setWorldGravity(g);
dynamic->forwardVelocity();
dynamic->forwardAcceleration();
std::cout << "xdd = " << dynamic->getOperationalAcceleration(0).linear().transpose() << " " << dynamic->getOperationalAcceleration(0).angular().transpose() << std::endl;
std::cout << "-------------------------------------------------------------------------------" << std::endl;
// Jd * qd
rl::math::Vector Jdqd(6 * dynamic->getOperationalDof());
dynamic->calculateJacobianDerivative(Jdqd);
std::cout << "Jd*qd = " << Jdqd.transpose() << std::endl;
// J * qdd + Jd * qd
tmp2 = J * qdd + Jdqd;
std::cout << "xdd = " << tmp2.transpose() << std::endl;
std::cout << "===============================================================================" << std::endl;
// RNE
dynamic->setPosition(q);
dynamic->setVelocity(qd);
dynamic->setAcceleration(qdd);
dynamic->inverseDynamics();
dynamic->getTorque(tau);
std::cout << "tau = " << tau.transpose() << std::endl;
std::cout << "-------------------------------------------------------------------------------" << std::endl;
// M
rl::math::Matrix M(dynamic->getDof(), dynamic->getDof());
dynamic->setPosition(q);
dynamic->calculateMassMatrix(M);
std::cout << "M = " << std::endl << M << std::endl;
// V
rl::math::Vector V(dynamic->getDof());
dynamic->setPosition(q);
dynamic->setVelocity(qd);
dynamic->calculateCentrifugalCoriolis(V);
std::cout << "V = " << V.transpose() << std::endl;
// G
rl::math::Vector G(dynamic->getDof());
dynamic->setPosition(q);
dynamic->setWorldGravity(g);
dynamic->calculateGravity(G);
std::cout << "G = " << G.transpose() << std::endl;
// M * qdd + V + G
tmp = M * qdd + V + G;
std::cout << "tau = " << tmp.transpose() << std::endl;
std::cout << "===============================================================================" << std::endl;
// FD
dynamic->setPosition(q);
dynamic->setVelocity(qd);
dynamic->setTorque(tau);
dynamic->forwardDynamics();
dynamic->getAcceleration(tmp);
std::cout << "qdd = " << tmp.transpose() << std::endl;
std::cout << "-------------------------------------------------------------------------------" << std::endl;
// M^{-1}
rl::math::Matrix invM(dynamic->getDof(), dynamic->getDof());
dynamic->setPosition(q);
dynamic->calculateMassMatrixInverse(invM);
std::cout << "M^{-1} = " << std::endl << invM << std::endl;
// V
std::cout << "V = " << V.transpose() << std::endl;
// G
std::cout << "G = " << G.transpose() << std::endl;
// M^{-1} * ( tau - V - G )
tmp = invM * (tau - V - G);
std::cout << "qdd = " << tmp.transpose() << std::endl;
std::cout << "===============================================================================" << std::endl;
}
catch (const std::exception& e)
{
std::cout << e.what() << std::endl;
return 1;
}
return 0;
}
void middle6::show_tabWidgetInfo(int index)
{
if(index == 0)
{
connect(pushButton_processkill2, SIGNAL(clicked()), this, SLOT(on_pushButton_pkill_clicked2()));
thread1->start("bash ps-amaz.sh");
thread2->start("bash ps-firfo.sh");
thread3->start("bash ps-gogl.sh");
thread4->start("bash ps-qc.sh");
thread5->start("bash ps-ql.sh");
thread6->start("bash ps-soft-cent.sh");
thread7->start("bash ps-term.sh");
thread8->start("bash ps-wp.sh");
thread9->start("bash ps-evi.sh");
if(!thread1->waitForStarted())
{
qDebug()<<"failed start!";
}
if(!thread2->waitForStarted())
{
qDebug()<<"failed start!";
}
if(!thread3->waitForStarted())
{
qDebug()<<"failed start!";
}
if(!thread4->waitForStarted())
{
qDebug()<<"failed start!";
}
if(!thread5->waitForStarted())
{
qDebug()<<"failed start!";
}
if(!thread6->waitForStarted())
{
qDebug()<<"failed start!";
}
if(!thread7->waitForStarted())
{
qDebug()<<"failed start!";
}
if(!thread8->waitForStarted())
{
qDebug()<<"failed start!";
}
if(!thread9->waitForStarted())
{
qDebug()<<"failed start!";
}
while(true == thread1->waitForFinished());
{
bool ok1;
QString ver1 = thread1->readAllStandardOutput();
int dec1 = ver1.toInt(&ok1,10);
if(dec1 > 1 && flag1 == 0)
{
item1 = new QListWidgetItem(listWidget2);
flag1 = 1;
item1->setIcon(QIcon(":images/amazon.png"));
item1->setText("亚马逊 正在运行中...");
listWidget2->addItem(item1);
}
if(dec1 < 2 && flag1 == 1)
{
listWidget2->removeItemWidget(item1);
delete item1;
flag1 = 0;
dec1 = 0;
}
}
while(true == thread2->waitForFinished());
{
bool ok2;
QString ver2 = thread2->readAllStandardOutput();
int dec2 = ver2.toInt(&ok2,10);
if(dec2 >1 && flag2 == 0)
{
item2 = new QListWidgetItem;
flag2 = 1;
item2->setIcon(QIcon(":images/firefox.png"));
item2->setText("火狐浏览器 正在运行中...");
listWidget2->addItem(item2);
}
if(dec2 < 2 && flag2 == 1)
{
listWidget2->removeItemWidget(item2);
delete item2;
flag2 = 0;
}
}
while(true == thread3->waitForFinished());
{
bool ok3;
QString ver3= thread3->readAllStandardOutput();
int dec3 = ver3.toInt(&ok3,10);
if(dec3 > 2 && flag3 == 0)
{
item3 = new QListWidgetItem;
flag3 = 1;
item3->setIcon(QIcon(":images/google.png"));
item3->setText("谷歌浏览器 正在运行中...");
listWidget2->addItem(item3);
}
if(dec3 < 3 && flag3 == 1)
{
listWidget2->removeItemWidget(item3);
delete item3;
dec3 = 0;
flag3 = 0;
}
}
while(true == thread4->waitForFinished());
{
bool ok4;
QString ver4 = thread4->readAllStandardOutput();
int dec4 = ver4.toInt(&ok4,10);
if(dec4 > 2 && flag4 == 0)
{
item4 = new QListWidgetItem;
flag4 = 1;
item4->setIcon(QIcon(":/images/qt.png"));
item4->setText("Qt Creater 正在运行中...");
listWidget2->addItem(item4);
}
if(dec4 < 2 && flag4 == 1)
{
listWidget2->removeItemWidget(item4);
delete item4;
dec4 = 0;
flag4 = 0;
}
}
while(true == thread5->waitForFinished());
{
bool ok5;
QString ver5 = thread5->readAllStandardOutput();
int dec5 = ver5.toInt(&ok5,10);
// qDebug()<< dec5;
if(dec5> 2 && flag5 == 0)
{
item5 = new QListWidgetItem;
flag5 = 1;
item5->setIcon(QIcon(":/images/qq.png"));
item5->setText("qq 正在运行中...");
listWidget2->addItem(item5);
}
if(dec5 < 3 && flag5 == 1)
{
listWidget2->removeItemWidget(item5);
delete item5;
flag5 = 0;
}
}
while(true == thread6->waitForFinished());
{
bool ok6;
QString ver6 = thread6->readAllStandardOutput();
int dec6 = ver6.toInt(&ok6,10);
if(dec6> 1 && flag6 == 0)
{
item6 = new QListWidgetItem;
flag6 = 1;
item6->setIcon(QIcon(":/images/software-center.png"));
item6->setText("ubuntu软件中心 正在运行中...");
listWidget2->addItem(item6);
}
if(dec6 < 2 && flag6 == 1)
{
listWidget2->removeItemWidget(item6);
delete item6;
flag6 = 0;
}
}
while(true == thread7->waitForFinished());
{
bool ok7;
QString ver7 = thread7->readAllStandardOutput();
int dec7 = ver7.toInt(&ok7,10);
if(dec7> 1 && flag7 == 0)
{
item7 = new QListWidgetItem;
flag7 = 1;
item7->setIcon(QIcon(":/images/terminal.png"));
item7->setText("终端 正在运行中...");
listWidget2->addItem(item7);
}
if(dec7 < 2 && flag7 == 1)
{
listWidget2->removeItemWidget(item7);
delete item7;
flag7 = 0;
}
}
while(true == thread8->waitForFinished());
{
bool ok8;
QString ver8 = thread8->readAllStandardOutput();
int dec8 = ver8.toInt(&ok8,10);
if(dec8> 1 && flag8 == 0)
{
item8 = new QListWidgetItem;
flag8 = 1;
item8->setIcon(QIcon(":/images/wps.png"));
item8->setText("wps文本编辑器 正在运行中...");
listWidget2->addItem(item8);
}
if(dec8 < 2 && flag8 == 1)
{
listWidget2->removeItemWidget(item8);
delete item8;
flag8 = 0;
}
}
while(true == thread9->waitForFinished());
{
bool ok9;
QString ver9 = thread9->readAllStandardOutput();
int dec9 = ver9.toInt(&ok9,10);
if(dec9> 1 && flag9 == 0)
{
item9 = new QListWidgetItem;
flag9 = 1;
item9->setIcon(QIcon(":/images/evnice.png"));
item9->setText("pdf文本编辑器 正在运行中...");
listWidget2->addItem(item9);
}
if(dec9 < 2 && flag9 == 1)
{
listWidget2->removeItemWidget(item9);
delete item9;
flag9 = 0;
}
}
}
if(index == 1)
{
QString tempStr; //读取文件信息字符串
QFile tempFile; //用于打开系统文件
// int pos; //读取文件的位置
this->listWidget->clear();
connect(this->pushButton_processfresh, SIGNAL(clicked()), this, SLOT(on_pushButton_prefresh_clicked()));
connect(this->pushButton_processkill, SIGNAL(clicked()), this, SLOT(on_pushButton_pkill_clicked()));
QDir qd("/proc");
QStringList qsList = qd.entryList();
QString qs = qsList.join("\n");
QString id_of_pro;
bool ok;
int find_start = 3;
int a, b;
int nProPid; //进程PID
int totalProNum = 0; //进程总数
QString proName; //进程名
QString proState; //进程状态
QString proPri; //进程优先级
QString proMem; //进程占用内存
QListWidgetItem *title = new QListWidgetItem("PID\t" + QString::fromUtf8("名称") + "\t\t" +
QString::fromUtf8("状态") + "\t" +
QString::fromUtf8("优先级") + "\t" +
QString::fromUtf8("占用内存"), this->listWidget);
//循环读取进程
while (1)
{
//获取进程PID
a = qs.indexOf("\n", find_start);
b = qs.indexOf("\n", a+1);
find_start = b;
id_of_pro = qs.mid(a+1, b-a-1);
totalProNum++;
nProPid = id_of_pro.toInt(&ok, 10);
if(!ok)
{
break;
}
//打开PID所对应的进程状态文件
tempFile.setFileName("/proc/" + id_of_pro + "/stat");
if ( !tempFile.open(QIODevice::ReadOnly) )
{
// QMessageBox::warning(this, tr("warning"), tr("The pid stat file can not open!"), QMessageBox::Yes);
return;
}
tempStr = tempFile.readLine();
if (tempStr.length() == 0)
{
break;
}
a = tempStr.indexOf("(");
b = tempStr.indexOf(")");
proName = tempStr.mid(a+1, b-a-1);
proName.trimmed(); //删除两端的空格
proState = tempStr.section(" ", 2, 2);
proPri = tempStr.section(" ", 17, 17);
proMem = tempStr.section(" ", 22, 22);
if (proName.length() >= 12)
{
QListWidgetItem *item = new QListWidgetItem(id_of_pro + "\t" +
proName + "\t" +
proState + "\t" +
proPri + "\t" +
proMem, this->listWidget);
}
else
{
QListWidgetItem *item = new QListWidgetItem(id_of_pro + "\t" +
proName + "\t\t" +
proState + "\t" +
proPri + "\t" +
proMem, this->listWidget);
}
}
tempFile.close(); //关闭该PID进程的状态文件
}
}
void KukaKDL::computeFrictionTorque(){
frictionTorque.data << FV1*qd(0) + FS1*sign(qd(0)),
FV2*qd(1) + FS2*sign(qd(1)),
FV3*qd(2) + FS3*sign(qd(2)),
FV4*qd(3) + FS4*sign(qd(3)),
FV5*qd(4) + FS5*sign(qd(4)),
FV6*qd(5) + FS6*sign(qd(5)),
FV7*qd(6) + FS7*sign(qd(6));
}
int main(int argc, char** argv) {
/*
Eigen::Vector4d mean, x;
KDL::Rotation rot;
rot.GetQuaternion(mean(0), mean(1), mean(2), mean(3));
double pdf_mean;
double sigma = 10.0/180.0*PI;
double Cp = misesFisherKernelConstant(sigma, 4);
pdf_mean = misesFisherKernel(mean, mean, sigma, Cp);
misesFisherKernel(x, mean, sigma, Cp);
std::cout << "pdf_mean: " << pdf_mean << std::endl;
for (double angle = 0.0; angle < 180.0/180 * PI; angle += 5.0/180.0*PI) {
KDL::Rotation::RotX(angle).GetQuaternion(x(0), x(1), x(2), x(3));
std::cout << (angle / PI * 180.0) << " " << misesFisherKernel(x, mean, sigma, Cp) << std::endl;
}
return 0;
*/
if (argc != 4) {
std::cout << "usage:" << std::endl;
std::cout << argv[0] << " object_model collision_model output_file" << std::endl;
return 0;
}
srand ( time(NULL) );
const double sigma_p = 0.01;//05;
const double sigma_q = 15.0/180.0*PI;//100.0;
int m_id = 101;
// generate object model
boost::shared_ptr<ObjectModel > om = ObjectModel::readFromXml(argv[1]);
std::cout << "om.getPointFeatures().size(): " << om->getPointFeatures().size() << std::endl;
// generate collision model
std::map<std::string, std::list<std::pair<int, double> > > link_pt_map;
boost::shared_ptr<CollisionModel > cm = CollisionModel::readFromXml(argv[2]);
boost::shared_ptr<HandConfigurationModel > hm = HandConfigurationModel::readFromXml(argv[2]);
// m_id = visualiseAllFeatures(markers_pub, m_id, om->getPointFeatures(), ob_name);
// m_id = visualiseRejectionSamplingVonMisesFisher3(markers_pub, m_id);
// m_id = visualiseRejectionSamplingVonMisesFisher4(markers_pub, m_id);
std::random_device rd;
std::mt19937 gen(rd());
boost::shared_ptr<QueryDensity > qd(new QueryDensity);
// om->setSamplerParameters(sigma_p, sigma_q, sigma_r);
qd->setSamplerParameters(sigma_p, sigma_q);
// generate query density using multiple threads
{
const int qd_sample_count = om->getPointFeatures().size()*10;//50000;
const int num_threads = cm->getLinkNamesCol().size();
std::unique_ptr<std::thread[] > t(new std::thread[num_threads]);
std::unique_ptr<std::vector<QueryDensity::QueryDensityElement >[] > qd_vec(new std::vector<QueryDensity::QueryDensityElement >[num_threads]);
for (int thread_id = 0; thread_id < num_threads; thread_id++) {
qd_vec[thread_id].resize(qd_sample_count);
std::vector<QueryDensity::QueryDensityElement > aaa;
t[thread_id] = std::thread(generateQueryDensity, gen(), std::cref(cm->getLinkNamesCol()[thread_id]), std::cref(cm), std::cref((*om)), std::ref(qd_vec[thread_id]));
}
for (int thread_id = 0; thread_id < num_threads; ++thread_id) {
t[thread_id].join();
qd->addQueryDensity(cm->getLinkNamesCol()[thread_id], qd_vec[thread_id]);
}
}
writeToXml(argv[3], qd, cm, hm, om);
return 0;
}
void mexFunction(int nlhs, mxArray *plhs[], int nrhs, const mxArray *prhs[])
{
if (nrhs<1) mexErrMsgTxt("usage: ptr = bodyMotionControlmex(0,robot_obj,Kp,Kd,body_index); y=bodyMotionControlmex(ptr,x,body_pose_des,body_v_des,body_vdot_des)");
if (nlhs<1) mexErrMsgTxt("take at least one output... please.");
struct BodyMotionControlData* pdata;
if (mxGetScalar(prhs[0])==0) { // then construct the data object and return
pdata = new struct BodyMotionControlData;
// get robot mex model ptr
if (!mxIsNumeric(prhs[1]) || mxGetNumberOfElements(prhs[1])!=1)
mexErrMsgIdAndTxt("DRC:bodyMotionControlmex:BadInputs","the second argument should be the robot mex ptr");
memcpy(&(pdata->r),mxGetData(prhs[1]),sizeof(pdata->r));
if (!mxIsNumeric(prhs[2]) || mxGetM(prhs[2])!=6 || mxGetN(prhs[2])!=1)
mexErrMsgIdAndTxt("DRC:bodyMotionControlmex:BadInputs","the third argument should be Kp");
memcpy(&(pdata->Kp),mxGetPrSafe(prhs[2]),sizeof(pdata->Kp));
if (!mxIsNumeric(prhs[3]) || mxGetM(prhs[3])!=6 || mxGetN(prhs[3])!=1)
mexErrMsgIdAndTxt("DRC:bodyMotionControlmex:BadInputs","the fourth argument should be Kd");
memcpy(&(pdata->Kd),mxGetPrSafe(prhs[3]),sizeof(pdata->Kd));
pdata->body_index = (int) mxGetScalar(prhs[4]) -1;
mxClassID cid;
if (sizeof(pdata)==4) cid = mxUINT32_CLASS;
else if (sizeof(pdata)==8) cid = mxUINT64_CLASS;
else mexErrMsgIdAndTxt("Drake:bodyMotionControlmex:PointerSize","Are you on a 32-bit machine or 64-bit machine??");
plhs[0] = mxCreateNumericMatrix(1,1,cid,mxREAL);
memcpy(mxGetData(plhs[0]),&pdata,sizeof(pdata));
return;
}
// first get the ptr back from matlab
if (!mxIsNumeric(prhs[0]) || mxGetNumberOfElements(prhs[0])!=1)
mexErrMsgIdAndTxt("DRC:bodyMotionControlmex:BadInputs","the first argument should be the ptr");
memcpy(&pdata,mxGetData(prhs[0]),sizeof(pdata));
int nq = pdata->r->num_positions;
int nv = pdata->r->num_velocities;
int narg = 1;
Map<VectorXd> q(mxGetPrSafe(prhs[narg]), nq);
Map<VectorXd> qd(mxGetPrSafe(prhs[narg++]) + nq, nv);
assert(mxGetM(prhs[narg])==6); assert(mxGetN(prhs[narg])==1);
Map< Vector6d > body_pose_des(mxGetPrSafe(prhs[narg++]));
assert(mxGetM(prhs[narg])==6); assert(mxGetN(prhs[narg])==1);
Map< Vector6d > body_v_des(mxGetPrSafe(prhs[narg++]));
assert(mxGetM(prhs[narg])==6); assert(mxGetN(prhs[narg])==1);
Map< Vector6d > body_vdot_des(mxGetPrSafe(prhs[narg++]));
pdata->r->doKinematics(q, qd);
// TODO: this must be updated to use quaternions/spatial velocity
auto body_pose_gradientvar = pdata->r->forwardKin(Vector3d::Zero().eval(), pdata->body_index, 0, 1, 1);
const auto& body_pose = body_pose_gradientvar.value();
const auto& J = body_pose_gradientvar.gradient().value();
Vector6d body_error;
body_error.head<3>()= body_pose_des.head<3>()-body_pose.head<3>();
Vector3d error_rpy,pose_rpy,des_rpy;
pose_rpy = body_pose.tail<3>();
des_rpy = body_pose_des.tail<3>();
angleDiff(pose_rpy,des_rpy,error_rpy);
body_error.tail(3) = error_rpy;
Vector6d body_vdot = (pdata->Kp.array()*body_error.array()).matrix() + (pdata->Kd.array()*(body_v_des-J*qd).array()).matrix() + body_vdot_des;
plhs[0] = eigenToMatlab(body_vdot);
}
