servo config yaml file about spotmicro

[ohmbrew]

It might help, here is the applicable part of my servo config file. Since the Pololu is commanded using microsecond values, I have converted all of your code to handle ~500-2500 values instead of the PWM values you had.

num_servos: 12
servo_max_angle_deg: 82.5
RF_3: {num: 18, center: 1500,range: 1816.8,direction: 1, center_angle_deg: 73.02}
RF_2: {num: 17, center: 1500,range: 1860.8,direction: 1, center_angle_deg: -36.27}
RF_1: {num: 16, center: 1500,range: 1932.3,direction: -1, center_angle_deg: 2.82}
RB_3: {num: 7, center: 1500,range: 1723.3,direction: 1, center_angle_deg: 47.87}
RB_2: {num: 6, center: 1500,range: 1851.7,direction: 1, center_angle_deg: -45.45}
RB_1: {num: 5, center: 1500,range: 1892,direction: 1, center_angle_deg: 0}
LB_3: {num: 3, center: 1500,range: 1833.3,direction: 1, center_angle_deg: -32.76}
LB_2: {num: 2, center: 1500,range: 1822.3,direction: 1, center_angle_deg: 40.93}
LB_1: {num: 1, center: 1500,range: 1650,direction: -1, center_angle_deg: 7}
LF_3: {num: 15, center: 1500,range: 1778.3,direction: 1, center_angle_deg: -68.01}
LF_2: {num: 14, center: 1500,range: 1750.8,direction: 1, center_angle_deg: 51.83}
LF_1: {num: 13, center: 1500,range: 1943.3,direction: 1, center_angle_deg: 8.32}

[ohmbrew]

Update for anyone reading this and using other servo controllers instead of the PCA. Along with other code changes mentioned above, I did fix the code in spot_micro_motion_cmd.cpp to allow for any channels to be used, and not have to default for the first 12 (connected in any configuration, too, as long as your servo config file lists the channel numbers correctly). Had to modify publishServoProportionalCommand() method, and replace:

servo_array_.servos[servo_num-1].servo = servo_num;
servo_array_.servos[servo_num-1].value = servo_proportional_cmd;

with

servo_array_.servos[array_ctr].servo = servo_num;
servo_array_.servos[array_ctr].value = servo_proportional_cmd;

and use array_ctr as an arbitrary index, instead of relying on the servo_num to specify index. Of course you'll have to increment array_ctr at the end of the for() loop.

I am still getting getting Joint LB_3 angles outside acceptable ranges, causing it to clip to -1, but it goes away after several messages are sent (for "stand") because the angle goes from -120 to within an acceptable value.

[mike4192]

I'm glad you were able to figure out how to modify the code to solve your issue. I'll reread through your comments again at some point to get a better understanding of your issue and see if there is a better way to architect the code so someone doesn't run into a similar problem, or so the code isn't dependent on monotonically increasing servo numbers.

The debug statements you saw indicate that a commanded servo angle was calculated that would have been beyond the servo's max angle specified in the configuration file. The command is instead clipped that max servo angle, and that debug statement is printed out. The debug statement prints out a range to -1 to 1 as the command angle is converted to a proportional value in the range between +/- 1, as that is what the servo control node takes in.

Since you say these messages go away after the robot stands, its probably because in the sit state some servo angles are indeed being commanded beyond the maximum angle allowed in your configuration, but after standing, the leg "straightens out" and gets within an achievable command angle.

You mentioned your left and right lower leg center angles are significantly different from mine (the knee joint). This is dependent on what angle you installed the servo horn onto your servo relative to the servo's center position. If my values for those center angles for the knee joint are about 90 degrees, that probably means I installed my servo horns such that when the servo is at it's center command value, the lower leg looks about a 90 degree angle to the upper leg link.

Since your values are more like 32 to 47 degrees, that probably means you installed your servo horns so when the servo's are at their center, the lower leg links appear more "straightened out" to the upper leg link. That seems to correlate with you experiencing issues when the robot is sitting, as sitting requires the lower leg link bent close together to the upper leg link.

I can draw some diagrams if the explanation doesn't make sense, but I think you should reposition your servo horn so the lower leg link is roughly at 90 deg to the upper leg link when the servo is at center, and then re calibrate those links. You want the servo's working range to line up with a leg link's typical range of motion. For example, you wouldn't want the servo to be able to command the knee beyond straight and then "backwards", as that's just wasted leg range.

[ohmbrew]

Thanks Mike. It seems I am installing the leg correctly, it is approximately 90 degrees from the upper leg, so I'm not sure what's going on there. I have since re-installed new servos, and made sure orientations were correct. No biggie...it still operates within parameters. Here is the full code I changed in spot_micro_motion_cmd.cpp. I simply added an array counter and used that value for indices instead of the servo number and incremented appropriately.

void SpotMicroMotionCmd::publishServoProportionalCommand() {
    int array_ctr = 0;  // using an index counter instead of servo number for servo array index

    for (std::map<std::string, std::map<std::string, float>>::iterator
       iter = smnc_.servo_config.begin();
       iter != smnc_.servo_config.end();
       ++iter) {
 
    std::string servo_name = iter->first;
    std::map<std::string, float> servo_config_params = iter->second;
    
    int servo_num = servo_config_params["num"];
    float cmd_ang_rad = servo_cmds_rad_[servo_name];
    float center_ang_rad = servo_config_params["center_angle_deg"]*M_PI/180.0f;
    float servo_proportional_cmd = (cmd_ang_rad - center_ang_rad) /
                                   (smnc_.servo_max_angle_deg*M_PI/180.0f);
 
    if (servo_proportional_cmd > 1.0f) {
      servo_proportional_cmd = 1.0f;
      ROS_WARN("Proportional Command above +1.0 was computed, clipped to 1.0");
      ROS_WARN("Joint %s, Angle: %1.2f", servo_name.c_str(), cmd_ang_rad*180.0/M_PI);
 
    } else if (servo_proportional_cmd < -1.0f) {
      servo_proportional_cmd = -1.0f;
      ROS_WARN("Proportional Command below -1.0 was computed, clipped to -1.0");
      ROS_WARN("Joint %s, Angle: %1.2f", servo_name.c_str(), cmd_ang_rad*180.0/M_PI);
    }
 
    servo_array_.servos[array_ctr].servo = servo_num;
    servo_array_.servos[array_ctr].value = servo_proportional_cmd; 
    array_ctr++;
 }

 // Publish message
 servos_proportional_pub_.publish(servo_array_);
}

[mike4192]

Thanks for the comment with your code modification, I think I understand the issue now. I'll try to refactor some things so an arbitrary servo numbering will be supported.