unitreerobotics / unitree_legged_sdk

SDK tools for control robots.
BSD 3-Clause "New" or "Revised" License
298 stars 165 forks source link

twp questions about low-level command #122

Open sizsJEon opened 1 year ago

sizsJEon commented 1 year ago

1. question about difference between position and torque command.

I'm currently testing go1's command performances, but torque control and position control is seems to be different in some way. for example,

position command (1000Hz)

joint_1_desired =  0.5*( 1- cos(1* angular_speed * current_t));
cmd.motorCmd[FR_1].q = joint_1_desired;
cmd.motorCmd[FR_1].dq = 0.;
cmd.motorCmd[FR_1].Kp = 20.;
cmd.motorCmd[FR_1].Kd = 0.5;
cmd.motorCmd[FR_1].tau= 0;

torque command (1000Hz)

joint_1_desired =  0.5*( 1- cos(1* angular_speed * current_t));
cmd.motorCmd[FR_1].q  = PosStopF;
cmd.motorCmd[FR_1].dq = VelStopF;
cmd.motorCmd[FR_1].Kp = 0.;
cmd.motorCmd[FR_1].Kd = 0.;
cmd.motorCmd[FR_1].tau= (joint_1_desired - state.motorState[FR_1].q)*20.0f + (0 - state.motorState[FR_1].dq)*0.5f;

when sending lowlevel command to go1 with sdk, these two command's performance is somehow different. especially, knee joint ( calf_joint. FR_2 ) is oscillate rapidly. (setting lower kd, kp gain results not oscillation. but position error rises)

*2. why cmd.motorCmd[].Kd is effect to torque command?**

as far as I know, Kp and Kd should not affect to torque command. but it does. (for example, ocilliation in knee joint was removed when setting Kd=0.5) Was this intentional? If so, can you share the code or formula within the controller?

heartInsert commented 4 months ago

mate , do you know how to get /gazebo/model_states and /aliengo_gazebo/joint_states in a real robot ?

sizsJEon commented 4 months ago

mate , do you know how to get /gazebo/model_states and /aliengo_gazebo/joint_states in a real robot ?

its a gazebo simulator's state. so If you want to get mode_state, maybe calculate from imu will be possible.

for joint_states, you can get joint information from robot. (by using state variable.) https://github.com/unitreerobotics/unitree_legged_sdk/blob/4539a6c10dfbc9781cea6fcb7d51bc6ddc6f71e1/example/example_position.cpp#L79C6-L79C43

heartInsert commented 4 months ago

Sorry , I am new to ROS . I mean , is there any possible that I can run a ros enviroment in the real robot ? And I just need to replace /gazebo/model_states to /imu /model_states ?

sizsJEon commented 4 months ago

Sorry , I am new to ROS . I mean , is there any possible that I can run a ros enviroment in the real robot ? And I just need to replace /gazebo/model_states to /imu /model_states ?

  1. real robot runs internally has ros system. but it does not publish its state to ros. (you should do it yourself.)
  2. If you have a real robot, simplely run this package's code (ex: ./example_position) will tell you something.
  3. Or, you could try other package. (https://github.com/unitreerobotics/unitree_ros_to_real) this package runs real robot in ros. (but you should Implement publisher in order to see the data in rostopic format)
heartInsert commented 4 months ago

Yeah , thanks for reply , now I have implemented https://github.com/unitreerobotics/unitree_ros_to_real successfully and my model walks well in ros-Gazebo by publish topics and subscribe topics . But the examples is all moved by sdk . It is not what I imaged that I can still just publish or subscribe topics likes in gazebo.

heartInsert commented 4 months ago

Doos your meanings is that I just need to put the https://github.com/unitreerobotics/unitree_ros_to_real into the
aliengo real Robot , then I can control the real robot through ros topic.

sizsJEon commented 4 months ago

Doos your meanings is that I just need to put the https://github.com/unitreerobotics/unitree_ros_to_real into the aliengo real Robot , then I can control the real robot through ros topic.

real robot does not have gazebo-like information. you should implement yourself it. I've already told you this. https://github.com/unitreerobotics/unitree_legged_sdk/issues/122#issuecomment-2208250532

checkout LowState and HighState. those values are only datas you can get from robot.

heartInsert commented 4 months ago

OK , thanks for your help. Your help save me a lot of time. I think I have fully understand the whole process from subscribe topics in c++ and public topics in python , my aliengo could move right now .