Skip to content
Snippets Groups Projects
Commit 72993fde authored by Sergi Hernandez's avatar Sergi Hernandez
Browse files

Modified the dynamixel_motor_group class to support relative motions

parent 49d3c74b
No related branches found
No related tags found
No related merge requests found
......@@ -22,9 +22,10 @@ CDynamixelMotorGroup::CDynamixelMotorGroup(std::string &group_id):CMotorGroup(gr
void CDynamixelMotorGroup::move(std::vector<float> &position,std::vector<float> &velocity,std::vector<float> &acceleration)
{
std::vector<float> pos_count,vel_count,accel_count;
std::vector<float> pos_count,vel_count,accel_count,tmp_pos;
std::vector< std::vector<unsigned char> > data;
std::vector<short int> count_value;
std::vector<bool> absolute;
unsigned int i=0;
if(position.size()!=this->servo_id.size())
......@@ -55,8 +56,17 @@ void CDynamixelMotorGroup::move(std::vector<float> &position,std::vector<float>
- goal_speed: 2 bytes */
data[i].resize(4);
}
absolute=this->is_motion_absolute();
for(i=0;i<servo_id.size();i++)
{
if(!absolute[i])
tmp_pos.push_back(position[i]+this->current_pos[i]);
else
tmp_pos.push_back(position[i]);
}
this->current_pos=tmp_pos;
// convert the input data
this->angles_to_controller(position,pos_count);
this->angles_to_controller(tmp_pos,pos_count);
this->speeds_to_controller(velocity,vel_count);
this->accels_to_controller(acceleration,accel_count);
for(i=0;i<servo_id.size();i++)
......@@ -76,12 +86,15 @@ void CDynamixelMotorGroup::move(std::vector<float> &position,std::vector<float>
std::string CDynamixelMotorGroup::add_motor_control(CDynamixelMotor *controller)
{
std::vector<float> current_position;
std::string cont_id;
try{
cont_id=CMotorGroup::add_motor_control(controller);
// if the controller is successfully added
this->servo_id.push_back(((CDynamixelMotor *)controller)->get_id());
current_position=controller->get_position();
this->current_pos.push_back(current_position[0]);
}catch(CException &e){
/* handle exceptions */
throw;
......@@ -94,6 +107,7 @@ void CDynamixelMotorGroup::remove_motor_control(std::string &cont_id)
{
std::vector<TMotorInfo>::iterator old_cont;
std::vector<unsigned char> new_servo_id;
std::vector<float> new_current_pos;
unsigned int i,old_id;
try{
......@@ -105,7 +119,10 @@ void CDynamixelMotorGroup::remove_motor_control(std::string &cont_id)
old_id=((CDynamixelMotor *)old_cont->controller)->get_id();
for(i=0;i<this->servo_id.size();i++)
if(this->servo_id[i]!=old_id)
{
new_servo_id.push_back(servo_id[i]);
new_current_pos.push_back(this->current_pos[i]);
}
this->servo_id=new_servo_id;
CMotorGroup::remove_motor_control(cont_id);
}catch(CException &e){
......
......@@ -13,8 +13,8 @@ class CDynamixelMotorGroup : public CMotorGroup
private:
std::vector<unsigned char> servo_id;
std::vector<float> current_pos;
CDynamixelServer *dyn_server;
public:
CDynamixelMotorGroup(std::string &group_id);
void move(std::vector<float> &position,std::vector<float> &velocity,std::vector<float> &acceleration);
......
0% Loading or .
You are about to add 0 people to the discussion. Proceed with caution.
Finish editing this message first!
Please register or to comment