From 72993fde8d36e38192a6b857cedcbf5d47e4737e Mon Sep 17 00:00:00 2001
From: =?UTF-8?q?Sergi=20Hern=C3=A0ndez=20Juan?= <shernand@iri.upc.edu>
Date: Mon, 2 Apr 2012 18:19:58 +0000
Subject: [PATCH] Modified the dynamixel_motor_group class to support relative
motions
---
src/dynamixel_motor_group.cpp | 21 +++++++++++++++++++--
src/dynamixel_motor_group.h | 2 +-
2 files changed, 20 insertions(+), 3 deletions(-)
diff --git a/src/dynamixel_motor_group.cpp b/src/dynamixel_motor_group.cpp
index 0bdf3a0..98fb2ef 100644
--- a/src/dynamixel_motor_group.cpp
+++ b/src/dynamixel_motor_group.cpp
@@ -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){
diff --git a/src/dynamixel_motor_group.h b/src/dynamixel_motor_group.h
index a4b6d7f..b4c6606 100644
--- a/src/dynamixel_motor_group.h
+++ b/src/dynamixel_motor_group.h
@@ -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);
--
GitLab