Skip to content

Commit

Permalink
[Spinal][Neuron][Servo] enable current control for dynamixel motor (#527
Browse files Browse the repository at this point in the history 
)

* [Neuron][Dynamixel] remove uncessary codes

* [Neuron][Dynamixel] enable to get model number in init phase

* [Neuron][Dynamixel][WIP] enable sending goal current (torque) in the init phase

* [Neuron][Dynamixel][WIP] use sync write for goal position regardless of RS485 TTL mix mode

* [Neuron][Dynamixel] enable polling the operating mode

* [Neuron][Dynamixel] refactor the current assignment

* [Neuron][Dynamixel] enable to send goal current every iteration after sending the goal position. Also enable receiving the goal current from CAN

* [Spinal][Servo][WIP] enable send goal current from rosserial to spinal, and finally to neuron

* [Spinal][Neuron] modify the CAN initialize message protocol to support sending the initial goal current from neuron to spinal

* [Servo Bridge] enable to pub/sub target torque via servo bridge

* [Servo Config] refactor Servo.yaml for all robots: removing unnecessary topic name assignment, use default one

* [Spinal][RQT] fix the import bug for python3

* [Servo][Spinal][Neuron] refactor the force servo off rule, which is determined by neuron but not spinal

* [Neuron][Dynamixel] add original rule to force switching off servo according to overload error

* [Neuron][Dynamixel] implement setup of pre-defined configuration

* [Spinal][RQT] fix the wrong list assignment

* [Spinal][CAN] set 1[ms] internal between motor pwn and servo target command.

---------

Co-authored-by: Moju Zhao <[email protected]>
  • Loading branch information
@tongtybj
tongtybj and Moju Zhao authored Jan 19, 2025
1 parent 2638f5f commit 49418eb
Show file tree
Hide file tree
Showing 23 changed files with 487 additions and 164 deletions.
36 changes: 30 additions & 6 deletions aerial_robot_model/include/aerial_robot_model/servo_bridge.h
View file
Original file line number Diff line number Diff line change
Expand Up @@ -125,9 +125,15 @@ class SingleServoHandle

inline void setCurrTorqueVal(const double& val)
{
curr_torque_val_ = torque_scale_ * angle_sgn_ * val;
curr_torque_val_ = torque_scale_ * angle_sgn_ * val;
}

inline void setTargetTorqueVal(const double& val)
{
target_torque_val_ = val;
}


inline void setName(const string& name){ name_ = name; }
inline void setId(const int& id){ id_ = id; }
inline void setAngleSgn(const int& sgn){ angle_sgn_ = sgn; }
Expand Down Expand Up @@ -164,6 +170,19 @@ class SingleServoHandle
return curr_torque_val_;
}

inline const double getTargetTorqueVal(int value_type) const
{
if(value_type == ValueType::BIT)
{
return target_torque_val_ * angle_sgn_ / torque_scale_;
}
else // Nm
{
return target_torque_val_;
}
}


inline const string& getName(){return name_; }
inline const int& getId() const {return id_; }
inline const int& getAngleSgn() const {return angle_sgn_; }
Expand All @@ -177,6 +196,7 @@ class SingleServoHandle
double curr_angle_val_; // radian
double target_angle_val_; // radian
double curr_torque_val_; // Nm
double target_torque_val_; // Nm
int angle_sgn_;
int zero_point_offset_;
double angle_scale_;
Expand Down Expand Up @@ -206,11 +226,14 @@ class ServoBridge
ros::Subscriber uav_info_sub_;
map<string, ros::Subscriber> servo_states_subs_;
map<string, ros::Subscriber> servo_ctrl_subs_;
map<string, ros::Subscriber> servo_torque_ctrl_subs_;
map<string, ros::Subscriber> servo_torque_subs_;
map<string, bool> no_real_state_flags_;
map<string, ros::Publisher> servo_ctrl_pubs_;
map<string, ros::ServiceServer> servo_torque_ctrl_srvs_;
map<string, ros::Publisher> servo_torque_ctrl_pubs_;
map<string, vector<ros::Publisher> > servo_ctrl_sim_pubs_; // TODO: should be actionlib, trajectory controller
map<string, ros::Publisher> servo_target_pos_pubs_;
map<string, ros::Publisher> servo_target_torque_pubs_;
map<string, ros::ServiceServer> servo_enable_srvs_;
map<string, ros::Publisher> servo_enable_pubs_;
map<string, vector<ros::Publisher> > servo_target_pos_sim_pubs_; // TODO: should be actionlib, trajectory controller

map<string, ServoGroupHandler> servos_handler_;
double moving_check_rate_;
Expand All @@ -222,7 +245,8 @@ class ServoBridge

void servoStatesCallback(const spinal::ServoStatesConstPtr& state_msg, const std::string& servo_group_name);
void servoCtrlCallback(const sensor_msgs::JointStateConstPtr& joints_ctrl_msg, const std::string& servo_group_name);
bool servoTorqueCtrlCallback(std_srvs::SetBool::Request &req, std_srvs::SetBool::Response &res, const std::string& servo_group_name);
void servoTorqueCtrlCallback(const sensor_msgs::JointStateConstPtr& joints_ctrl_msg, const std::string& servo_group_name);
bool servoEnableCallback(std_srvs::SetBool::Request &req, std_srvs::SetBool::Response &res, const std::string& servo_group_name);
void uavInfoCallback(const spinal::UavInfoConstPtr& uav_msg);

public:
Expand Down
148 changes: 123 additions & 25 deletions aerial_robot_model/src/servo_bridge/servo_bridge.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -62,17 +62,20 @@ ServoBridge::ServoBridge(ros::NodeHandle nh, ros::NodeHandle nhp): nh_(nh),nhp_(

/* common param and topics (between servo_bridge and spinal_ros_bridge) */
ros::NodeHandle nh_controller(nh_, "servo_controller");
std::string state_sub_topic, ctrl_pub_topic, torque_pub_topic;
std::string state_sub_topic, pos_pub_topic, torque_pub_topic, servo_enable_pub_topic;
nh_controller.param("state_sub_topic", state_sub_topic, std::string("servo/states"));
nh_controller.param("ctrl_pub_topic", ctrl_pub_topic, std::string("servo/target_states"));
nh_controller.param("torque_pub_topic", torque_pub_topic, std::string("servo/torque_enable"));
nh_controller.param("pos_pub_topic", pos_pub_topic, std::string("servo/target_states"));
nh_controller.param("torque_pub_topic", torque_pub_topic, std::string("servo/target_current"));
nh_controller.param("servo_enable_pub_topic", servo_enable_pub_topic, std::string("servo/torque_enable"));
/* common subsriber: get servo states (i.e. joint angles) from real machine (spinal_ros_bridge) */
servo_states_subs_.insert(make_pair("common", nh_.subscribe<spinal::ServoStates>(state_sub_topic, 10, boost::bind(&ServoBridge::servoStatesCallback, this, _1, "common"))));
/* common publisher: target servo state to real machine (spinal_ros_bridge) */
servo_ctrl_pubs_.insert(make_pair("common", nh_.advertise<spinal::ServoControlCmd>(ctrl_pub_topic, 1)));
/* common publisher: target servo position to real machine (spinal_ros_bridge) */
servo_target_pos_pubs_.insert(make_pair("common", nh_.advertise<spinal::ServoControlCmd>(pos_pub_topic, 1)));
mujoco_control_input_pub_ = nh_.advertise<sensor_msgs::JointState>("mujoco/ctrl_input", 1);
/* common publisher: torque on/off command */
servo_torque_ctrl_pubs_.insert(make_pair("common", nh_.advertise<spinal::ServoTorqueCmd>(torque_pub_topic, 1)));
/* common publisher: target servo torque to real machine (spinal_ros_bridge) */
servo_target_torque_pubs_.insert(make_pair("common", nh_.advertise<spinal::ServoControlCmd>(torque_pub_topic, 1)));
/* common publisher: servo on/off flag to real machine (spinal_ros_bridge) */
servo_enable_pubs_.insert(make_pair("common", nh_.advertise<spinal::ServoTorqueCmd>(servo_enable_pub_topic, 1)));
/* common publisher: joint profiles */
joint_profile_pub_ = nh_.advertise<spinal::JointProfiles>("joint_profiles",1);
/* subscriber: uav info */
Expand All @@ -96,27 +99,35 @@ ServoBridge::ServoBridge(ros::NodeHandle nh, ros::NodeHandle nhp): nh_(nh),nhp_(
no_real_state_flags_.at(group_name) = servo_group_params.second["no_real_state"];

/* pub, sub, and srv */
/* mandatory subscriber: target servo state from controller */
/* mandatory subscriber: target servo command (e.g., position, torque) from controller */
servo_ctrl_subs_.insert(make_pair(group_name, nh_.subscribe<sensor_msgs::JointState>(group_name + string("_ctrl"), 10, boost::bind(&ServoBridge::servoCtrlCallback, this, _1, group_name))));
/* mandatory subscriber: only target servo troque command from controller */
servo_torque_ctrl_subs_.insert(make_pair(group_name, nh_.subscribe<sensor_msgs::JointState>(group_name + string("_torque_ctrl"), 10, boost::bind(&ServoBridge::servoTorqueCtrlCallback, this, _1, group_name))));
/* mandatory service: get torque enalbe/disable flag */
servo_torque_ctrl_srvs_.insert(make_pair(group_name, nh_.advertiseService<std_srvs::SetBool::Request, std_srvs::SetBool::Response>(servo_group_params.first + string("/torque_enable"), boost::bind(&ServoBridge::servoTorqueCtrlCallback, this, _1, _2, servo_group_params.first))));
servo_enable_srvs_.insert(make_pair(group_name, nh_.advertiseService<std_srvs::SetBool::Request, std_srvs::SetBool::Response>(servo_group_params.first + string("/torque_enable"), boost::bind(&ServoBridge::servoEnableCallback, this, _1, _2, servo_group_params.first))));

if(servo_group_params.second.hasMember("state_sub_topic"))
{
/* option: servo states (i.e. joint angles) from real machine (spinal_ros_bridge) */
servo_states_subs_.insert(make_pair(group_name, nh_.subscribe<spinal::ServoStates>((string)servo_group_params.second["state_sub_topic"], 10, boost::bind(&ServoBridge::servoStatesCallback, this, _1, servo_group_params.first))));
}

if(servo_group_params.second.hasMember("ctrl_pub_topic"))
if(servo_group_params.second.hasMember("pos_pub_topic"))
{
/* option: publish target servo state to real machine (spinal_ros_bridge) */
servo_ctrl_pubs_.insert(make_pair(group_name, nh_.advertise<spinal::ServoControlCmd>(servo_group_params.second["ctrl_pub_topic"], 1)));
servo_target_pos_pubs_.insert(make_pair(group_name, nh_.advertise<spinal::ServoControlCmd>(servo_group_params.second["pos_pub_topic"], 1)));
}

if(servo_group_params.second.hasMember("torque_pub_topic"))
{
/* option: torque on/off */
servo_torque_ctrl_pubs_.insert(make_pair(group_name, nh_.advertise<spinal::ServoTorqueCmd>((string)servo_group_params.second["torque_pub_topic"], 1)));
servo_target_torque_pubs_.insert(make_pair(group_name, nh_.advertise<spinal::ServoControlCmd>(servo_group_params.second["torque_pub_topic"], 1)));
}

if(servo_group_params.second.hasMember("servo_enable_pub_topic"))
{
/* option: torque on/off */
servo_enable_pubs_.insert(make_pair(group_name, nh_.advertise<spinal::ServoTorqueCmd>((string)servo_group_params.second["servo_enable_pub_topic"], 1)));
}

/* servo handler */
Expand Down Expand Up @@ -202,15 +213,15 @@ ServoBridge::ServoBridge(ros::NodeHandle nh, ros::NodeHandle nhp): nh_(nh),nhp_(
ROS_ERROR("Failed to call service %s", controller_starter.getService().c_str());

/* init the servo command publisher to the controller */
servo_ctrl_sim_pubs_[servo_group_params.first].push_back(nh_.advertise<std_msgs::Float64>(load_srv.request.name + string("/command"), 1));
servo_target_pos_sim_pubs_[servo_group_params.first].push_back(nh_.advertise<std_msgs::Float64>(load_srv.request.name + string("/command"), 1));
// wait for the publisher initialization
while(servo_ctrl_sim_pubs_[servo_group_params.first].back().getNumSubscribers() == 0 && ros::ok())
while(servo_target_pos_sim_pubs_[servo_group_params.first].back().getNumSubscribers() == 0 && ros::ok())
ros::Duration(0.1).sleep();

/* set the initial angle */
std_msgs::Float64 msg;
nh_.getParam(string("servo_controller/") + servo_group_params.first + string("/") + servo_params.first + string("/simulation/init_value"), msg.data);
servo_ctrl_sim_pubs_[servo_group_params.first].back().publish(msg);
servo_target_pos_sim_pubs_[servo_group_params.first].back().publish(msg);
}
}
}
Expand Down Expand Up @@ -303,6 +314,7 @@ void ServoBridge::servoStatesCallback(const spinal::ServoStatesConstPtr& state_m
void ServoBridge::servoCtrlCallback(const sensor_msgs::JointStateConstPtr& servo_ctrl_msg, const string& servo_group_name)
{
spinal::ServoControlCmd target_angle_msg;
spinal::ServoControlCmd target_torque_msg;
sensor_msgs::JointState mujoco_control_input_msg;

if(servo_ctrl_msg->name.size() > 0)
Expand Down Expand Up @@ -333,11 +345,20 @@ void ServoBridge::servoCtrlCallback(const sensor_msgs::JointStateConstPtr& servo
target_angle_msg.index.push_back((*servo_handler)->getId());
target_angle_msg.angles.push_back((*servo_handler)->getTargetAngleVal(ValueType::BIT));

// process torque command if necessary
if(servo_ctrl_msg->effort.size() == servo_ctrl_msg->name.size())
{
double torque = servo_ctrl_msg->effort.at(i);
(*servo_handler)->setTargetTorqueVal(torque);
target_torque_msg.index.push_back((*servo_handler)->getId());
target_torque_msg.angles.push_back((*servo_handler)->getTargetTorqueVal(ValueType::BIT));
}

if(simulation_mode_)
{
std_msgs::Float64 msg;
msg.data = servo_ctrl_msg->position[i];
servo_ctrl_sim_pubs_[servo_group_name].at(distance(servos_handler_[servo_group_name].begin(), servo_handler)).publish(msg);
servo_target_pos_sim_pubs_[servo_group_name].at(distance(servos_handler_[servo_group_name].begin(), servo_handler)).publish(msg);
}
}
}
Expand All @@ -346,7 +367,7 @@ void ServoBridge::servoCtrlCallback(const sensor_msgs::JointStateConstPtr& servo

if(servo_ctrl_msg->position.size() != servos_handler_[servo_group_name].size())
{
ROS_ERROR("[servo bridge, servo control control]: the joint num from rosparam %d is not equal with ros msgs %d",
ROS_ERROR("[servo bridge, servo ctrl control]: the joint num from rosparam %d is not equal with ros msgs %d",
(int)servos_handler_[servo_group_name].size(), (int)servo_ctrl_msg->position.size());
return;
}
Expand All @@ -359,27 +380,104 @@ void ServoBridge::servoCtrlCallback(const sensor_msgs::JointStateConstPtr& servo
target_angle_msg.index.push_back(servo_handler->getId());
target_angle_msg.angles.push_back(servo_handler->getTargetAngleVal(ValueType::BIT));

// process torque command if necessary
if(servo_ctrl_msg->effort.size() == servo_ctrl_msg->position.size())
{
double torque = servo_ctrl_msg->effort.at(i);
servo_handler->setTargetTorqueVal(torque);
target_torque_msg.index.push_back(servo_handler->getId());
target_torque_msg.angles.push_back(servo_handler->getTargetTorqueVal(ValueType::BIT));
}

mujoco_control_input_msg.name.push_back(servo_handler->getName());
mujoco_control_input_msg.position.push_back(servo_ctrl_msg->position.at(i));

if(simulation_mode_)
{
std_msgs::Float64 msg;
msg.data = servo_ctrl_msg->position[i];
servo_ctrl_sim_pubs_[servo_group_name].at(i).publish(msg);
servo_target_pos_sim_pubs_[servo_group_name].at(i).publish(msg);
}
}
}

mujoco_control_input_pub_.publish(mujoco_control_input_msg);

if (servo_ctrl_pubs_.find(servo_group_name) != servo_ctrl_pubs_.end())
servo_ctrl_pubs_[servo_group_name].publish(target_angle_msg);
if (servo_target_pos_pubs_.find(servo_group_name) != servo_target_pos_pubs_.end())
servo_target_pos_pubs_[servo_group_name].publish(target_angle_msg);
else
servo_target_pos_pubs_["common"].publish(target_angle_msg);


// process torque command if necessary
if (target_torque_msg.index.size() == 0) return;
if (servo_target_torque_pubs_.find(servo_group_name) != servo_target_torque_pubs_.end())
servo_target_torque_pubs_[servo_group_name].publish(target_torque_msg);
else
servo_target_torque_pubs_["common"].publish(target_torque_msg);
}

void ServoBridge::servoTorqueCtrlCallback(const sensor_msgs::JointStateConstPtr& servo_ctrl_msg, const string& servo_group_name)
{
spinal::ServoControlCmd target_torque_msg;

if(servo_ctrl_msg->name.size() > 0)
{
for(int i = 0; i < servo_ctrl_msg->name.size(); i++)
{/* servo name is assigned */

if(servo_ctrl_msg->effort.size() != servo_ctrl_msg->name.size())
{
ROS_ERROR("[servo bridge, servo torque control]: the servo effort (torque) num and name num are different in ros msgs [%d vs %d]",
(int)servo_ctrl_msg->effort.size(), (int)servo_ctrl_msg->name.size());
return;
}

// use servo_name to search the servo_handler
auto servo_handler = find_if(servos_handler_[servo_group_name].begin(), servos_handler_[servo_group_name].end(),
[&](SingleServoHandlePtr s) {return servo_ctrl_msg->name.at(i) == s->getName();});

if(servo_handler == servos_handler_[servo_group_name].end())
{
ROS_ERROR("[servo bridge, servo control callback]: no matching servo handler for %s", servo_ctrl_msg->name.at(i).c_str());
return;
}

double torque = servo_ctrl_msg->effort.at(i);
(*servo_handler)->setTargetTorqueVal(torque);
target_torque_msg.index.push_back((*servo_handler)->getId());
target_torque_msg.angles.push_back((*servo_handler)->getTargetTorqueVal(ValueType::BIT));
}
}
else
{ /* for fast tranmission: no searching process, in the predefine order */

if(servo_ctrl_msg->position.size() != servos_handler_[servo_group_name].size())
{
ROS_ERROR("[servo bridge, servo control control]: the joint num from rosparam %d is not equal with ros msgs %d",
(int)servos_handler_[servo_group_name].size(), (int)servo_ctrl_msg->position.size());
return;
}

for(int i = 0; i < servo_ctrl_msg->position.size(); i++)
{
/* use the kinematics order (e.g. joint1 ~ joint N, gimbal_roll -> gimbal_pitch) */
SingleServoHandlePtr servo_handler = servos_handler_[servo_group_name].at(i);
double torque = servo_ctrl_msg->effort.at(i);

servo_handler->setTargetTorqueVal(torque);
target_torque_msg.index.push_back(servo_handler->getId());
target_torque_msg.angles.push_back(servo_handler->getTargetTorqueVal(ValueType::BIT));
}
}

if (servo_target_torque_pubs_.find(servo_group_name) != servo_target_torque_pubs_.end())
servo_target_torque_pubs_[servo_group_name].publish(target_torque_msg);
else
servo_ctrl_pubs_["common"].publish(target_angle_msg);
servo_target_torque_pubs_["common"].publish(target_torque_msg);
}

bool ServoBridge::servoTorqueCtrlCallback(std_srvs::SetBool::Request &req, std_srvs::SetBool::Response &res, const std::string& servo_group_name)
bool ServoBridge::servoEnableCallback(std_srvs::SetBool::Request &req, std_srvs::SetBool::Response &res, const std::string& servo_group_name)
{
spinal::ServoTorqueCmd torque_msg;
for(auto servo_handler: servos_handler_[servo_group_name])
Expand All @@ -388,10 +486,10 @@ bool ServoBridge::servoTorqueCtrlCallback(std_srvs::SetBool::Request &req, std_s
torque_msg.torque_enable.push_back(req.data);
}

if (servo_torque_ctrl_pubs_.find(servo_group_name) != servo_torque_ctrl_pubs_.end())
servo_torque_ctrl_pubs_[servo_group_name].publish(torque_msg);
if (servo_enable_pubs_.find(servo_group_name) != servo_enable_pubs_.end())
servo_enable_pubs_[servo_group_name].publish(torque_msg);
else
servo_torque_ctrl_pubs_["common"].publish(torque_msg);
servo_enable_pubs_["common"].publish(torque_msg);

return true;
}
Expand Down
1 change: 1 addition & 0 deletions aerial_robot_nerve/neuron/neuronlib/CAN/can_constants.h
View file
Original file line number Diff line number Diff line change
Expand Up @@ -30,6 +30,7 @@ namespace CAN {
constexpr uint8_t MESSAGEID_RECEIVE_PWM_0_5 = 0;
constexpr uint8_t MESSAGEID_RECEIVE_PWM_6_11 = 1;
constexpr uint8_t MESSAGEID_RECEIVE_SERVO_ANGLE = 0;
constexpr uint8_t MESSAGEID_RECEIVE_SERVO_CURRENT = 1;
constexpr uint8_t MESSAGEID_RECEIVE_SERVO_CONFIG = 15;
constexpr uint8_t MESSAGEID_SEND_SERVO_LIST[4] = {0, 1, 2, 3};
constexpr uint8_t MESSAGEID_RECEIVE_ENUM_REQUEST = 0;
Expand Down
17 changes: 9 additions & 8 deletions aerial_robot_nerve/neuron/neuronlib/Initializer/initializer.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -39,15 +39,16 @@ void Initializer::sendBoardConfig()
data[5] = s.current_limit_ & 0xFF;
data[6] = (s.current_limit_ >> 8) & 0xFF;
data[7] = (s.send_data_flag_ ? 1 : 0);
data[7] += ((s.external_encoder_flag_ ? 1 : 0) << 1);
sendMessage(CAN::MESSAGEID_SEND_INITIAL_CONFIG_2, m_slave_id, 8, data, 1);
data[0] = i;
data[1] = s.hardware_error_status_;
data[2] = (s.external_encoder_flag_ ? 1 : 0);
data[3] = s.joint_resolution_ & 0xFF;
data[4] = (s.joint_resolution_ >> 8) & 0xFF;
data[5] = s.servo_resolution_ & 0xFF;
data[6] = (s.servo_resolution_ >> 8) & 0xFF;
data[7] = 0;
data[0] = i;
data[1] = s.hardware_error_status_;
data[2] = s.goal_current_ & 0xFF;
data[3] = (s.goal_current_ >> 8) & 0xFF;
data[4] = s.joint_resolution_ & 0xFF;
data[5] = (s.joint_resolution_ >> 8) & 0xFF;
data[6] = s.servo_resolution_ & 0xFF;
data[7] = (s.servo_resolution_ >> 8) & 0xFF;
sendMessage(CAN::MESSAGEID_SEND_INITIAL_CONFIG_3, m_slave_id, 8, data, 1);
}
}
Expand Down
Loading

0 comments on commit 49418eb

Please sign in to comment.