Affine3 EuclidDistHeuristic::createPose( double x, double y, double z, double Y, double P, double R) const { return Affine3( Translation3(x, y, z) * AngleAxis(Y, Vector3::UnitZ()) * AngleAxis(P, Vector3::UnitY()) * AngleAxis(R, Vector3::UnitX())); }
bool WorkspaceLatticeBase::stateWorkspaceToRobot( const WorkspaceState& state, const RobotState& seed, RobotState& ostate) const { Affine3 pose = Translation3(state[0], state[1], state[2]) * AngleAxis(state[5], Vector3::UnitZ()) * AngleAxis(state[4], Vector3::UnitY()) * AngleAxis(state[3], Vector3::UnitX()); // TODO: unrestricted variant? return m_rm_iface->computeFastIK(pose, seed, ostate); }
bool WorkspaceLatticeBase::stateWorkspaceToRobot( const WorkspaceState& state, RobotState& ostate) const { RobotState seed(robot()->jointVariableCount(), 0); for (size_t fai = 0; fai < freeAngleCount(); ++fai) { seed[m_fangle_indices[fai]] = state[6 + fai]; } Affine3 pose = Translation3(state[0], state[1], state[2]) * AngleAxis(state[5], Vector3::UnitZ()) * AngleAxis(state[4], Vector3::UnitY()) * AngleAxis(state[3], Vector3::UnitX()); return m_rm_iface->computeFastIK(pose, seed, ostate); }
AngleAxis EulerBall::getRotation() const { QQuaternion xRotation = QQuaternion:: fromAxisAndAngle(QVector3D(0.0, 1.0, 0.0), 180 * phi_now / M_PI); QQuaternion yRotation = QQuaternion:: fromAxisAndAngle(QVector3D(1.0, 0.0, 0.0), 180 * theta_now / M_PI); return AngleAxis(yRotation * xRotation); }
static Affine3 ComputeJointTransform(const Joint* joint, const double* variables) { switch (joint->type) { case JointType::Fixed: return Affine3::Identity(); case JointType::Revolute: return Affine3(AngleAxis(variables[0], joint->axis)); case JointType::Prismatic: return Affine3(Translation3(variables[0] * joint->axis)); case JointType::Planar: return Translation3(variables[0], variables[1], 0.0) * AngleAxis(variables[2], Vector3::UnitZ()); case JointType::Floating: return Translation3(variables[0], variables[1], variables[2]) * Quaternion(variables[6], variables[3], variables[4], variables[5]); default: assert(0); } }
void Node::Roll(float angle, TransformSpace space) { Rotate(AngleAxis(angle, VECTOR3_FORWARD), space); }
void Node::Pitch(float angle, TransformSpace space) { Rotate(AngleAxis(angle, VECTOR3_RIGHT), space); }
void Node::Yaw(float angle, TransformSpace space) { Rotate(AngleAxis(angle, VECTOR3_UP), space); }
AngleAxis ArcBall::getRotation() const { return AngleAxis(q_now); }