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

Jump to bottom

Use Eigen::Ref<Eigen vectorXd> in robot state for better performance? #4

Open
wants to merge 3 commits into
base: master
Choose a base branch
from
Open

Use Eigen::Ref<Eigen vectorXd> in robot state for better performance? #4

Show file tree
Hide file tree
Changes from all commits
Commits
Show all changes
3 commits
Select commit Hold shift + click to select a range
8a067a1
fix moveit core robot state benchmark script
JeroenDM Jul 24, 2020
5665861
add moveit core robot state benchmark for state copy to eigen vector
JeroenDM Jul 24, 2020
04c1029
Use Eigen::Ref for all Eigen::VectorXd references in robot state func…
JeroenDM Jul 24, 2020
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
37 changes: 19 additions & 18 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 @@ -632,7 +632,7 @@ class RobotState

/** \brief Given positions for the variables that make up a group, in the order found in the group (including values
* of mimic joints), set those as the new values that correspond to the group */
void setJointGroupPositions(const std::string& joint_group_name, const Eigen::VectorXd& values)
void setJointGroupPositions(const std::string& joint_group_name, const Eigen::Ref<const Eigen::VectorXd> values)
{
const JointModelGroup* jmg = robot_model_->getJointModelGroup(joint_group_name);
if (jmg)
Expand All @@ -641,7 +641,7 @@ class RobotState

/** \brief Given positions for the variables that make up a group, in the order found in the group (including values
* of mimic joints), set those as the new values that correspond to the group */
void setJointGroupPositions(const JointModelGroup* group, const Eigen::VectorXd& values);
void setJointGroupPositions(const JointModelGroup* group, const Eigen::Ref<const Eigen::VectorXd> values);

/** \brief For a given group, copy the position values of the variables that make up the group into another location,
* in the order that the variables are found in the group. This is not necessarily a contiguous block of memory in the
Expand Down Expand Up @@ -683,7 +683,7 @@ class RobotState
/** \brief For a given group, copy the position values of the variables that make up the group into another location,
* in the order that the variables are found in the group. This is not necessarily a contiguous block of memory in the
* RobotState itself, so we copy instead of returning a pointer.*/
void copyJointGroupPositions(const std::string& joint_group_name, Eigen::VectorXd& values) const
void copyJointGroupPositions(const std::string& joint_group_name, Eigen::Ref<Eigen::VectorXd> values) const
{
const JointModelGroup* jmg = robot_model_->getJointModelGroup(joint_group_name);
if (jmg)
Expand All @@ -693,7 +693,7 @@ class RobotState
/** \brief For a given group, copy the position values of the variables that make up the group into another location,
* in the order that the variables are found in the group. This is not necessarily a contiguous block of memory in the
* RobotState itself, so we copy instead of returning a pointer.*/
void copyJointGroupPositions(const JointModelGroup* group, Eigen::VectorXd& values) const;
void copyJointGroupPositions(const JointModelGroup* group, Eigen::Ref<Eigen::VectorXd> values) const;

/** @} */

Expand Down Expand Up @@ -732,7 +732,7 @@ class RobotState

/** \brief Given velocities for the variables that make up a group, in the order found in the group (including values
* of mimic joints), set those as the new values that correspond to the group */
void setJointGroupVelocities(const std::string& joint_group_name, const Eigen::VectorXd& values)
void setJointGroupVelocities(const std::string& joint_group_name, const Eigen::Ref<const Eigen::VectorXd> values)
{
const JointModelGroup* jmg = robot_model_->getJointModelGroup(joint_group_name);
if (jmg)
Expand All @@ -741,7 +741,7 @@ class RobotState

/** \brief Given velocities for the variables that make up a group, in the order found in the group (including values
* of mimic joints), set those as the new values that correspond to the group */
void setJointGroupVelocities(const JointModelGroup* group, const Eigen::VectorXd& values);
void setJointGroupVelocities(const JointModelGroup* group, const Eigen::Ref<const Eigen::VectorXd> values);

/** \brief For a given group, copy the velocity values of the variables that make up the group into another location,
* in the order that the variables are found in the group. This is not necessarily a contiguous block of memory in the
Expand Down Expand Up @@ -783,7 +783,7 @@ class RobotState
/** \brief For a given group, copy the velocity values of the variables that make up the group into another location,
* in the order that the variables are found in the group. This is not necessarily a contiguous block of memory in the
* RobotState itself, so we copy instead of returning a pointer.*/
void copyJointGroupVelocities(const std::string& joint_group_name, Eigen::VectorXd& values) const
void copyJointGroupVelocities(const std::string& joint_group_name, Eigen::Ref<Eigen::VectorXd> values) const
{
const JointModelGroup* jmg = robot_model_->getJointModelGroup(joint_group_name);
if (jmg)
Expand All @@ -793,7 +793,7 @@ class RobotState
/** \brief For a given group, copy the velocity values of the variables that make up the group into another location,
* in the order that the variables are found in the group. This is not necessarily a contiguous block of memory in the
* RobotState itself, so we copy instead of returning a pointer.*/
void copyJointGroupVelocities(const JointModelGroup* group, Eigen::VectorXd& values) const;
void copyJointGroupVelocities(const JointModelGroup* group, Eigen::Ref<Eigen::VectorXd> values) const;

/** @} */

Expand Down Expand Up @@ -832,7 +832,7 @@ class RobotState

/** \brief Given accelerations for the variables that make up a group, in the order found in the group (including
* values of mimic joints), set those as the new values that correspond to the group */
void setJointGroupAccelerations(const std::string& joint_group_name, const Eigen::VectorXd& values)
void setJointGroupAccelerations(const std::string& joint_group_name, const Eigen::Ref<const Eigen::VectorXd> values)
{
const JointModelGroup* jmg = robot_model_->getJointModelGroup(joint_group_name);
if (jmg)
Expand All @@ -841,7 +841,7 @@ class RobotState

/** \brief Given accelerations for the variables that make up a group, in the order found in the group (including
* values of mimic joints), set those as the new values that correspond to the group */
void setJointGroupAccelerations(const JointModelGroup* group, const Eigen::VectorXd& values);
void setJointGroupAccelerations(const JointModelGroup* group, const Eigen::Ref<const Eigen::VectorXd> values);

/** \brief For a given group, copy the acceleration values of the variables that make up the group into another
* location, in the order that the variables are found in the group. This is not necessarily a contiguous block of
Expand Down Expand Up @@ -883,7 +883,7 @@ class RobotState
/** \brief For a given group, copy the acceleration values of the variables that make up the group into another
* location, in the order that the variables are found in the group. This is not necessarily a contiguous block of
* memory in the RobotState itself, so we copy instead of returning a pointer.*/
void copyJointGroupAccelerations(const std::string& joint_group_name, Eigen::VectorXd& values) const
void copyJointGroupAccelerations(const std::string& joint_group_name, Eigen::Ref<Eigen::VectorXd> values) const
{
const JointModelGroup* jmg = robot_model_->getJointModelGroup(joint_group_name);
if (jmg)
Expand All @@ -893,7 +893,7 @@ class RobotState
/** \brief For a given group, copy the acceleration values of the variables that make up the group into another
* location, in the order that the variables are found in the group. This is not necessarily a contiguous block of
* memory in the RobotState itself, so we copy instead of returning a pointer.*/
void copyJointGroupAccelerations(const JointModelGroup* group, Eigen::VectorXd& values) const;
void copyJointGroupAccelerations(const JointModelGroup* group, Eigen::Ref<Eigen::VectorXd> values) const;

/** @} */

Expand Down Expand Up @@ -1085,7 +1085,8 @@ class RobotState
* @param dt a time interval (seconds)
* @param st a secondary task computation function
*/
bool setFromDiffIK(const JointModelGroup* group, const Eigen::VectorXd& twist, const std::string& tip, double dt,
bool setFromDiffIK(const JointModelGroup* group, const Eigen::Ref<const Eigen::VectorXd> twist,
const std::string& tip, double dt,
const GroupStateValidityCallbackFn& constraint = GroupStateValidityCallbackFn());

/** \brief Set the joint values from a Cartesian velocity applied during a time dt
Expand Down Expand Up @@ -1223,13 +1224,13 @@ class RobotState

/** \brief Given a twist for a particular link (\e tip), compute the corresponding velocity for every variable and
* store it in \e qdot */
void computeVariableVelocity(const JointModelGroup* jmg, Eigen::VectorXd& qdot, const Eigen::VectorXd& twist,
const LinkModel* tip) const;
void computeVariableVelocity(const JointModelGroup* jmg, Eigen::Ref<Eigen::VectorXd> qdot,
const Eigen::Ref<const Eigen::VectorXd> twist, const LinkModel* tip) const;

/** \brief Given a twist for a particular link (\e tip), compute the corresponding velocity for every variable and
* store it in \e qdot */
void computeVariableVelocity(const JointModelGroup* jmg, Eigen::VectorXd& qdot, const Eigen::VectorXd& twist,
const LinkModel* tip)
void computeVariableVelocity(const JointModelGroup* jmg, Eigen::Ref<Eigen::VectorXd> qdot,
const Eigen::Ref<const Eigen::VectorXd> twist, const LinkModel* tip)
{
updateLinkTransforms();
static_cast<const RobotState*>(this)->computeVariableVelocity(jmg, qdot, twist, tip);
Expand All @@ -1238,7 +1239,7 @@ class RobotState
/** \brief Given the velocities for the variables in this group (\e qdot) and an amount of time (\e dt),
update the current state using the Euler forward method. If the constraint specified is satisfied, return true,
otherwise return false. */
bool integrateVariableVelocity(const JointModelGroup* jmg, const Eigen::VectorXd& qdot, double dt,
bool integrateVariableVelocity(const JointModelGroup* jmg, const Eigen::Ref<const Eigen::VectorXd> qdot, double dt,
const GroupStateValidityCallbackFn& constraint = GroupStateValidityCallbackFn());

/** @} */
Expand Down
25 changes: 13 additions & 12 deletions moveit_core/robot_state/src/robot_state.cpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -504,7 +504,7 @@ void RobotState::setJointGroupPositions(const JointModelGroup* group, const doub
updateMimicJoints(group);
}

void RobotState::setJointGroupPositions(const JointModelGroup* group, const Eigen::VectorXd& values)
void RobotState::setJointGroupPositions(const JointModelGroup* group, const Eigen::Ref<const Eigen::VectorXd> values)
{
const std::vector<int>& il = group->getVariableIndexList();
for (std::size_t i = 0; i < il.size(); ++i)
Expand All @@ -522,7 +522,7 @@ void RobotState::copyJointGroupPositions(const JointModelGroup* group, double* g
gstate[i] = position_[il[i]];
}

void RobotState::copyJointGroupPositions(const JointModelGroup* group, Eigen::VectorXd& values) const
void RobotState::copyJointGroupPositions(const JointModelGroup* group, Eigen::Ref<Eigen::VectorXd> values) const
{
const std::vector<int>& il = group->getVariableIndexList();
values.resize(il.size());
Expand All @@ -543,7 +543,7 @@ void RobotState::setJointGroupVelocities(const JointModelGroup* group, const dou
}
}

void RobotState::setJointGroupVelocities(const JointModelGroup* group, const Eigen::VectorXd& values)
void RobotState::setJointGroupVelocities(const JointModelGroup* group, const Eigen::Ref<const Eigen::VectorXd> values)
{
markVelocity();
const std::vector<int>& il = group->getVariableIndexList();
Expand All @@ -561,7 +561,7 @@ void RobotState::copyJointGroupVelocities(const JointModelGroup* group, double*
gstate[i] = velocity_[il[i]];
}

void RobotState::copyJointGroupVelocities(const JointModelGroup* group, Eigen::VectorXd& values) const
void RobotState::copyJointGroupVelocities(const JointModelGroup* group, Eigen::Ref<Eigen::VectorXd> values) const
{
const std::vector<int>& il = group->getVariableIndexList();
values.resize(il.size());
Expand All @@ -582,7 +582,8 @@ void RobotState::setJointGroupAccelerations(const JointModelGroup* group, const
}
}

void RobotState::setJointGroupAccelerations(const JointModelGroup* group, const Eigen::VectorXd& values)
void RobotState::setJointGroupAccelerations(const JointModelGroup* group,
const Eigen::Ref<const Eigen::VectorXd> values)
{
markAcceleration();
const std::vector<int>& il = group->getVariableIndexList();
Expand All @@ -600,7 +601,7 @@ void RobotState::copyJointGroupAccelerations(const JointModelGroup* group, doubl
gstate[i] = acceleration_[il[i]];
}

void RobotState::copyJointGroupAccelerations(const JointModelGroup* group, Eigen::VectorXd& values) const
void RobotState::copyJointGroupAccelerations(const JointModelGroup* group, Eigen::Ref<Eigen::VectorXd> values) const
{
const std::vector<int>& il = group->getVariableIndexList();
values.resize(il.size());
Expand Down Expand Up @@ -1330,8 +1331,8 @@ bool RobotState::getJacobian(const JointModelGroup* group, const LinkModel* link
return true;
}

bool RobotState::setFromDiffIK(const JointModelGroup* jmg, const Eigen::VectorXd& twist, const std::string& tip,
double dt, const GroupStateValidityCallbackFn& constraint)
bool RobotState::setFromDiffIK(const JointModelGroup* jmg, const Eigen::Ref<const Eigen::VectorXd> twist,
const std::string& tip, double dt, const GroupStateValidityCallbackFn& constraint)
{
Eigen::VectorXd qdot;
computeVariableVelocity(jmg, qdot, twist, getLinkModel(tip));
Expand All @@ -1346,8 +1347,8 @@ bool RobotState::setFromDiffIK(const JointModelGroup* jmg, const geometry_msgs::
return setFromDiffIK(jmg, t, tip, dt, constraint);
}

void RobotState::computeVariableVelocity(const JointModelGroup* jmg, Eigen::VectorXd& qdot,
const Eigen::VectorXd& twist, const LinkModel* tip) const
void RobotState::computeVariableVelocity(const JointModelGroup* jmg, Eigen::Ref<Eigen::VectorXd> qdot,
const Eigen::Ref<const Eigen::VectorXd> twist, const LinkModel* tip) const
{
// Get the Jacobian of the group at the current configuration
Eigen::MatrixXd j(6, jmg->getVariableCount());
Expand Down Expand Up @@ -1387,8 +1388,8 @@ void RobotState::computeVariableVelocity(const JointModelGroup* jmg, Eigen::Vect
qdot = jinv * twist;
}

bool RobotState::integrateVariableVelocity(const JointModelGroup* jmg, const Eigen::VectorXd& qdot, double dt,
const GroupStateValidityCallbackFn& constraint)
bool RobotState::integrateVariableVelocity(const JointModelGroup* jmg, const Eigen::Ref<const Eigen::VectorXd> qdot,
double dt, const GroupStateValidityCallbackFn& constraint)
{
Eigen::VectorXd q(jmg->getVariableCount());
copyJointGroupPositions(jmg, q);
Expand Down
17 changes: 16 additions & 1 deletion moveit_core/robot_state/test/robot_state_benchmark.cpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -95,7 +95,7 @@ class Timing : public testing::Test

TEST_F(Timing, stateUpdate)
{
moveit::core::RobotModelPtr model = moveit::core::loadTestingRobotModel("pr2_description");
moveit::core::RobotModelPtr model = moveit::core::loadTestingRobotModel("pr2");
ASSERT_TRUE(bool(model));
moveit::core::RobotState state(model);
ScopedTimer t("RobotState updates: ");
Expand All @@ -106,6 +106,21 @@ TEST_F(Timing, stateUpdate)
}
}

TEST_F(Timing, stateCopy)
{
moveit::core::RobotModelPtr model = moveit::core::loadTestingRobotModel("fanuc");
ASSERT_TRUE(bool(model));
moveit::core::RobotState state(model);
moveit::core::JointModelGroup* jmg(model->getJointModelGroup("manipulator"));
ScopedTimer t("Copy state to Eigen vector: ");
Eigen::VectorXd joint_positions(6);
for (unsigned i = 0; i < 1e5; ++i)
{
state.setToRandomPositions();
state.copyJointGroupPositions(jmg, joint_positions);
}
}

TEST_F(Timing, multiply)
{
size_t runs = 1e7;
Expand Down