Skip to content

Commit 9a793dd

Browse files
juanjqojuanjqo
authored
Added the Franka Emika Panda robot (#49)
* Added the Franka Emika Panda Robot. * [FrankaEmikaPandaRobot.h, .cpp] Added some comments.
1 parent 7d42e28 commit 9a793dd

File tree

2 files changed

+182
-0
lines changed

2 files changed

+182
-0
lines changed

‎include/dqrobotics/robots/FrankaEmikaPandaRobot.h‎

Lines changed: 33 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,33 @@
1+
/**
2+
(C) Copyright 2022 DQ Robotics Developers
3+
This file is part of DQ Robotics.
4+
DQ Robotics is free software: you can redistribute it and/or modify
5+
it under the terms of the GNU Lesser General Public License as published by
6+
the Free Software Foundation, either version 3 of the License, or
7+
(at your option) any later version.
8+
DQ Robotics is distributed in the hope that it will be useful,
9+
but WITHOUT ANY WARRANTY; without even the implied warranty of
10+
MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
11+
GNU Lesser General Public License for more details.
12+
You should have received a copy of the GNU Lesser General Public License
13+
along with DQ Robotics. If not, see <http://www.gnu.org/licenses/>.
14+
Contributors:
15+
- Juan Jose Quiroz Omana (juanjqo@g.ecc.u-tokyo.ac.jp)
16+
*/
17+
18+
#pragma once
19+
#include<dqrobotics/robot_modeling/DQ_SerialManipulatorMDH.h>
20+
21+
namespace DQ_robotics
22+
{
23+
24+
class FrankaEmikaPandaRobot
25+
{
26+
public:
27+
static DQ_SerialManipulatorMDH kinematics();
28+
//static DQ_SerialManipulatorMDH dynamics(); To be implemented
29+
};
30+
31+
}
32+
33+

‎src/robots/FrankaEmikaPandaRobot.cpp‎

Lines changed: 149 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,149 @@
1+
/**
2+
(C) Copyright 2022 DQ Robotics Developers
3+
This file is part of DQ Robotics.
4+
DQ Robotics is free software: you can redistribute it and/or modify
5+
it under the terms of the GNU Lesser General Public License as published by
6+
the Free Software Foundation, either version 3 of the License, or
7+
(at your option) any later version.
8+
DQ Robotics is distributed in the hope that it will be useful,
9+
but WITHOUT ANY WARRANTY; without even the implied warranty of
10+
MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
11+
GNU Lesser General Public License for more details.
12+
You should have received a copy of the GNU Lesser General Public License
13+
along with DQ Robotics. If not, see <http://www.gnu.org/licenses/>.
14+
Contributors:
15+
- Juan Jose Quiroz Omana (juanjqo@g.ecc.u-tokyo.ac.jp)
16+
*/
17+
18+
#include<dqrobotics/utils/DQ_Constants.h>
19+
#include <dqrobotics/robots/FrankaEmikaPandaRobot.h>
20+
#include <Eigen/StdVector>
21+
22+
namespace DQ_robotics
23+
{
24+
25+
///****************************************************************************************
26+
/// PRIVATE FUNCTIONS
27+
/// ***************************************************************************************
28+
/**
29+
* @brief _get_mdh_matrix returns a matrix related to the modified D-H parameters of the
30+
* Franka Emika Panda robot, which is
31+
* defined as
32+
*
33+
* Matrix<double, 5, 6> raw_franka_mdh;
34+
* raw_franka_mdh << theta,
35+
* d,
36+
* a,
37+
* alpha,
38+
* type_of_joints;
39+
* Source: https://frankaemika.github.io/docs/control_parameters.html
40+
*
41+
* @return MatrixXd raw_franka_mdh a matrix related to the modified D-H parameters
42+
*/
43+
MatrixXd _get_mdh_matrix()
44+
{
45+
const double pi2 = pi/2.0;
46+
Matrix<double,5,7> raw_franka_mdh(5,7);
47+
raw_franka_mdh << 0, 0, 0, 0, 0, 0, 0,
48+
0.333, 0, 3.16e-1, 0, 3.84e-1, 0, 0,
49+
0, 0, 0, 8.25e-2, -8.25e-2, 0, 8.8e-2,
50+
0, -pi2, pi2, pi2, -pi2, pi2, pi2,
51+
0, 0, 0, 0, 0, 0, 0;
52+
53+
return raw_franka_mdh;
54+
}
55+
56+
57+
/**
58+
* @brief _get_offset_base returns the base offset of the Franka Emika Panda robot.
59+
* @return a unit dual quatenion representing the base offset of the robot.
60+
*/
61+
DQ _get_offset_base()
62+
{
63+
return 1 + E_ * 0.5 * DQ(0, 0.0413, 0, 0);
64+
}
65+
66+
/**
67+
* @brief _get_offset_flange returns the end-effector offset of the Franka Emika Panda robot,
68+
* which is called "Flange" by the manufacturer.
69+
* This offset does not correspond to the end-effector tool but is part
70+
* of the modeling based on Craig's convention.
71+
* Source: https://frankaemika.github.io/docs/control_parameters.html
72+
* @return
73+
*/
74+
DQ _get_offset_flange()
75+
{
76+
return 1+E_*0.5*k_*1.07e-1;
77+
}
78+
79+
/**
80+
* @brief _get_q_limits returns the joint limits of the Franka Emika Panda robot
81+
* as suggested by the manufacturer.
82+
*
83+
* source: https://frankaemika.github.io/docs/control_parameters.html
84+
*
85+
* @return a std::make_tuple(q_min_, q_max_) containing the joint limits.
86+
*/
87+
std::tuple<const VectorXd, const VectorXd> _get_q_limits()
88+
{
89+
const VectorXd q_max_ = ((VectorXd(7) << 2.3093, 1.5133, 2.4937, -0.4461, 2.4800, 4.2094, 2.6895).finished());
90+
const VectorXd q_min_ = ((VectorXd(7) << -2.3093,-1.5133,-2.4937, -2.7478,-2.4800, 0.8521, -2.6895).finished());
91+
return std::make_tuple(q_min_, q_max_);
92+
}
93+
94+
95+
/**
96+
* @brief _get_q_dot_limits returns the joint velocity limits of the Franka Emika Panda robot
97+
* as suggested by the manufacturer.
98+
*
99+
* source: https://frankaemika.github.io/docs/control_parameters.html
100+
*
101+
* @return a std::make_tuple(q_min_dot_, q_max_dot_) containing the joint velocity limits.
102+
*/
103+
std::tuple<const VectorXd, const VectorXd> _get_q_dot_limits()
104+
{
105+
const VectorXd q_min_dot_ = ((VectorXd(7) << -2, -1, -1.5, -1.25, -3, -1.5, -3).finished());
106+
const VectorXd q_max_dot_ = ((VectorXd(7) << 2, 1, 1.5, 1.25, 3, 1.5, 3).finished());
107+
return std::make_tuple(q_min_dot_, q_max_dot_);
108+
}
109+
110+
111+
112+
113+
///****************************************************************************************
114+
/// PUBLIC FUNCTIONS
115+
/// ***************************************************************************************
116+
117+
118+
/**
119+
* @brief FrankaEmikaPandaRobot::kinematics returns an object of the class DQ_SerialManipulatorMDH()
120+
* that contain the kinematic model of the Franka Emika Panda robot.
121+
* Example of use:
122+
*
123+
* #include <dqrobotics/robots/FrankaEmikaPandaRobot.h>
124+
* DQ_SerialManipulatorMDH franka = FrankaEmikaPandaRobot::kinematics();
125+
* DQ x = franka.fkm(VectorXd::Zero(franka.get_dim_configuration_space()));
126+
*
127+
* @return A DQ_SerialManipulatorMDH() object of the desire robot
128+
*/
129+
DQ_SerialManipulatorMDH FrankaEmikaPandaRobot::kinematics()
130+
{
131+
DQ_SerialManipulatorMDH franka(_get_mdh_matrix());
132+
franka.set_base_frame(_get_offset_base());
133+
franka.set_reference_frame(_get_offset_base());
134+
franka.set_effector(_get_offset_flange());
135+
VectorXd q_min;
136+
VectorXd q_max;
137+
VectorXd q_dot_min;
138+
VectorXd q_dot_max;
139+
std::tie(q_min, q_max) = _get_q_limits();
140+
std::tie(q_dot_min, q_dot_max) = _get_q_dot_limits();
141+
franka.set_lower_q_limit(q_min);
142+
franka.set_upper_q_limit(q_max);
143+
franka.set_lower_q_dot_limit(q_dot_min);
144+
franka.set_upper_q_dot_limit(q_dot_max);
145+
return franka;
146+
}
147+
148+
}
149+

0 commit comments

Comments
 (0)