Joints trembling at startup, on video

[elpimous]

Hello @qiayuanliao a kp kd problem ? where could I correct those values ?

https://github.com/qiayuanliao/legged_control/assets/8529940/ca4d8ec3-5e38-4c19-8412-9fdfac9e6a38

[elpimous]

@qiayuanliao @LoveThinkinghard any updade, please ?! my send command is in radians/s position mode. pos value is real radian turn, from zero position.

[demirciomer]

hello @elpimous did you find any solution? same issue for us.

[elpimous]

Hey, could you share any video ? Perhaps could we share ?! [email protected]. (joints commands format, refresh, robot specs...) This framework is really impressive. Hope we'll have success on it.

[evan-brooks]

Looks like not enough torque?

[elpimous]

Hey,.... Not silly... I did tests with low maxtorque..

3Nm. Need to test. Thanks friend.

@evan-brooks tested with 10Nm, same result !!

Any other idea ? where could I modify kp/kd ?

[elpimous]

@demirciomer , what robot do you use ? I suspect any task.info bad compatibility param !

[demirciomer]

@elpimous we have a custom robot like you, but its size is bigger. We increased the torque limit in urdf and operated the robot in standing mode. Tries to increase body height when we switch to the controller from position control. still no fix available.

[elpimous]

@demirciomer Hello. Motors always trembling ?? You don t have foot sensors, correct ? If yes, how did you faked foot sensors values ? If anyone of us two find solution, please don t forget the other 😉

[demirciomer]

@elpimous We dont have foot sensors. We are just looking for the force in Z and above treshold we set foot contact as true. We are looking MPC observation topic for forces in Z direction.

[elpimous]

And you have same reaction as me ? Robot cant maintain sitted down position. Correct ?

[demirciomer]

We tried another thing. We stand up the robot with position control and implemented a controller switch which switches the controller from position to legged control. it tries to move up from standing position. foot contacts are all true no problem in foot contacts but body tries to go up. may be the state estimation problem or controller task.info parameter problem.

[demirciomer]

just be sure about the torque limits in the urdf file for your case. increase them to 100nm etc.

[elpimous]

@demirciomer

modified to into const.xacro AND ylo2.urdf

and under task.info :

; Whole body control torqueLimitsTask { (0,0) 100.0 ; HAA (1,0) 100.0 ; HFE (2,0) 100.0 ; KFE }

frictionConeTask { frictionCoefficient 0.3 }

swingLegTask { kp 350 kd 80 }

Same problem !!

[demirciomer]

@elpimous also check the motor controllers default parameters if there is an integral coefficient, make it zero, that is also problem. We are still dealing with it :+1:

[elpimous]

@demirciomer. Thanks friend. My Ki is =0.

Could you confirm this :

• It uses position mode,
• position is signed and in rad/s,
• velocity is signed and in rad/s,
• torque is fftorque unsigned, in Nm,
• kp and kd are unsigned, in Nm/rad,

Could you confirm that you use same params ?

[demirciomer]

@elpimous yeah same parameters, we also tried to smooth the control signals with low pass filters, it keeps the position better but this is not a good practice. probably state estimation of both of us are problematic. It might be a noise issue. Keep try to solve it.

[demirciomer]

@elpimous torque values should be signed?

[elpimous]

Hello on my controllers, maxtorque and kp kd are always positive or 0. Pos, vel, fftorque are signed

[elpimous]

Could we have state estimation errors, due to bad legs segment lenght, compared to body com/imu,? Any error on urdf/xacro ? Need to investigate. I'll tell you.

[yyagin]

Hi, @elpimous. In literature, position control is used only when the feet are in the swing phase. During the stance phase, only torque control is applied. Therefore, you should provide only feedforward values (pos_des, vel_des, kp, and kd, all set to zero) while the robot is in the stance phase. In contrast, during the swing phase, all desired variables, including feedforward values, should be sent. Perhaps that's the issue you're encountering. By the way, @demirciomer and I are working together, and this fix seems to solve our issue. We're continuing to test it.

[elpimous]

Hi @yyagin @demirciomer Nice. after a French comprehension of your answer, you seem to use 2 different orders ?! a Torque mode for stance, and a position mode for swing ? How do you did that ? could you share a piece of your controller code ? Don't know how to switch from one to other in process ? Thanks to integrate me in your equation Lol. Have a nice day, friends.

[yyagin]

I apologize for any confusion in my previous message regarding the position and torque controllers. Currently, our approach involves the following code:

command_.send_moteus_TX_frame(ids, port, 0, 0, joint_fftorque + joint_kp*(desired_position - measured_position) + joint_kd * (desired_velocity - measured_velocity), 0, 0);

in the : GitHub - legged_ylo2/legged_ylo2_hw/src/Ylo2HW.cpp#L143

After implementing this change, the trembling has lowered. You can try it and check if it is standing well. Additionally, I recommend reviewing the MIT Cheetah 3 paper for further insights about force and torque controllers. If you have the opportunity to try it, please update us as well :)

Have a nice day too!

[elpimous]

Hi @yyagin , @demirciomer Hello friends. Yes, this is my github package. ok to test with only fftorque feeded.

void Ylo2HW::read(const ros::Time& /*time*/, const ros::Duration& /*period*/) {

  // read the 12 joints, and store values into legged controller
  for (int i = 0; i < 12; ++i) {

    // Reset values
    RX_pos = 0.0;
    RX_vel = 0.0;
    RX_tor = 0.0;
    RX_volt = 0.0;
    RX_temp = 0.0;
    RX_fault = 0.0;

    auto ids  = command_.motor_adapters_[i].getIdx(); // moteus controller id
    int port  = command_.motor_adapters_[i].getPort(); // select correct port on Peak canfd board
    auto sign = command_.motor_adapters_[i].getSign(); // in case of joint reverse rotation

    // call ylo2 moteus lib
    command_.read_moteus_RX_queue(ids, port, 
                                  RX_pos, RX_vel, RX_tor, 
                                  RX_volt, RX_temp, RX_fault);      // query values;

    jointData_[i].pos_ = static_cast<double>(sign*RX_pos)*6.28318531;
    jointData_[i].vel_ = static_cast<double>(sign*RX_vel)*6.28318531;
    jointData_[i].tau_ = static_cast<double>(sign*RX_tor);


    // The imu variable is actualized into callback !

    // TODO read volt, temp, faults for Diagnostics

    usleep(150); // needed
  }

void Ylo2HW::write(const ros::Time& /*time*/, const ros::Duration& /*period*/) {

  // ask security switch status
  // if pressed, it directly set maxtorque to 0
  command_.security_switch();

  //ROS_INFO("write function");
  for (int i = 0; i < 12; ++i) {

    auto ids  = command_.motor_adapters_[i].getIdx(); // moteus controller id
    int port  = command_.motor_adapters_[i].getPort(); // select correct port on Peak canfd board
    auto sign = command_.motor_adapters_[i].getSign(); // in case of joint reverse rotation
    
    joint_position = static_cast<float>(sign*(jointData_[i].posDes_)/6.28318531);
    joint_velocity = static_cast<float>(sign*(jointData_[i].velDes_)/6.28318531);
    joint_fftorque = static_cast<float>(sign*(jointData_[i].ff_));
    joint_kp       = static_cast<float>((jointData_[i].kp_)/6.28318531);
    joint_kd       = static_cast<float>((jointData_[i].kd_)/6.28318531);

    command_.send_moteus_TX_frame(ids, port, 0, 0, joint_fftorque  + joint_kp * (joint_position - jointData_[i].pos_) + joint_kd * (joint_velocity - jointData_[i].vel_), 0, 0);

    usleep(150); // needed

  }
}

trying your idea with command_.send_moteus_TX_frame(...)

cross fingers ! I'll tell you. Good night friends Could you share a small video ?

[demirciomer]

@elpimous first you can try to send just ff_torque values to joints withouth any other parameters pos vel kp kd, make them zero and just send ff torques. This would prevent trembling behaviour. later add pos error times kp + vel error times kd to the ff torque to follow pos and velocity