Skip to content

Commit 9c45f13

Browse files
ThomasDuquetteThomasDuquette
authored
Merge pull request #55 from Dynamium-Lab/Add-manipulators
Added Gen3 and UR5e, Validated Gen3 Collision Model
2 parents 9929cab + 8d7bcf7 commit 9c45f13

2 files changed

Lines changed: 267 additions & 0 deletions

File tree

‎blast/manipulator/Gen3.hpp‎

Lines changed: 123 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,123 @@
1+
#pragma once
2+
#include <blast>
3+
4+
namespace blast {
5+
6+
Manipulator make_kinova_gen3() {
7+
// Manipulator
8+
u32 joints = 7;
9+
10+
// limits
11+
ManipulatorLimits limits;
12+
limits.position_max = {INF_REAL, 2.25, INF_REAL, 2.58f, INF_REAL, 2.1f, INF_REAL}; // rad
13+
limits.position_min = -limits.position_max;
14+
15+
limits.velocity_max = {1.745f, 1.745f, 1.745f, 1.745f, 2.443f, 2.443f, 2.443f}; // rad/s
16+
// limits.vmin = -limits.velocity_max;
17+
18+
limits.acceleration_max = {INF_REAL, INF_REAL, INF_REAL, INF_REAL, INF_REAL, INF_REAL, INF_REAL}; // rad/s^2
19+
// limits.amin = -limits.acceleration_max;
20+
21+
limits.torque_max = {52, 52, 52, 52, 17, 17, 17}; // Nm
22+
// limits.tau_min = -limits.torque_max;
23+
24+
// kinematic properties
25+
ManipulatorKinematics kinematics; // using default Q_base
26+
kinematics.joint_offsets = {
27+
Vec3{0.0, 0.0054, -0.1284},
28+
{0.0, -0.2104, -0.0064},
29+
{0.0, -0.0064, -0.2104},
30+
{0.0, -0.2084, -0.0064},
31+
{0.0, 0.0, -0.1059},
32+
{0.0, -0.1059, 0.0},
33+
{0.0, 0.0, -0.0615}}; // vector to next joint
34+
kinematics.joint_axes = {
35+
Vec3{0, 0, 1},
36+
{0, 0, 1},
37+
{0, 0, 1},
38+
{0, 0, 1},
39+
{0, 0, 1},
40+
{0, 0, 1},
41+
{0, 0, 1}}; // direction vectors of joint
42+
kinematics.static_rotations[0] = {1, 0, 0, 0, -1, 0, 0, 0, -1};
43+
kinematics.static_rotations[1] = {1, 0, 0, 0, 0, 1, 0, -1, 0};
44+
kinematics.static_rotations[2] = {1, 0, 0, 0, 0, -1, 0, 1, 0};
45+
kinematics.static_rotations[3] = {1, 0, 0, 0, 0, 1, 0, -1, 0};
46+
kinematics.static_rotations[4] = {1, 0, 0, 0, 0, -1, 0, 1, 0};
47+
kinematics.static_rotations[5] = {1, 0, 0, 0, 0, 1, 0, -1, 0};
48+
kinematics.static_rotations[6] = {1, 0, 0, 0, 0, -1, 0, 1, 0};
49+
kinematics.first_joint_position = {0, 0, 0.1564f};
50+
kinematics.base_position = {0, 0, 0};
51+
kinematics.base_rotation = {1, 0, 0, 0, 1, 0, 0, 0, 1};
52+
53+
// dynamic properties
54+
ManipulatorDynamics dynamics;
55+
56+
dynamics.link_masses = {
57+
1.377f,
58+
1.1636f,
59+
1.1636f,
60+
0.93f,
61+
0.678f,
62+
0.678f,
63+
0.364f}; // link masses
64+
dynamics.inertia_tensors = {
65+
Mat3{0.004570f, 0.000001f, 0.000002f, 0.000001f, 0.004831f, 0.000448f, 0.000002f, 0.000448f, 0.001409f},
66+
{0.011088f, 0.000005f, 0.000000f, 0.000005f, 0.001072f, -0.000691f, 0.000000f, -0.000691f, 0.011255f},
67+
{0.010932f, 0.000000f, -0.000007f, 0.000000f, 0.011127f, 0.000606f, -0.000007f, 0.000606f, 0.001043f},
68+
{0.008147f, -0.000001f, 0.000000f, -0.000001f, 0.000631f, -0.000500f, 0.000000f, -0.000500f, 0.008316f},
69+
{0.001596f, 0.000000f, 0.000000f, 0.000000f, 0.001607f, 0.000256f, 0.000000f, 0.000256f, 0.000399f},
70+
{0.001641f, 0.000000f, 0.000000f, 0.000000f, 0.000410f, -0.000278f, 0.000000f, -0.000278f, 0.001641f},
71+
{0.000214f, 0.000000f, 0.000001f, 0.000000f, 0.000223f, -0.000002f, 0.000001f, -0.000002f, 0.000240f}}; // Inertial tensors
72+
dynamics.cog_offsets = {
73+
Vec3{-0.000023f, -0.010364f, -0.073360f},
74+
{-0.000044f, -0.099580f, -0.013278f},
75+
{-0.000044f, -0.006641f, -0.117892f},
76+
{-0.000018f, -0.075478f, -0.015006f},
77+
{0.000001f, -0.009432f, -0.063883f},
78+
{0.000001f, -0.045483f, -0.009650f},
79+
{-0.000093f, 0.000132f, -0.022905f}}; // centers of mass
80+
81+
// create manipulator gen3
82+
Manipulator generic_manip(joints, limits, kinematics, &dynamics);
83+
84+
// Collision model
85+
ManipulatorCapsules collisions;
86+
Sphere sphere;
87+
sphere.center = {0, 0, 0.1214};
88+
sphere.radius = 0.14;
89+
collisions.base_sphere = sphere;
90+
collisions.collision_base = {0, 0, 1};
91+
92+
collisions.collision_matrix.resize(3, 3);
93+
collisions.collision_matrix(2, 0) = 1;
94+
95+
// Collision model
96+
CollisionModelCapsule model_caps;
97+
98+
// Capsule 1
99+
model_caps.joint_frame = 1;
100+
model_caps.p1 = {0, 0.035, 0};
101+
model_caps.p2 = {0, -0.425, 0};
102+
model_caps.radius = 0.06;
103+
collisions.capsule_list.push_back(model_caps);
104+
105+
// Capsule 2
106+
model_caps.joint_frame = 3;
107+
model_caps.p1 = {0, 0, -0.025};
108+
model_caps.p2 = {0, -0.3, -0.01};
109+
model_caps.radius = 0.06;
110+
collisions.capsule_list.push_back(model_caps);
111+
112+
// Capsule 3
113+
model_caps.joint_frame = 5;
114+
model_caps.p1 = {0, 0, -0.015};
115+
model_caps.p2 = {0.0, -0.15, -0.015};
116+
model_caps.radius = 0.055;
117+
collisions.capsule_list.push_back(model_caps);
118+
119+
generic_manip.set_capsules(collisions);
120+
121+
return generic_manip;
122+
}
123+
} // namespace blast

‎blast/manipulator/UR5e.hpp‎

Lines changed: 144 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,144 @@
1+
#pragma once
2+
#include <blast>
3+
4+
namespace blast {
5+
6+
Manipulator make_UR5e() {
7+
// Manipulator
8+
blast::u32 joints = 6;
9+
10+
// limits
11+
blast::ManipulatorLimits limits;
12+
limits.position_max = {6.283200, 6.283200, 3.141600, 6.283200, 6.283200, 6.283200};
13+
limits.position_min = {-6.283200, -6.283200, -3.141600, -6.283200, -6.283200, -6.283200};
14+
limits.velocity_max = {3.141600, 3.141600, 3.141600, 3.141600, 3.141600, 3.141600};
15+
// limits.vmin = {-3.141600, -3.141600, -3.141600, -3.141600, -3.141600, -3.141600};
16+
limits.acceleration_max = {13.96, 13.96, 13.96, 13.96, 13.96, 13.96};
17+
// limits.amin = {-13.96, -13.96, -13.96, -13.96, -13.96, -13.96};
18+
limits.torque_max = {150.000000, 150.000000, 150.000000, 28.000000, 28.000000, 28.000000};
19+
// limits.tau_min = {-150.000000, -150.000000, -150.000000, -28.000000, -28.000000, -28.000000};
20+
limits.tool_speed_max = 2.0; // todo: verify this value
21+
22+
blast::ManipulatorKinematics kinematics;
23+
kinematics.joint_offsets = {
24+
Vec3{0, 0, 0},
25+
{-0.425, 0, 0},
26+
{-0.3922, 0, 0.1333},
27+
{0, -0.0997, -0},
28+
{0, 0.0996, -0},
29+
{0, 0, 0} // to end effector
30+
};
31+
kinematics.joint_axes = {
32+
Vec3{0, 0, 1},
33+
{0, 0, 1},
34+
{0, 0, 1},
35+
{0, 0, 1},
36+
{0, 0, 1},
37+
{0, 0, 1}};
38+
// kinematics.static_rotations.resize(6);
39+
kinematics.static_rotations[0] = {-1.000000, 0.000000, 0.000000, -0.000000, -1.000000, 0.000000, 0.000000, -0.000000, 1.000000}; // modified
40+
kinematics.static_rotations[1] = {1.000000, 0.000000, 0.000000, -0.000000, -0.000000, 1.000000, 0.000000, -1.000000, -0.000000};
41+
kinematics.static_rotations[2] = {1.000000, 0.000000, 0.000000, -0.000000, 1.000000, 0.000000, 0.000000, -0.000000, 1.000000};
42+
kinematics.static_rotations[3] = {1.000000, 0.000000, 0.000000, -0.000000, 1.000000, 0.000000, 0.000000, -0.000000, 1.000000};
43+
kinematics.static_rotations[4] = {1.000000, 0.000000, 0.000000, -0.000000, -0.000000, 1.000000, 0.000000, -1.000000, -0.000000};
44+
kinematics.static_rotations[5] = {1.000000, -0.000000, 0.000000, 0.000000, -0.000000, -1.000000, 0.000000, 1.000000, -0.000000};
45+
// kinematics.static_rotations[6] = {1.000000, 0.000000, 0.000000, 0.000000, 1.000000, 0.000000, 0.000000, 0.000000, 1.000000};
46+
kinematics.first_joint_position = {0.000000, 0.000000, 0.162500};
47+
48+
blast::ManipulatorDynamics dynamics;
49+
dynamics.link_masses = {3.700000, 8.393000, 2.275000, 1.219000, 1.219000, 0.187900};
50+
dynamics.inertia_tensors = {
51+
Mat3{0.0103, 0, 0, 0, 0.0103, 0, 0, 0, 0.0067},
52+
{0.1339, 0, 0, 0, 0.1339, 0, 0, 0, 0.0151},
53+
{0.0312, 0, 0, 0, 0.0312, 0, 0, 0, 0.0041},
54+
{0.0026, 0, 0, 0, 0.0026, 0, 0, 0, 0.0022},
55+
{0.0026, 0, 0, 0, 0.0026, 0, 0, 0, 0.0022},
56+
{0.0001, 0, 0, 0, 0.0001, 0, 0, 0, 0.0001}};
57+
dynamics.cog_offsets = {
58+
Vec3{0, 0, 0},
59+
{-0.2125, 0, 0.138},
60+
{-0.1961, 0, 0.007},
61+
{0, 0, 0},
62+
{0, 0, 0},
63+
{0, 0, -0.0229}};
64+
65+
blast::ManipulatorCapsules collisions;
66+
67+
blast::Sphere base;
68+
base.center = {0, 0, 0.0325};
69+
base.radius = 0.09;
70+
collisions.base_sphere = base;
71+
collisions.collision_base = {0, 0, 0, 1, 1, 1, 1};
72+
73+
collisions.collision_matrix.resize(7, 7);
74+
collisions.collision_matrix(3, 0) = 1;
75+
collisions.collision_matrix(4, 0) = 1;
76+
collisions.collision_matrix(5, 0) = 1;
77+
collisions.collision_matrix(6, 0) = 1;
78+
79+
collisions.collision_matrix(4, 1) = 1;
80+
collisions.collision_matrix(5, 1) = 1;
81+
collisions.collision_matrix(6, 1) = 1;
82+
83+
collisions.collision_matrix(6, 3) = 1;
84+
85+
// mirrored just for consistency, technically not used
86+
collisions.collision_matrix(0, 3) = 1;
87+
collisions.collision_matrix(0, 4) = 1;
88+
collisions.collision_matrix(0, 5) = 1;
89+
collisions.collision_matrix(0, 6) = 1;
90+
91+
collisions.collision_matrix(1, 4) = 1;
92+
collisions.collision_matrix(1, 5) = 1;
93+
collisions.collision_matrix(1, 6) = 1;
94+
95+
collisions.collision_matrix(3, 6) = 1;
96+
97+
blast::CollisionModelCapsule capsule;
98+
capsule.joint_frame = 0;
99+
capsule.p1 = {0, 0, 0};
100+
capsule.p2 = {0, -0.15, 0};
101+
capsule.radius = 0.09;
102+
collisions.capsule_list.push_back(capsule);
103+
104+
capsule.joint_frame = 1;
105+
capsule.p1 = {-0.42, 0, 0.1375};
106+
capsule.p2 = {0, 0, 0.1375};
107+
capsule.radius = 0.06;
108+
collisions.capsule_list.push_back(capsule);
109+
110+
capsule.joint_frame = 2;
111+
capsule.p1 = {0, 0, 0.02};
112+
capsule.p2 = {0, 0, 0.18};
113+
capsule.radius = 0.065;
114+
collisions.capsule_list.push_back(capsule);
115+
116+
capsule.joint_frame = 2;
117+
capsule.p1 = {-0.373156, 0, 0.00850418};
118+
capsule.p2 = {0.000440611, 0.000121443, 0.00850418};
119+
capsule.radius = 0.05;
120+
collisions.capsule_list.push_back(capsule);
121+
122+
capsule.joint_frame = 3;
123+
capsule.p1 = {0, 0, 0};
124+
capsule.p2 = {0, 0, -0.155};
125+
capsule.radius = 0.0425;
126+
collisions.capsule_list.push_back(capsule);
127+
128+
capsule.joint_frame = 3;
129+
capsule.p1 = {0, 0.03, 0};
130+
capsule.p2 = {0, -0.1, 0};
131+
capsule.radius = 0.0425;
132+
collisions.capsule_list.push_back(capsule);
133+
134+
capsule.joint_frame = 5;
135+
capsule.p1 = {0, 0, -0.14};
136+
capsule.p2 = {0, 0, -0.01};
137+
capsule.radius = 0.038;
138+
collisions.capsule_list.push_back(capsule);
139+
140+
blast::Manipulator generic_manip(joints, limits, kinematics, &dynamics, &collisions);
141+
return generic_manip;
142+
}
143+
144+
} // namespace blast

0 commit comments

Comments
 (0)