void jointPublish(ros::Publisher jP) {
updateMotor();
/* ************** Publish joint angles ************** */
sensor_msgs::JointState msg;// = boost::make_shared<sensor_msgs::JointState>();
// msg.reset(new sensor_msgs::JointState);
std::vector<std::string> joint_names;
std::vector<float> angles;
std::vector<float> vels;
getJointNames(joint_names);
getJointAngles(angles);
getJointVelocities(vels);
for (int i = 0; i < 2 ; ++i) {
msg.name.push_back(joint_names[i]);
msg.position.push_back(angles[i]);
msg.velocity.push_back(vels[i]);
}
msg.header.stamp = ros::Time::now();
jP.publish(msg);
}
bool simulatedArm::MoveJointSpaceSync(const std::vector<double>& target)
{
std::cerr << "======================TUTUTUTUT\n";
PSIM_CHECK_INITIALIZED();
std::vector<double> acc(m_DOF);
std::vector<double> vel(m_DOF);
double TG = 0;
try
{
// Ermittle Joint, der bei max Geschw. und Beschl. am längsten braucht:
std::vector<double> posnow;
posnow.resize(m_DOF);
if ( getConfig(posnow) == false )
return false;
std::vector<double> velnow;
velnow.resize(m_DOF);
if ( getJointVelocities(velnow) == false )
return false;
std::vector<double> times(m_DOF);
for (int i=0; i < m_DOF; i++)
{
RampCommand rm(posnow[i], velnow[i], target[i], m_maxAcc[i], m_maxVel[i]);
times[i] = rm.getTotalTime();
}
// determine the joint index that has the greates value for time
int furthest = 0;
double max = times[0];
for (int i=1; i<m_DOF; i++)
{
if (times[i] > max)
{
max = times[i];
furthest = i;
}
}
RampCommand rm_furthest(posnow[furthest], velnow[furthest], target[furthest], m_maxAcc[furthest], m_maxVel[furthest]);
double T1 = rm_furthest.T1();
double T2 = rm_furthest.T2();
double T3 = rm_furthest.T3();
// Gesamtzeit:
TG = T1 + T2 + T3;
// Jetzt Geschwindigkeiten und Beschl. für alle:
acc[furthest] = m_maxAcc[furthest];
vel[furthest] = m_maxVel[furthest];
for (int i = 0; i < m_DOF; i++)
{
if (i != furthest)
{
double a; double v;
// a und v berechnen:
RampCommand::calculateAV(
posnow[i],
velnow[i],
target[i],
TG, T3,
m_maxAcc[i],
m_maxVel[i],
a,
v);
acc[i] = a;
vel[i] = v;
}
}
}
catch(...)
{
m_ErrorMessage.assign("Problem during calculation of a and av.");
return false;
}
// Jetzt Bewegung starten:
for (int i=0; i < m_DOF; i++)
{
std::cout << "moving motor " << i << ": " << target[i] << ": " << vel[i] << ": " << acc[i] << "\n";
m_motors[i].moveRamp(target[i], vel[i], acc[i]);
}
return true;
}
