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

Performance improvements found using callgrind #852

Merged
merged 3 commits into from Mar 1, 2023
Merged

Performance improvements found using callgrind #852

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
e56c6e7
Performance improvements found using callgrind
Levi-Armstrong Mar 1, 2023
132a216
Merge branch 'master' into feat/ProfilingImprovements
Levi-Armstrong Mar 1, 2023
e54535c
Merge branch 'master' into feat/ProfilingImprovements
Levi-Armstrong Mar 1, 2023
File filter

Filter by extension

Filter by extension
Conversations
Failed to load comments.
Jump to
Jump to file
Failed to load files.
Diff view
Diff view
2 changes: 1 addition & 1 deletion tesseract_common/include/tesseract_common/yaml_utils.h
View file
Open in desktop
Expand Up @@ -295,7 +295,7 @@ struct convert<Eigen::Isometry3d>
Eigen::AngleAxisd pitchAngle(p, Eigen::Vector3d::UnitY());
Eigen::AngleAxisd yawAngle(y, Eigen::Vector3d::UnitZ());

Eigen::Quaterniond rpy = yawAngle * pitchAngle * rollAngle;
Eigen::Quaterniond rpy{ yawAngle * pitchAngle * rollAngle };

out.linear() = rpy.toRotationMatrix();
}
Expand Down
4 changes: 2 additions & 2 deletions tesseract_common/src/utils.cpp
View file
Open in desktop
Expand Up @@ -133,7 +133,7 @@ Eigen::Vector3d calcRotationalError(const Eigen::Ref<const Eigen::Matrix3d>& R)
// Make sure that the angle is on [-pi, pi]
const static double two_pi = 2.0 * M_PI;
double angle = s * r12.angle();
Eigen::Vector3d axis = s * r12.axis();
Eigen::Vector3d axis{ s * r12.axis() };
angle = copysign(fmod(fabs(angle), two_pi), angle);
if (angle < -M_PI)
angle += two_pi;
Expand All @@ -157,7 +157,7 @@ Eigen::Vector3d calcRotationalError2(const Eigen::Ref<const Eigen::Matrix3d>& R)
// Make sure that the angle is on [0, 2 * pi]
const static double two_pi = 2.0 * M_PI;
double angle = s * r12.angle();
Eigen::Vector3d axis = s * r12.axis();
Eigen::Vector3d axis{ s * r12.axis() };
angle = copysign(fmod(fabs(angle), two_pi), angle);
if (angle < 0)
angle += two_pi;
Expand Down
4 changes: 4 additions & 0 deletions tesseract_geometry/include/tesseract_geometry/mesh_parser.h
View file
Open in desktop
Expand Up @@ -83,6 +83,7 @@ std::vector<std::shared_ptr<T>> extractMeshData(const aiScene* scene,
bool material_and_texture)
{
std::vector<std::shared_ptr<T>> meshes;
meshes.reserve(node->mNumMeshes);

aiMatrix4x4 transform = parent_transform;
transform *= node->mTransformation;
Expand All @@ -96,6 +97,7 @@ std::vector<std::shared_ptr<T>> extractMeshData(const aiScene* scene,
std::shared_ptr<std::vector<MeshTexture::Ptr>> textures = nullptr;

const aiMesh* a = scene->mMeshes[node->mMeshes[j]];
vertices->reserve(a->mNumVertices);
for (unsigned int i = 0; i < a->mNumVertices; ++i)
{
aiVector3D v = transform * a->mVertices[i];
Expand Down Expand Up @@ -129,6 +131,7 @@ std::vector<std::shared_ptr<T>> extractMeshData(const aiScene* scene,
if (normals && a->HasNormals())
{
vertex_normals = std::make_shared<tesseract_common::VectorVector3d>();
vertex_normals->reserve(a->mNumVertices);
for (unsigned int i = 0; i < a->mNumVertices; ++i)
{
aiVector3D v = transform * a->mNormals[i];
Expand All @@ -141,6 +144,7 @@ std::vector<std::shared_ptr<T>> extractMeshData(const aiScene* scene,
if (vertex_colors && a->HasVertexColors(0))
{
vertex_colors = std::make_shared<tesseract_common::VectorVector4d>();
vertex_colors->reserve(a->mNumVertices);
for (unsigned int i = 0; i < a->mNumVertices; ++i)
{
aiColor4D v = a->mColors[0][i];
Expand Down
9 changes: 4 additions & 5 deletions tesseract_kinematics/core/include/tesseract_kinematics/core/utils.h
View file
Open in desktop
Expand Up @@ -62,8 +62,7 @@ inline static void numericalJacobian(Eigen::Ref<Eigen::MatrixXd> jacobian,
Eigen::VectorXd njvals;
double delta = 1e-8;
tesseract_common::TransformMap poses = kin.calcFwdKin(joint_values);
Eigen::Isometry3d pose = poses[link_name];
pose = change_base * pose;
Eigen::Isometry3d pose{ change_base * poses[link_name] };

for (int i = 0; i < static_cast<int>(joint_values.size()); ++i)
{
Expand All @@ -72,13 +71,13 @@ inline static void numericalJacobian(Eigen::Ref<Eigen::MatrixXd> jacobian,
Eigen::Isometry3d updated_pose = kin.calcFwdKin(njvals)[link_name];
updated_pose = change_base * updated_pose;

Eigen::Vector3d temp = pose * link_point;
Eigen::Vector3d temp2 = updated_pose * link_point;
Eigen::Vector3d temp{ pose * link_point };
Eigen::Vector3d temp2{ updated_pose * link_point };
jacobian(0, i) = (temp2.x() - temp.x()) / delta;
jacobian(1, i) = (temp2.y() - temp.y()) / delta;
jacobian(2, i) = (temp2.z() - temp.z()) / delta;

Eigen::VectorXd omega = (pose.rotation() * tesseract_common::calcRotationalError(pose.rotation().transpose() *
Eigen::Vector3d omega = (pose.rotation() * tesseract_common::calcRotationalError(pose.rotation().transpose() *
updated_pose.rotation())) /
delta;
jacobian(3, i) = omega(0);
Expand Down
43 changes: 33 additions & 10 deletions tesseract_scene_graph/src/kdl_parser.cpp
View file
Open in desktop
Expand Up @@ -41,28 +41,51 @@ namespace tesseract_scene_graph
KDL::Frame convert(const Eigen::Isometry3d& transform)
{
KDL::Frame frame;
frame.Identity();

for (int i = 0; i < 3; ++i)
frame.p[i] = transform(i, 3);
// translation
frame.p[0] = transform(0, 3);
frame.p[1] = transform(1, 3);
frame.p[2] = transform(2, 3);

for (int i = 0; i < 9; ++i)
frame.M.data[i] = transform(i / 3, i % 3);
// rotation matrix
frame.M.data[0] = transform(0, 0);
frame.M.data[1] = transform(0, 1);
frame.M.data[2] = transform(0, 2);
frame.M.data[3] = transform(1, 0);
frame.M.data[4] = transform(1, 1);
frame.M.data[5] = transform(1, 2);
frame.M.data[6] = transform(2, 0);
frame.M.data[7] = transform(2, 1);
frame.M.data[8] = transform(2, 2);

return frame;
}

Eigen::Isometry3d convert(const KDL::Frame& frame)
{
Eigen::Isometry3d transform{ Eigen::Isometry3d::Identity() };
Eigen::Isometry3d transform;

// translation
for (int i = 0; i < 3; ++i)
transform(i, 3) = frame.p[i];
transform(0, 3) = frame.p[0];
transform(1, 3) = frame.p[1];
transform(2, 3) = frame.p[2];

// rotation matrix
for (int i = 0; i < 9; ++i)
transform(i / 3, i % 3) = frame.M.data[i];
transform(0, 0) = frame.M.data[0];
transform(0, 1) = frame.M.data[1];
transform(0, 2) = frame.M.data[2];
transform(1, 0) = frame.M.data[3];
transform(1, 1) = frame.M.data[4];
transform(1, 2) = frame.M.data[5];
transform(2, 0) = frame.M.data[6];
transform(2, 1) = frame.M.data[7];
transform(2, 2) = frame.M.data[8];

// Bottom row
transform(3, 0) = 0;
transform(3, 1) = 0;
transform(3, 2) = 0;
transform(3, 3) = 1;

return transform;
}
Expand Down
2 changes: 1 addition & 1 deletion tesseract_srdf/src/group_tool_center_points.cpp
View file
Open in desktop
Expand Up @@ -139,7 +139,7 @@ GroupTCPs parseGroupTCPs(const tesseract_scene_graph::SceneGraph& /*scene_graph*
Eigen::AngleAxisd pitchAngle(p, Eigen::Vector3d::UnitY());
Eigen::AngleAxisd yawAngle(y, Eigen::Vector3d::UnitZ());

Eigen::Quaterniond rpy = yawAngle * pitchAngle * rollAngle;
Eigen::Quaterniond rpy{ yawAngle * pitchAngle * rollAngle };

tcp.linear() = rpy.toRotationMatrix();
}
Expand Down
4 changes: 2 additions & 2 deletions tesseract_state_solver/src/kdl_state_solver.cpp
View file
Open in desktop
Expand Up @@ -217,7 +217,7 @@ bool KDLStateSolver::isActiveLinkName(const std::string& link_name) const
{
return (std::find(data_.active_link_names.begin(), data_.active_link_names.end(), link_name) !=
data_.active_link_names.end());
};
}

bool KDLStateSolver::hasLinkName(const std::string& link_name) const
{
Expand Down Expand Up @@ -316,7 +316,7 @@ void KDLStateSolver::calculateTransformsHelper(SceneState& state,
current_frame = GetTreeElementSegment(current_element).pose(0);

Eigen::Isometry3d local_frame = convert(current_frame);
Eigen::Isometry3d global_frame = parent_frame * local_frame;
Eigen::Isometry3d global_frame{ parent_frame * local_frame };
state.link_transforms[current_element.segment.getName()] = global_frame;
if (current_element.segment.getName() != data_.tree.getRootSegment()->first)
state.joint_transforms[current_element.segment.getJoint().getName()] = global_frame;
Expand Down
2 changes: 1 addition & 1 deletion tesseract_state_solver/src/ofkt_state_solver.cpp
View file
Open in desktop
Expand Up @@ -327,7 +327,7 @@ Eigen::MatrixXd OFKTStateSolver::calcJacobianHelper(const std::unordered_map<std
OFKTNode* node = link_map_.at(link_name);
Eigen::MatrixXd jacobian = Eigen::MatrixXd::Zero(6, static_cast<Eigen::Index>(active_joint_names_.size()));

Eigen::Isometry3d total_tf = Eigen::Isometry3d::Identity();
Eigen::Isometry3d total_tf{ Eigen::Isometry3d::Identity() };
while (node != root_.get())
{
if (node->getType() == JointType::FIXED || node->getType() == JointType::FLOATING)
Expand Down
2 changes: 1 addition & 1 deletion tesseract_urdf/src/origin.cpp
View file
Open in desktop
Expand Up @@ -90,7 +90,7 @@ Eigen::Isometry3d tesseract_urdf::parseOrigin(const tinyxml2::XMLElement* xml_el
Eigen::AngleAxisd pitchAngle(p, Eigen::Vector3d::UnitY());
Eigen::AngleAxisd yawAngle(y, Eigen::Vector3d::UnitZ());

Eigen::Quaterniond rpy = yawAngle * pitchAngle * rollAngle;
Eigen::Quaterniond rpy{ yawAngle * pitchAngle * rollAngle };

origin.linear() = rpy.toRotationMatrix();
}
Expand Down