diff --git a/legged_examples/legged_unitree/legged_unitree_hw/src/UnitreeHW.cpp b/legged_examples/legged_unitree/legged_unitree_hw/src/UnitreeHW.cpp index c79db267..a06c6911 100644 --- a/legged_examples/legged_unitree/legged_unitree_hw/src/UnitreeHW.cpp +++ b/legged_examples/legged_unitree/legged_unitree_hw/src/UnitreeHW.cpp @@ -83,12 +83,12 @@ void UnitreeHW::read(const ros::Time& time, const ros::Duration& /*period*/) { } // Set feedforward and velocity cmd to zero to avoid for safety when not controller setCommand - std::vector names = hybridJointInterface_.getNames(); - for (const auto& name : names) { - HybridJointHandle handle = hybridJointInterface_.getHandle(name); - handle.setFeedforward(0.); - handle.setVelocityDesired(0.); - handle.setKd(3.); + for(size_t i=0; i<12; ++i){ + jointData_[i].posDes_ = jointData_[i].pos_; + jointData_[i].velDes_ = jointData_[i].vel_; + jointData_[i].ff_ = 0.0; + jointData_[i].kp_ = 0.0; + jointData_[i].kd_ = 0.0; } updateJoystick(time);