-
Notifications
You must be signed in to change notification settings - Fork 1.2k
/
gripper_brick.h
139 lines (111 loc) · 4 KB
/
gripper_brick.h
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
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55
56
57
58
59
60
61
62
63
64
65
66
67
68
69
70
71
72
73
74
75
76
77
78
79
80
81
82
83
84
85
86
87
88
89
90
91
92
93
94
95
96
97
98
99
100
101
102
103
104
105
106
107
108
109
110
111
112
113
114
115
116
117
118
119
120
121
122
123
124
125
126
127
128
129
130
131
132
133
134
135
136
137
138
139
#pragma once
#include <array>
#include <memory>
#include <string>
#include "drake/geometry/scene_graph.h"
#include "drake/multibody/plant/multibody_plant.h"
#include "drake/systems/framework/diagram.h"
namespace drake {
namespace examples {
namespace planar_gripper {
enum class Finger {
kFinger1,
kFinger2,
kFinger3,
};
std::string to_string(Finger finger);
enum class BrickFace {
kPosZ,
kNegZ,
kPosY,
kNegY,
};
/**
* The helper class that contains the diagram of the planar gripper (3 planar
* fingers) with a brick.
*/
template <typename T>
class GripperBrickHelper {
public:
GripperBrickHelper();
const systems::Diagram<T>& diagram() const { return *diagram_; }
systems::Diagram<T>* get_mutable_diagram() { return diagram_.get(); }
const multibody::MultibodyPlant<T>& plant() const { return *plant_; }
multibody::MultibodyPlant<T>* get_mutable_plant() { return plant_; }
/** The index of the base joint for a given finger in MultibodyPlant. */
int finger_base_position_index(Finger finger) const;
/** The index of the middle joint for a given finger in MultibodyPlant. */
int finger_mid_position_index(Finger finger) const;
int brick_translate_y_position_index() const {
return brick_translate_y_position_index_;
}
int brick_translate_z_position_index() const {
return brick_translate_z_position_index_;
}
int brick_revolute_x_position_index() const {
return brick_revolute_x_position_index_;
}
/** Position of the finger tip sphere center "Tip" in the finger_link2 frame.
*/
Eigen::Vector3d p_L2Fingertip() const { return p_L2Fingertip_; }
const multibody::Frame<double>& brick_frame() const { return *brick_frame_; }
const multibody::Frame<double>& finger_link2_frame(Finger finger) const;
double finger_tip_radius() const { return finger_tip_radius_; }
Eigen::Vector3d brick_size() const { return brick_size_; }
/**
* Return the orientation of link 2. Notice that since the finger only moves
* in the planar surface, the orientation can be represented by the rotation
* angle around the world x axis.
*/
template <typename U>
U CalcFingerLink2Orientation(Finger finger, const U& base_joint_angle,
const U& middle_joint_angle) const {
// base_theta is the yaw angle to weld the finger base. Keep the values
// synchronized with WeldGripperFrames() in planar_gripper_common.h. The
// test in gripper_brick_test.cc guarantees that base_theta is synchronized.
double base_theta;
switch (finger) {
case Finger::kFinger1: {
base_theta = 1.0 / 3 * M_PI;
break;
}
case Finger::kFinger2: {
base_theta = -1.0 / 3 * M_PI;
break;
}
case Finger::kFinger3: {
base_theta = M_PI;
break;
}
default: { throw std::runtime_error("Unknown finger."); }
}
return base_theta + base_joint_angle + middle_joint_angle;
}
geometry::GeometryId finger_tip_sphere_geometry_id(Finger finger) const;
geometry::GeometryId brick_geometry_id() const {
return plant_->GetCollisionGeometriesForBody(brick_frame().body())[0];
}
multibody::CoulombFriction<T> GetFingerTipBrickCoulombFriction(
Finger finger) const;
private:
std::unique_ptr<systems::Diagram<T>> diagram_;
multibody::MultibodyPlant<T>* plant_;
geometry::SceneGraph<T>* scene_graph_;
static constexpr int kNumFingers{3};
std::array<int, kNumFingers> finger_base_position_indices_;
std::array<int, kNumFingers> finger_mid_position_indices_;
int brick_translate_y_position_index_;
int brick_translate_z_position_index_;
int brick_revolute_x_position_index_;
const multibody::Frame<double>* brick_frame_;
std::array<const multibody::Frame<double>*, kNumFingers>
finger_link2_frames_;
std::array<geometry::GeometryId, kNumFingers>
finger_tip_sphere_geometry_ids_;
Eigen::Vector3d p_L2Fingertip_;
double finger_tip_radius_;
Eigen::Vector3d brick_size_;
};
} // namespace planar_gripper
} // namespace examples
} // namespace drake