bool PowerCubeCtrl::MoveVel(const std::vector<double>& velocities)
{
PCTRL_CHECK_INITIALIZED();
unsigned int DOF = m_params->GetDOF();
/// getting limits
std::vector<double> lowerLimits = m_params->GetLowerLimits();
std::vector<double> upperLimits = m_params->GetUpperLimits();
std::vector<double> maxVels = m_params->GetMaxVel();
/// check dimensions
if (velocities.size() != DOF)
{
m_ErrorMessage = "Skipping command: Commanded velocities and DOF are not same dimension.";
return false;
}
for (unsigned int i = 0; i < DOF; i++)
{
/// check velocity limit
if(velocities[i] > maxVels[i])
{
std::ostringstream errorMsg;
errorMsg << "Skipping command: Velocity " << velocities[i] << " exceeds limit " << maxVels[i] << "for axis " << i;
m_ErrorMessage = errorMsg.str();
return false;
}
/// check position limits
double targetpos = m_positions[i] + (0.02 * velocities[i]);
if (targetpos < lowerLimits[i] || targetpos > upperLimits[i])
{
std::ostringstream errorMsg;
errorMsg << "Skipping command: Target position " << targetpos << " exceeds limit " << lowerLimits[i] << " - " << upperLimits[i] << "for axis " << i;
m_ErrorMessage = errorMsg.str();
return false;
}
}
std::vector<std::string> errorMessages;
PC_CTRL_STATUS status;
getStatus(status, errorMessages);
if ((status != PC_CTRL_OK))
{
m_ErrorMessage.assign("");
for (unsigned int i = 0; i < DOF; i++)
{
m_ErrorMessage.append(errorMessages[i]);
m_ErrorMessage.append("\n");
}
return false;
}
float delta_t;
delta_t = ros::Time::now().toSec() - m_last_time_pub.toSec();
m_last_time_pub = ros::Time::now();
/// Display Timegap with time before and after update
//std::cout << "\n-------\nTimegap\n-------\n time now = " << ros::Time::now() << "\n last time = " << m_last_time_pub << "\n difference = " << delta_t << std::endl;
for (unsigned int i = 0; i < DOF; i++)
{
float pos;
float cmd_pos;
unsigned short cmd_time; // time in milliseconds
// limit step time to 20msec
if (delta_t >= 0.020)
{
cmd_time = 0.020*1000; //msec
}
else
{
cmd_time = delta_t * 1000; //msec
}
cmd_pos = (cmd_time/1000.0) * velocities[i];
pthread_mutex_lock(&m_mutex);
//std::cout << "Modul_id = " << m_params->GetModuleID(i) <<", step= "<< m_positions[i] + cmd_pos << ", time = " << cmd_time << std::endl;
//int ret = PCube_moveVelExtended(m_DeviceHandle, m_params->GetModuleID(i), velocities[i], &m_status[i], &m_dios[i], &pos);
int ret = PCube_moveStepExtended(m_DeviceHandle, m_params->GetModuleID(i), m_positions[i] + cmd_pos, 100*cmd_time, &m_status[i], &m_dios[i], &pos);
pthread_mutex_unlock(&m_mutex);
if (ret != 0)
{
m_pc_status = PC_CTRL_ERR;
}
m_positions[i] = (double)pos;
}
pthread_mutex_lock(&m_mutex);
PCube_startMotionAll(m_DeviceHandle);
pthread_mutex_unlock(&m_mutex);
return true;
}
bool PowerCubeCtrl::MoveVel(const std::vector<double>& velocities)
{
PCTRL_CHECK_INITIALIZED();
unsigned int DOF = m_params->GetDOF();
std::vector<double> lowerLimits = m_params->GetLowerLimits();
std::vector<double> upperLimits = m_params->GetUpperLimits();
// check dimensions
if (velocities.size() != DOF)
{
m_ErrorMessage = "Skipping command: Commanded velocities and DOF are not same dimension.";
return false;
}
for (unsigned int i = 0; i < DOF; i++)
{
// check limits
if (velocities[i] < lowerLimits[i] || velocities[i] > upperLimits[i])
{
std::ostringstream errorMsg;
errorMsg << "Skipping command: Velocity " << velocities[i] << " exceeds limit " << lowerLimits[i] << " - " << upperLimits[i] << "for axis " << i;
m_ErrorMessage = errorMsg.str();
return false;
}
}
std::vector<std::string> errorMessages;
PC_CTRL_STATUS status;
getStatus(status, errorMessages);
if ((status != PC_CTRL_OK))
{
m_ErrorMessage.assign("");
for (unsigned int i = 0; i < DOF; i++)
{
m_ErrorMessage.append(errorMessages[i]);
m_ErrorMessage.append("\n");
}
return false;
}
//#############################################
//############ D E B U G ################
//#############################################
//Init
float delta_t;
//Berechne delta_t
delta_t = ros::Time::now().toSec() - last_time_pub_.toSec();
//Display Timegap with time before and after update
std::cout << "\n-------\nTimegap\n-------\n time now = " << ros::Time::now() << "\n last time = " << last_time_pub_ << "\n difference = " << delta_t << std::endl;
last_time_pub_ = ros::Time::now();
for (unsigned int i = 0; i < DOF; i++) //vorher i < DOF
{
float pos;
float cmd_pos;
float cmd_pos_abs; // absolute position
unsigned short cmd_time; // time in milliseconds
// ToDo: Hier getHorizon() Funktion benutzen um die aktuelle Frequenz auszulesen die in arm_sdh.yaml gespeichert ist. Daraus mit 1/getHorizon() den ersten Wert ermitteln.
if (countr == 0)
{
cmd_time = 17;
if (i == DOF-1)
countr = countr + 1;
}
else
{
cmd_time = delta_t * 1000;
}
cmd_pos = (cmd_time/1000.0) * velocities[i];
pthread_mutex_lock(&m_mutex);
PCube_moveStepExtended(m_DeviceHandle, m_params->GetModuleID(i), m_positions[i] + cmd_pos, cmd_time, &m_status[i], &m_dios[i], &pos);
pthread_mutex_unlock(&m_mutex);
m_positions[i] = (double)pos;
}
pthread_mutex_lock(&m_mutex);
//std::cout << "------------------------------> PCube_startMotionAll()" << std::endl;
PCube_startMotionAll(m_DeviceHandle);
pthread_mutex_unlock(&m_mutex);
return true;
}
bool PowerCubeCtrl::MoveVel(const std::vector<double>& velocities)
{
PCTRL_CHECK_INITIALIZED();
unsigned int DOF = m_params->GetDOF();
/// getting limits
std::vector<double> lowerLimits = m_params->GetLowerLimits();
std::vector<double> upperLimits = m_params->GetUpperLimits();
std::vector<double> maxVels = m_params->GetMaxVel();
/// check dimensions
if (velocities.size() != DOF)
{
m_ErrorMessage = "Skipping command: Commanded velocities and DOF are not same dimension.";
return false;
}
for (unsigned int i = 0; i < DOF; i++)
{
/// check velocity limit
if(velocities[i] > maxVels[i])
{
//velocities[i] = maxVels[i];
std::ostringstream errorMsg;
errorMsg << "Velocity " << velocities[i] << " exceeds limit " << maxVels[i] << "for axis " << i << " moving with " << maxVels[i] << " instead";
m_ErrorMessage = errorMsg.str();
}
/// check position limits
double targetpos = m_positions[i] + (0.02 * velocities[i]);
if (targetpos < lowerLimits[i] || targetpos > upperLimits[i])
{
std::ostringstream errorMsg;
errorMsg << "Skipping command: Target position " << targetpos << " exceeds limit " << lowerLimits[i] << " - " << upperLimits[i] << "for axis " << i;
m_ErrorMessage = errorMsg.str();
return false;
}
}
std::vector<std::string> errorMessages;
PC_CTRL_STATUS status;
getStatus(status, errorMessages);
if ((status != PC_CTRL_OK))
{
m_ErrorMessage.assign("");
for (unsigned int i = 0; i < DOF; i++)
{
m_ErrorMessage.append(errorMessages[i]);
}
return false;
}
float delta_t;
delta_t = ros::Time::now().toSec() - m_last_time_pub.toSec();
m_last_time_pub = ros::Time::now();
std::vector<double> pos_temp;
pos_temp.resize(DOF);
for (unsigned int i = 0; i < DOF; i++)
{
float pos;
float cmd_pos;
double cmd_time; // time in milliseconds
// limit step time to 20msec
if (delta_t >= 0.050)
{
cmd_time = 0.050; //sec
}
else
cmd_time = delta_t; //sec
cmd_pos = cmd_time * velocities[i];
pthread_mutex_lock(&m_mutex);
//std::cout << "Modul_id = " << m_params->GetModuleID(i) <<", step= "<< m_positions[i] + cmd_pos << ", time = " << cmd_time << std::endl;
//int ret = PCube_moveVelExtended(m_DeviceHandle, m_params->GetModuleID(i), velocities[i], &m_status[i], &m_dios[i], &pos);
int ret = PCube_moveStepExtended(m_DeviceHandle, m_params->GetModuleID(i), m_positions[i] + cmd_pos, (cmd_time+m_horizon), &m_status[i], &m_dios[i], &pos);
pthread_mutex_unlock(&m_mutex);
if (ret != 0)
{
pos = m_positions[i];
std::cout << ret << std::endl;
// m_pc_status = PC_CTRL_ERR;
}
// !!! Position in pos is position before moveStep movement, to get the expected position after the movement (required as input to the next moveStep command) we add the delta position (cmd_pos) !!!
m_positions[i] = (double)pos + cmd_pos;
pos_temp[i] = (double)pos;
}
// Calculation of velocities based on vel = 1/(6*dt) * (-pos(t-3) - 3*pos(t-2) + 3*pos(t-1) + pos(t))
if(m_cached_pos.size() < 4)
{
m_cached_pos.push_back(pos_temp);
for(unsigned int i = 0; i < DOF; i++)
m_velocities[i] = 0.0;
}
else
{
m_cached_pos.push_back(pos_temp);
m_cached_pos.pop_front();
for(unsigned int i = 0; i < DOF; i++)
{
m_velocities[i] = 1/(6*delta_t) * (-m_cached_pos[0][i]-(3*m_cached_pos[1][i])+(3*m_cached_pos[2][i])+m_cached_pos[3][i]);
}
}
pthread_mutex_lock(&m_mutex);
PCube_startMotionAll(m_DeviceHandle);
pthread_mutex_unlock(&m_mutex);
return true;
}
bool PowerCubeCtrl::MoveVel(const std::vector<double>& velocities)
{
PCTRL_CHECK_INITIALIZED();
unsigned int DOF = m_params->GetDOF();
std::vector<double> lowerLimits = m_params->GetLowerLimits();
std::vector<double> upperLimits = m_params->GetUpperLimits();
// check dimensions
if (velocities.size() != DOF)
{
m_ErrorMessage = "Skipping command: Commanded velocities and DOF are not same dimension.";
return false;
}
for (unsigned int i = 0; i < DOF; i++)
{
// check limits
if (velocities[i] < lowerLimits[i] || velocities[i] > upperLimits[i])
{
m_ErrorMessage = "Skipping command: Velocity exceeds limits.";
return false;
}
}
std::vector<std::string> errorMessages;
PC_CTRL_STATUS status;
getStatus(status, errorMessages);
if ((status != PC_CTRL_OK))
{
m_ErrorMessage.assign("");
for (unsigned int i = 0; i < DOF; i++)
{
m_ErrorMessage.append(errorMessages[i]);
m_ErrorMessage.append("\n");
}
return false;
}
for (unsigned int i = 0; i < DOF; i++)
{
float pos;
float cmd_pos;
unsigned short cmd_time; // time in milliseconds
// calculate horizon
cmd_time = getHorizon() * 1000; // time in milliseconds
cmd_pos = cmd_time * velocities[i];
pthread_mutex_lock(&m_mutex);
//PCube_moveVelExtended(m_DeviceHandle, m_params->GetModuleID(i), velocities[i], &m_status[i], &m_dios[i], &pos);
PCube_moveStepExtended(m_DeviceHandle, m_params->GetModuleID(i), cmd_pos, cmd_time, &m_status[i], &m_dios[i], &pos);
m_positions[i] = (double)pos;
pthread_mutex_unlock(&m_mutex);
}
pthread_mutex_lock(&m_mutex);
PCube_startMotionAll( m_DeviceHandle);
pthread_mutex_unlock(&m_mutex);
return true;
}
