/*!
* \brief Returns the current states
*/
bool PowerCubeCtrl::updateStates()
{
PCTRL_CHECK_INITIALIZED();
unsigned int DOF = m_params->GetDOF();
unsigned long state;
unsigned char dio;
float position;
int ret = 0;
for (unsigned int i = 0; i < DOF; i++)
{
pthread_mutex_lock(&m_mutex);
ret = PCube_getStateDioPos(m_DeviceHandle, m_params->GetModuleID(i), &state, &dio, &position);
pthread_mutex_unlock(&m_mutex);
if (ret != 0)
{
m_pc_status = PC_CTRL_ERR;
}
m_status[i] = state;
m_dios[i] = dio;
m_positions[i] = position;
/// ToDo: calculate vel and acc
///m_velocities = ???;
///m_accelerations = ???
}
return true;
}
/// @brief Returns the current states
bool PowerCubeCtrl::updateStates()
{
PCTRL_CHECK_INITIALIZED();
unsigned int DOF = m_params->GetDOF();
unsigned long state;
unsigned char dio;
float position;
for (unsigned int i = 0; i < DOF; i++)
{
pthread_mutex_lock(&m_mutex);
//std::cout << "------------------------------> PCube_getStateDioPos()" << std::endl;
PCube_getStateDioPos(m_DeviceHandle, m_params->GetModuleID(i), &state, &dio, &position);
pthread_mutex_unlock(&m_mutex);
m_status[i] = state;
m_dios[i] = dio;
m_positions[i] = position;
// @todo calculate vel and acc
//m_velocities = ???;
//m_accelerations = ???
}
return true;
}
/*!
* \brief Returns true if some cubes are still moving
*/
bool PowerCubeCtrl::statusMoving()
{
PCTRL_CHECK_INITIALIZED();
for (int i = 0; i < m_params->GetDOF(); i++)
{
if (m_status[i] & STATEID_MOD_MOTION)
return true;
}
return false;
}
/*!
* \brief Sets the maximum angular acceleration (rad/s^2) for the Joints, use with care!
*/
bool PowerCubeCtrl::setMaxAcceleration(const std::vector<double>& maxAccelerations)
{
PCTRL_CHECK_INITIALIZED();
for (int i = 0; i < m_params->GetDOF(); i++)
{
pthread_mutex_lock(&m_mutex);
PCube_setMaxAcc(m_DeviceHandle, m_params->GetModuleID(i), maxAccelerations[i]);
pthread_mutex_unlock(&m_mutex);
m_params->SetMaxAcc(maxAccelerations);
}
return true;
}
/*!
* \brief Sets the maximum angular velocity (rad/s) for the Joints, use with care!
*/
bool PowerCubeCtrl::setMaxVelocity(const std::vector<double>& maxVelocities)
{
PCTRL_CHECK_INITIALIZED();
for (int i = 0; i < m_params->GetDOF(); i++)
{
pthread_mutex_lock(&m_mutex);
//std::cout << "------------------------------> PCube_setMaxVel()" << std::endl;
PCube_setMaxVel(m_DeviceHandle, m_params->GetModuleID(i), maxVelocities[i]);
pthread_mutex_unlock(&m_mutex);
m_params->SetMaxVel(maxVelocities);
}
return true;
}
/*!
* \brief Sets the maximum angular velocity (rad/s) for the Joints, use with care!
*
* A Value of 0.5 is already pretty fast, you probably don't want anything more than one...
*/
bool PowerCubeCtrl::setMaxVelocity(double maxVelocity)
{
PCTRL_CHECK_INITIALIZED();
for (int i = 0; i < m_params->GetDOF(); i++)
{
pthread_mutex_lock(&m_mutex);
PCube_setMaxVel(m_DeviceHandle, m_params->GetModuleID(i), maxVelocity);
pthread_mutex_unlock(&m_mutex);
std::vector<double> maxVelocities(maxVelocity);
m_params->SetMaxVel(maxVelocities);
}
return true;
}
/*!
* \brief Sets the maximum angular acceleration (rad/s^2) for the Joints, use with care!
*
* A Value of 0.5 is already pretty fast, you probably don't want anything more than one...
*/
bool PowerCubeCtrl::setMaxAcceleration(double maxAcceleration)
{
PCTRL_CHECK_INITIALIZED();
for (int i = 0; i < m_params->GetDOF(); i++)
{
pthread_mutex_lock(&m_mutex);
//std::cout << "------------------------------> PCube_setMaxAcc()" << std::endl;
PCube_setMaxAcc(m_DeviceHandle, m_params->GetModuleID(i), maxAcceleration);
pthread_mutex_unlock(&m_mutex);
std::vector<double> maxAccelerations(maxAcceleration);
m_params->SetMaxAcc(maxAccelerations);
}
return true;
}
/*!
* \brief Returns the current states
*/
bool PowerCubeCtrl::updateStates()
{
PCTRL_CHECK_INITIALIZED();
unsigned int DOF = m_params->GetDOF();
unsigned long state;
unsigned long state_s; // for debug
unsigned char dio;
float position;
int ret = 0;
for (unsigned int i = 0; i < DOF; i++)
{
state_s = m_status[i];
pthread_mutex_lock(&m_mutex);
ret = PCube_getStateDioPos(m_DeviceHandle, m_params->GetModuleID(i), &state, &dio, &position);
pthread_mutex_unlock(&m_mutex);
if (ret != 0)
{
//m_pc_status = PC_CTRL_ERR;
std::cout << "State: Error com" << std::endl;
}
else
{ if( state_s != state)
{
std::cout << "State: " << state << "Joint: " << i << std::endl;
}
m_status[i] = state;
m_dios[i] = dio;
m_positions[i] = position;
}
/// ToDo: calculate vel and acc
///m_velocities = ???;
///m_accelerations = ???
}
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])
{
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;
}
/*!
* \brief Move joints synchronous
*
* Adjusting velocity of all joints to reach the final angules at the same time
*/
bool PowerCubeCtrl::MoveJointSpaceSync(const std::vector<double>& target)
{
PCTRL_CHECK_INITIALIZED();
unsigned int DOF = m_params->GetDOF();
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;
}
std::vector<double> vel(DOF);
std::vector<double> acc(DOF);
double TG = 0;
try
{
/// calculate which joint takes the longest time to reach goal
std::vector<double> times(DOF);
for (unsigned int i = 0; i < DOF; i++)
{
RampCommand rm(m_positions[i], m_velocities[i], target[i], m_params->GetMaxAcc()[i],
m_params->GetMaxVel()[i]);
times[i] = rm.getTotalTime();
}
/// determine the joint index that has the greatest value for time
int furthest = 0;
double max = times[0];
for (unsigned int i = 1; i < DOF; i++)
{
if (times[i] > max)
{
max = times[i];
furthest = i;
}
}
RampCommand rm_furthest(m_positions[furthest], m_velocities[furthest], target[furthest],
m_params->GetMaxAcc()[furthest], m_params->GetMaxVel()[furthest]);
double T1 = rm_furthest.T1();
double T2 = rm_furthest.T2();
double T3 = rm_furthest.T3();
/// total time:
TG = T1 + T2 + T3;
/// calculate velocity and acceleration for all joints:
acc[furthest] = m_params->GetMaxAcc()[furthest];
vel[furthest] = m_params->GetMaxVel()[furthest];
for (unsigned int i = 0; i < DOF; i++)
{
if (int(i) != furthest)
{
double a;
double v;
RampCommand::calculateAV(m_positions[i], m_velocities[i], target[i], TG, T3, m_params->GetMaxAcc()[i],
m_params->GetMaxVel()[i], a, v);
acc[i] = a;
vel[i] = v;
}
}
}
catch (...)
{
return false;
}
/// Send motion commands to hardware
for (unsigned int i = 0; i < DOF; i++)
{
pthread_mutex_lock(&m_mutex);
PCube_moveRamp(m_DeviceHandle, m_params->GetModuleIDs()[i], target[i], fabs(vel[i]), fabs(acc[i]));
pthread_mutex_unlock(&m_mutex);
}
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();
/// 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])
{
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();
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;
}
