Skip to content

Commit

Permalink
apply clang-format
Browse files Browse the repository at this point in the history
  • Loading branch information
@Alpaca-zip
Alpaca-zip committed Sep 9, 2023
1 parent 490dd4e commit 933801c
Show file tree
Hide file tree
Showing 11 changed files with 227 additions and 150 deletions.
12 changes: 6 additions & 6 deletions include/dynamixel_control.h
View file
Original file line number Diff line number Diff line change
Expand Up @@ -55,17 +55,17 @@ class dynamixelControl
int _model_number_int;
uint8_t _dxl_id[12];
uint16_t _model_number;
const char * _log;
const char * _port_name;
const char* _log;
const char* _port_name;
DynamixelWorkbench _dxl_wb;

public:
dynamixelControl();
void controlLoop();
void monitorLFLegCallback(const trajectory_msgs::JointTrajectory & LF_leg);
void monitorLRLegCallback(const trajectory_msgs::JointTrajectory & LR_leg);
void monitorRRLegCallback(const trajectory_msgs::JointTrajectory & RR_leg);
void monitorRFLegCallback(const trajectory_msgs::JointTrajectory & RF_leg);
void monitorLFLegCallback(const trajectory_msgs::JointTrajectory& LF_leg);
void monitorLRLegCallback(const trajectory_msgs::JointTrajectory& LR_leg);
void monitorRRLegCallback(const trajectory_msgs::JointTrajectory& RR_leg);
void monitorRFLegCallback(const trajectory_msgs::JointTrajectory& RF_leg);
void dxlInit();
void dxlTorqueOn();
void dxlAddSyncWriteHandler();
Expand Down
2 changes: 1 addition & 1 deletion include/inverse_kinematics.h
View file
Original file line number Diff line number Diff line change
Expand Up @@ -39,5 +39,5 @@ class inverseKinematics

public:
inverseKinematics();
void inverseKinematicsCallback(const std_msgs::Float64MultiArray & leg_position);
void inverseKinematicsCallback(const std_msgs::Float64MultiArray& leg_position);
};
6 changes: 3 additions & 3 deletions include/posture_stabilization.h
View file
Original file line number Diff line number Diff line change
Expand Up @@ -59,7 +59,7 @@ class postureStabilization
public:
postureStabilization();
void controlLoop();
void imuCallback(const sensor_msgs::Imu & msg);
void quatToRPY(const geometry_msgs::Quaternion quat, double & roll, double & pitch, double & yaw);
void postureControlCallback(const std_msgs::Bool & posture_control);
void imuCallback(const sensor_msgs::Imu& msg);
void quatToRPY(const geometry_msgs::Quaternion quat, double& roll, double& pitch, double& yaw);
void postureControlCallback(const std_msgs::Bool& posture_control);
};
2 changes: 1 addition & 1 deletion include/standing_motion.h
View file
Original file line number Diff line number Diff line change
Expand Up @@ -35,5 +35,5 @@ class standingMotion

public:
standingMotion();
void standingMotionCallback(const std_msgs::Bool & stand);
void standingMotionCallback(const std_msgs::Bool& stand);
};
8 changes: 4 additions & 4 deletions include/trot_gait.h
View file
Original file line number Diff line number Diff line change
Expand Up @@ -46,10 +46,10 @@ class trotGait
public:
trotGait();
void controlLoop();
void trotFowardMotionCallback(const std_msgs::Float64 & direction_x);
void trotTurnMotionCallback(const std_msgs::Float64 & turn);
void stopSignalCallback(const std_msgs::Bool & stop);
void stabilizationVariableCallback(const std_msgs::Float64MultiArray & MV);
void trotFowardMotionCallback(const std_msgs::Float64& direction_x);
void trotTurnMotionCallback(const std_msgs::Float64& turn);
void stopSignalCallback(const std_msgs::Bool& stop);
void stabilizationVariableCallback(const std_msgs::Float64MultiArray& MV);
void trot(double c0_x, double c0_y, bool inv);
void countC(const double step_extent_x, const int l);
double rDirX();
Expand Down
75 changes: 45 additions & 30 deletions src/dynamixel_control.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -21,11 +21,15 @@ dynamixelControl::dynamixelControl() : _pnh("~")
_pnh.param<std::string>("port", _port_name_str, "/dev/ttyACM1");
_pnh.param<int>("model_number", _model_number_int, 12);
_pnh.param<int>("baudrate", _baudrate, 1000000);
if (_pnh.getParam("dxl_ids", _dxl_id_vector) && _dxl_id_vector.size() == 12) {
for (int i = 0; i < 12; ++i) {
if (_pnh.getParam("dxl_ids", _dxl_id_vector) && _dxl_id_vector.size() == 12)
{
for (int i = 0; i < 12; ++i)
{
_dxl_id[i] = static_cast<uint8_t>(_dxl_id_vector[i]);
}
} else {
}
else
{
_dxl_id[0] = LF_LEG_SHOULDER_ID;
_dxl_id[1] = LF_LEG_UPPER_ID;
_dxl_id[2] = LF_LEG_LOWER_ID;
Expand All @@ -40,14 +44,10 @@ dynamixelControl::dynamixelControl() : _pnh("~")
_dxl_id[11] = RF_LEG_LOWER_ID;
}

_sub_LF_leg = _nh.subscribe(
"leftfront_leg_controller/command", 10, &dynamixelControl::monitorLFLegCallback, this);
_sub_LR_leg = _nh.subscribe(
"leftback_leg_controller/command", 10, &dynamixelControl::monitorLRLegCallback, this);
_sub_RR_leg = _nh.subscribe(
"rightback_leg_controller/command", 10, &dynamixelControl::monitorRRLegCallback, this);
_sub_RF_leg = _nh.subscribe(
"rightfront_leg_controller/command", 10, &dynamixelControl::monitorRFLegCallback, this);
_sub_LF_leg = _nh.subscribe("leftfront_leg_controller/command", 10, &dynamixelControl::monitorLFLegCallback, this);
_sub_LR_leg = _nh.subscribe("leftback_leg_controller/command", 10, &dynamixelControl::monitorLRLegCallback, this);
_sub_RR_leg = _nh.subscribe("rightback_leg_controller/command", 10, &dynamixelControl::monitorRRLegCallback, this);
_sub_RF_leg = _nh.subscribe("rightfront_leg_controller/command", 10, &dynamixelControl::monitorRFLegCallback, this);

_joint_pos[0].data = 0; // LF_leg_shoulder_joint
_joint_pos[1].data = 1.5; // LF_leg_upper_joint
Expand All @@ -70,30 +70,34 @@ dynamixelControl::dynamixelControl() : _pnh("~")
dxlAddSyncWriteHandler();
}

void dynamixelControl::monitorLFLegCallback(const trajectory_msgs::JointTrajectory & LF_leg)
void dynamixelControl::monitorLFLegCallback(const trajectory_msgs::JointTrajectory& LF_leg)
{
for (int i = 0; i < 3; i++) {
for (int i = 0; i < 3; i++)
{
_joint_pos[i].data = LF_leg.points[0].positions[i];
}
}

void dynamixelControl::monitorLRLegCallback(const trajectory_msgs::JointTrajectory & LR_leg)
void dynamixelControl::monitorLRLegCallback(const trajectory_msgs::JointTrajectory& LR_leg)
{
for (int i = 3; i < 6; i++) {
for (int i = 3; i < 6; i++)
{
_joint_pos[i].data = LR_leg.points[0].positions[i - 3];
}
}

void dynamixelControl::monitorRRLegCallback(const trajectory_msgs::JointTrajectory & RR_leg)
void dynamixelControl::monitorRRLegCallback(const trajectory_msgs::JointTrajectory& RR_leg)
{
for (int i = 6; i < 9; i++) {
for (int i = 6; i < 9; i++)
{
_joint_pos[i].data = RR_leg.points[0].positions[i - 6];
}
}

void dynamixelControl::monitorRFLegCallback(const trajectory_msgs::JointTrajectory & RF_leg)
void dynamixelControl::monitorRFLegCallback(const trajectory_msgs::JointTrajectory& RF_leg)
{
for (int i = 9; i < 12; i++) {
for (int i = 9; i < 12; i++)
{
_joint_pos[i].data = RF_leg.points[0].positions[i - 9];
}
}
Expand All @@ -102,11 +106,13 @@ void dynamixelControl::controlLoop()
{
int32_t goal_position[12];
const uint8_t handler_index = 0;
for (int i = 0; i < 12; i++) {
for (int i = 0; i < 12; i++)
{
goal_position[i] = 512 + 3.41 * 180 * (_joint_pos[i].data / M_PI);
}
_result = _dxl_wb.syncWrite(handler_index, &goal_position[0], &_log);
if (_result == false) {
if (_result == false)
{
ROS_ERROR("%s", _log);
ROS_ERROR("Failed to sync write position");
}
Expand All @@ -115,44 +121,53 @@ void dynamixelControl::controlLoop()
void dynamixelControl::dxlInit()
{
_result = _dxl_wb.init(_port_name, _baudrate, &_log);
if (_result == false) {
if (_result == false)
{
ROS_ERROR("%s", _log);
ROS_ERROR("Failed to init");
} else
}
else
ROS_WARN("Succeed to init(%d)", _baudrate);
}

void dynamixelControl::dxlTorqueOn()
{
for (int cnt = 0; cnt < 12; cnt++) {
for (int cnt = 0; cnt < 12; cnt++)
{
_result = _dxl_wb.ping(_dxl_id[cnt], &_model_number, &_log);
if (_result == false) {
if (_result == false)
{
ROS_ERROR("%s", _log);
ROS_ERROR("Failed to ping");
} else {
}
else
{
ROS_WARN("Succeeded to ping");
ROS_WARN("id : %d, model_number : %d", _dxl_id[cnt], _model_number);
}
_result = _dxl_wb.torqueOn(_dxl_id[cnt]);
if (_result == false) ROS_ERROR("torqueOn ERROR");
if (_result == false)
ROS_ERROR("torqueOn ERROR");
}
}

void dynamixelControl::dxlAddSyncWriteHandler()
{
_result = _dxl_wb.addSyncWriteHandler(_dxl_id[0], "Goal_Position", &_log);
if (_result == false) {
if (_result == false)
{
ROS_ERROR("%s", _log);
ROS_ERROR("Failed to add sync write handler");
}
}

int main(int argc, char ** argv)
int main(int argc, char** argv)
{
ros::init(argc, argv, "dynamixel_control");
dynamixelControl DC;
ros::Rate loop_rate(100);
while (ros::ok()) {
while (ros::ok())
{
DC.controlLoop();
ros::spinOnce();
loop_rate.sleep();
Expand Down
44 changes: 23 additions & 21 deletions src/inverse_kinematics.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -20,16 +20,11 @@ inverseKinematics::inverseKinematics() : _pnh("~")
{
_pnh.param<double>("duration", _duration, 0.01);

_pub_LF_leg =
_nh.advertise<trajectory_msgs::JointTrajectory>("leftfront_leg_controller/command", 10);
_pub_LR_leg =
_nh.advertise<trajectory_msgs::JointTrajectory>("leftback_leg_controller/command", 10);
_pub_RF_leg =
_nh.advertise<trajectory_msgs::JointTrajectory>("rightfront_leg_controller/command", 10);
_pub_RR_leg =
_nh.advertise<trajectory_msgs::JointTrajectory>("rightback_leg_controller/command", 10);
_sub_leg_position =
_nh.subscribe("leg_position", 10, &inverseKinematics::inverseKinematicsCallback, this);
_pub_LF_leg = _nh.advertise<trajectory_msgs::JointTrajectory>("leftfront_leg_controller/command", 10);
_pub_LR_leg = _nh.advertise<trajectory_msgs::JointTrajectory>("leftback_leg_controller/command", 10);
_pub_RF_leg = _nh.advertise<trajectory_msgs::JointTrajectory>("rightfront_leg_controller/command", 10);
_pub_RR_leg = _nh.advertise<trajectory_msgs::JointTrajectory>("rightback_leg_controller/command", 10);
_sub_leg_position = _nh.subscribe("leg_position", 10, &inverseKinematics::inverseKinematicsCallback, this);

_LF_leg.joint_names.resize(3);
_LF_leg.points.resize(1);
Expand Down Expand Up @@ -57,22 +52,23 @@ inverseKinematics::inverseKinematics() : _pnh("~")
_RF_leg.joint_names[2] = "rightfront_leg_lower_joint";
}

void inverseKinematics::inverseKinematicsCallback(const std_msgs::Float64MultiArray & leg_position)
void inverseKinematics::inverseKinematicsCallback(const std_msgs::Float64MultiArray& leg_position)
{
double x, y, z, a0, a1, b0;
double angle1, angle2, angle3;
double target_leg_shoulder_joint, target_L_leg_upper_joint, target_L_leg_lower_joint,
target_R_leg_upper_joint, target_R_leg_lower_joint;
double target_leg_shoulder_joint, target_L_leg_upper_joint, target_L_leg_lower_joint, target_R_leg_upper_joint,
target_R_leg_lower_joint;

for (int l = 0; l < 4; l++) {
for (int l = 0; l < 4; l++)
{
x = leg_position.data[3 * l];
y = leg_position.data[3 * l + 1];
z = leg_position.data[3 * l + 2];

a0 = (y * y + z * z - LENGTH * LENGTH + x * x - 2 * BONE_LENGTH * BONE_LENGTH) /
(2 * BONE_LENGTH * BONE_LENGTH);
a0 = (y * y + z * z - LENGTH * LENGTH + x * x - 2 * BONE_LENGTH * BONE_LENGTH) / (2 * BONE_LENGTH * BONE_LENGTH);

if (l == 0) {
if (l == 0)
{
angle1 = -atan2(-z, -y) - atan2(sqrt(y * y + z * z - LENGTH * LENGTH), LENGTH);
angle3 = atan2(sqrt(1 - a0 * a0), a0);
angle2 = atan2(x, sqrt(y * y + z * z - LENGTH * LENGTH)) -
Expand All @@ -86,7 +82,9 @@ void inverseKinematics::inverseKinematicsCallback(const std_msgs::Float64MultiAr
_LF_leg.header.stamp = ros::Time::now();
_LF_leg.points[0].time_from_start = ros::Duration(_duration);
_pub_LF_leg.publish(_LF_leg);
} else if (l == 1) {
}
else if (l == 1)
{
angle1 = -atan2(-z, -y) - atan2(sqrt(y * y + z * z - LENGTH * LENGTH), LENGTH);
angle3 = atan2(sqrt(1 - a0 * a0), a0);
angle2 = atan2(x, sqrt(y * y + z * z - LENGTH * LENGTH)) -
Expand All @@ -100,7 +98,9 @@ void inverseKinematics::inverseKinematicsCallback(const std_msgs::Float64MultiAr
_LR_leg.header.stamp = ros::Time::now();
_LR_leg.points[0].time_from_start = ros::Duration(_duration);
_pub_LR_leg.publish(_LR_leg);
} else if (l == 2) {
}
else if (l == 2)
{
angle1 = -atan2(-z, -y) - atan2(sqrt(y * y + z * z - LENGTH * LENGTH), -LENGTH);
angle3 = atan2(sqrt(1 - a0 * a0), a0);
angle2 = atan2(x, sqrt(y * y + z * z - LENGTH * LENGTH)) -
Expand All @@ -114,7 +114,9 @@ void inverseKinematics::inverseKinematicsCallback(const std_msgs::Float64MultiAr
_RR_leg.header.stamp = ros::Time::now();
_RR_leg.points[0].time_from_start = ros::Duration(_duration);
_pub_RR_leg.publish(_RR_leg);
} else if (l == 3) {
}
else if (l == 3)
{
angle1 = -atan2(-z, -y) - atan2(sqrt(y * y + z * z - LENGTH * LENGTH), -LENGTH);
angle3 = atan2(sqrt(1 - a0 * a0), a0);
angle2 = atan2(x, sqrt(y * y + z * z - LENGTH * LENGTH)) -
Expand All @@ -132,7 +134,7 @@ void inverseKinematics::inverseKinematicsCallback(const std_msgs::Float64MultiAr
}
}

int main(int argc, char ** argv)
int main(int argc, char** argv)
{
ros::init(argc, argv, "inverse_kinematics");
inverseKinematics IK;
Expand Down
Loading

0 comments on commit 933801c

Please sign in to comment.