snake_visualization/robotic.cpp at master · nimasarli/snake_visualization · GitHub

1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
/* Implentation file for robotic namespace
 * Author: Nima Sarli
 */
#include "robotic.h"

/****************************************************************************
 *  Functions
 ****************************************************************************/

//
Eigen::Matrix3d robotic::rotX(double angle)
{
    Eigen::Matrix3d rotMat;
    rotMat << 1.0, 0.0, 0.0,
            0.0, std::cos(angle), -std::sin(angle),
            0.0, std::sin(angle), std::cos(angle);
    return rotMat;
}

//
Eigen::Matrix3d robotic::rotY(double angle)
{
    Eigen::Matrix3d rotMat;
    rotMat << std::cos(angle), 0, std::sin(angle),
            0.0, 1.0, 0.0,
            -std::sin(angle), 0.0, std::cos(angle);
    return rotMat;
}

//
Eigen::Matrix3d robotic::rotZ(double angle)
{
    Eigen::Matrix3d rotMat;
    rotMat << std::cos(angle), -std::sin(angle), 0.0,
            std::sin(angle), std::cos(angle), 0.0,
            0.0, 0.0, 1.0;
    return rotMat;
}