From 68a1e342c82576bdd402d9bc72d9c9bf024134da Mon Sep 17 00:00:00 2001 From: Thore Goll Date: Tue, 1 Feb 2022 15:39:45 +0100 Subject: [PATCH] CHANGE: Set desired joint quantities of robot state to zero This field is only ever set when using motion generators, i.e. the position- or velocity interface of the FCI. When sending a target value, the FCI sends you back the generated trajectory trajectory in `q_d`, `dq_d` and `ddq_d`. However franka_gazebo does not yet support any motion generator interface. Via torque control interface the robot sets these values to zero indicating that no motion is generated, which we replicate in franka_gazebo simulation. Note that `q_d` is a special case. This updates also in torque control mode and also during guiding to reset motion generators on replanning attempts. For brewity we just set it to `q` in franka_gazebo --- franka_gazebo/src/franka_hw_sim.cpp | 8 +++++--- 1 file changed, 5 insertions(+), 3 deletions(-) diff --git a/franka_gazebo/src/franka_hw_sim.cpp b/franka_gazebo/src/franka_hw_sim.cpp index dac452faa..d34d344fe 100644 --- a/franka_gazebo/src/franka_hw_sim.cpp +++ b/franka_gazebo/src/franka_hw_sim.cpp @@ -480,9 +480,11 @@ void FrankaHWSim::updateRobotState(ros::Time time) { this->robot_state_.tau_J[i] = joint->effort; this->robot_state_.dtau_J[i] = joint->jerk; - this->robot_state_.q_d[i] = joint->position; - this->robot_state_.dq_d[i] = joint->velocity; - this->robot_state_.ddq_d[i] = joint->acceleration; + // since we don't support position or velocity interface yet, we set the desired joint + // trajectory to zero indicating we are in torque control mode + this->robot_state_.q_d[i] = joint->position; // special case for resetting motion generators + this->robot_state_.dq_d[i] = 0; + this->robot_state_.ddq_d[i] = 0; this->robot_state_.tau_J_d[i] = joint->command; // For now we assume no flexible joints