15 std::lock_guard<std::mutex> lg(m_mtx);
21 std::lock_guard<std::mutex> lg(m_mtx);
28 std::lock_guard<std::mutex> lg(m_mtx);
36 std::lock_guard<std::mutex> lg(m_mtx);
37 m_current_action->
print();
43 std::lock_guard<std::mutex> lg(m_mtx);
52 std::lock_guard<std::mutex> lg(m_mtx);
64 m_current_action->
forever =
true;
70 std::lock_guard<std::mutex> lg(m_mtx);
82 m_current_action->
forever =
false;
88 std::lock_guard<std::mutex> lg(m_mtx);
89 m_current_action = action;
90 m_current_driver = driver;
99 std::lock_guard<std::mutex> lg(m_mtx);
109 std::lock_guard<std::mutex> lg(m_mtx);
120 yInfo() <<
"Control thread period set to: " << period;
121 m_module_name = name;
127 m_port_command_out.
close();
129 m_port_command_joints.
close();
134 if (!m_port_command_out.
open(std::string(
"/")+m_module_name+
"/port_command_out:o"))
139 if (!m_port_command_joints.
open(std::string(
"/")+m_module_name+
"/port_joints:o"))
146bool ControlThread::execute_joint_command(
int frame_id)
148 if (!m_current_driver)
return false;
154 for (
size_t j = 0; j < nj; j++)
161void ControlThread::compute_and_send_command(
int frame_id)
170 m_port_command_out.
write();
173 if (!execute_joint_command(frame_id))
175 yError(
"failed to execute command");
179 std::vector<double> encs;
180 if (m_current_driver)
184 for (
size_t j = 0; j < nj; j++)
186 m_current_driver->
getEncoder((
int)j, &encs[j]);
192 yError(
"Critical error: invalid driver");
203 for (
size_t ix=0;ix<nj;ix++)
208 for (
size_t ix=0;ix<nj;ix++)
212 this->m_port_command_joints.
write();
225 for (i = 0; i < dq.size(); i++)
227 if (dq[i].time > elapsed)
235 std::lock_guard<std::mutex> lck(m_mtx);
246 yInfo() <<
"ACTION_STOP";
252 yInfo() <<
"ACTION_RESET";
254 for (
size_t j = 0; j < nj; j++)
269 yError(
"ACTION_RUNNING: sequence empty!");
282 compute_and_send_command((
int)m_current_action->
current_frame);
297 for (
size_t j = 0; j < nj; j++)
316 yDebug() <<
"ACTION_START: switch to position mode";
317 for (
size_t j = 0; j < nj; j++)
322 for (
size_t j = 0; j < nj; j++)
328 yInfo() <<
"ACTION_START: going to start position";
331 std::vector<double> err(nj);
336 for (
size_t j = 0; j < nj; j++)
339 err[j] = fabs(enc - ll[j]);
345 yInfo() <<
"ACTION_START: start position reached successfully";
350 yWarning() <<
"ACTION_START: timeout while trying to reach start position";
351 for (
size_t j = 0; j < nj; j++)
362 yError() <<
"ACTION_START: unable to reach start position!";
363 if (m_home_position_strict_check_enabled)
367 yError() <<
"ACTION_START: m_home_position_strict_check_enabled, stopping!";
372 for (
int j = 0; j <nj; j++)
381 yError() <<
"ACTION_START: control mode check failed!";
388 yDebug() <<
"ACTION_START: switch to position direct mode";
389 for (
int j = 0; j <nj; j++)
396 yDebug() <<
"ACTION_START: moving to step 0 in direct mode";
397 compute_and_send_command(0);
402 yInfo() <<
"ACTION_START: sequence started";
413 yError() <<
"unknown current_status";
421 for (
size_t i=0; i< m_current_action->
get_njoints(); i++)
432 m_home_position_timeout = timeout;
438 m_home_position_strict_check_enabled=enable;
443 if (m_current_driver)
bool threadInit() override
Initialization method.
bool action_getname(std::string &name)
void setPositionStrictCheck(bool enable)
bool m_enable_execute_joint_command
void setInitialMoveTime(double t)
void run() override
Loop function.
action_status_enum getStatus()
bool action_resample(double value)
bool action_setSpeedFactor(double factor=1)
void setPositionTolerance(double tolerance)
bool action_change(action_class *action, robotDriver *driver)
ControlThread(std::string name, double period)
void setPositionTimeout(double timeloops)
void interpolate_action_frames(double timestep)
std::vector< double > m_tolerances
std::deque< action_frame > action_frames_vector
bool setTrajectoryTime(double t)
bool setPosition(int j, double ref)
bool setControlMode(const int j, const yarp::dev::SelectableControlModeEnum mode)
bool getEncoder(int j, double *v)
bool storeRefVelocities()
bool getControlMode(const int j, yarp::dev::ControlModeEnum &mode)
bool positionMove(int j, double ref)
A simple collection of objects that can be described and transmitted in a portable way.
void addFloat64(yarp::conf::float64_t x)
Places a 64-bit floating point number in the bottle, at the end of the list.
void clear()
Empties the bottle of any objects it contains.
void addInt32(std::int32_t x)
Places a 32-bit integer in the bottle, at the end of the list.
void addString(const char *str)
Places a string in the bottle, at the end of the list.
void close() override
Stop port activity.
bool open(const std::string &name) override
Start port operation, with a specific name, with automatically-chosen network parameters.
void interrupt() override
Interrupt any current reads or writes attached to the port.
void write(bool forceStrict=false)
Write the current object being returned by BufferedPort::prepare.
T & prepare()
Access the object which will be transmitted by the next call to yarp::os::BufferedPort::write.
size_t getCurrentFrame(double elapsed, std::deque< action_frame > &dq)
@ VOCAB_CM_POSITION_DIRECT
double now()
Return the current time in seconds, relative to an arbitrary starting point.
void delay(double seconds)
Wait for a certain number of seconds.