servo config yaml file

[ohmbrew]

Mike,

First off, thank you for contributing and actively responding to this awesome project. I have a question about the servo config file and, my implementation of controlling the servos.

My status: I've 3D printed a spot micro, with some cls6336HV servos. Everything's put together, running a Pi 3B and ros melodic with your code (slightly modified). I am using a Pololu Maestro controller (18 channels) instead of your PCA board. I've modded your code to allow me to control any of the 18 channels on the maestro (but see question 2), and I can correctly control any servo from the servo calibration python script.

Question 1: After running through the calibration steps, most of the calculated angles for my servos are generally close to your example config file. However, my lines for Right Back Lower Leg and Left Back Lower Leg appear drastically different. My calculated center angles are 47.87 (yours is 90.42), and -32.76 (yours is -84.08), respectively. When I try to run the servoMoveKeyboard script, I see debug warnings about Joint LB_3, pretty big coincidence. But I have double checked my calibration measurements...any idea what might be going on? The legs do appear to move correctly to a stand position, but the warnings are that the Proportional Command is below -1.0 and is being clipped to -1.0 (the angle is very negative, approx -120). Seems like that should be way beyond what the servo should be going, if I am supposed to be calibrating between 0 and -90 values...

Question 2: The servo wires inside the spot micro are tight, and I'm choosing to use the Maestro channels 0-5 for the rear legs, and channels 12-17 for the front legs. Your code seems to be written for 12 servo channels only (on a PCA), and they must be in the first 12 channel positions on your 16 channel PCA. I've already modded your code to allow for 18 channels, but the discontinuous "array" is causing warnings about non-configured servos (because of the gap). Can I just remove those warnings in the servo controller ros node, or are there other dependencies I need to address?

Again, thanks for your hard work and contributions!

Greg

[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.