bool ControlUtils::setJointSubset(const std::vector<int>& joints, const std::vector<double>& positions)
{
int16_t tmp_tick[positions.size()];
std::vector<int16_t> positions_to_send;
std::vector<int> joints_to_send;
for (int i = 0; i < positions.size(); i++) {
tmp_tick[i] = rad2tick(positions[i], i);
if (tmp_tick[i]!= ticks_to[i]) {
joints_to_send.push_back(joints[i]);
positions_to_send.push_back(tmp_tick[i]);
ticks_to[i] = tmp_tick[i];
}
}
return syncWriteWord(ADDR_GOAL_POSITION_L, joints_to_send, positions_to_send);
}
bool ControlUtils::setJoints(const double a[TOTAL_JOINTS])
{
int16_t tmp_tick[TOTAL_JOINTS];
std::vector<int16_t> vals;
std::vector<int> joints;
for (int i = 0; i < TOTAL_JOINTS; i++) {
tmp_tick[i] = rad2tick(a[i], i);
if (tmp_tick[i] != ticks_to[i]) {
vals.push_back(tmp_tick[i]);
joints.push_back(i);
ticks_to[i] = tmp_tick[i];
}
}
return syncWriteWord(ADDR_GOAL_POSITION_L, joints, vals);
}
bool ControlUtils::setJoints(const double a[TOTAL_JOINTS])
{
int16_t tmp_tick[TOTAL_JOINTS];
std::vector<int16_t> vals;
std::vector<int> joints;
for (int i = 0; i < TOTAL_JOINTS; i++) {
tmp_tick[i] = rad2tick(a[i], i);
if (tmp_tick[i] != ticks_to[i]) {
//printf("changing tick!\n");
//std::cout << tmp_tick[i] << std::endl;
vals.push_back(tmp_tick[i]);
joints.push_back(i);
//printf("joint being moved\n");
//std::cout << i << std::endl;
ticks_to[i] = tmp_tick[i];
}
}
return syncWriteWord(ADDR_GOAL_POSITION_L, joints, vals);
}