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

Replaced euler angles in Affine::vector_with_elbow with rotation vectors #3

Open
wants to merge 1 commit into
base: main
Choose a base branch
from
Open

Replaced euler angles in Affine::vector_with_elbow with rotation vectors #3

Changes from all commits
Commits
Show all changes
1 commit
Select commit
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
33 changes: 32 additions & 1 deletion include/affx/affine.hpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -50,6 +50,32 @@ class Affine {
data = affine;
}

static Affine fromPosRotVec(double p_x, double p_y, double p_z, double r_x, double r_y, double r_z) {
auto output = Affine();
output.data.translation() = Eigen::Vector3d(p_x, p_y, p_z);
auto rot_vec = Eigen::Vector3d(r_x, r_y, r_z);
auto axis = rot_vec.normalized();
auto angle = rot_vec.norm();
output.data.linear() = Eigen::AngleAxis(angle, axis).toRotationMatrix();
return output;
}

static Affine fromPosRotVec(const std::array<double, 6> &v) {
return fromPosRotVec(v[0], v[1], v[2], v[3], v[4], v[5]);
}

static Affine fromPosRotVec(const std::array<double, 7> &v) {
return fromPosRotVec(v[0], v[1], v[2], v[3], v[4], v[5]);
}

static Affine fromPosRotVec(const Vector6d &v) {
return fromPosRotVec(v[0], v[1], v[2], v[3], v[4], v[5]);
}

static Affine fromPosRotVec(const Vector7d &v) {
return fromPosRotVec(v[0], v[1], v[2], v[3], v[4], v[5]);
}

Affine operator *(const Affine &a) const {
Type result;
result = data * a.data;
Expand Down Expand Up @@ -80,9 +106,14 @@ class Affine {
return result;
}

Eigen::Vector3d rotation_vector() const {
Eigen::AngleAxis<double> result(data.rotation());
return result.axis() * result.angle();
}

Vector7d vector_with_elbow(double elbow) const {
Vector7d result;
result << data.translation(), angles(), elbow;
result << data.translation(), rotation_vector(), elbow;
return result;
}

Expand Down