HiveCoreRD/hivecore_robot_motion
6
0
Fork 0
Code Issues Pull Requests Actions Packages Projects Releases Wiki Activity

add more motors

Browse Source
This commit is contained in:
NuoDaJia02
2025-10-21 17:50:12 +08:00
parent fe42a1e25e
commit 5cdb04dc9d
3 changed files with 10 additions and 11 deletions
Download Patch File Download Diff File Expand all files Collapse all files

2
src/robot_control/include/motion_parameters.hpp
View File

@@ -114,7 +114,7 @@ namespace robot_control
//TODO: check the ratio
joint_gear_ratio_ = {100.0, 100.0, 100.0, 100.0, 100.0, 100.0, 100.0, 100.0, 100.0, 100.0}; // 传动比 都一样
joint_resolution_ = {524288, 131072 , 131072, 131072, 131072, 131072, 131072, 131072, 131072, 131072}; // 分辨率 都一样
joint_resolution_ = {131072, 131072 , 131072, 131072, 524288, 131072, 131072, 131072, 131072, 131072}; // 分辨率 都一样
//TODO: 限位需要修改

17
src/robot_control/src/robot_control_manager.cpp
View File

@@ -145,8 +145,8 @@ void RobotControlManager::UpdateJointStates(const sensor_msgs::msg::JointState::
jointVelocities_.resize(motionParams_.total_joints_);
jointEfforts_.resize(motionParams_.total_joints_);
// for (size_t i = 0; i < motionParams_.total_joints_; i++)
for (size_t i = 0; i < 3; i++)
for (size_t i = 0; i < motionParams_.total_joints_; i++)
// for (size_t i = 0; i < 3; i++)
{
std::string joint_name = jointStates_.name[i];
size_t index = 1e5;
@@ -177,8 +177,7 @@ void RobotControlManager::UpdateJointStates(const sensor_msgs::msg::JointState::
// TODO: This block can be deleted
// for (size_t i = 0; i < motionParams_.total_joints_; i++)
for (size_t i = 0; i < 3; i++)
for (size_t i = 0; i < motionParams_.total_joints_; i++)
{
if(!jointInited_[i])
{
@@ -216,11 +215,11 @@ void RobotControlManager::UpdateJointStates(const sensor_msgs::msg::JointState::
waistController_->UpdateJointStates(waistPositions, waistVelocities, waistEfforts);
// Update the leg controller
// size_t legJointSize = motionParams_.leg_joint_indices_.size();
// std::vector<double> legPositions(jointPositions_.begin() + motionParams_.leg_joint_indices_[0], jointPositions_.begin() + motionParams_.leg_joint_indices_[0] + legJointSize);
// std::vector<double> legVelocities(jointVelocities_.begin() + motionParams_.leg_joint_indices_[0], jointVelocities_.begin() + motionParams_.leg_joint_indices_[0] + legJointSize);
// std::vector<double> legEfforts(jointEfforts_.begin() + motionParams_.leg_joint_indices_[0], jointEfforts_.begin() + motionParams_.leg_joint_indices_[0] + legJointSize);
// legController_->UpdateJointStates(legPositions, legVelocities, legEfforts);
size_t legJointSize = motionParams_.leg_joint_indices_.size();
std::vector<double> legPositions(jointPositions_.begin() + motionParams_.leg_joint_indices_[0], jointPositions_.begin() + motionParams_.leg_joint_indices_[0] + legJointSize);
std::vector<double> legVelocities(jointVelocities_.begin() + motionParams_.leg_joint_indices_[0], jointVelocities_.begin() + motionParams_.leg_joint_indices_[0] + legJointSize);
std::vector<double> legEfforts(jointEfforts_.begin() + motionParams_.leg_joint_indices_[0], jointEfforts_.begin() + motionParams_.leg_joint_indices_[0] + legJointSize);
legController_->UpdateJointStates(legPositions, legVelocities, legEfforts);
}
MotionParameters RobotControlManager::GetMotionParameters()

2
src/robot_control/src/robot_control_node.cpp
View File

@@ -525,7 +525,7 @@ void RobotControlNode::Publish_joint_trajectory()
std::stringstream msg_stream;
size_t total_joints = robotControlManager_.GetMotionParameters().total_joints_;
for (size_t i = 0; i < 3; ++i)
for (size_t i = 0; i < total_joints; ++i)
{
double current_pos = cmd_msg.data[i];