Go1 Motor jittering with controller running on Jetson Orin (arm64)

echoGee commented 1 year ago

We use the legged_control to run the go1 with the controller running on an jetson orin (arm64), but the motors are jittering back and forth.

The simulation works fine on the jetson orin (arm64)
We ran the same controller on an intel 64bit machine and it works fine with go1 robot
the jetson orin (arm64) controller also works fine for the A1 robot (v3.3.4)
The unitree legged SDK(Unitree Legged SDK version : v3.8.0 and v3.5.1) sample codes also work for go1 robot

Do you know where this issue could be with running the go1 robot with the controller pc as arm64?

qiayuanl commented 1 year ago

but the motors are jittering back and forth.

Can you provide videos of the real robot and RViz?

echoGee commented 1 year ago

This is the behavior on init: https://drive.google.com/file/d/1FZz6N4_DqZ7NbxNMoruPjOeZVQz2e_e7/view?usp=sharing The rviz does see the twitching behavior in the simulation. Just didn't get the video for it.

qiayuanl commented 1 year ago

I am sorry that I don't have experience with either Go1 or arm64. Maybe we need to wait for someone who has the same problem.

Kashu7100 commented 1 year ago

@qiayuanliao Is there any way to decrease the standing-up speed? I think the sudden base height change in the init state (laying down) to COM_HEIGHT is possibly causing the failure in real Go1.

Kashu7100 commented 1 year ago

Solved, thanks

suihu-mk commented 1 year ago

I tried to control the real go1 on my pc, there have been some errors.

[ERROR] [1672636716.067159468]: Unable to parse component [$0] to a double (while parsing a vector value) [ERROR] [1672636716.067708510]: Could not parse collision element for Link [RF_thigh] [ERROR] [1672636716.067826536]: Unable to parse component [$0] to a double (while parsing a vector value) [ERROR] [1672636716.067838151]: Could not parse collision element for Link [LF_thigh] [ERROR] [1672636716.067966565]: Unable to parse component [$0] to a double (while parsing a vector value) [ERROR] [1672636716.067976649]: Could not parse collision element for Link [RH_thigh] [ERROR] [1672636716.068075496]: Unable to parse component [$0] to a double (while parsing a vector value) [ERROR] [1672636716.068084957]: Could not parse collision element for Link [LH_thigh] [ERROR] [1672636716.089933332]: could not retrieve one of the required parameters: loop_hz or cycle_time_error_threshold or thread_priority terminate called after throwing an instance of 'std::runtime_error' what(): could not retrieve one of the required parameters: loop_hz or cycle_time_error_threshold or thread_priority

Can you help me with this?

qiayuanl commented 1 year ago

There seems to be something wrong with your urdf. Another error message is obviously that you didn't set those three parameters via rosparam

suihu-mk commented 1 year ago

There seems to be something wrong with your urdf. Another error message is obviously that you didn't set those three parameters via rosparam

Thank you, this problem is solved. When I run it again I get some warnings and the legged_controller won't start when loaded, is it because of these warnings?

WARNING: Failed to set threads priority (one possible reason could be that the user and the group permissions are not set properly.) [ WARN] [1672641837.610638079]: The root link base has an inertia specified in the URDF, but KDL does not support a root link with an inertia. As a workaround, you can add an extra dummy link to your URDF.

suihu-mk commented 1 year ago

There seems to be something wrong with your urdf. Another error message is obviously that you didn't set those three parameters via rosparam

Thank you, this problem is solved. When I run it again I get some warnings and the legged_controller won't start when loaded, is it because of these warnings?

WARNING: Failed to set threads priority (one possible reason could be that the user and the group permissions are not set properly.) [ WARN] [1672641837.610638079]: The root link base has an inertia specified in the URDF, but KDL does not support a root link with an inertia. As a workaround, you can add an extra dummy link to your URDF.

legged_controller fails to start and is stuck.

abrobotics commented 1 year ago

@Kashu7100 @qiayuanliao How did you decrease the standing-up speed?

qiayuanl commented 1 year ago

@Kashu7100 @qiayuanliao How did you decrease the standing-up speed?

You can modify the target_trajectories_publisher to add some setup behavior.

Or you can add more cost in z velocity at the task.info

abrobotics commented 1 year ago

@suihu-mk If you still stuck:

Add this to /etc/security/limits.conf for thread priorities: * hard rtprio 99 * soft rtprio 99 You will still have the last warning but it will work.

Then as said in the README, load the controller:

roslaunch legged_controllers load_controller.launch

Then publish some cmd_vel with:

rosrun rqt_publisher rqt_publisher

or

rosrun rqt_robot_steering rqt_robot_steering

dkanou commented 1 year ago

Hi @Kashu7100 how was this solved? Thanks!

abrobotics commented 1 year ago

@dkanou you can add an integrator in legged_controllers as following:

Edit target_trajectories_publisher.h

  TargetTrajectoriesPublisher(::ros::NodeHandle& nh, const std::string& topic_prefix,
                              CmdToTargetTrajectories goal_to_target_trajectories,
                              CmdToTargetTrajectories cmd_vel_to_target_trajectories,
                              CmdToTargetTrajectories setup_behavior)
    : goal_to_target_trajectories_(std::move(goal_to_target_trajectories))
    , cmd_vel_to_target_trajectories_(std::move(cmd_vel_to_target_trajectories))
    , setup_behavior_(std::move(setup_behavior))
    , tf2_(buffer_)
    , flag_setup_(true)
    , z_coeff_(vector_t::Zero(1))
  {
    // Trajectories publisher
    target_trajectories_publisher_.reset(new TargetTrajectoriesRosPublisher(nh, topic_prefix));

    // observation subscriber
    auto observation_callback = [this](const ocs2_msgs::mpc_observation::ConstPtr& msg) {
      std::lock_guard<std::mutex> lock(latest_observation_mutex_);
      latest_observation_ = ros_msg_conversions::readObservationMsg(*msg);

      if(flag_setup_){  // Setup
        if (z_coeff_(0) < 1.0){
          z_coeff_(0) += 0.005; //Will take 200 loops to stand up 
          const auto trajectories = setup_behavior_(z_coeff_, latest_observation_);
          target_trajectories_publisher_->publishTargetTrajectories(trajectories);
        }
        else {
          flag_setup_ = false;  // Allow command by cmd_vel & goal
          ROS_INFO("--> Robot is up");
        }
      }
    };

private:
  CmdToTargetTrajectories goal_to_target_trajectories_, cmd_vel_to_target_trajectories_;
  CmdToTargetTrajectories setup_behavior_;
  bool flag_setup_;
  vector_t z_coeff_;

Edit target_trajectories_publisher.cpp

TargetTrajectories setupBehavior(const vector_t& z_coeff, const SystemObservation& observation)
{
  const vector_t current_pose = observation.state.segment<6>(6);
  const vector_t target_pose = [&]() {
    vector_t target(6);
    target(0) = 0;
    target(1) = 0;
    target(2) = COM_HEIGHT*z_coeff(0);
    target(3) = 0;
    target(4) = 0;
    target(5) = 0;
    return target;
  }();
  // Increase target_reaching_time didn't work.
  const scalar_t target_reaching_time = observation.time + estimateTimeToTarget(target_pose - current_pose);
  // ROS_INFO("z_target = %.4f\n", COM_HEIGHT*z_coeff(0)); // Print z value each time robot goes up to stand
  return targetPoseToTargetTrajectories(target_pose, observation, target_reaching_time);
}

Edit main

  TargetTrajectoriesPublisher target_pose_command(node_handle, robot_name, &goalToTargetTrajectories,
                                                  &cmdVelToTargetTrajectories, &setupBehavior);

AndrewZheng-1011 commented 1 year ago

Hello,

I am having similar issues of running the go1 on this platform. I encounter the same issue of the motor jittering and making the same beeping sounds. I am communicating to the go1 robot through a PC (with an AMD Ryzen 7 processor) and ethernet port.

Likewise with @echoGee , I am able to run the simulator fine, however, I encounter the issue on hardware. Has there been any updates on this issue?

qiayuanl commented 1 year ago

What unitree_sdk are you using? The SDK originally in legged_unitree_hw can not work with Go1 (according to the Unitree official repo)

AndrewZheng-1011 commented 1 year ago

I am using the unitree_legged_sdk v3.8.0 which supports go1. To note the changes I made in the legged control repository, I changed the following (with a working go1 simulation model):

1) Some CMakeLists path changes to include the different lib folder (has a different structure than a1 unitree_legged_sdk) 2) Switched out the unitree_legged_sdk folders 3) Following changes in UnitreeHW.cpp

Call udp object with a different set of constructor parameters (calling low level UDP requires 4 input parameters)

Enable option for robot_type to be go1

...
udp_ = std::make_shared<UNITREE_LEGGED_SDK>(UNITREE_LEGGED_SDK::LOWLEVEL, 8090, "192.168.123.10", 8007);
...
} elseif (robot_type == "go1") {
safety_ = std::make_shared<UNITREE_LEGGED_SDK::Safety>(UNITREE_LEGGED_SDK::LeggedType:Go1);
} else {
...

So, I believe these are the crucial minor changes needed to make the unitree_legged_sdk go1 version to work.

I have also tried tuning the Kp and Kd values sent, to see if it is a motor issue, however, I have had no luck with that.

Please let me know if I'm missing anything crucial.

AndrewZheng-1011 commented 1 year ago

Hey @qiayuanliao

I was able to fix my jittering and beeping motor issue. I don't think this was explicitly stated in this issue page, but my problem of beeping and jittering motors was fixed by fixing the following thread priority warning

...
WARNING: Failed to set threads priority (one possible reason could be that the user and the group permissions are not set properly.) 
...

by temporarily changing the thread priority through the terminal

ulimit -r 99
ulimit -r # Verify that thread priority set to max

See attached gyf for semi-working version of your algorithm with go1. go1_legged_control_leaning

I am not sure why this solved the jittering motor problems and any insight to this would be helpful for me as a learning experience.

I also have a followup trivial questions that I hope you may not mind answering.

I want to permanently change the thread priority by editing the /etc/security/limits.conf file by adding the following

<username> hard rtprio 99
<username> soft rtprio 99

but this method did not work for me. Sources are stating that these configuration may be overwritten by files in /etc/security/limits.d/ directory, but I have no files within there. I would ideally like to change my thread priority through these files rather than temporarily setting up the thread priority everytime (or temporarily setting thread priority through ~/.bashrc file). Could you let me know if you ever faced these issues.

Thanks.

qiayuanl commented 1 year ago

@AndrewZheng-1011 check here to solve the prioty problem.

suihu-mk commented 1 year ago

I have a "[SafetyChecker] Orientation safety check failed!" problem when running on go1 hardware, how do I solve this? @AndrewZheng-1011

elpimous commented 1 year ago

@dkanou you can add an integrator in legged_controllers as following:

@abrobotics , Some changes needed in namings, but it works perfectly ( I have a nice standup with zcoeff(0) += 0.0005; )

qiayuanl commented 1 year ago

@suihu-mk @AndrewZheng-1011 @elpimous check this repo for a working version legged_control in GO1. However, I wouldn't say I like the macro method to solve the problem of different sdk . I believe the best solution is to add two ros packages for different robots.

elpimous commented 1 year ago

@qiayuanliao Correct. When I'll have it working on my real homemade robot, I'll do a renamed package, with a full Help file for all those interested to une your ros package on their robot.

qiayuanl commented 1 year ago

@suihu-mk @AndrewZheng-1011 @elpimous check this repo for a working version legged_control in GO1. However, I wouldn't say I like the macro method to solve the problem of different sdk . I believe the best solution is to add two ros packages for different robots.

I forgot to add the link, updated.

AndrewZheng-1011 commented 1 year ago

@AndrewZheng-1011 check here to solve the prioty problem.

@qiayuanliao Thanks, I got the permanent priority threading to work. @suihu-mk Can you elaborate on the problem as I do not get that error at all. I also have a working go1 github repo linked here. I believe you just need to build the packages and run the same commands stated in this repo and the following should work.

suihu-mk commented 1 year ago

@AndrewZheng-1011 check here to solve the prioty problem.

@qiayuanliao Thanks, I got the permanent priority threading to work. @suihu-mk Can you elaborate on the problem as I do not get that error at all. I also have a working go1 github repo linked here. I believe you just need to build the packages and run the same commands stated in this repo and the following should work.

@AndrewZheng-1011 Thank you for sharing the repo. I have run your repo successfully. However，I have the same problem as you: the motor is jittering and beeping. The motor jitter and beeping occurs when the legged_unitree_hw is executed directly after starting go1 in the standing state. I found that the motor jitter and beeping disappeared when I switched go1 to a damped state via the handle "L2+B" and then executed legged_unitree_hw and the controller.

neozoxix commented 3 months ago

Hello @echoGee, I just learned about this program and want to implement it on the Unitree A1 robot with Jetson Xavier AGX. can you tell me how to run this program on the arm64 platform (Jetson Xavier/Orin), because I encountered several failures when trying to compile the OCS2 program and this program

nanbwrn commented 2 days ago

hi,@neozoxix
we might be facing the same issue. Have you managed to resolve it?

I encountered an error when running this command on Jetson AGX Orin with Ubuntu 20.04 ARM64. The error occurred while compiling 'ocs2_legged_robot_ros' and 'ocs2_self_collision_visualization'. However, when I compile the same code on Ubuntu AMD64, there are no errors. It's very strange. Does anyone know how to solve this?

Errors << hpipm_catkin:make /home/ubuntu/disk/catkin_legged_control/logs/hpipm_catkin/build.make.001.log cc: error: unrecognized command line option ‘-m64’ cc: error: unrecognized command line option ‘-m64’ cc: error: unrecognized command line option ‘-mavx’ cc: error: unrecognized command line option ‘-mavx’ cc: error: unrecognized command line option ‘-m64’ cc: error: unrecognized command line option ‘-m64’ cc: error: unrecognized command line option ‘-mavx’ cc: error: unrecognized command line option ‘-mavx’ make[2]: [build/CMakeFiles/hpipm.dir/build.make:66：build/CMakeFiles/hpipm.dir/cond/d_cast_qcqp.c.o] 错误 1 make[2]: 正在等待未完成的任务.... make[2]: [build/CMakeFiles/hpipm.dir/build.make:92：build/CMakeFiles/hpipm.dir/cond/d_cond_aux.c.o] 错误 1 make[2]: [build/CMakeFiles/hpipm.dir/build.make:105：build/CMakeFiles/hpipm.dir/cond/d_cond_qcqp.c.o] 错误 1 make[2]: [build/CMakeFiles/hpipm.dir/build.make:79：build/CMakeFiles/hpipm.dir/cond/d_cond.c.o] 错误 1 make[1]: [CMakeFiles/Makefile2:691：build/CMakeFiles/hpipm.dir/all] 错误 2 make: *** [Makefile:141：all] 错误 2 legged_control_erro

qiayuanl / legged_control