7 #include "pinocchio/math/rpy.hpp" 21 if(axis.length() != 1
U)
22 throw std::invalid_argument(std::string(
"Invalid axis: ").append(axis));
25 const char axis_ = axis[0];
28 case 'x': u[0] = 1.;
break;
29 case 'y': u[1] = 1.;
break;
30 case 'z': u[2] = 1.;
break;
31 default:
throw std::invalid_argument(std::string(
"Invalid axis: ").
append(1
U,axis_));
34 return Eigen::AngleAxisd(ang, u).matrix();
39 using namespace Eigen;
46 bp::def(
"rpyToMatrix",
47 static_cast<Matrix3d (*)(
const double&,
const double&,
const double&)
>(&
rpyToMatrix),
48 bp::args(
"roll",
"pitch",
"yaw"),
49 "Given (r, p, y), the rotation is given as R = R_z(y)R_y(p)R_x(r)," 50 " where R_a(theta) denotes the rotation of theta radians axis a");
52 bp::def(
"rpyToMatrix",
53 static_cast<Matrix3d (*)(
const MatrixBase<Vector3d>&)
>(&
rpyToMatrix),
55 "Given (r, p, y), the rotation is given as R = R_z(y)R_y(p)R_x(r)," 56 " where R_a(theta) denotes the rotation of theta radians axis a");
58 bp::def(
"matrixToRpy",
59 &matrixToRpy<Matrix3d>,
61 "Given a rotation matrix R, the angles (r, p, y) are given so that R = R_z(y)R_y(p)R_x(r)," 62 " where R_a(theta) denotes the rotation of theta radians axis a." 63 " The angles are guaranteed to be in the ranges: r in [-pi,pi]," 64 " p in[-pi/2,pi/2], y in [-pi,pi]");
68 bp::args(
"axis",
"ang"),
69 "Rotation matrix corresponding to a rotation about x, y or z" 70 " e.g. R = rot('x', pi / 4): rotate pi/4 rad about x axis");
72 bp::def(
"computeRpyJacobian",
73 &computeRpyJacobian<Vector3d>,
74 computeRpyJacobian_overload(
75 bp::args(
"rpy",
"reference_frame"),
76 "Compute the Jacobian of the Roll-Pitch-Yaw conversion" 77 " Given phi = (r, p, y) such that that R = R_z(y)R_y(p)R_x(r)" 78 " and reference frame F (either LOCAL or WORLD)," 79 " the Jacobian is such that omega_F = J_F(phi)phidot," 80 " where omega_F is the angular velocity expressed in frame F" 81 " and J_F is the Jacobian computed with reference frame F" 83 "\trpy Roll-Pitch-Yaw vector" 84 "\treference_frame Reference frame in which the angular velocity is expressed." 85 " Notice LOCAL_WORLD_ALIGNED is equivalent to WORLD" 89 bp::def(
"computeRpyJacobianInverse",
90 &computeRpyJacobianInverse<Vector3d>,
91 computeRpyJacobianInverse_overload(
92 bp::args(
"rpy",
"reference_frame"),
93 "Compute the inverse Jacobian of the Roll-Pitch-Yaw conversion" 94 " Given phi = (r, p, y) such that that R = R_z(y)R_y(p)R_x(r)" 95 " and reference frame F (either LOCAL or WORLD)," 96 " the Jacobian is such that omega_F = J_F(phi)phidot," 97 " where omega_F is the angular velocity expressed in frame F" 98 " and J_F is the Jacobian computed with reference frame F" 100 "\trpy Roll-Pitch-Yaw vector" 101 "\treference_frame Reference frame in which the angular velocity is expressed." 102 " Notice LOCAL_WORLD_ALIGNED is equivalent to WORLD" 106 bp::def(
"computeRpyJacobianTimeDerivative",
107 &computeRpyJacobianTimeDerivative<Vector3d, Vector3d>,
108 computeRpyJacobianTimeDerivative_overload(
109 bp::args(
"rpy",
"rpydot",
"reference_frame"),
110 "Compute the time derivative of the Jacobian of the Roll-Pitch-Yaw conversion" 111 " Given phi = (r, p, y) such that that R = R_z(y)R_y(p)R_x(r)" 112 " and reference frame F (either LOCAL or WORLD)," 113 " the Jacobian is such that omega_F = J_F(phi)phidot," 114 " where omega_F is the angular velocity expressed in frame F" 115 " and J_F is the Jacobian computed with reference frame F" 117 "\trpy Roll-Pitch-Yaw vector" 118 "\treference_frame Reference frame in which the angular velocity is expressed." 119 " Notice LOCAL_WORLD_ALIGNED is equivalent to WORLD" result_of::push_front< V const, T >::type append(T const &t, V const &v)
Append the element T at the front of boost fusion vector V.
Eigen::Matrix< typename Vector3Like::Scalar, 3, 3, PINOCCHIO_EIGEN_PLAIN_TYPE(Vector3Like)::Options > computeRpyJacobian(const Eigen::MatrixBase< Vector3Like > &rpy, const ReferenceFrame rf=LOCAL)
Compute the Jacobian of the Roll-Pitch-Yaw conversion.
BOOST_PYTHON_FUNCTION_OVERLOADS(computeKKTContactDynamicMatrixInverse_overload, computeKKTContactDynamicMatrixInverse_proxy, 4, 5) static const Eigen
Eigen::Matrix3d rotate(const std::string &axis, const double ang)
JointCollectionTpl const Eigen::MatrixBase< ConfigVectorIn1 > const Eigen::MatrixBase< ConfigVectorIn2 > const Scalar & u
boost::python::object getOrCreatePythonNamespace(const std::string &submodule_name)
Helper to create or simply return an existing namespace in Python.
Main pinocchio namespace.
Eigen::Matrix< typename Vector3Like::Scalar, 3, 3, PINOCCHIO_EIGEN_PLAIN_TYPE(Vector3Like)::Options > computeRpyJacobianInverse(const Eigen::MatrixBase< Vector3Like > &rpy, const ReferenceFrame rf=LOCAL)
Compute the inverse Jacobian of the Roll-Pitch-Yaw conversion.
Eigen::Matrix< Scalar, 3, 3 > rpyToMatrix(const Scalar &r, const Scalar &p, const Scalar &y)
Convert from Roll, Pitch, Yaw to rotation Matrix.
Eigen::Matrix< typename Vector3Like0::Scalar, 3, 3, PINOCCHIO_EIGEN_PLAIN_TYPE(Vector3Like0)::Options > computeRpyJacobianTimeDerivative(const Eigen::MatrixBase< Vector3Like0 > &rpy, const Eigen::MatrixBase< Vector3Like1 > &rpydot, const ReferenceFrame rf=LOCAL)
Compute the time derivative Jacobian of the Roll-Pitch-Yaw conversion.
Roll-pitch-yaw operations.