19 #include "../include/open_manipulator_libs/dynamixel.h" 44 if (get_arg_[0] ==
"position_mode" || get_arg_[0] ==
"current_based_position_mode")
70 const char* log = NULL;
73 for (uint32_t index = 0; index < dynamixel_.num; index++)
75 result = dynamixel_workbench_->torqueOn(dynamixel_.id.at(index), &log);
81 enabled_state_ =
true;
86 const char* log = NULL;
89 for (uint32_t index = 0; index < dynamixel_.num; index++)
91 result = dynamixel_workbench_->torqueOff(dynamixel_.id.at(index), &log);
97 enabled_state_ =
false;
104 std::vector<double> radian_vector;
105 for(uint32_t index = 0; index < value_vector.size(); index++)
107 radian_vector.push_back(value_vector.at(index).position);
128 const char* log = NULL;
130 STRING return_delay_time_st =
"Return_Delay_Time";
131 const char * return_delay_time_char = return_delay_time_st.c_str();
133 dynamixel_.id = actuator_id;
134 dynamixel_.num = actuator_id.size();
138 result = dynamixel_workbench_->
init(dxl_device_name.c_str(), std::atoi(dxl_baud_rate.c_str()), &log);
144 uint16_t get_model_number;
145 for (uint8_t index = 0; index < dynamixel_.num; index++)
147 uint8_t
id = dynamixel_.id.at(index);
148 result = dynamixel_workbench_->ping(
id, &get_model_number, &log);
153 log::error(
"Please check your Dynamixel ID");
158 sprintf(str,
"Joint Dynamixel ID : %d, Model Name : %s",
id, dynamixel_workbench_->getModelName(
id));
161 result = dynamixel_workbench_->setVelocityBasedProfile(
id, &log);
165 log::error(
"Please check your Dynamixel firmware version (v38~)");
168 result = dynamixel_workbench_->writeRegister(
id, return_delay_time_char, 0, &log);
172 log::error(
"Please check your Dynamixel firmware version");
181 const char* log = NULL;
184 const uint32_t velocity = 0;
185 const uint32_t acceleration = 0;
186 const uint32_t current = 0;
188 if (dynamixel_mode ==
"position_mode")
190 for (uint8_t num = 0; num < actuator_id.size(); num++)
192 result = dynamixel_workbench_->jointMode(actuator_id.at(num), velocity, acceleration, &log);
199 else if (dynamixel_mode ==
"current_based_position_mode")
201 for (uint8_t num = 0; num < actuator_id.size(); num++)
203 result = dynamixel_workbench_->currentBasedPositionMode(actuator_id.at(num), current, &log);
212 for (uint8_t num = 0; num < actuator_id.size(); num++)
214 result = dynamixel_workbench_->jointMode(actuator_id.at(num), velocity, acceleration, &log);
228 const char* log = NULL;
230 result = dynamixel_workbench_->addSyncWriteHandler(actuator_id,
"Goal_Position", &log);
249 const char* log = NULL;
252 const char * char_profile_mode = profile_mode.c_str();
254 for (uint8_t num = 0; num < actuator_id.size(); num++)
256 result = dynamixel_workbench_->writeRegister(actuator_id.at(num), char_profile_mode, value, &log);
269 const char* log = NULL;
271 uint8_t id_array[actuator_id.size()];
272 int32_t goal_position[actuator_id.size()];
274 for (uint8_t index = 0; index < actuator_id.size(); index++)
276 id_array[index] = actuator_id.at(index);
277 goal_position[index] = dynamixel_workbench_->convertRadian2Value(actuator_id.at(index), radian_vector.at(index));
280 result = dynamixel_workbench_->syncWrite(
SYNC_WRITE_HANDLER, id_array, actuator_id.size(), goal_position, 1, &log);
292 const char* log = NULL;
294 std::vector<robotis_manipulator::ActuatorValue> all_actuator;
296 uint8_t id_array[actuator_id.size()];
297 for (uint8_t index = 0; index < actuator_id.size(); index++)
298 id_array[index] = actuator_id.at(index);
300 int32_t get_current[actuator_id.size()];
301 int32_t get_velocity[actuator_id.size()];
302 int32_t get_position[actuator_id.size()];
349 for (uint8_t index = 0; index < actuator_id.size(); index++)
352 actuator.
effort = dynamixel_workbench_->convertValue2Current(get_current[index]);
353 actuator.
velocity = dynamixel_workbench_->convertValue2Velocity(actuator_id.at(index), get_velocity[index]);
354 actuator.
position = dynamixel_workbench_->convertValue2Radian(actuator_id.at(index), get_position[index]);
356 all_actuator.push_back(actuator);
368 control_loop_time_ = control_loop_time;
388 if (get_arg_[0] ==
"position_mode" || get_arg_[0] ==
"current_based_position_mode")
409 return dynamixel_.id;
414 const char* log = NULL;
417 for (uint32_t index = 0; index < dynamixel_.num; index++)
419 result = dynamixel_workbench_->torqueOn(dynamixel_.id.at(index), &log);
425 enabled_state_ =
true;
430 const char* log = NULL;
433 for (uint32_t index = 0; index < dynamixel_.num; index++)
435 result = dynamixel_workbench_->torqueOff(dynamixel_.id.at(index), &log);
441 enabled_state_ =
false;
467 const char* log = NULL;
469 STRING return_delay_time_st =
"Return_Delay_Time";
470 const char * return_delay_time_char = return_delay_time_st.c_str();
472 dynamixel_.id = actuator_id;
473 dynamixel_.num = actuator_id.size();
477 result = dynamixel_workbench_->
init(dxl_device_name.c_str(), std::atoi(dxl_baud_rate.c_str()), &log);
483 uint16_t get_model_number;
484 for (uint8_t index = 0; index < dynamixel_.num; index++)
486 uint8_t
id = dynamixel_.id.at(index);
487 result = dynamixel_workbench_->ping(
id, &get_model_number, &log);
492 log::error(
"Please check your Dynamixel ID");
497 sprintf(str,
"Joint Dynamixel ID : %d, Model Name : %s",
id, dynamixel_workbench_->getModelName(
id));
500 result = dynamixel_workbench_->setTimeBasedProfile(
id, &log);
504 log::error(
"Please check your Dynamixel firmware version (v38~)");
507 result = dynamixel_workbench_->writeRegister(
id, return_delay_time_char, 0, &log);
511 log::error(
"Please check your Dynamixel firmware version");
520 const char* log = NULL;
523 const uint32_t velocity = uint32_t(control_loop_time_*1000) * 3;
524 const uint32_t acceleration = uint32_t(control_loop_time_*1000);
525 const uint32_t current = 0;
527 if (dynamixel_mode ==
"position_mode")
529 for (uint8_t num = 0; num < actuator_id.size(); num++)
531 result = dynamixel_workbench_->jointMode(actuator_id.at(num), velocity, acceleration, &log);
538 else if (dynamixel_mode ==
"current_based_position_mode")
540 for (uint8_t num = 0; num < actuator_id.size(); num++)
542 result = dynamixel_workbench_->currentBasedPositionMode(actuator_id.at(num), current, &log);
551 for (uint8_t num = 0; num < actuator_id.size(); num++)
553 result = dynamixel_workbench_->jointMode(actuator_id.at(num), velocity, acceleration, &log);
567 const char* log = NULL;
569 result = dynamixel_workbench_->addSyncWriteHandler(actuator_id,
"Goal_Position", &log);
588 const char* log = NULL;
591 const char * char_profile_mode = profile_mode.c_str();
593 for (uint8_t num = 0; num < actuator_id.size(); num++)
595 result = dynamixel_workbench_->writeRegister(actuator_id.at(num), char_profile_mode, value, &log);
607 const char* log = NULL;
609 uint8_t id_array[actuator_id.size()];
610 int32_t goal_value[actuator_id.size()];
613 for(uint8_t index = 0; index < actuator_id.size(); index++)
615 float result_position;
616 float time_control = control_loop_time_;
618 if(previous_goal_value_.find(actuator_id.at(index)) == previous_goal_value_.end())
620 previous_goal_value_.insert(std::make_pair(actuator_id.at(index), value_vector.at(index)));
623 result_position = value_vector.at(index).position + 3*(value_vector.at(index).velocity * (time_control))/2;
625 id_array[index] = actuator_id.at(index);
626 goal_value[index] = dynamixel_workbench_->convertRadian2Value(actuator_id.at(index), result_position);
628 previous_goal_value_[actuator_id.at(index)] = value_vector.at(index);
631 result = dynamixel_workbench_->syncWrite(
SYNC_WRITE_HANDLER, id_array, actuator_id.size(), goal_value, 1, &log);
642 const char* log = NULL;
644 std::vector<robotis_manipulator::ActuatorValue> all_actuator;
646 uint8_t id_array[actuator_id.size()];
647 for (uint8_t index = 0; index < actuator_id.size(); index++)
648 id_array[index] = actuator_id.at(index);
650 int32_t get_current[actuator_id.size()];
651 int32_t get_velocity[actuator_id.size()];
652 int32_t get_position[actuator_id.size()];
699 for (uint8_t index = 0; index < actuator_id.size(); index++)
702 actuator.
effort = dynamixel_workbench_->convertValue2Current(get_current[index]);
703 actuator.
velocity = dynamixel_workbench_->convertValue2Velocity(actuator_id.at(index), get_velocity[index]);
704 actuator.
position = dynamixel_workbench_->convertValue2Radian(actuator_id.at(index), get_position[index]);
706 all_actuator.push_back(actuator);
733 if (get_arg_[0] ==
"position_mode" || get_arg_[0] ==
"current_based_position_mode")
753 return dynamixel_.id.at(0);
758 const char* log = NULL;
761 result = dynamixel_workbench_->torqueOn(dynamixel_.id.at(0), &log);
766 enabled_state_ =
true;
771 const char* log = NULL;
774 result = dynamixel_workbench_->torqueOff(dynamixel_.id.at(0), &log);
779 enabled_state_ =
false;
803 const char* log = NULL;
806 STRING return_delay_time_st =
"Return_Delay_Time";
807 const char * return_delay_time_char = return_delay_time_st.c_str();
809 dynamixel_.id.push_back(actuator_id);
814 result = dynamixel_workbench_->
init(dxl_device_name.c_str(), std::atoi(dxl_baud_rate.c_str()), &log);
820 uint16_t get_model_number;
821 result = dynamixel_workbench_->ping(dynamixel_.id.at(0), &get_model_number, &log);
825 log::error(
"Please check your Dynamixel ID");
830 sprintf(str,
"Gripper Dynamixel ID : %d, Model Name :", dynamixel_.id.at(0));
831 strcat(str, dynamixel_workbench_->getModelName(dynamixel_.id.at(0)));
834 result = dynamixel_workbench_->setVelocityBasedProfile(dynamixel_.id.at(0), &log);
838 log::error(
"Please check your Dynamixel firmware version (v38~)");
841 result = dynamixel_workbench_->writeRegister(dynamixel_.id.at(0), return_delay_time_char, 0, &log);
845 log::error(
"Please check your Dynamixel firmware version");
854 const char* log = NULL;
857 const uint32_t velocity = 0;
858 const uint32_t acceleration = 0;
859 const uint32_t current = 200;
861 if (dynamixel_mode ==
"position_mode")
863 result = dynamixel_workbench_->jointMode(dynamixel_.id.at(0), velocity, acceleration, &log);
869 else if (dynamixel_mode ==
"current_based_position_mode")
871 result = dynamixel_workbench_->currentBasedPositionMode(dynamixel_.id.at(0), current, &log);
879 result = dynamixel_workbench_->jointMode(dynamixel_.id.at(0), velocity, acceleration, &log);
891 const char* log = NULL;
894 const char * char_profile_mode = profile_mode.c_str();
896 result = dynamixel_workbench_->writeRegister(dynamixel_.id.at(0), char_profile_mode, value, &log);
908 const char* log = NULL;
910 result = dynamixel_workbench_->addSyncWriteHandler(dynamixel_.id.at(0),
"Goal_Position", &log);
916 result = dynamixel_workbench_->addSyncReadHandler(dynamixel_.id.at(0),
930 const char* log = NULL;
932 int32_t goal_position = 0;
934 goal_position = dynamixel_workbench_->convertRadian2Value(dynamixel_.id.at(0), radian);
948 const char* log = NULL;
950 int32_t get_value = 0;
951 uint8_t id_array[1] = {dynamixel_.id.at(0)};
972 return dynamixel_workbench_->convertValue2Radian(dynamixel_.id.at(0), get_value);
virtual bool sendJointActuatorValue(std::vector< uint8_t > actuator_id, std::vector< robotis_manipulator::ActuatorValue > value_vector)
bool writeGoalPosition(std::vector< uint8_t > actuator_id, std::vector< double > radian_vector)
#define SYNC_WRITE_HANDLER
virtual std::vector< robotis_manipulator::ActuatorValue > receiveJointActuatorValue(std::vector< uint8_t > actuator_id)
bool writeGoalProfilingControlValue(std::vector< uint8_t > actuator_id, std::vector< robotis_manipulator::ActuatorValue > value_vector)
#define LENGTH_PRESENT_CURRENT_2
bool initialize(std::vector< uint8_t > actuator_id, STRING dxl_device_name, STRING dxl_baud_rate)
bool init(const char *device_name="/dev/ttyUSB0", uint32_t baud_rate=57600, const char **log=NULL)
virtual std::vector< uint8_t > getId()
bool setOperatingMode(STRING dynamixel_mode="position_mode")
bool setOperatingMode(std::vector< uint8_t > actuator_id, STRING dynamixel_mode="position_mode")
#define ADDR_PRESENT_CURRENT_2
virtual robotis_manipulator::ActuatorValue receiveToolActuatorValue()
virtual bool sendJointActuatorValue(std::vector< uint8_t > actuator_id, std::vector< robotis_manipulator::ActuatorValue > value_vector)
virtual void init(uint8_t actuator_id, const void *arg)
bool setSDKHandler(uint8_t actuator_id)
bool writeProfileValue(STRING profile_mode, uint32_t value)
virtual void setMode(const void *arg)
virtual void setMode(std::vector< uint8_t > actuator_id, const void *arg)
virtual void init(std::vector< uint8_t > actuator_id, const void *arg)
#define SYNC_READ_HANDLER_FOR_PRESENT_POSITION_VELOCITY_CURRENT
bool writeProfileValue(std::vector< uint8_t > actuator_id, STRING profile_mode, uint32_t value)
bool setSDKHandler(uint8_t actuator_id)
virtual std::vector< uint8_t > getId()
virtual void init(std::vector< uint8_t > actuator_id, const void *arg)
#define LENGTH_PRESENT_VELOCITY_2
bool setOperatingMode(std::vector< uint8_t > actuator_id, STRING dynamixel_mode="position_mode")
std::vector< robotis_manipulator::ActuatorValue > receiveAllDynamixelValue(std::vector< uint8_t > actuator_id)
std::vector< robotis_manipulator::ActuatorValue > receiveAllDynamixelValue(std::vector< uint8_t > actuator_id)
bool writeGoalPosition(double radian)
bool initialize(uint8_t actuator_id, STRING dxl_device_name, STRING dxl_baud_rate)
bool initialize(std::vector< uint8_t > actuator_id, STRING dxl_device_name, STRING dxl_baud_rate)
virtual std::vector< robotis_manipulator::ActuatorValue > receiveJointActuatorValue(std::vector< uint8_t > actuator_id)
#define ADDR_PRESENT_POSITION_2
JointDynamixelProfileControl(float control_loop_time=0.010)
virtual bool sendToolActuatorValue(robotis_manipulator::ActuatorValue value)
#define ADDR_PRESENT_VELOCITY_2
#define LENGTH_PRESENT_POSITION_2
virtual void setMode(std::vector< uint8_t > actuator_id, const void *arg)
bool writeProfileValue(std::vector< uint8_t > actuator_id, STRING profile_mode, uint32_t value)
double receiveDynamixelValue()