Skip to content

Commit

Permalink
Benchmark robot state (#2546)
Browse files Browse the repository at this point in the history 
* simplify memory management in RobotState

* further changes

* avoid pointer arithmetic where possible

* fix memory access issue on root joint with 0 variables

* fix vector size

* remove unused header
  • Loading branch information
@marioprats
marioprats authored Nov 30, 2023
1 parent 81094a6 commit f7b0c15
Show file tree
Hide file tree
Showing 2 changed files with 125 additions and 178 deletions.
121 changes: 58 additions & 63 deletions moveit_core/robot_state/include/moveit/robot_state/robot_state.h
View file
Original file line number Diff line number Diff line change
Expand Up @@ -146,14 +146,14 @@ class RobotState
the state by calling update(true). */
double* getVariablePositions()
{
return position_;
return position_.data();
}

/** \brief Get a raw pointer to the positions of the variables
stored in this state. */
const double* getVariablePositions() const
{
return position_;
return position_.data();
}

/** \brief It is assumed \e positions is an array containing the new
Expand Down Expand Up @@ -236,14 +236,14 @@ class RobotState
double* getVariableVelocities()
{
markVelocity();
return velocity_;
return velocity_.data();
}

/** \brief Get const access to the velocities of the variables that make up this state. The values are in the same
* order as reported by getVariableNames() */
const double* getVariableVelocities() const
{
return velocity_;
return velocity_.data();
}

/** \brief Set all velocities to 0.0 */
Expand All @@ -254,7 +254,7 @@ class RobotState
{
has_velocity_ = true;
// assume everything is in order in terms of array lengths (for efficiency reasons)
memcpy(velocity_, velocity, robot_model_->getVariableCount() * sizeof(double));
memcpy(velocity_.data(), velocity, robot_model_->getVariableCount() * sizeof(double));
}

/** \brief Given an array with velocity values for all variables, set those values as the velocities in this state */
Expand Down Expand Up @@ -330,14 +330,14 @@ class RobotState
double* getVariableAccelerations()
{
markAcceleration();
return acceleration_;
return effort_or_acceleration_.data();
}

/** \brief Get const raw access to the accelerations of the variables that make up this state. The values are in the
* same order as reported by getVariableNames() */
const double* getVariableAccelerations() const
{
return acceleration_;
return effort_or_acceleration_.data();
}

/** \brief Set all accelerations to 0.0 */
Expand All @@ -351,7 +351,7 @@ class RobotState
has_effort_ = false;

// assume everything is in order in terms of array lengths (for efficiency reasons)
memcpy(acceleration_, acceleration, robot_model_->getVariableCount() * sizeof(double));
memcpy(effort_or_acceleration_.data(), acceleration, robot_model_->getVariableCount() * sizeof(double));
}

/** \brief Given an array with acceleration values for all variables, set those values as the accelerations in this
Expand Down Expand Up @@ -388,21 +388,21 @@ class RobotState
void setVariableAcceleration(int index, double value)
{
markAcceleration();
acceleration_[index] = value;
effort_or_acceleration_[index] = value;
}

/** \brief Get the acceleration of a particular variable. An exception is thrown if the variable is not known. */
double getVariableAcceleration(const std::string& variable) const
{
return acceleration_[robot_model_->getVariableIndex(variable)];
return effort_or_acceleration_[robot_model_->getVariableIndex(variable)];
}

/** \brief Get the acceleration of a particular variable. The variable is
specified by its index. No checks are performed for the validity
of the index passed */
double getVariableAcceleration(int index) const
{
return acceleration_[index];
return effort_or_acceleration_[index];
}

/** \brief Remove accelerations from this state (this differs from setting them to zero) */
Expand All @@ -428,14 +428,14 @@ class RobotState
double* getVariableEffort()
{
markEffort();
return effort_;
return effort_or_acceleration_.data();
}

/** \brief Get const raw access to the effort of the variables that make up this state. The values are in the same
* order as reported by getVariableNames(). */
const double* getVariableEffort() const
{
return effort_;
return effort_or_acceleration_.data();
}

/** \brief Set all effort values to 0.0 */
Expand All @@ -447,7 +447,7 @@ class RobotState
has_effort_ = true;
has_acceleration_ = false;
// assume everything is in order in terms of array lengths (for efficiency reasons)
memcpy(effort_, effort, robot_model_->getVariableCount() * sizeof(double));
memcpy(effort_or_acceleration_.data(), effort, robot_model_->getVariableCount() * sizeof(double));
}

/** \brief Given an array with effort values for all variables, set those values as the effort in this state */
Expand Down Expand Up @@ -479,21 +479,21 @@ class RobotState
void setVariableEffort(int index, double value)
{
markEffort();
effort_[index] = value;
effort_or_acceleration_[index] = value;
}

/** \brief Get the effort of a particular variable. An exception is thrown if the variable is not known. */
double getVariableEffort(const std::string& variable) const
{
return effort_[robot_model_->getVariableIndex(variable)];
return effort_or_acceleration_[robot_model_->getVariableIndex(variable)];
}

/** \brief Get the effort of a particular variable. The variable is
specified by its index. No checks are performed for the validity
of the index passed */
double getVariableEffort(int index) const
{
return effort_[index];
return effort_or_acceleration_[index];
}

/** \brief Remove effort values from this state (this differs from setting them to zero) */
Expand Down Expand Up @@ -529,7 +529,7 @@ class RobotState

void setJointPositions(const JointModel* joint, const double* position)
{
memcpy(position_ + joint->getFirstVariableIndex(), position, joint->getVariableCount() * sizeof(double));
memcpy(&position_.at(joint->getFirstVariableIndex()), position, joint->getVariableCount() * sizeof(double));
markDirtyJointTransforms(joint);
updateMimicJoint(joint);
}
Expand All @@ -541,15 +541,15 @@ class RobotState

void setJointPositions(const JointModel* joint, const Eigen::Isometry3d& transform)
{
joint->computeVariablePositions(transform, position_ + joint->getFirstVariableIndex());
joint->computeVariablePositions(transform, &position_.at(joint->getFirstVariableIndex()));
markDirtyJointTransforms(joint);
updateMimicJoint(joint);
}

void setJointVelocities(const JointModel* joint, const double* velocity)
{
has_velocity_ = true;
memcpy(velocity_ + joint->getFirstVariableIndex(), velocity, joint->getVariableCount() * sizeof(double));
memcpy(&velocity_.at(joint->getFirstVariableIndex()), velocity, joint->getVariableCount() * sizeof(double));
}

void setJointEfforts(const JointModel* joint, const double* effort);
Expand All @@ -561,7 +561,7 @@ class RobotState

const double* getJointPositions(const JointModel* joint) const
{
return position_ + joint->getFirstVariableIndex();
return &position_.at(joint->getFirstVariableIndex());
}

const double* getJointVelocities(const std::string& joint_name) const
Expand All @@ -571,7 +571,7 @@ class RobotState

const double* getJointVelocities(const JointModel* joint) const
{
return velocity_ + joint->getFirstVariableIndex();
return &velocity_.at(joint->getFirstVariableIndex());
}

const double* getJointAccelerations(const std::string& joint_name) const
Expand All @@ -581,7 +581,7 @@ class RobotState

const double* getJointAccelerations(const JointModel* joint) const
{
return acceleration_ + joint->getFirstVariableIndex();
return &effort_or_acceleration_.at(joint->getFirstVariableIndex());
}

const double* getJointEffort(const std::string& joint_name) const
Expand All @@ -591,7 +591,7 @@ class RobotState

const double* getJointEffort(const JointModel* joint) const
{
return effort_ + joint->getFirstVariableIndex();
return &effort_or_acceleration_.at(joint->getFirstVariableIndex());
}

/** @} */
Expand Down Expand Up @@ -1190,11 +1190,10 @@ class RobotState
* Resulting values are clamped within default bounds. */
void setToRandomPositionsNearBy(const JointModelGroup* group, const RobotState& seed, double distance);

/** \brief Set all joints in \e group to random values near the value in \e seed, using a specified random number generator.
* \e distance is the maximum amount each joint value will vary from the
* corresponding value in \e seed. \distance represents meters for
* prismatic/positional joints and radians for revolute/orientation joints.
* Resulting values are clamped within default bounds. */
/** \brief Set all joints in \e group to random values near the value in \e seed, using a specified random number
* generator. \e distance is the maximum amount each joint value will vary from the corresponding value in \e seed.
* \distance represents meters for prismatic/positional joints and radians for revolute/orientation joints. Resulting
* values are clamped within default bounds. */
void setToRandomPositionsNearBy(const JointModelGroup* group, const RobotState& seed, double distance,
random_numbers::RandomNumberGenerator& rng);

Expand All @@ -1208,13 +1207,11 @@ class RobotState
void setToRandomPositionsNearBy(const JointModelGroup* group, const RobotState& seed,
const std::vector<double>& distances);

/** \brief Set all joints in \e group to random values near the value in \e seed, using a specified random number generator.
* \e distances \b MUST have the same size as \c
* group.getActiveJointModels(). Each value in \e distances is the maximum
* amount the corresponding active joint in \e group will vary from the
* corresponding value in \e seed. \distance represents meters for
* prismatic/positional joints and radians for revolute/orientation joints.
* Resulting values are clamped within default bounds. */
/** \brief Set all joints in \e group to random values near the value in \e seed, using a specified random number
* generator. \e distances \b MUST have the same size as \c group.getActiveJointModels(). Each value in \e distances
* is the maximum amount the corresponding active joint in \e group will vary from the corresponding value in \e seed.
* \distance represents meters for prismatic/positional joints and radians for revolute/orientation joints. Resulting
* values are clamped within default bounds. */
void setToRandomPositionsNearBy(const JointModelGroup* group, const RobotState& seed,
const std::vector<double>& distances, random_numbers::RandomNumberGenerator& rng);

Expand Down Expand Up @@ -1341,7 +1338,7 @@ class RobotState
unsigned char& dirty = dirty_joint_transforms_[idx];
if (dirty)
{
joint->computeTransform(position_ + joint->getFirstVariableIndex(), variable_joint_transforms_[idx]);
joint->computeTransform(&position_.at(joint->getFirstVariableIndex()), variable_joint_transforms_[idx]);
dirty = 0;
}
return variable_joint_transforms_[idx];
Expand Down Expand Up @@ -1388,7 +1385,7 @@ class RobotState
/** \brief Return the sum of joint distances to "other" state. An L1 norm. Only considers active joints. */
double distance(const RobotState& other) const
{
return robot_model_->distance(position_, other.getVariablePositions());
return robot_model_->distance(position_.data(), other.getVariablePositions());
}

/** \brief Return the sum of joint distances to "other" state. An L1 norm. Only considers active joints. */
Expand All @@ -1398,7 +1395,7 @@ class RobotState
double distance(const RobotState& other, const JointModel* joint) const
{
const int idx = joint->getFirstVariableIndex();
return joint->distance(position_ + idx, other.position_ + idx);
return joint->distance(&position_.at(idx), &other.position_.at(idx));
}

/**
Expand Down Expand Up @@ -1434,7 +1431,7 @@ class RobotState
void interpolate(const RobotState& to, double t, RobotState& state, const JointModel* joint) const
{
const int idx = joint->getFirstVariableIndex();
joint->interpolate(position_ + idx, to.position_ + idx, t, state.position_ + idx);
joint->interpolate(&position_.at(idx), &to.position_.at(idx), t, &state.position_.at(idx));
state.markDirtyJointTransforms(joint);
state.updateMimicJoint(joint);
}
Expand All @@ -1449,7 +1446,7 @@ class RobotState
}
void enforcePositionBounds(const JointModel* joint)
{
if (joint->enforcePositionBounds(position_ + joint->getFirstVariableIndex()))
if (joint->enforcePositionBounds(&position_.at(joint->getFirstVariableIndex())))
{
markDirtyJointTransforms(joint);
updateMimicJoint(joint);
Expand All @@ -1461,7 +1458,7 @@ class RobotState
void harmonizePositions(const JointModelGroup* joint_group);
void harmonizePosition(const JointModel* joint)
{
if (joint->harmonizePosition(position_ + joint->getFirstVariableIndex()))
if (joint->harmonizePosition(&position_.at(joint->getFirstVariableIndex())))
{
// no need to mark transforms dirty, as the transform hasn't changed
updateMimicJoint(joint);
Expand All @@ -1470,7 +1467,7 @@ class RobotState

void enforceVelocityBounds(const JointModel* joint)
{
joint->enforceVelocityBounds(velocity_ + joint->getFirstVariableIndex());
joint->enforceVelocityBounds(&velocity_.at(joint->getFirstVariableIndex()));
}

bool satisfiesBounds(double margin = 0.0) const;
Expand Down Expand Up @@ -1624,7 +1621,7 @@ class RobotState
random_numbers::RandomNumberGenerator& getRandomNumberGenerator()
{
if (!rng_)
rng_ = new random_numbers::RandomNumberGenerator();
rng_ = std::make_unique<random_numbers::RandomNumberGenerator>();
return *rng_;
}

Expand Down Expand Up @@ -1731,7 +1728,7 @@ class RobotState

private:
void allocMemory();
void initTransforms();
void init();
void copyFrom(const RobotState& other);

void markDirtyJointTransforms(const JointModel* joint)
Expand Down Expand Up @@ -1792,26 +1789,25 @@ class RobotState
bool checkCollisionTransforms() const;

RobotModelConstPtr robot_model_;
void* memory_;

double* position_;
double* velocity_;
double* acceleration_;
double* effort_;
bool has_velocity_;
bool has_acceleration_;
bool has_effort_;
std::vector<double> position_;
std::vector<double> velocity_;
std::vector<double> effort_or_acceleration_;
bool has_velocity_ = false;
bool has_acceleration_ = false;
bool has_effort_ = false;

const JointModel* dirty_link_transforms_;
const JointModel* dirty_collision_body_transforms_;
const JointModel* dirty_link_transforms_ = nullptr;
const JointModel* dirty_collision_body_transforms_ = nullptr;

// All the following transform variables point into aligned memory in memory_
// All the following transform variables point into aligned memory.
// They are updated lazily, based on the flags in dirty_joint_transforms_
// resp. the pointers dirty_link_transforms_ and dirty_collision_body_transforms_
Eigen::Isometry3d* variable_joint_transforms_; ///< Local transforms of all joints
Eigen::Isometry3d* global_link_transforms_; ///< Transforms from model frame to link frame for each link
Eigen::Isometry3d* global_collision_body_transforms_; ///< Transforms from model frame to collision bodies
unsigned char* dirty_joint_transforms_;
std::vector<Eigen::Isometry3d> variable_joint_transforms_; ///< Local transforms of all joints
std::vector<Eigen::Isometry3d> global_link_transforms_; ///< Transforms from model frame to link frame for each link
std::vector<Eigen::Isometry3d> global_collision_body_transforms_; ///< Transforms from model frame to collision
///< bodies
std::vector<unsigned char> dirty_joint_transforms_;

/** \brief All attached bodies that are part of this state, indexed by their name */
std::map<std::string, std::unique_ptr<AttachedBody>> attached_body_map_;
Expand All @@ -1823,9 +1819,8 @@ class RobotState
/** \brief For certain operations a state needs a random number generator. However, it may be slightly expensive
to allocate the random number generator if many state instances are generated. For this reason, the generator
is allocated on a need basis, by the getRandomNumberGenerator() function. Never use the rng_ member directly, but
call
getRandomNumberGenerator() instead. */
random_numbers::RandomNumberGenerator* rng_;
call getRandomNumberGenerator() instead. */
std::unique_ptr<random_numbers::RandomNumberGenerator> rng_;
};

/** \brief Operator overload for printing variable bounds to a stream */
Expand Down
Loading

0 comments on commit f7b0c15

Please sign in to comment.