Skip to content
New issue

Have a question about this project? Sign up for a free GitHub account to open an issue and contact its maintainers and the community.

Sign up for GitHub

By clicking “Sign up for GitHub”, you agree to our terms of service and privacy statement. We’ll occasionally send you account related emails.

Already on GitHub? Sign in to your account

Benchmark robot state #2546

Merged
merged 9 commits into from
Nov 30, 2023
Merged

Benchmark robot state #2546

Show file tree
Hide file tree
Changes from all commits
Commits
Show all changes
9 commits
Select commit Hold shift + click to select a range
23e93e4
simplify memory management in RobotState
marioprats Nov 12, 2023
7ee883b
further changes
marioprats Nov 14, 2023
31957ac
Merge branch 'main' into benchmark_robot_state
marioprats Nov 28, 2023
1e80743
avoid pointer arithmetic where possible
marioprats Nov 28, 2023
c6b04f6
fix memory access issue on root joint with 0 variables
marioprats Nov 28, 2023
1748fd6
Merge branch 'main' into benchmark_robot_state
marioprats Nov 28, 2023
cb60855
fix vector size
marioprats Nov 29, 2023
1e94a58
remove unused header
marioprats Nov 29, 2023
27b842e
Merge branch 'main' into benchmark_robot_state
marioprats Nov 30, 2023
File filter

Filter by extension

Filter by extension
Conversations
Failed to load comments.
Loading
Jump to
Jump to file
Failed to load files.
Loading
Diff view
Diff view
121 changes: 58 additions & 63 deletions moveit_core/robot_state/include/moveit/robot_state/robot_state.h
View file
Open in desktop
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
Loading