Merge pull request chvmp#48 from peci1/support-sim-time

Add support for running using sim time
xu-yang16 · Mar 24, 2021 · f6b1ee2 · f6b1ee2
2 parents f99cc04 + 3e75026
commit f6b1ee2
Show file tree

Hide file tree

Showing 2 changed files with 8 additions and 3 deletions.
diff --git a/champ/include/champ b/champ/include/champ
diff --git a/champ_base/src/quadruped_controller.cpp b/champ_base/src/quadruped_controller.cpp
@@ -27,9 +27,14 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
 
 #include <quadruped_controller.h>
 
+champ::PhaseGenerator::Time rosTimeToChampTime(const ros::Time& time)
+{
+  return time.toNSec() / 1000ul;
+}
+
 QuadrupedController::QuadrupedController(ros::NodeHandle *nh, ros::NodeHandle *pnh):
     body_controller_(base_),
-    leg_controller_(base_),
+    leg_controller_(base_, rosTimeToChampTime(ros::Time::now())),
     kinematics_(base_)
 {
     std::string joint_control_topic = "joint_group_position_controller/command";
@@ -91,7 +96,7 @@ void QuadrupedController::controlLoop_(const ros::TimerEvent& event)
     bool foot_contacts[4];
 
     body_controller_.poseCommand(target_foot_positions, req_pose_);
-    leg_controller_.velocityCommand(target_foot_positions, req_vel_);
+    leg_controller_.velocityCommand(target_foot_positions, req_vel_, rosTimeToChampTime(ros::Time::now()));
     kinematics_.inverse(target_joint_positions, target_foot_positions);
 
     for(size_t i = 0; i < 4; i++)
+4 −4		leg_controller/leg_controller.h
+13 −8		leg_controller/phase_generator.h