Merge pull request moveit#261 from davetcoleman/jade-devel-velocity-e…

…ffort-robot-state Added setJointVelocity and setJointEffort functions
Levi-Armstrong · Nov 3, 2015 · 3c4c7a6 · 3c4c7a6
2 parents bf47bfe + dbec73f
commit 3c4c7a6
Showing 1 changed file with 18 additions and 0 deletions.
diff --git a/robot_state/include/moveit/robot_state/robot_state.h b/robot_state/include/moveit/robot_state/robot_state.h
@@ -482,6 +482,24 @@ class RobotState
     updateMimicJoint(joint);
   }
 
+  void setJointVelocities(const JointModel *joint, const double *velocity)
+  {
+    has_velocity_ = true;
+    memcpy(velocity_ + joint->getFirstVariableIndex(), velocity, joint->getVariableCount() * sizeof(double));
+  }
+
+  void setJointEfforts(const JointModel *joint, const double *effort)
+  {
+    if (has_acceleration_)
+    {
+      logError("Unable to set joint efforts because array is being used for accelerations");
+      return;
+    }
+    has_effort_ = true;
+
+    memcpy(effort_ + joint->getFirstVariableIndex(), effort, joint->getVariableCount() * sizeof(double));
+  }
+
   const double* getJointPositions(const std::string &joint_name) const
   {
     return getJointPositions(robot_model_->getJointModel(joint_name));