├── README.md
├── communication
├── CMakeLists.txt
├── msg
│ ├── ActuatorCmds.msg
│ └── MotionCommands.msg
├── package.xml
└── srv
│ ├── GaitSwitch.srv
│ └── SimulationReset.srv
├── core
├── CMakeLists.txt
├── cmake
│ ├── deepbreak_cxx_flags.cmake
│ └── pinocchio_config.cmake
├── include
│ └── core
│ │ ├── Types.h
│ │ └── misc
│ │ ├── Benchmark.h
│ │ ├── Buffer.h
│ │ ├── LinearAlgebra.h
│ │ ├── LoadData.h
│ │ ├── Lookup.h
│ │ ├── NumericTraits.h
│ │ └── Numerics.h
├── package.xml
└── src
│ ├── Types.cpp
│ └── misc
│ └── LinearAlgebra.cpp
├── description
├── CMakeLists.txt
├── model
│ └── xml
│ │ ├── digit.xml
│ │ └── meshes
│ │ ├── achilles-rod.stl
│ │ ├── arm-L1.stl
│ │ ├── arm-L2.stl
│ │ ├── arm-L3.stl
│ │ ├── arm-L4.stl
│ │ ├── heel-spring.stl
│ │ ├── hip-pitch-housing.stl
│ │ ├── hip-pitch.stl
│ │ ├── hip-roll-housing.stl
│ │ ├── hip-yaw-housing.stl
│ │ ├── knee.stl
│ │ ├── left-shoulder-cap.stl
│ │ ├── left-waist-cap.stl
│ │ ├── right-shoulder-cap.stl
│ │ ├── right-waist-cap.stl
│ │ ├── shin.stl
│ │ ├── shoulder-roll-housing.stl
│ │ ├── tarsus.stl
│ │ ├── toe-A-rod.stl
│ │ ├── toe-B-rod.stl
│ │ ├── toe-output-ll.stl
│ │ ├── toe-output-lr.stl
│ │ ├── toe-output-rl.stl
│ │ ├── toe-output-rr.stl
│ │ ├── toe-output.stl
│ │ ├── toe-pitch.stl
│ │ ├── toe-roll.stl
│ │ └── torso.stl
└── package.xml
├── results.png
└── simulation
└── mujoco
├── CMakeLists.txt
├── cmake
├── CheckAvxSupport.cmake
├── FindOrFetch.cmake
├── MujocoHarden.cmake
├── MujocoLinkOptions.cmake
├── MujocoMacOS.cmake
├── SimulateDependencies.cmake
└── SimulateOptions.cmake
├── include
├── MuJoCoMessageHandler.h
├── array_safety.h
├── glfw_adapter.h
├── glfw_dispatch.h
├── lodepng.h
├── mujoco
│ ├── mjdata.h
│ ├── mjexport.h
│ ├── mjmodel.h
│ ├── mjplugin.h
│ ├── mjrender.h
│ ├── mjtnum.h
│ ├── mjui.h
│ ├── mjvisualize.h
│ ├── mjxmacro.h
│ └── mujoco.h
├── platform_ui_adapter.h
└── simulate.h
├── launch
└── simulation_launch.py
├── lib
├── libmujoco.so
└── libmujoco.so.2.3.2
├── package.xml
└── src
├── MuJoCoMessageHandler.cpp
├── glfw_adapter.cc
├── glfw_dispatch.cc
├── lodepng.cpp
├── main.cc
├── platform_ui_adapter.cc
└── simulate.cc
/README.md:
--------------------------------------------------------------------------------
1 | # Digit MuJoCo ROS2
2 |
3 |
4 |
5 |
116 | * matrixName
117 | * {
118 | * scaling 1e+0
119 | * (0,0) value ; M(0,0)
120 | * (1,0) value ; M(1,0)
121 | * (0,1) value ; M(0,1)
122 | * (1,1) value ; M(1,1)
123 | * }
124 | *
125 | * If a value for a specific element is not defined it will set by default to zero.
126 | *
127 | * @param [in] filename: File name which contains the configuration data.
128 | * @param [in] matrixName: The key name assigned to the matrix in the config file.
129 | * @param [out] matrix: The loaded matrix, must have desired size.
130 | */
131 | template
132 | inline void loadEigenMatrix(const std::string& filename, const std::string& matrixName, Eigen::MatrixBase& matrix) {
133 | using scalar_t = typename Eigen::MatrixBase::Scalar;
134 |
135 | size_t rows = matrix.rows();
136 | size_t cols = matrix.cols();
137 |
138 | if (rows == 0 && cols == 0) {
139 | throw std::runtime_error("[loadEigenMatrix] Loading empty matrix \"" + matrixName + "\" is not allowed.");
140 | }
141 |
142 | boost::property_tree::ptree pt;
143 | boost::property_tree::read_info(filename, pt);
144 |
145 | const scalar_t scaling = pt.get(matrixName + ".scaling", 1.0);
146 | const scalar_t defaultValue = pt.get(matrixName + ".default", 0.0);
147 |
148 | size_t numFailed = 0;
149 | for (size_t i = 0; i < rows; i++) {
150 | for (size_t j = 0; j < cols; j++) {
151 | scalar_t aij;
152 | try {
153 | aij = pt.get(matrixName + "." + "(" + std::to_string(i) + "," + std::to_string(j) + ")");
154 | } catch (const std::exception&) {
155 | aij = defaultValue;
156 | numFailed++;
157 | }
158 | matrix(i, j) = scaling * aij;
159 | }
160 | }
161 |
162 | if (numFailed == matrix.size()) {
163 | throw std::runtime_error("[loadEigenMatrix] Could not load matrix \"" + matrixName + "\" from file \"" + filename + "\".");
164 | } else if (numFailed > 0) {
165 | std::cerr << "WARNING: Loaded at least one default value in matrix: \"" + matrixName + "\"\n";
166 | }
167 | }
168 |
169 | template
170 | inline void loadStdVector(const std::string& filename, const std::string& topicName, std::vector& loadVector, bool verbose = true) {
171 | boost::property_tree::ptree pt;
172 | boost::property_tree::read_info(filename, pt);
173 |
174 | std::vector backup;
175 | backup.swap(loadVector);
176 | loadVector.clear();
177 |
178 | size_t vectorSize = 0;
179 | while (true) {
180 | try {
181 | loadVector.push_back(pt.get(topicName + ".[" + std::to_string(vectorSize) + "]"));
182 | vectorSize++;
183 | } catch (const std::exception&) {
184 | if (vectorSize == 0) {
185 | loadVector.swap(backup); // if nothing could be loaded, keep the old data
186 | }
187 | break;
188 | }
189 | } // end of while loop
190 |
191 | // display
192 | if (verbose) {
193 | std::cerr << " #### '" << topicName << "': {";
194 | if (vectorSize == 0) {
195 | std::cerr << " }\n";
196 | } else {
197 | for (size_t i = 0; i < vectorSize; i++) {
198 | std::cerr << loadVector[i] << ", ";
199 | }
200 | std::cerr << "\b\b}\n";
201 | }
202 | }
203 | }
204 |
205 | } // namespace loadData
206 | } // namespace deepbreak
207 |
--------------------------------------------------------------------------------
/core/include/core/misc/Lookup.h:
--------------------------------------------------------------------------------
1 | /******************************************************************************
2 | Copyright (c) 2020, Farbod Farshidian. All rights reserved.
3 |
4 | Redistribution and use in source and binary forms, with or without
5 | modification, are permitted provided that the following conditions are met:
6 |
7 | * Redistributions of source code must retain the above copyright notice, this
8 | list of conditions and the following disclaimer.
9 |
10 | * Redistributions in binary form must reproduce the above copyright notice,
11 | this list of conditions and the following disclaimer in the documentation
12 | and/or other materials provided with the distribution.
13 |
14 | * Neither the name of the copyright holder nor the names of its
15 | contributors may be used to endorse or promote products derived from
16 | this software without specific prior written permission.
17 |
18 | THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
19 | AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
20 | IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE
21 | DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE
22 | FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL
23 | DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR
24 | SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
25 | CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY,
26 | OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE
27 | OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
28 | ******************************************************************************/
29 |
30 | #pragma once
31 |
32 | #include
33 | #include
34 | #include
35 |
36 | #include
37 | #include
38 |
39 | namespace deepbreak {
40 | namespace lookup {
41 |
42 | /**
43 | * finds the index of an element in a sorted dataArray which is equal to value
44 | * (epsilon distance)
45 | *
46 | * @tparam SCALAR : scalar type
47 | * @param [in] dataArray: data array
48 | * @param [in] value: enquiry value
49 | * @return: index
50 | */
51 | template
52 | size_t
53 | findFirstIndexWithinTol(const std::vector &dataArray, SCALAR value,
54 | SCALAR eps = numeric_traits::weakEpsilon()) {
55 | // Search for a match by linearly traversing the data, returning first match
56 | for (size_t i = 0; i < dataArray.size(); i++) {
57 | if (std::abs(dataArray[i] - value) < eps) {
58 | return i;
59 | }
60 | }
61 |
62 | // If we reach here, no match was found
63 | std::stringstream msg;
64 | msg << "[findFirstIndexWithinTol] Value not found within tolerance, with "
65 | "\n\t dataArray: ";
66 | for (const auto d : dataArray) {
67 | msg << d << ", ";
68 | }
69 | msg << "\n\t value: " << value << "\n\t epsilon: " << eps << std::endl;
70 | throw std::runtime_error(msg.str());
71 | }
72 |
73 | /**
74 | * Find index into a sorted time Array
75 | *
76 | * Indices are counted as follows:
77 | * ------ | ----- | --- ... --- | -----
78 | * t0 t1 t(n-1)
79 | * Index 0 1 2 ... (n-1) n
80 | *
81 | * Corner cases:
82 | * - If time equal to a time in the timeArray is requested, the lower index
83 | * is taken (e.g. t = t1 -> index = 1)
84 | * - If multiple times in the timeArray are equal, the index before the
85 | * first occurrence is taken. for example: if t1 = t2 = t3 and the requested
86 | * time t <= t3 -> index = 1
87 | *
88 | *
89 | * @tparam SCALAR : numerical type of time
90 | * @param timeArray : sorted time array to perform the lookup in
91 | * @param time : enquiry time
92 | * @return index between [0, size(timeArray)]
93 | */
94 | template
95 | int findIndexInTimeArray(const std::vector &timeArray, SCALAR time) {
96 | auto firstLargerValueIterator =
97 | std::lower_bound(timeArray.begin(), timeArray.end(), time);
98 | return static_cast(firstLargerValueIterator - timeArray.begin());
99 | }
100 |
101 | /**
102 | * Find interval into a sorted time Array
103 | *
104 | * Intervals are counted as follows:
105 | * ------ | ----- | --- ... --- | -----
106 | * t0 t1 t(n-1)
107 | * Interval -1 0 1 ... (n-2) (n-1)
108 | *
109 | * Corner cases are handled as in findIndexInTimeArray
110 | *
111 | * @tparam SCALAR : numerical type of time
112 | * @param timeArray : sorted time array to perform the lookup in
113 | * @param time : enquiry time
114 | * @return interval between [-1, size(timeArray)-1]
115 | */
116 | template
117 | int findIntervalInTimeArray(const std::vector &timeArray, SCALAR time) {
118 | if (!timeArray.empty()) {
119 | return findIndexInTimeArray(timeArray, time) - 1;
120 | } else {
121 | return 0;
122 | }
123 | }
124 |
125 | /**
126 | * Same as findIntervalInTimeArray except for 1 rule:
127 | * if t = t0, a 0 is returned instead of -1
128 | * This means that any query t_front <= t <= t_back gets assigned to a partition
129 | * [0, size -2]
130 | *
131 | * @tparam SCALAR : scalar type
132 | * @param timeArray : sorted time array to perform the lookup in
133 | * @param time : enquiry time
134 | * @return partition between [-1, size(timeArray)-1]
135 | */
136 | template
137 | int findActiveIntervalInTimeArray(const std::vector &timeArray,
138 | SCALAR time) {
139 | if (!timeArray.empty() && !numerics::almost_eq(time, timeArray.front())) {
140 | return findIntervalInTimeArray(timeArray, time);
141 | } else { // t = t0
142 | return 0;
143 | }
144 | }
145 |
146 | /**
147 | * Wraps findActiveIntervalInTimeArray with bound check
148 | * throws an error if time before of after the end is selected.
149 | * Also throws on timeArrays that are empty or with 1 element only.
150 | *
151 | * @tparam SCALAR : scalar type
152 | * @param timeArray : sorted time array to perform the lookup in
153 | * @param time : enquiry time
154 | * @return partition between [0, size(timeArray)-2]
155 | */
156 | template
157 | int findBoundedActiveIntervalInTimeArray(const std::vector &timeArray,
158 | SCALAR time) {
159 | auto partition = findActiveIntervalInTimeArray(timeArray, time);
160 |
161 | if (timeArray.empty()) {
162 | std::string mesg =
163 | "[findBoundedActiveIntervalInTimeArray] Time array is empty";
164 | throw std::runtime_error(mesg);
165 | }
166 |
167 | if (partition < 0) {
168 | std::string mesg =
169 | "[findBoundedActiveIntervalInTimeArray] Given time is less than the "
170 | "start time (i.e. givenTime < timeArray.front()): " +
171 | std::to_string(time) + " < " + std::to_string(timeArray.front());
172 | throw std::runtime_error(mesg);
173 | }
174 |
175 | if (partition >= timeArray.size() - 1) {
176 | std::string mesg =
177 | "[findBoundedActiveIntervalInTimeArray] Given time is greater than the "
178 | "final time (i.e. timeArray.back() < givenTime): " +
179 | std::to_string(timeArray.back()) + " < " + std::to_string(time);
180 | throw std::runtime_error(mesg);
181 | }
182 |
183 | return partition;
184 | }
185 |
186 | } // namespace lookup
187 | } // namespace deepbreak
188 |
--------------------------------------------------------------------------------
/core/include/core/misc/NumericTraits.h:
--------------------------------------------------------------------------------
1 | /******************************************************************************
2 | Copyright (c) 2017, Farbod Farshidian. All rights reserved.
3 |
4 | Redistribution and use in source and binary forms, with or without
5 | modification, are permitted provided that the following conditions are met:
6 |
7 | * Redistributions of source code must retain the above copyright notice, this
8 | list of conditions and the following disclaimer.
9 |
10 | * Redistributions in binary form must reproduce the above copyright notice,
11 | this list of conditions and the following disclaimer in the documentation
12 | and/or other materials provided with the distribution.
13 |
14 | * Neither the name of the copyright holder nor the names of its
15 | contributors may be used to endorse or promote products derived from
16 | this software without specific prior written permission.
17 |
18 | THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
19 | AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
20 | IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE
21 | DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE
22 | FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL
23 | DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR
24 | SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
25 | CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY,
26 | OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE
27 | OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
28 | ******************************************************************************/
29 |
30 | #pragma once
31 |
32 | namespace deepbreak {
33 | namespace numeric_traits {
34 | /**
35 | * If a floating number v is in limit epsilon vicinity of w, then we assume that v approaches to w.
36 | * This limit_epsilon value should be greater than the weak_epsilon value.
37 | *
38 | * @return limit epsilon value.
39 | */
40 | template
41 | constexpr T limitEpsilon() {
42 | return T(1e-6);
43 | }
44 |
45 | /**
46 | * Defines the precision during comparison.
47 | *
48 | * @return weak epsilon value
49 | */
50 | template
51 | constexpr T weakEpsilon() {
52 | return T(1e-9);
53 | }
54 |
55 | } // namespace numeric_traits
56 | } // namespace deepbreak
57 |
--------------------------------------------------------------------------------
/core/include/core/misc/Numerics.h:
--------------------------------------------------------------------------------
1 | /******************************************************************************
2 | Copyright (c) 2020, Farbod Farshidian. All rights reserved.
3 |
4 | Redistribution and use in source and binary forms, with or without
5 | modification, are permitted provided that the following conditions are met:
6 |
7 | * Redistributions of source code must retain the above copyright notice, this
8 | list of conditions and the following disclaimer.
9 |
10 | * Redistributions in binary form must reproduce the above copyright notice,
11 | this list of conditions and the following disclaimer in the documentation
12 | and/or other materials provided with the distribution.
13 |
14 | * Neither the name of the copyright holder nor the names of its
15 | contributors may be used to endorse or promote products derived from
16 | this software without specific prior written permission.
17 |
18 | THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
19 | AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
20 | IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE
21 | DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE
22 | FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL
23 | DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR
24 | SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
25 | CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY,
26 | OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE
27 | OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
28 | ******************************************************************************/
29 |
30 | #pragma once
31 |
32 | #include
33 | #include
34 | #include
35 | #include
36 |
37 | namespace deepbreak {
38 | namespace numerics {
39 |
40 | /**
41 | * Almost equal which uses machine epsilon to compare floating-point values for equality.
42 | * refer to: https://en.cppreference.com/w/cpp/types/numeric_limits/epsilon
43 | *
44 | * @tparam T1: data type of x.
45 | * @tparam T2: data type of y.
46 | * @tparam T3: data type of prec.
47 | * @param [in] x: First floating-point number.
48 | * @param [in] y: Second floating-point number.
49 | * @param [in] prec: The comparison precision.
50 | * @return bool: true if x=y.
51 | */
52 | template
53 | bool almost_eq(T1&& x, T2&& y, T3&& prec) {
54 | rcpputils::assert_true(std::is_floating_point::type>::value, "First argument is not floating point!");
55 | rcpputils::assert_true(std::is_floating_point::type>::value, "Second argument is not floating point!");
56 | rcpputils::assert_true(std::is_floating_point::type>::value, "prec is not floating point!");
57 | // the machine epsilon has to be scaled to the magnitude of the values used
58 | // and multiplied by the desired precision unless the result is subnormal
59 | using Type = const std::remove_reference_t;
60 | const auto absDiff = std::abs(x - static_cast(y));
61 | const auto magnitude = std::min(std::abs(x), std::abs(static_cast(y)));
62 | return absDiff <= static_cast(prec) * magnitude || absDiff < std::numeric_limits::min();
63 | }
64 |
65 | /**
66 | * Almost equal which uses machine epsilon to compare floating-point values for equality.
67 | * refer to: https://en.cppreference.com/w/cpp/types/numeric_limits/epsilon
68 | *
69 | * @tparam T1: data type of x.
70 | * @tparam T2: data type of y.
71 | * @param [in] x: First floating-point number.
72 | * @param [in] y: Second floating-point number.
73 | * @return bool: true if x=y.
74 | */
75 | template
76 | bool almost_eq(T1&& x, T2&& y) {
77 | const auto prec = std::numeric_limits>::epsilon();
78 | return almost_eq(x, y, prec);
79 | }
80 |
81 | /**
82 | * Almost less-equal which uses machine epsilon to compare floating-point values for equality.
83 | *
84 | * @tparam T1: data type of x.
85 | * @tparam T2: data type of y.
86 | * @tparam T3: data type of prec.
87 | * @param [in] x: First floating-point number.
88 | * @param [in] y: Second floating-point number.
89 | * @param [in] prec: The comparison precision.
90 | * @return bool: true if x<=y.
91 | */
92 | template
93 | bool almost_le(T1&& x, T2&& y, T3&& prec) {
94 | return x < y || almost_eq(x, y, prec);
95 | }
96 |
97 | /**
98 | * Almost less-equal which uses machine epsilon to compare floating-point values for equality.
99 | *
100 | * @tparam T1: data type of x.
101 | * @tparam T2: data type of y.
102 | * @param [in] x: First floating-point number.
103 | * @param [in] y: Second floating-point number.
104 | * @return bool: true if x<=y.
105 | */
106 | template
107 | bool almost_le(T1&& x, T2&& y) {
108 | return x < y || almost_eq(x, y);
109 | }
110 |
111 | /**
112 | * Almost greater-equal which uses machine epsilon to compare floating-point values for equality.
113 | *
114 | * @tparam T1: data type of x.
115 | * @tparam T2: data type of y.
116 | * @tparam T3: data type of prec.
117 | * @param [in] x: First floating-point number.
118 | * @param [in] y: Second floating-point number.
119 | * @param [in] prec: The comparison precision.
120 | * @return bool: true if x>=y.
121 | */
122 | template
123 | bool almost_ge(T1&& x, T2&& y, T3&& prec) {
124 | return x > y || almost_eq(x, y, prec);
125 | }
126 |
127 | /**
128 | * Almost greater-equal which uses machine epsilon to compare floating-point values for equality.
129 | *
130 | * @tparam T1: data type of x.
131 | * @tparam T2: data type of y.
132 | * @param [in] x: First floating-point number.
133 | * @param [in] y: Second floating-point number.
134 | * @return bool: true if x>=y.
135 | */
136 | template
137 | bool almost_ge(T1&& x, T2&& y) {
138 | return x > y || almost_eq(x, y);
139 | }
140 |
141 | } // namespace numerics
142 | } // namespace deepbreak
143 |
--------------------------------------------------------------------------------
/core/package.xml:
--------------------------------------------------------------------------------
1 |
2 |
3 |
4 | core
5 | 0.0.0
6 | TODO: Package description
7 | DeepBreak
8 | BSD
9 |
10 | ament_cmake
11 |
12 | ament_lint_auto
13 | ament_lint_common
14 |
15 | Boost
16 | Eigen3
17 | rcpputils
18 |
19 |
20 | ament_cmake
21 |
22 |
23 |
--------------------------------------------------------------------------------
/core/src/misc/LinearAlgebra.cpp:
--------------------------------------------------------------------------------
1 | /******************************************************************************
2 | Copyright (c) 2017, Farbod Farshidian. All rights reserved.
3 |
4 | Redistribution and use in source and binary forms, with or without
5 | modification, are permitted provided that the following conditions are met:
6 |
7 | * Redistributions of source code must retain the above copyright notice, this
8 | list of conditions and the following disclaimer.
9 |
10 | * Redistributions in binary form must reproduce the above copyright notice,
11 | this list of conditions and the following disclaimer in the documentation
12 | and/or other materials provided with the distribution.
13 |
14 | * Neither the name of the copyright holder nor the names of its
15 | contributors may be used to endorse or promote products derived from
16 | this software without specific prior written permission.
17 |
18 | THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
19 | AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
20 | IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE
21 | DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE
22 | FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL
23 | DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR
24 | SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
25 | CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY,
26 | OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE
27 | OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
28 | ******************************************************************************/
29 |
30 | #include
31 | #include
32 |
33 | namespace deepbreak {
34 | namespace LinearAlgebra {
35 |
36 | /******************************************************************************************************/
37 | /******************************************************************************************************/
38 | /******************************************************************************************************/
39 | void setTriangularMinimumEigenvalues(matrix_t& Lr, scalar_t minEigenValue) {
40 | for (Eigen::Index i = 0; i < Lr.rows(); ++i) {
41 | scalar_t& eigenValue = Lr(i, i); // diagonal element is the eigenvalue
42 | if (eigenValue < 0.0) {
43 | eigenValue = std::min(-minEigenValue, eigenValue);
44 | } else {
45 | eigenValue = std::max(minEigenValue, eigenValue);
46 | }
47 | }
48 | }
49 |
50 | /******************************************************************************************************/
51 | /******************************************************************************************************/
52 | /******************************************************************************************************/
53 | void makePsdEigenvalue(matrix_t& squareMatrix, scalar_t minEigenvalue) {
54 | rcpputils::assert_true(squareMatrix.rows() == squareMatrix.cols());
55 |
56 | Eigen::SelfAdjointEigenSolver eig(squareMatrix, Eigen::EigenvaluesOnly);
57 | vector_t lambda = eig.eigenvalues();
58 |
59 | bool hasNegativeEigenValue = false;
60 | for (int j = 0; j < lambda.size(); j++) {
61 | if (lambda(j) < minEigenvalue) {
62 | hasNegativeEigenValue = true;
63 | lambda(j) = minEigenvalue;
64 | }
65 | }
66 |
67 | if (hasNegativeEigenValue) {
68 | eig.compute(squareMatrix, Eigen::ComputeEigenvectors);
69 | squareMatrix = eig.eigenvectors() * lambda.asDiagonal() * eig.eigenvectors().inverse();
70 | } else {
71 | squareMatrix = 0.5 * (squareMatrix + squareMatrix.transpose()).eval();
72 | }
73 | }
74 |
75 | /******************************************************************************************************/
76 | /******************************************************************************************************/
77 | /******************************************************************************************************/
78 | void makePsdGershgorin(matrix_t& squareMatrix, scalar_t minEigenvalue) {
79 | rcpputils::assert_true(squareMatrix.rows() == squareMatrix.cols());
80 | squareMatrix = 0.5 * (squareMatrix + squareMatrix.transpose()).eval();
81 | for (int i = 0; i < squareMatrix.rows(); i++) {
82 | // Gershgorin radius: since the matrix is symmetric we use column sum instead of row sum
83 | auto Ri = squareMatrix.col(i).cwiseAbs().sum() - std::abs(squareMatrix(i, i));
84 | squareMatrix(i, i) = std::max(squareMatrix(i, i), Ri + minEigenvalue);
85 | }
86 | }
87 |
88 | /******************************************************************************************************/
89 | /******************************************************************************************************/
90 | /******************************************************************************************************/
91 | void makePsdCholesky(matrix_t& A, scalar_t minEigenvalue) {
92 | using sparse_matrix_t = Eigen::SparseMatrix;
93 |
94 | rcpputils::assert_true(A.rows() == A.cols());
95 |
96 | // set the minimum eigenvalue
97 | A.diagonal().array() -= minEigenvalue;
98 |
99 | // S P' (A + E) P S = L L'
100 | sparse_matrix_t squareMatrix = 0.5 * A.sparseView();
101 | A.transposeInPlace();
102 | squareMatrix += 0.5 * A.sparseView();
103 | Eigen::IncompleteCholesky incompleteCholesky(squareMatrix);
104 | // P' A P = M M'
105 | sparse_matrix_t M = (incompleteCholesky.scalingS().asDiagonal().inverse() * incompleteCholesky.matrixL());
106 | // L L' = P M M' P'
107 | sparse_matrix_t LmTwisted;
108 | LmTwisted.selfadjointView() = M.selfadjointView().twistedBy(incompleteCholesky.permutationP());
109 | const matrix_t L(LmTwisted);
110 | // A = L L'
111 | A = L * L.transpose();
112 |
113 | // correction for the minimum eigenvalue
114 | A.diagonal().array() += minEigenvalue;
115 | }
116 |
117 | /******************************************************************************************************/
118 | /******************************************************************************************************/
119 | /******************************************************************************************************/
120 | void computeInverseMatrixUUT(const matrix_t& Am, matrix_t& AmInvUmUmT) {
121 | // Am = Lm Lm^T --> inv(Am) = inv(Lm^T) inv(Lm) where Lm^T is upper triangular
122 | Eigen::LLT lltOfA(Am);
123 | AmInvUmUmT.setIdentity(Am.rows(), Am.cols()); // for dynamic size matrices
124 | lltOfA.matrixU().solveInPlace(AmInvUmUmT);
125 | }
126 |
127 | /******************************************************************************************************/
128 | /******************************************************************************************************/
129 | /******************************************************************************************************/
130 | void computeConstraintProjection(const matrix_t& Dm, const matrix_t& RmInvUmUmT, matrix_t& DmDagger, matrix_t& DmDaggerTRmDmDaggerUUT,
131 | matrix_t& RmInvConstrainedUUT) {
132 | const auto numConstraints = Dm.rows();
133 | const auto numInputs = Dm.cols();
134 |
135 | // Constraint Projectors are based on the QR decomposition
136 | Eigen::HouseholderQR QRof_RmInvUmUmTT_DmT(RmInvUmUmT.transpose() * Dm.transpose());
137 |
138 | matrix_t QRof_RmInvUmUmTT_DmT_Rc = QRof_RmInvUmUmTT_DmT.matrixQR().topRows(numConstraints).triangularView();
139 | setTriangularMinimumEigenvalues(QRof_RmInvUmUmTT_DmT_Rc);
140 |
141 | // Computes the inverse of Rc with an efficient in-place forward-backward substitution
142 | // Turns out that this is equal to the UUT decomposition of DmDagger^T * R * DmDagger after simplification
143 | DmDaggerTRmDmDaggerUUT.setIdentity(numConstraints, numConstraints);
144 | QRof_RmInvUmUmTT_DmT_Rc.triangularView().solveInPlace(DmDaggerTRmDmDaggerUUT);
145 |
146 | matrix_t QRof_RmInvUmUmTT_DmT_Q = QRof_RmInvUmUmTT_DmT.householderQ();
147 | // Auto take reference to the column view here without making a temporary
148 | auto QRof_RmInvUmUmTT_DmT_Qc = QRof_RmInvUmUmTT_DmT_Q.leftCols(numConstraints);
149 | auto QRof_RmInvUmUmTT_DmT_Qu = QRof_RmInvUmUmTT_DmT_Q.rightCols(numInputs - numConstraints);
150 |
151 | // Compute Weighted Pseudo Inverse, brackets used to compute the smaller, right-side product first
152 | DmDagger.noalias() = RmInvUmUmT * (QRof_RmInvUmUmTT_DmT_Qc * DmDaggerTRmDmDaggerUUT.transpose());
153 |
154 | // Constraint input cost UUT decomposition
155 | RmInvConstrainedUUT.noalias() = RmInvUmUmT * QRof_RmInvUmUmTT_DmT_Qu;
156 | }
157 |
158 | /******************************************************************************************************/
159 | /******************************************************************************************************/
160 | /******************************************************************************************************/
161 | std::pair qrConstraintProjection(const VectorFunctionLinearApproximation& constraint) {
162 | // Constraint Projectors are based on the QR decomposition
163 | const auto numConstraints = constraint.dfdu.rows();
164 | const auto numInputs = constraint.dfdu.cols();
165 | const Eigen::HouseholderQR QRof_DT(constraint.dfdu.transpose());
166 |
167 | const matrix_t Q = QRof_DT.householderQ();
168 | const auto Q1 = Q.leftCols(numConstraints);
169 |
170 | const auto R = QRof_DT.matrixQR().topRows(numConstraints).triangularView();
171 | const matrix_t pseudoInverse = R.solve(Q1.transpose()); // left pseudo-inverse of D^T
172 |
173 | VectorFunctionLinearApproximation projectionTerms;
174 | projectionTerms.dfdu = Q.rightCols(numInputs - numConstraints);
175 | projectionTerms.dfdx.noalias() = -pseudoInverse.transpose() * constraint.dfdx;
176 | projectionTerms.f.noalias() = -pseudoInverse.transpose() * constraint.f;
177 |
178 | return std::make_pair(std::move(projectionTerms), std::move(pseudoInverse));
179 | }
180 |
181 | /******************************************************************************************************/
182 | /******************************************************************************************************/
183 | /******************************************************************************************************/
184 | std::pair luConstraintProjection(const VectorFunctionLinearApproximation& constraint,
185 | bool extractPseudoInverse) {
186 | // Constraint Projectors are based on the LU decomposition
187 | const Eigen::FullPivLU lu(constraint.dfdu);
188 |
189 | VectorFunctionLinearApproximation projectionTerms;
190 | projectionTerms.dfdu = lu.kernel();
191 | projectionTerms.dfdx.noalias() = -lu.solve(constraint.dfdx);
192 | projectionTerms.f.noalias() = -lu.solve(constraint.f);
193 |
194 | matrix_t pseudoInverse;
195 | if (extractPseudoInverse) {
196 | pseudoInverse = lu.solve(matrix_t::Identity(constraint.f.size(), constraint.f.size())).transpose(); // left pseudo-inverse of D^T
197 | }
198 |
199 | return std::make_pair(std::move(projectionTerms), std::move(pseudoInverse));
200 | }
201 |
202 | // Explicit instantiations for dynamic sized matrices
203 | template int rank(const matrix_t& A);
204 | template Eigen::VectorXcd eigenvalues(const matrix_t& A);
205 | template vector_t symmetricEigenvalues(const matrix_t& A);
206 |
207 | } // namespace LinearAlgebra
208 | } // namespace deepbreak
209 |
--------------------------------------------------------------------------------
/description/CMakeLists.txt:
--------------------------------------------------------------------------------
1 | cmake_minimum_required(VERSION 3.8)
2 | project(description)
3 |
4 | if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang")
5 | add_compile_options(-Wall -Wextra -Wpedantic)
6 | endif()
7 |
8 | # find dependencies
9 | find_package(ament_cmake REQUIRED)
10 |
11 | install(DIRECTORY model
12 | DESTINATION share/${PROJECT_NAME})
13 |
14 | if(BUILD_TESTING)
15 | find_package(ament_lint_auto REQUIRED)
16 | # the following line skips the linter which checks for copyrights
17 | # comment the line when a copyright and license is added to all source files
18 | set(ament_cmake_copyright_FOUND TRUE)
19 | # the following line skips cpplint (only works in a git repo)
20 | # comment the line when this package is in a git repo and when
21 | # a copyright and license is added to all source files
22 | set(ament_cmake_cpplint_FOUND TRUE)
23 | ament_lint_auto_find_test_dependencies()
24 | endif()
25 |
26 | ament_package()
27 |
--------------------------------------------------------------------------------
/description/model/xml/meshes/achilles-rod.stl:
--------------------------------------------------------------------------------
https://raw.githubusercontent.com/MindSpaceInc/Digit-MuJoCo-ROS2/1a263e07331cb22cf86b80ee2435febdb3be3bf2/description/model/xml/meshes/achilles-rod.stl
--------------------------------------------------------------------------------
/description/model/xml/meshes/arm-L1.stl:
--------------------------------------------------------------------------------
https://raw.githubusercontent.com/MindSpaceInc/Digit-MuJoCo-ROS2/1a263e07331cb22cf86b80ee2435febdb3be3bf2/description/model/xml/meshes/arm-L1.stl
--------------------------------------------------------------------------------
/description/model/xml/meshes/arm-L2.stl:
--------------------------------------------------------------------------------
https://raw.githubusercontent.com/MindSpaceInc/Digit-MuJoCo-ROS2/1a263e07331cb22cf86b80ee2435febdb3be3bf2/description/model/xml/meshes/arm-L2.stl
--------------------------------------------------------------------------------
/description/model/xml/meshes/arm-L3.stl:
--------------------------------------------------------------------------------
https://raw.githubusercontent.com/MindSpaceInc/Digit-MuJoCo-ROS2/1a263e07331cb22cf86b80ee2435febdb3be3bf2/description/model/xml/meshes/arm-L3.stl
--------------------------------------------------------------------------------
/description/model/xml/meshes/arm-L4.stl:
--------------------------------------------------------------------------------
https://raw.githubusercontent.com/MindSpaceInc/Digit-MuJoCo-ROS2/1a263e07331cb22cf86b80ee2435febdb3be3bf2/description/model/xml/meshes/arm-L4.stl
--------------------------------------------------------------------------------
/description/model/xml/meshes/heel-spring.stl:
--------------------------------------------------------------------------------
https://raw.githubusercontent.com/MindSpaceInc/Digit-MuJoCo-ROS2/1a263e07331cb22cf86b80ee2435febdb3be3bf2/description/model/xml/meshes/heel-spring.stl
--------------------------------------------------------------------------------
/description/model/xml/meshes/hip-pitch-housing.stl:
--------------------------------------------------------------------------------
https://raw.githubusercontent.com/MindSpaceInc/Digit-MuJoCo-ROS2/1a263e07331cb22cf86b80ee2435febdb3be3bf2/description/model/xml/meshes/hip-pitch-housing.stl
--------------------------------------------------------------------------------
/description/model/xml/meshes/hip-pitch.stl:
--------------------------------------------------------------------------------
https://raw.githubusercontent.com/MindSpaceInc/Digit-MuJoCo-ROS2/1a263e07331cb22cf86b80ee2435febdb3be3bf2/description/model/xml/meshes/hip-pitch.stl
--------------------------------------------------------------------------------
/description/model/xml/meshes/hip-roll-housing.stl:
--------------------------------------------------------------------------------
https://raw.githubusercontent.com/MindSpaceInc/Digit-MuJoCo-ROS2/1a263e07331cb22cf86b80ee2435febdb3be3bf2/description/model/xml/meshes/hip-roll-housing.stl
--------------------------------------------------------------------------------
/description/model/xml/meshes/hip-yaw-housing.stl:
--------------------------------------------------------------------------------
https://raw.githubusercontent.com/MindSpaceInc/Digit-MuJoCo-ROS2/1a263e07331cb22cf86b80ee2435febdb3be3bf2/description/model/xml/meshes/hip-yaw-housing.stl
--------------------------------------------------------------------------------
/description/model/xml/meshes/knee.stl:
--------------------------------------------------------------------------------
https://raw.githubusercontent.com/MindSpaceInc/Digit-MuJoCo-ROS2/1a263e07331cb22cf86b80ee2435febdb3be3bf2/description/model/xml/meshes/knee.stl
--------------------------------------------------------------------------------
/description/model/xml/meshes/left-shoulder-cap.stl:
--------------------------------------------------------------------------------
https://raw.githubusercontent.com/MindSpaceInc/Digit-MuJoCo-ROS2/1a263e07331cb22cf86b80ee2435febdb3be3bf2/description/model/xml/meshes/left-shoulder-cap.stl
--------------------------------------------------------------------------------
/description/model/xml/meshes/left-waist-cap.stl:
--------------------------------------------------------------------------------
https://raw.githubusercontent.com/MindSpaceInc/Digit-MuJoCo-ROS2/1a263e07331cb22cf86b80ee2435febdb3be3bf2/description/model/xml/meshes/left-waist-cap.stl
--------------------------------------------------------------------------------
/description/model/xml/meshes/right-shoulder-cap.stl:
--------------------------------------------------------------------------------
https://raw.githubusercontent.com/MindSpaceInc/Digit-MuJoCo-ROS2/1a263e07331cb22cf86b80ee2435febdb3be3bf2/description/model/xml/meshes/right-shoulder-cap.stl
--------------------------------------------------------------------------------
/description/model/xml/meshes/right-waist-cap.stl:
--------------------------------------------------------------------------------
https://raw.githubusercontent.com/MindSpaceInc/Digit-MuJoCo-ROS2/1a263e07331cb22cf86b80ee2435febdb3be3bf2/description/model/xml/meshes/right-waist-cap.stl
--------------------------------------------------------------------------------
/description/model/xml/meshes/shin.stl:
--------------------------------------------------------------------------------
https://raw.githubusercontent.com/MindSpaceInc/Digit-MuJoCo-ROS2/1a263e07331cb22cf86b80ee2435febdb3be3bf2/description/model/xml/meshes/shin.stl
--------------------------------------------------------------------------------
/description/model/xml/meshes/shoulder-roll-housing.stl:
--------------------------------------------------------------------------------
https://raw.githubusercontent.com/MindSpaceInc/Digit-MuJoCo-ROS2/1a263e07331cb22cf86b80ee2435febdb3be3bf2/description/model/xml/meshes/shoulder-roll-housing.stl
--------------------------------------------------------------------------------
/description/model/xml/meshes/tarsus.stl:
--------------------------------------------------------------------------------
https://raw.githubusercontent.com/MindSpaceInc/Digit-MuJoCo-ROS2/1a263e07331cb22cf86b80ee2435febdb3be3bf2/description/model/xml/meshes/tarsus.stl
--------------------------------------------------------------------------------
/description/model/xml/meshes/toe-A-rod.stl:
--------------------------------------------------------------------------------
https://raw.githubusercontent.com/MindSpaceInc/Digit-MuJoCo-ROS2/1a263e07331cb22cf86b80ee2435febdb3be3bf2/description/model/xml/meshes/toe-A-rod.stl
--------------------------------------------------------------------------------
/description/model/xml/meshes/toe-B-rod.stl:
--------------------------------------------------------------------------------
https://raw.githubusercontent.com/MindSpaceInc/Digit-MuJoCo-ROS2/1a263e07331cb22cf86b80ee2435febdb3be3bf2/description/model/xml/meshes/toe-B-rod.stl
--------------------------------------------------------------------------------
/description/model/xml/meshes/toe-output-ll.stl:
--------------------------------------------------------------------------------
https://raw.githubusercontent.com/MindSpaceInc/Digit-MuJoCo-ROS2/1a263e07331cb22cf86b80ee2435febdb3be3bf2/description/model/xml/meshes/toe-output-ll.stl
--------------------------------------------------------------------------------
/description/model/xml/meshes/toe-output-lr.stl:
--------------------------------------------------------------------------------
https://raw.githubusercontent.com/MindSpaceInc/Digit-MuJoCo-ROS2/1a263e07331cb22cf86b80ee2435febdb3be3bf2/description/model/xml/meshes/toe-output-lr.stl
--------------------------------------------------------------------------------
/description/model/xml/meshes/toe-output-rl.stl:
--------------------------------------------------------------------------------
https://raw.githubusercontent.com/MindSpaceInc/Digit-MuJoCo-ROS2/1a263e07331cb22cf86b80ee2435febdb3be3bf2/description/model/xml/meshes/toe-output-rl.stl
--------------------------------------------------------------------------------
/description/model/xml/meshes/toe-output-rr.stl:
--------------------------------------------------------------------------------
https://raw.githubusercontent.com/MindSpaceInc/Digit-MuJoCo-ROS2/1a263e07331cb22cf86b80ee2435febdb3be3bf2/description/model/xml/meshes/toe-output-rr.stl
--------------------------------------------------------------------------------
/description/model/xml/meshes/toe-output.stl:
--------------------------------------------------------------------------------
https://raw.githubusercontent.com/MindSpaceInc/Digit-MuJoCo-ROS2/1a263e07331cb22cf86b80ee2435febdb3be3bf2/description/model/xml/meshes/toe-output.stl
--------------------------------------------------------------------------------
/description/model/xml/meshes/toe-pitch.stl:
--------------------------------------------------------------------------------
https://raw.githubusercontent.com/MindSpaceInc/Digit-MuJoCo-ROS2/1a263e07331cb22cf86b80ee2435febdb3be3bf2/description/model/xml/meshes/toe-pitch.stl
--------------------------------------------------------------------------------
/description/model/xml/meshes/toe-roll.stl:
--------------------------------------------------------------------------------
https://raw.githubusercontent.com/MindSpaceInc/Digit-MuJoCo-ROS2/1a263e07331cb22cf86b80ee2435febdb3be3bf2/description/model/xml/meshes/toe-roll.stl
--------------------------------------------------------------------------------
/description/model/xml/meshes/torso.stl:
--------------------------------------------------------------------------------
https://raw.githubusercontent.com/MindSpaceInc/Digit-MuJoCo-ROS2/1a263e07331cb22cf86b80ee2435febdb3be3bf2/description/model/xml/meshes/torso.stl
--------------------------------------------------------------------------------
/description/package.xml:
--------------------------------------------------------------------------------
1 |
2 |
3 |
4 | description
5 | 0.0.0
6 | TODO: Package description
7 | DeepBreak
8 | BSD
9 |
10 | ament_cmake
11 |
12 | ament_lint_auto
13 | ament_lint_common
14 |
15 |
16 | ament_cmake
17 |
18 |
19 |
--------------------------------------------------------------------------------
/results.png:
--------------------------------------------------------------------------------
https://raw.githubusercontent.com/MindSpaceInc/Digit-MuJoCo-ROS2/1a263e07331cb22cf86b80ee2435febdb3be3bf2/results.png
--------------------------------------------------------------------------------
/simulation/mujoco/CMakeLists.txt:
--------------------------------------------------------------------------------
1 | cmake_minimum_required(VERSION 3.5)
2 | project(mujoco)
3 |
4 | # if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang")
5 | # add_compile_options(-Wall -Wextra -Wpedantic)
6 | # endif()
7 |
8 | # find dependencies
9 | find_package(ament_cmake REQUIRED)
10 | # uncomment the following section in order to fill in
11 | # further dependencies manually.
12 | # find_package( REQUIRED)
13 |
14 | list(APPEND ${PROJECT_NAME}_CONFIG_EXTRAS
15 | cmake/CheckAvxSupport.cmake
16 | cmake/FindOrFetch.cmake
17 | cmake/MujocoHarden.cmake
18 | cmake/MujocoLinkOptions.cmake
19 | cmake/MujocoMacOS.cmake
20 | cmake/SimulateDependencies.cmake
21 | cmake/SimulateOptions.cmake
22 | )
23 |
24 | # find dependencies
25 | find_package(ament_cmake REQUIRED)
26 | find_package(sensor_msgs REQUIRED)
27 | find_package(communication REQUIRED)
28 | find_package(rclcpp REQUIRED)
29 | find_package(geometry_msgs REQUIRED)
30 | find_package(tf2 REQUIRED)
31 | find_package(tf2_ros REQUIRED)
32 | find_package(nav_msgs REQUIRED)
33 | find_package(cv_bridge REQUIRED)
34 | # BLASFEO Option
35 | if(NOT TARGET mujoco)
36 | add_library(mujoco SHARED IMPORTED)
37 | set_target_properties(mujoco PROPERTIES IMPORTED_LOCATION ${CMAKE_CURRENT_SOURCE_DIR}/lib/libmujoco.so)
38 | endif()
39 |
40 | file(GLOB_RECURSE source ${CMAKE_CURRENT_SOURCE_DIR}/src/*.cpp ${CMAKE_CURRENT_SOURCE_DIR}/src/*.cc)
41 | add_executable(simulation ${source})
42 | target_link_libraries(simulation mujoco glfw pthread rt ${CMAKE_DL_LIBS})
43 | ament_target_dependencies(simulation sensor_msgs rclcpp communication geometry_msgs tf2 tf2_ros nav_msgs cv_bridge)
44 | target_include_directories(simulation PUBLIC
45 | $
46 | $)
47 | target_compile_options(simulation PUBLIC ${DEEPBREAK_CXX_FLAGS})
48 |
49 |
50 | install(TARGETS simulation
51 | DESTINATION lib/${PROJECT_NAME})
52 |
53 | install(DIRECTORY launch
54 | DESTINATION share/${PROJECT_NAME})
55 |
56 | ament_package()
57 |
--------------------------------------------------------------------------------
/simulation/mujoco/cmake/CheckAvxSupport.cmake:
--------------------------------------------------------------------------------
1 | # Copyright 2021 DeepMind Technologies Limited
2 | #
3 | # Licensed under the Apache License, Version 2.0 (the "License");
4 | # you may not use this file except in compliance with the License.
5 | # You may obtain a copy of the License at
6 | #
7 | # https://www.apache.org/licenses/LICENSE-2.0
8 | #
9 | # Unless required by applicable law or agreed to in writing, software
10 | # distributed under the License is distributed on an "AS IS" BASIS,
11 | # WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
12 | # See the License for the specific language governing permissions and
13 | # limitations under the License.
14 |
15 | include(CheckCSourceCompiles)
16 |
17 | # Assigns compiler options to the given variable based on availability of AVX.
18 | function(get_avx_compile_options OUTPUT_VAR)
19 | message(VERBOSE "Checking if AVX is available...")
20 |
21 | if(MSVC)
22 | set(CMAKE_REQUIRED_FLAGS "/arch:AVX")
23 | elseif(WIN32)
24 | # Abseil LTS 20230125.0 assumes that AVX implies PCLMUL on Windows.
25 | set(CMAKE_REQUIRED_FLAGS "-mavx" "-mpclmul")
26 | else()
27 | set(CMAKE_REQUIRED_FLAGS "-mavx")
28 | endif()
29 |
30 | if(APPLE AND "x86_64" IN_LIST CMAKE_OSX_ARCHITECTURES)
31 | message(STATUS "Building x86_64 on macOS, forcing CAN_BUILD_AVX to TRUE.")
32 | set(CAN_BUILD_AVX TRUE)
33 | else()
34 | check_c_source_compiles(
35 | "
36 | #include
37 | int main(int argc, char* argv[]) {
38 | __m256d ymm;
39 | return 0;
40 | }
41 | "
42 | CAN_BUILD_AVX
43 | )
44 | endif()
45 |
46 | if(CAN_BUILD_AVX)
47 | message(VERBOSE "Checking if AVX is available... AVX available.")
48 | set("${OUTPUT_VAR}"
49 | ${CMAKE_REQUIRED_FLAGS}
50 | PARENT_SCOPE
51 | )
52 | else()
53 | message(VERBOSE "Checking if AVX is available... AVX not available.")
54 | set("${OUTPUT_VAR}" PARENT_SCOPE)
55 | endif()
56 | endfunction()
57 |
--------------------------------------------------------------------------------
/simulation/mujoco/cmake/FindOrFetch.cmake:
--------------------------------------------------------------------------------
1 | # Copyright 2021 DeepMind Technologies Limited
2 | #
3 | # Licensed under the Apache License, Version 2.0 (the "License");
4 | # you may not use this file except in compliance with the License.
5 | # You may obtain a copy of the License at
6 | #
7 | # https://www.apache.org/licenses/LICENSE-2.0
8 | #
9 | # Unless required by applicable law or agreed to in writing, software
10 | # distributed under the License is distributed on an "AS IS" BASIS,
11 | # WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
12 | # See the License for the specific language governing permissions and
13 | # limitations under the License.
14 | #
15 | #.rst:
16 | # FindOrFetch
17 | # ----------------------
18 | #
19 | # Find or fetch a package in order to satisfy target dependencies.
20 | #
21 | # FindOrFetch([USE_SYSTEM_PACKAGE [ON/OFF]]
22 | # [PACKAGE_NAME [name]]
23 | # [LIBRARY_NAME [name]]
24 | # [GIT_REPO [repo]]
25 | # [GIT_TAG [tag]]
26 | # [PATCH_COMMAND [cmd] [args]]
27 | # [TARGETS [targets]]
28 | # [EXCLUDE_FROM_ALL])
29 | #
30 | # The command has the following parameters:
31 | #
32 | # Arguments:
33 | # - ``USE_SYSTEM_PACKAGE`` one-value argument on whether to search for the
34 | # package in the system (ON) or whether to fetch the library using
35 | # FetchContent from the specified Git repository (OFF). Note that
36 | # FetchContent variables will override this behaviour.
37 | # - ``PACKAGE_NAME`` name of the system-package. Ignored if
38 | # ``USE_SYSTEM_PACKAGE`` is ``OFF``.
39 | # - ``LIBRARY_NAME`` name of the library. Ignored if
40 | # ``USE_SYSTEM_PACKAGE`` is ``ON``.
41 | # - ``GIT_REPO`` git repository to fetch the library from. Ignored if
42 | # ``USE_SYSTEM_PACKAGE`` is ``ON``.
43 | # - ``GIT_TAG`` tag reference when fetching the library from the git
44 | # repository. Ignored if ``USE_SYSTEM_PACKAGE`` is ``ON``.
45 | # - ``PATCH_COMMAND`` Specifies a custom command to patch the sources after an
46 | # update. See https://cmake.org/cmake/help/latest/module/ExternalProject.html#command:externalproject_add
47 | # for details on the parameter.
48 | # - ``TARGETS`` list of targets to be satisfied. If any of these targets are
49 | # not currently defined, this macro will attempt to either find or fetch the
50 | # package.
51 | # - ``EXCLUDE_FROM_ALL`` if specified, the targets are not added to the ``all``
52 | # metatarget.
53 | #
54 | # Note: if ``USE_SYSTEM_PACKAGE`` is ``OFF``, FetchContent will be used to
55 | # retrieve the specified targets. It is possible to specify any variable in
56 | # https://cmake.org/cmake/help/latest/module/FetchContent.html#variables to
57 | # override this macro behaviour.
58 |
59 | if(COMMAND FindOrFetch)
60 | return()
61 | endif()
62 |
63 | macro(FindOrFetch)
64 | if(NOT FetchContent)
65 | include(FetchContent)
66 | endif()
67 |
68 | # Parse arguments.
69 | set(options EXCLUDE_FROM_ALL)
70 | set(one_value_args
71 | USE_SYSTEM_PACKAGE
72 | PACKAGE_NAME
73 | LIBRARY_NAME
74 | GIT_REPO
75 | GIT_TAG
76 | )
77 | set(multi_value_args PATCH_COMMAND TARGETS)
78 | cmake_parse_arguments(
79 | _ARGS
80 | "${options}"
81 | "${one_value_args}"
82 | "${multi_value_args}"
83 | ${ARGN}
84 | )
85 |
86 | # Check if all targets are found.
87 | if(NOT _ARGS_TARGETS)
88 | message(FATAL_ERROR "mujoco::FindOrFetch: TARGETS must be specified.")
89 | endif()
90 |
91 | set(targets_found TRUE)
92 | message(CHECK_START
93 | "mujoco::FindOrFetch: checking for targets in package `${_ARGS_PACKAGE_NAME}`"
94 | )
95 | foreach(target ${_ARGS_TARGETS})
96 | if(NOT TARGET ${target})
97 | message(CHECK_FAIL "target `${target}` not defined.")
98 | set(targets_found FALSE)
99 | break()
100 | endif()
101 | endforeach()
102 |
103 | # If targets are not found, use `find_package` or `FetchContent...` to get it.
104 | if(NOT targets_found)
105 | if(${_ARGS_USE_SYSTEM_PACKAGE})
106 | message(CHECK_START
107 | "mujoco::FindOrFetch: finding `${_ARGS_PACKAGE_NAME}` in system packages..."
108 | )
109 | find_package(${_ARGS_PACKAGE_NAME} REQUIRED)
110 | message(CHECK_PASS "found")
111 | else()
112 | message(CHECK_START
113 | "mujoco::FindOrFetch: Using FetchContent to retrieve `${_ARGS_LIBRARY_NAME}`"
114 | )
115 | FetchContent_Declare(
116 | ${_ARGS_LIBRARY_NAME}
117 | GIT_REPOSITORY ${_ARGS_GIT_REPO}
118 | GIT_TAG ${_ARGS_GIT_TAG}
119 | GIT_SHALLOW FALSE
120 | PATCH_COMMAND ${_ARGS_PATCH_COMMAND}
121 | )
122 | if(${_ARGS_EXCLUDE_FROM_ALL})
123 | FetchContent_GetProperties(${_ARGS_LIBRARY_NAME})
124 | if(NOT ${${_ARGS_LIBRARY_NAME}_POPULATED})
125 | FetchContent_Populate(${_ARGS_LIBRARY_NAME})
126 | add_subdirectory(
127 | ${${_ARGS_LIBRARY_NAME}_SOURCE_DIR} ${${_ARGS_LIBRARY_NAME}_BINARY_DIR}
128 | EXCLUDE_FROM_ALL
129 | )
130 | endif()
131 | else()
132 | FetchContent_MakeAvailable(${_ARGS_LIBRARY_NAME})
133 | endif()
134 | message(CHECK_PASS "Done")
135 | endif()
136 | else()
137 | message(CHECK_PASS "found")
138 | endif()
139 | endmacro()
140 |
--------------------------------------------------------------------------------
/simulation/mujoco/cmake/MujocoHarden.cmake:
--------------------------------------------------------------------------------
1 | # Copyright 2022 DeepMind Technologies Limited
2 | #
3 | # Licensed under the Apache License, Version 2.0 (the "License");
4 | # you may not use this file except in compliance with the License.
5 | # You may obtain a copy of the License at
6 | #
7 | # https://www.apache.org/licenses/LICENSE-2.0
8 | #
9 | # Unless required by applicable law or agreed to in writing, software
10 | # distributed under the License is distributed on an "AS IS" BASIS,
11 | # WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
12 | # See the License for the specific language governing permissions and
13 | # limitations under the License.
14 |
15 | option(MUJOCO_HARDEN "Enable build hardening for MuJoCo." OFF)
16 | if(MUJOCO_HARDEN
17 | AND NOT
18 | CMAKE_CXX_COMPILER_ID
19 | MATCHES
20 | ".*Clang.*"
21 | )
22 | message(FATAL_ERROR "MUJOCO_HARDEN is only supported when building with Clang")
23 | endif()
24 |
25 | if(MUJOCO_HARDEN)
26 | set(MUJOCO_HARDEN_COMPILE_OPTIONS -D_FORTIFY_SOURCE=2 -fstack-protector)
27 | if(${CMAKE_SYSTEM_NAME} MATCHES "Darwin")
28 | set(MUJOCO_HARDEN_LINK_OPTIONS -Wl,-bind_at_load)
29 | elseif(${CMAKE_SYSTEM_NAME} MATCHES "Linux")
30 | set(MUJOCO_HARDEN_LINK_OPTIONS -Wl,-z,relro -Wl,-z,now)
31 | endif()
32 | else()
33 | set(MUJOCO_HARDEN_COMPILE_OPTIONS "")
34 | set(MUJOCO_HARDEN_LINK_OPTIONS "")
35 | endif()
36 |
--------------------------------------------------------------------------------
/simulation/mujoco/cmake/MujocoLinkOptions.cmake:
--------------------------------------------------------------------------------
1 | # Copyright 2021 DeepMind Technologies Limited
2 | #
3 | # Licensed under the Apache License, Version 2.0 (the "License");
4 | # you may not use this file except in compliance with the License.
5 | # You may obtain a copy of the License at
6 | #
7 | # https://www.apache.org/licenses/LICENSE-2.0
8 | #
9 | # Unless required by applicable law or agreed to in writing, software
10 | # distributed under the License is distributed on an "AS IS" BASIS,
11 | # WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
12 | # See the License for the specific language governing permissions and
13 | # limitations under the License.
14 |
15 | include(CheckCSourceCompiles)
16 |
17 | # Gets the appropriate linker options for building MuJoCo, based on features available on the
18 | # linker.
19 | function(get_mujoco_extra_link_options OUTPUT_VAR)
20 | if(MSVC)
21 | set(EXTRA_LINK_OPTIONS /OPT:REF /OPT:ICF=5)
22 | else()
23 | set(EXTRA_LINK_OPTIONS)
24 |
25 | if(WIN32)
26 | set(CMAKE_REQUIRED_FLAGS "-fuse-ld=lld-link")
27 | check_c_source_compiles("int main() {}" SUPPORTS_LLD)
28 | if(SUPPORTS_LLD)
29 | set(EXTRA_LINK_OPTIONS
30 | ${EXTRA_LINK_OPTIONS}
31 | -fuse-ld=lld-link
32 | -Wl,/OPT:REF
33 | -Wl,/OPT:ICF
34 | )
35 | endif()
36 | else()
37 | set(CMAKE_REQUIRED_FLAGS "-fuse-ld=lld")
38 | check_c_source_compiles("int main() {}" SUPPORTS_LLD)
39 | if(SUPPORTS_LLD)
40 | set(EXTRA_LINK_OPTIONS ${EXTRA_LINK_OPTIONS} -fuse-ld=lld)
41 | else()
42 | set(CMAKE_REQUIRED_FLAGS "-fuse-ld=gold")
43 | check_c_source_compiles("int main() {}" SUPPORTS_GOLD)
44 | if(SUPPORTS_GOLD)
45 | set(EXTRA_LINK_OPTIONS ${EXTRA_LINK_OPTIONS} -fuse-ld=gold)
46 | endif()
47 | endif()
48 |
49 | set(CMAKE_REQUIRED_FLAGS ${EXTRA_LINK_OPTIONS} "-Wl,--gc-sections")
50 | check_c_source_compiles("int main() {}" SUPPORTS_GC_SECTIONS)
51 | if(SUPPORTS_GC_SECTIONS)
52 | set(EXTRA_LINK_OPTIONS ${EXTRA_LINK_OPTIONS} -Wl,--gc-sections)
53 | else()
54 | set(CMAKE_REQUIRED_FLAGS ${EXTRA_LINK_OPTIONS} "-Wl,-dead_strip")
55 | check_c_source_compiles("int main() {}" SUPPORTS_DEAD_STRIP)
56 | if(SUPPORTS_DEAD_STRIP)
57 | set(EXTRA_LINK_OPTIONS ${EXTRA_LINK_OPTIONS} -Wl,-dead_strip)
58 | endif()
59 | endif()
60 | endif()
61 | endif()
62 |
63 | set("${OUTPUT_VAR}"
64 | ${EXTRA_LINK_OPTIONS}
65 | PARENT_SCOPE
66 | )
67 | endfunction()
68 |
--------------------------------------------------------------------------------
/simulation/mujoco/cmake/MujocoMacOS.cmake:
--------------------------------------------------------------------------------
1 | # Copyright 2022 DeepMind Technologies Limited
2 | #
3 | # Licensed under the Apache License, Version 2.0 (the "License");
4 | # you may not use this file except in compliance with the License.
5 | # You may obtain a copy of the License at
6 | #
7 | # https://www.apache.org/licenses/LICENSE-2.0
8 | #
9 | # Unless required by applicable law or agreed to in writing, software
10 | # distributed under the License is distributed on an "AS IS" BASIS,
11 | # WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
12 | # See the License for the specific language governing permissions and
13 | # limitations under the License.
14 |
15 | if(APPLE)
16 | # 10.12 is the oldest version of macOS that supports C++17, launched 2016.
17 | set(MUJOCO_MACOSX_VERSION_MIN 10.12)
18 |
19 | # We are setting the -mmacosx-version-min compiler flag directly rather than using the
20 | # CMAKE_OSX_DEPLOYMENT_TARGET variable since we do not want to affect choice of SDK,
21 | # and also we only want to apply the version restriction locally.
22 | set(MUJOCO_MACOS_COMPILE_OPTIONS -mmacosx-version-min=${MUJOCO_MACOSX_VERSION_MIN}
23 | -Werror=partial-availability -Werror=unguarded-availability
24 | )
25 | set(MUJOCO_MACOS_LINK_OPTIONS -mmacosx-version-min=${MUJOCO_MACOSX_VERSION_MIN}
26 | -Wl,-no_weak_imports
27 | )
28 | else()
29 | set(MUJOCO_MACOS_COMPILE_OPTIONS "")
30 | set(MUJOCO_MACOS_LINK_OPTIONS "")
31 | endif()
32 |
33 | function(enforce_mujoco_macosx_min_version)
34 | if(APPLE)
35 | add_compile_options(${MUJOCO_MACOS_COMPILE_OPTIONS})
36 | add_link_options(${MUJOCO_MACOS_LINK_OPTIONS})
37 | endif()
38 | endfunction()
39 |
--------------------------------------------------------------------------------
/simulation/mujoco/cmake/SimulateDependencies.cmake:
--------------------------------------------------------------------------------
1 | # Copyright 2021 DeepMind Technologies Limited
2 | #
3 | # Licensed under the Apache License, Version 2.0 (the "License");
4 | # you may not use this file except in compliance with the License.
5 | # You may obtain a copy of the License at
6 | #
7 | # https://www.apache.org/licenses/LICENSE-2.0
8 | #
9 | # Unless required by applicable law or agreed to in writing, software
10 | # distributed under the License is distributed on an "AS IS" BASIS,
11 | # WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
12 | # See the License for the specific language governing permissions and
13 | # limitations under the License.
14 |
15 | include(FindOrFetch)
16 |
17 | if(SIMULATE_STANDALONE)
18 | # If standalone, by default look for MuJoCo binary version.
19 | set(DEFAULT_USE_SYSTEM_MUJOCO ON)
20 | else()
21 | set(DEFAULT_USE_SYSTEM_MUJOCO OFF)
22 | endif()
23 |
24 | option(MUJOCO_SIMULATE_USE_SYSTEM_MUJOCO "Use installed MuJoCo version."
25 | ${DEFAULT_USE_SYSTEM_MUJOCO}
26 | )
27 | unset(DEFAULT_USE_SYSTEM_MUJOCO)
28 |
29 | option(MUJOCO_SIMULATE_USE_SYSTEM_MUJOCO "Use installed MuJoCo version." OFF)
30 | option(MUJOCO_SIMULATE_USE_SYSTEM_GLFW "Use installed GLFW version." OFF)
31 |
32 | set(MUJOCO_DEP_VERSION_glfw3
33 | 7482de6071d21db77a7236155da44c172a7f6c9e # 3.3.8
34 | CACHE STRING "Version of `glfw` to be fetched."
35 | )
36 | mark_as_advanced(MUJOCO_DEP_VERSION_glfw3)
37 |
38 | find_package(Threads REQUIRED)
39 |
40 | set(MUJOCO_BUILD_EXAMPLES OFF)
41 | set(MUJOCO_BUILD_TESTS OFF)
42 | set(MUJOCO_BUILD_PYTHON OFF)
43 | set(MUJOCO_TEST_PYTHON_UTIL OFF)
44 |
45 | findorfetch(
46 | USE_SYSTEM_PACKAGE
47 | MUJOCO_SIMULATE_USE_SYSTEM_MUJOCO
48 | PACKAGE_NAME
49 | mujoco
50 | LIBRARY_NAME
51 | mujoco
52 | GIT_REPO
53 | https://github.com/deepmind/mujoco.git
54 | GIT_TAG
55 | main
56 | TARGETS
57 | mujoco
58 | EXCLUDE_FROM_ALL
59 | )
60 |
61 | option(MUJOCO_EXTRAS_STATIC_GLFW
62 | "Link MuJoCo sample apps and simulate libraries against GLFW statically." ON
63 | )
64 | if(MUJOCO_EXTRAS_STATIC_GLFW)
65 | set(BUILD_SHARED_LIBS_OLD ${BUILD_SHARED_LIBS})
66 | set(BUILD_SHARED_LIBS
67 | OFF
68 | CACHE INTERNAL "Build SHARED libraries"
69 | )
70 | endif()
71 |
72 | set(GLFW_BUILD_EXAMPLES OFF)
73 | set(GLFW_BUILD_TESTS OFF)
74 | set(GLFW_BUILD_DOCS OFF)
75 | set(GLFW_INSTALL OFF)
76 |
77 | findorfetch(
78 | USE_SYSTEM_PACKAGE
79 | MUJOCO_SIMULATE_USE_SYSTEM_GLFW
80 | PACKAGE_NAME
81 | glfw3
82 | LIBRARY_NAME
83 | glfw3
84 | GIT_REPO
85 | https://github.com/glfw/glfw.git
86 | GIT_TAG
87 | ${MUJOCO_DEP_VERSION_glfw3}
88 | TARGETS
89 | glfw
90 | EXCLUDE_FROM_ALL
91 | )
92 |
93 | if(MUJOCO_EXTRAS_STATIC_GLFW)
94 | set(BUILD_SHARED_LIBS
95 | ${BUILD_SHARED_LIBS_OLD}
96 | CACHE BOOL "Build SHARED libraries" FORCE
97 | )
98 | unset(BUILD_SHARED_LIBS_OLD)
99 | endif()
100 |
101 | if(NOT SIMULATE_STANDALONE)
102 | target_compile_options(glfw PRIVATE ${MUJOCO_MACOS_COMPILE_OPTIONS})
103 | target_link_options(glfw PRIVATE ${MUJOCO_MACOS_LINK_OPTIONS})
104 | endif()
105 |
--------------------------------------------------------------------------------
/simulation/mujoco/cmake/SimulateOptions.cmake:
--------------------------------------------------------------------------------
1 | # Copyright 2021 DeepMind Technologies Limited
2 | #
3 | # Licensed under the Apache License, Version 2.0 (the "License");
4 | # you may not use this file except in compliance with the License.
5 | # You may obtain a copy of the License at
6 | #
7 | # https://www.apache.org/licenses/LICENSE-2.0
8 | #
9 | # Unless required by applicable law or agreed to in writing, software
10 | # distributed under the License is distributed on an "AS IS" BASIS,
11 | # WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
12 | # See the License for the specific language governing permissions and
13 | # limitations under the License.
14 |
15 | # Global compilation settings
16 | set(CMAKE_C_STANDARD 11)
17 | set(CMAKE_C_STANDARD_REQUIRED ON)
18 | set(CMAKE_CXX_STANDARD 17)
19 | set(CMAKE_CXX_STANDARD_REQUIRED ON)
20 | set(CMAKE_CXX_EXTENSIONS OFF)
21 | set(CMAKE_C_EXTENSIONS OFF)
22 | set(CMAKE_EXPORT_COMPILE_COMMANDS ON) # For LLVM tooling
23 |
24 | if(NOT CMAKE_CONFIGURATION_TYPES)
25 | if(NOT CMAKE_BUILD_TYPE)
26 | message(STATUS "Setting build type to 'Release' as none was specified.")
27 | set(CMAKE_BUILD_TYPE
28 | "Release"
29 | CACHE STRING "Choose the type of build, recommanded options are: Debug or Release" FORCE
30 | )
31 | endif()
32 | set(BUILD_TYPES
33 | "Debug"
34 | "Release"
35 | "MinSizeRel"
36 | "RelWithDebInfo"
37 | )
38 | set_property(CACHE CMAKE_BUILD_TYPE PROPERTY STRINGS ${BUILD_TYPES})
39 | endif()
40 |
41 | include(GNUInstallDirs)
42 |
43 | # Change the default output directory in the build structure. This is not stricly needed, but helps
44 | # running in Windows, such that all built executables have DLLs in the same folder as the .exe
45 | # files.
46 | set(CMAKE_RUNTIME_OUTPUT_DIRECTORY "${CMAKE_BINARY_DIR}/${CMAKE_INSTALL_BINDIR}")
47 | set(CMAKE_LIBRARY_OUTPUT_DIRECTORY "${CMAKE_BINARY_DIR}/${CMAKE_INSTALL_LIBDIR}")
48 | set(CMAKE_ARCHIVE_OUTPUT_DIRECTORY "${CMAKE_BINARY_DIR}/${CMAKE_INSTALL_LIBDIR}")
49 |
50 | set(OpenGL_GL_PREFERENCE GLVND)
51 |
52 | set(CMAKE_POSITION_INDEPENDENT_CODE ON)
53 | set(CMAKE_C_VISIBILITY_PRESET hidden)
54 | set(CMAKE_CXX_VISIBILITY_PRESET hidden)
55 | set(CMAKE_VISIBILITY_INLINES_HIDDEN ON)
56 |
57 | if(MSVC)
58 | add_compile_options(/Gy /Gw /Oi)
59 | elseif(CMAKE_CXX_COMPILER_ID STREQUAL "GNU" OR CMAKE_CXX_COMPILER_ID MATCHES "Clang")
60 | add_compile_options(-fdata-sections -ffunction-sections)
61 | endif()
62 |
63 | # We default to shared library.
64 | set(BUILD_SHARED_LIBS
65 | ON
66 | CACHE BOOL "Build Mujoco as shared library."
67 | )
68 |
69 | option(MUJOCO_ENABLE_AVX "Build binaries that require AVX instructions, if possible." ON)
70 | option(MUJOCO_ENABLE_AVX_INTRINSICS "Make use of hand-written AVX intrinsics, if possible." ON)
71 | option(MUJOCO_ENABLE_RPATH "Enable RPath support when installing Mujoco." ON)
72 | mark_as_advanced(MUJOCO_ENABLE_RPATH)
73 |
74 | if(MUJOCO_ENABLE_AVX)
75 | include(CheckAvxSupport)
76 | get_avx_compile_options(AVX_COMPILE_OPTIONS)
77 | else()
78 | set(AVX_COMPILE_OPTIONS)
79 | endif()
80 |
81 | option(MUJOCO_BUILD_MACOS_FRAMEWORKS "Build libraries as macOS Frameworks" OFF)
82 |
83 | # Get some extra link options.
84 | include(MujocoLinkOptions)
85 | get_mujoco_extra_link_options(EXTRA_LINK_OPTIONS)
86 |
87 | if(CMAKE_CXX_COMPILER_ID STREQUAL "GNU" OR (CMAKE_CXX_COMPILER_ID MATCHES "Clang" AND NOT MSVC))
88 | set(EXTRA_COMPILE_OPTIONS -Wall -Werror)
89 | if(CMAKE_CXX_COMPILER_ID STREQUAL "GNU")
90 | # TODO: This should add to EXTRA_COMPILE_OPTIONS rather than overwrite it.
91 | set(EXTRA_COMPILE_OPTIONS
92 | -Wno-int-in-bool-context
93 | -Wno-maybe-uninitialized
94 | -Wno-sign-compare
95 | -Wno-stringop-overflow
96 | -Wno-stringop-truncation
97 | )
98 | endif()
99 | endif()
100 |
101 | if(WIN32)
102 | add_compile_definitions(_CRT_SECURE_NO_WARNINGS)
103 | endif()
104 |
105 | include(MujocoHarden)
106 | set(EXTRA_COMPILE_OPTIONS ${EXTRA_COMPILE_OPTIONS} ${MUJOCO_HARDEN_COMPILE_OPTIONS})
107 | set(EXTRA_LINK_OPTIONS ${EXTRA_LINK_OPTIONS} ${MUJOCO_HARDEN_LINK_OPTIONS})
108 |
--------------------------------------------------------------------------------
/simulation/mujoco/include/MuJoCoMessageHandler.h:
--------------------------------------------------------------------------------
1 | #pragma once
2 |
3 | #include
4 | #include
5 | #include
6 | #include
7 | #include
8 | #include
9 | #include
10 | #include
11 | #include
12 |
13 | #include "communication/msg/actuator_cmds.hpp"
14 | #include "communication/srv/simulation_reset.hpp"
15 |
16 | #include "array_safety.h"
17 | #include "simulate.h"
18 |
19 | using namespace rclcpp;
20 |
21 | using namespace std::chrono_literals;
22 |
23 | namespace deepbreak {
24 | namespace mj = ::mujoco;
25 | namespace mju = ::mujoco::sample_util;
26 |
27 | class MuJoCoMessageHandler : public rclcpp::Node {
28 | public:
29 | struct ActuatorCmds {
30 | double time = 0.0;
31 | std::vector actuators_name;
32 | std::vector kp;
33 | std::vector pos;
34 | std::vector kd;
35 | std::vector vel;
36 | std::vector torque;
37 | };
38 |
39 | MuJoCoMessageHandler(mj::Simulate *sim);
40 | ~MuJoCoMessageHandler();
41 |
42 | std::shared_ptr get_actuator_cmds_ptr();
43 |
44 | private:
45 | void reset_callback(
46 | const std::shared_ptr request,
47 | std::shared_ptr response);
48 |
49 | void imu_callback();
50 |
51 | void odom_callback();
52 |
53 | void joint_callback();
54 |
55 | void actuator_cmd_callback(
56 | const communication::msg::ActuatorCmds::SharedPtr msg) const;
57 |
58 | void parameter_callback(const rclcpp::Parameter &);
59 |
60 | void drop_old_message();
61 |
62 |
63 | mj::Simulate *sim_;
64 | std::string name_prefix, model_param_name;
65 | std::vector timers_;
66 | rclcpp::Publisher::SharedPtr imu_publisher_;
67 | rclcpp::Publisher::SharedPtr
68 | joint_state_publisher_;
69 | rclcpp::Publisher::SharedPtr odom_publisher_;
70 |
71 | rclcpp::Subscription::SharedPtr
72 | actuator_cmd_subscription_;
73 | rclcpp::Service::SharedPtr reset_service_;
74 |
75 | std::shared_ptr param_subscriber_;
76 |
77 | std::shared_ptr cb_handle_;
78 |
79 | std::shared_ptr actuator_cmds_ptr_;
80 |
81 | std::thread spin_thread;
82 | };
83 |
84 | } // namespace deepbreak
85 |
--------------------------------------------------------------------------------
/simulation/mujoco/include/array_safety.h:
--------------------------------------------------------------------------------
1 | // Copyright 2021 DeepMind Technologies Limited
2 | //
3 | // Licensed under the Apache License, Version 2.0 (the "License");
4 | // you may not use this file except in compliance with the License.
5 | // You may obtain a copy of the License at
6 | //
7 | // http://www.apache.org/licenses/LICENSE-2.0
8 | //
9 | // Unless required by applicable law or agreed to in writing, software
10 | // distributed under the License is distributed on an "AS IS" BASIS,
11 | // WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
12 | // See the License for the specific language governing permissions and
13 | // limitations under the License.
14 |
15 | #ifndef MUJOCO_SAMPLE_ARRAY_SAFETY_H_
16 | #define MUJOCO_SAMPLE_ARRAY_SAFETY_H_
17 |
18 | #include
19 | #include
20 | #include
21 | #include
22 | #include
23 |
24 | // Provides safe alternatives to the sizeof() operator and standard library functions for handling
25 | // null-terminated (C-style) strings in raw char arrays.
26 | //
27 | // These functions make use of compile-time array sizes to limit read and write operations to within
28 | // the array bounds. They are designed to trigger a compile error if the array size cannot be
29 | // determined at compile time (e.g. when an array has decayed into a pointer).
30 | //
31 | // They do not perform runtime bound checks.
32 |
33 | namespace mujoco {
34 | namespace sample_util {
35 |
36 | // returns sizeof(arr)
37 | // use instead of sizeof() to avoid unintended array-to-pointer decay
38 | template
39 | static constexpr std::size_t sizeof_arr(const T(&arr)[N]) {
40 | return sizeof(arr);
41 | }
42 |
43 | // like std::strcmp but it will not read beyond the bound of either lhs or rhs
44 | template
45 | static inline int strcmp_arr(const char (&lhs)[N1], const char (&rhs)[N2]) {
46 | return std::strncmp(lhs, rhs, std::min(N1, N2));
47 | }
48 |
49 | // like std::strlen but it will not read beyond the bound of str
50 | // if str is not null-terminated, returns sizeof(str)
51 | template
52 | static inline std::size_t strlen_arr(const char (&str)[N]) {
53 | for (std::size_t i = 0; i < N; ++i) {
54 | if (str[i] == '\0') {
55 | return i;
56 | }
57 | }
58 | return N;
59 | }
60 |
61 | // like std::sprintf but will not write beyond the bound of dest
62 | // dest is guaranteed to be null-terminated
63 | template
64 | static inline int sprintf_arr(char (&dest)[N], const char* format, ...) {
65 | std::va_list vargs;
66 | va_start(vargs, format);
67 | int retval = std::vsnprintf(dest, N, format, vargs);
68 | va_end(vargs);
69 | return retval;
70 | }
71 |
72 | // like std::strcat but will not write beyond the bound of dest
73 | // dest is guaranteed to be null-terminated
74 | template
75 | static inline char* strcat_arr(char (&dest)[N], const char* src) {
76 | return std::strncat(dest, src, sizeof_arr(dest) - strlen_arr(dest) - 1);
77 | }
78 |
79 | // like std::strcpy but won't write beyond the bound of dest
80 | // dest is guaranteed to be null-terminated
81 | template
82 | static inline char* strcpy_arr(char (&dest)[N], const char* src) {
83 | {
84 | std::size_t i = 0;
85 | for (; src[i] && i < N - 1; ++i) {
86 | dest[i] = src[i];
87 | }
88 | dest[i] = '\0';
89 | }
90 | return &dest[0];
91 | }
92 |
93 | } // namespace sample_util
94 | } // namespace mujoco
95 |
96 | #endif // MUJOCO_SAMPLE_ARRAY_SAFETY_H_
97 |
--------------------------------------------------------------------------------
/simulation/mujoco/include/glfw_adapter.h:
--------------------------------------------------------------------------------
1 | // Copyright 2023 DeepMind Technologies Limited
2 | //
3 | // Licensed under the Apache License, Version 2.0 (the "License");
4 | // you may not use this file except in compliance with the License.
5 | // You may obtain a copy of the License at
6 | //
7 | // http://www.apache.org/licenses/LICENSE-2.0
8 | //
9 | // Unless required by applicable law or agreed to in writing, software
10 | // distributed under the License is distributed on an "AS IS" BASIS,
11 | // WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
12 | // See the License for the specific language governing permissions and
13 | // limitations under the License.
14 |
15 | #ifndef MUJOCO_SIMULATE_GLFW_ADAPTER_H_
16 | #define MUJOCO_SIMULATE_GLFW_ADAPTER_H_
17 |
18 | #include
19 |
20 | #include
21 | #include
22 | #include "platform_ui_adapter.h"
23 |
24 | namespace mujoco {
25 | class GlfwAdapter : public PlatformUIAdapter {
26 | public:
27 | GlfwAdapter();
28 | ~GlfwAdapter() override;
29 |
30 | std::pair GetCursorPosition() const override;
31 | double GetDisplayPixelsPerInch() const override;
32 | std::pair GetFramebufferSize() const override;
33 | std::pair GetWindowSize() const override;
34 | bool IsGPUAccelerated() const override;
35 | void PollEvents() override;
36 | void SetClipboardString(const char* text) override;
37 | void SetVSync(bool enabled) override;
38 | void SetWindowTitle(const char* title) override;
39 | bool ShouldCloseWindow() const override;
40 | void SwapBuffers() override;
41 | void ToggleFullscreen() override;
42 |
43 | bool IsLeftMouseButtonPressed() const override;
44 | bool IsMiddleMouseButtonPressed() const override;
45 | bool IsRightMouseButtonPressed() const override;
46 |
47 | bool IsAltKeyPressed() const override;
48 | bool IsCtrlKeyPressed() const override;
49 | bool IsShiftKeyPressed() const override;
50 |
51 | bool IsMouseButtonDownEvent(int act) const override;
52 | bool IsKeyDownEvent(int act) const override;
53 |
54 | int TranslateKeyCode(int key) const override;
55 | mjtButton TranslateMouseButton(int button) const override;
56 |
57 | private:
58 | GLFWvidmode vidmode_;
59 | GLFWwindow* window_;
60 |
61 | // store last window information when going to full screen
62 | std::pair window_pos_;
63 | std::pair window_size_;
64 | };
65 | } // namespace mujoco
66 |
67 | #endif // MUJOCO_SIMULATE_GLFW_ADAPTER_H_
68 |
--------------------------------------------------------------------------------
/simulation/mujoco/include/glfw_dispatch.h:
--------------------------------------------------------------------------------
1 | // Copyright 2022 DeepMind Technologies Limited
2 | //
3 | // Licensed under the Apache License, Version 2.0 (the "License");
4 | // you may not use this file except in compliance with the License.
5 | // You may obtain a copy of the License at
6 | //
7 | // http://www.apache.org/licenses/LICENSE-2.0
8 | //
9 | // Unless required by applicable law or agreed to in writing, software
10 | // distributed under the License is distributed on an "AS IS" BASIS,
11 | // WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
12 | // See the License for the specific language governing permissions and
13 | // limitations under the License.
14 |
15 | #ifndef MUJOCO_SIMULATE_GLFW_DISPATCH_H_
16 | #define MUJOCO_SIMULATE_GLFW_DISPATCH_H_
17 |
18 | #include
19 |
20 | namespace mujoco {
21 | // Dynamic dispatch table for GLFW functions required by Simulate.
22 | // This allows us to use GLFW without introducing a link-time dependency on the
23 | // library, which is useful e.g. when using GLFW via Python.
24 | struct Glfw {
25 | #define mjGLFW_DECLARE_SYMBOL(func) decltype(&::func) func
26 | // go/keep-sorted start
27 | mjGLFW_DECLARE_SYMBOL(glfwCreateWindow);
28 | mjGLFW_DECLARE_SYMBOL(glfwDestroyWindow);
29 | mjGLFW_DECLARE_SYMBOL(glfwGetCursorPos);
30 | mjGLFW_DECLARE_SYMBOL(glfwGetFramebufferSize);
31 | mjGLFW_DECLARE_SYMBOL(glfwGetKey);
32 | mjGLFW_DECLARE_SYMBOL(glfwGetMonitorPhysicalSize);
33 | mjGLFW_DECLARE_SYMBOL(glfwGetMouseButton);
34 | mjGLFW_DECLARE_SYMBOL(glfwGetPrimaryMonitor);
35 | mjGLFW_DECLARE_SYMBOL(glfwGetTime);
36 | mjGLFW_DECLARE_SYMBOL(glfwGetVideoMode);
37 | mjGLFW_DECLARE_SYMBOL(glfwGetWindowMonitor);
38 | mjGLFW_DECLARE_SYMBOL(glfwGetWindowPos);
39 | mjGLFW_DECLARE_SYMBOL(glfwGetWindowSize);
40 | mjGLFW_DECLARE_SYMBOL(glfwGetWindowUserPointer);
41 | mjGLFW_DECLARE_SYMBOL(glfwInit);
42 | mjGLFW_DECLARE_SYMBOL(glfwMakeContextCurrent);
43 | mjGLFW_DECLARE_SYMBOL(glfwPollEvents);
44 | mjGLFW_DECLARE_SYMBOL(glfwSetClipboardString);
45 | mjGLFW_DECLARE_SYMBOL(glfwSetCursorPosCallback);
46 | mjGLFW_DECLARE_SYMBOL(glfwSetDropCallback);
47 | mjGLFW_DECLARE_SYMBOL(glfwSetKeyCallback);
48 | mjGLFW_DECLARE_SYMBOL(glfwSetMouseButtonCallback);
49 | mjGLFW_DECLARE_SYMBOL(glfwSetScrollCallback);
50 | mjGLFW_DECLARE_SYMBOL(glfwSetWindowMonitor);
51 | mjGLFW_DECLARE_SYMBOL(glfwSetWindowRefreshCallback);
52 | mjGLFW_DECLARE_SYMBOL(glfwSetWindowSizeCallback);
53 | mjGLFW_DECLARE_SYMBOL(glfwSetWindowTitle);
54 | mjGLFW_DECLARE_SYMBOL(glfwSetWindowUserPointer);
55 | mjGLFW_DECLARE_SYMBOL(glfwSwapBuffers);
56 | mjGLFW_DECLARE_SYMBOL(glfwSwapInterval);
57 | mjGLFW_DECLARE_SYMBOL(glfwTerminate);
58 | mjGLFW_DECLARE_SYMBOL(glfwWindowHint);
59 | mjGLFW_DECLARE_SYMBOL(glfwWindowShouldClose);
60 | // go/keep-sorted end
61 | #undef mjGLFW_DECLARE_SYMBOL
62 | };
63 |
64 | const struct Glfw& Glfw(void* dlhandle = nullptr);
65 | } // namespace mujoco
66 |
67 | #endif // MUJOCO_SIMULATE_GLFW_DISPATCH_H_
68 |
--------------------------------------------------------------------------------
/simulation/mujoco/include/mujoco/mjexport.h:
--------------------------------------------------------------------------------
1 | // Copyright 2021 DeepMind Technologies Limited
2 | //
3 | // Licensed under the Apache License, Version 2.0 (the "License");
4 | // you may not use this file except in compliance with the License.
5 | // You may obtain a copy of the License at
6 | //
7 | // http://www.apache.org/licenses/LICENSE-2.0
8 | //
9 | // Unless required by applicable law or agreed to in writing, software
10 | // distributed under the License is distributed on an "AS IS" BASIS,
11 | // WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
12 | // See the License for the specific language governing permissions and
13 | // limitations under the License.
14 |
15 | #ifndef MUJOCO_MJEXPORT_H_
16 | #define MUJOCO_MJEXPORT_H_
17 |
18 | #if defined _WIN32 || defined __CYGWIN__
19 | #define MUJOCO_HELPER_DLL_IMPORT __declspec(dllimport)
20 | #define MUJOCO_HELPER_DLL_EXPORT __declspec(dllexport)
21 | #define MUJOCO_HELPER_DLL_LOCAL
22 | #else
23 | #if __GNUC__ >= 4
24 | #define MUJOCO_HELPER_DLL_IMPORT __attribute__ ((visibility ("default")))
25 | #define MUJOCO_HELPER_DLL_EXPORT __attribute__ ((visibility ("default")))
26 | #define MUJOCO_HELPER_DLL_LOCAL __attribute__ ((visibility ("hidden")))
27 | #else
28 | #define MUJOCO_HELPER_DLL_IMPORT
29 | #define MUJOCO_HELPER_DLL_EXPORT
30 | #define MUJOCO_HELPER_DLL_LOCAL
31 | #endif
32 | #endif
33 |
34 | #ifdef MJ_STATIC
35 | // static library
36 | #define MJAPI
37 | #define MJLOCAL
38 | #else
39 | #ifdef MUJOCO_DLL_EXPORTS
40 | #define MJAPI MUJOCO_HELPER_DLL_EXPORT
41 | #else
42 | #define MJAPI MUJOCO_HELPER_DLL_IMPORT
43 | #endif
44 | #define MJLOCAL MUJOCO_HELPER_DLL_LOCAL
45 | #endif
46 |
47 | #endif // MUJOCO_MJEXPORT_H_
48 |
--------------------------------------------------------------------------------
/simulation/mujoco/include/mujoco/mjplugin.h:
--------------------------------------------------------------------------------
1 | // Copyright 2022 DeepMind Technologies Limited
2 | //
3 | // Licensed under the Apache License, Version 2.0 (the "License");
4 | // you may not use this file except in compliance with the License.
5 | // You may obtain a copy of the License at
6 | //
7 | // http://www.apache.org/licenses/LICENSE-2.0
8 | //
9 | // Unless required by applicable law or agreed to in writing, software
10 | // distributed under the License is distributed on an "AS IS" BASIS,
11 | // WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
12 | // See the License for the specific language governing permissions and
13 | // limitations under the License.
14 |
15 | #ifndef MUJOCO_INCLUDE_MJPLUGIN_H_
16 | #define MUJOCO_INCLUDE_MJPLUGIN_H_
17 |
18 | #include
19 | #include
20 | #include
21 |
22 |
23 | typedef enum mjtPluginCapabilityBit_ {
24 | mjPLUGIN_ACTUATOR = 1<<0,
25 | mjPLUGIN_SENSOR = 1<<1,
26 | mjPLUGIN_PASSIVE = 1<<2,
27 | } mjtPluginCapabilityBit;
28 |
29 | struct mjpPlugin_ {
30 | const char* name; // globally unique name identifying the plugin
31 |
32 | int nattribute; // number of configuration attributes
33 | const char* const* attributes; // name of configuration attributes
34 |
35 | int capabilityflags; // bitfield of mjtPluginCapabilityBit specifying plugin capabilities
36 | int needstage; // an mjtStage enum value specifying the sensor computation stage
37 |
38 | // number of mjtNums needed to store the state of a plugin instance (required)
39 | int (*nstate)(const mjModel* m, int instance);
40 |
41 | // dimension of the specified sensor's output (required only for sensor plugins)
42 | int (*nsensordata)(const mjModel* m, int instance, int sensor_id);
43 |
44 | // called when a new mjData is being created (required), returns 0 on success or -1 on failure
45 | int (*init)(const mjModel* m, mjData* d, int instance);
46 |
47 | // called when an mjData is being freed (optional)
48 | void (*destroy)(mjData* d, int instance);
49 |
50 | // called when an mjData is being copied (optional)
51 | void (*copy)(mjData* dest, const mjModel* m, const mjData* src, int instance);
52 |
53 | // called when an mjData is being reset (required)
54 | void (*reset)(const mjModel* m, double* plugin_state, void* plugin_data, int instance);
55 |
56 | // called when the plugin needs to update its outputs (required)
57 | void (*compute)(const mjModel* m, mjData* d, int instance, int capability_bit);
58 |
59 | // called when time integration occurs (optional)
60 | void (*advance)(const mjModel* m, mjData* d, int instance);
61 |
62 | // called by mjv_updateScene (optional)
63 | void (*visualize)(const mjModel*m, mjData* d, mjvScene* scn, int instance);
64 | };
65 | typedef struct mjpPlugin_ mjpPlugin;
66 |
67 | #if defined(__has_attribute)
68 |
69 | #if __has_attribute(constructor)
70 | #define mjPLUGIN_DYNAMIC_LIBRARY_INIT \
71 | __attribute__((constructor)) static void _mjplugin_init(void)
72 | #endif
73 |
74 | #elif defined(_MSC_VER)
75 |
76 | #ifndef mjDLLMAIN
77 | #define mjDLLMAIN DllMain
78 | #endif
79 |
80 | #if !defined(mjEXTERNC)
81 | #if defined(__cplusplus)
82 | #define mjEXTERNC extern "C"
83 | #else
84 | #define mjEXTERNC
85 | #endif // defined(__cplusplus)
86 | #endif // !defined(mjEXTERNC)
87 |
88 | // NOLINTBEGIN(runtime/int)
89 | #define mjPLUGIN_DYNAMIC_LIBRARY_INIT \
90 | static void _mjplugin_dllmain(void); \
91 | mjEXTERNC int __stdcall mjDLLMAIN(void* hinst, unsigned long reason, void* reserved) { \
92 | if (reason == 1) { \
93 | _mjplugin_dllmain(); \
94 | } \
95 | return 1; \
96 | } \
97 | static void _mjplugin_dllmain(void)
98 | // NOLINTEND(runtime/int)
99 |
100 | #endif // defined(_MSC_VER)
101 |
102 | // function pointer type for mj_loadAllPluginLibraries callback
103 | typedef void (*mjfPluginLibraryLoadCallback)(const char* filename, int first, int count);
104 |
105 | #endif // MUJOCO_INCLUDE_MJPLUGIN_H_
106 |
--------------------------------------------------------------------------------
/simulation/mujoco/include/mujoco/mjrender.h:
--------------------------------------------------------------------------------
1 | // Copyright 2021 DeepMind Technologies Limited
2 | //
3 | // Licensed under the Apache License, Version 2.0 (the "License");
4 | // you may not use this file except in compliance with the License.
5 | // You may obtain a copy of the License at
6 | //
7 | // http://www.apache.org/licenses/LICENSE-2.0
8 | //
9 | // Unless required by applicable law or agreed to in writing, software
10 | // distributed under the License is distributed on an "AS IS" BASIS,
11 | // WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
12 | // See the License for the specific language governing permissions and
13 | // limitations under the License.
14 |
15 | #ifndef MUJOCO_MJRENDER_H_
16 | #define MUJOCO_MJRENDER_H_
17 |
18 | #include
19 |
20 | #if defined(__cplusplus)
21 | extern "C" {
22 | #endif
23 |
24 | #define mjNAUX 10 // number of auxiliary buffers
25 | #define mjMAXTEXTURE 1000 // maximum number of textures
26 |
27 |
28 | //---------------------------------- primitive types (mjt) -----------------------------------------
29 |
30 | typedef enum mjtGridPos_ { // grid position for overlay
31 | mjGRID_TOPLEFT = 0, // top left
32 | mjGRID_TOPRIGHT, // top right
33 | mjGRID_BOTTOMLEFT, // bottom left
34 | mjGRID_BOTTOMRIGHT // bottom right
35 | } mjtGridPos;
36 |
37 |
38 | typedef enum mjtFramebuffer_ { // OpenGL framebuffer option
39 | mjFB_WINDOW = 0, // default/window buffer
40 | mjFB_OFFSCREEN // offscreen buffer
41 | } mjtFramebuffer;
42 |
43 |
44 | typedef enum mjtFontScale_ { // font scale, used at context creation
45 | mjFONTSCALE_50 = 50, // 50% scale, suitable for low-res rendering
46 | mjFONTSCALE_100 = 100, // normal scale, suitable in the absence of DPI scaling
47 | mjFONTSCALE_150 = 150, // 150% scale
48 | mjFONTSCALE_200 = 200, // 200% scale
49 | mjFONTSCALE_250 = 250, // 250% scale
50 | mjFONTSCALE_300 = 300 // 300% scale
51 | } mjtFontScale;
52 |
53 |
54 | typedef enum mjtFont_ { // font type, used at each text operation
55 | mjFONT_NORMAL = 0, // normal font
56 | mjFONT_SHADOW, // normal font with shadow (for higher contrast)
57 | mjFONT_BIG // big font (for user alerts)
58 | } mjtFont;
59 |
60 |
61 | struct mjrRect_ { // OpenGL rectangle
62 | int left; // left (usually 0)
63 | int bottom; // bottom (usually 0)
64 | int width; // width (usually buffer width)
65 | int height; // height (usually buffer height)
66 | };
67 | typedef struct mjrRect_ mjrRect;
68 |
69 |
70 | //---------------------------------- mjrContext ----------------------------------------------------
71 |
72 | struct mjrContext_ { // custom OpenGL context
73 | // parameters copied from mjVisual
74 | float lineWidth; // line width for wireframe rendering
75 | float shadowClip; // clipping radius for directional lights
76 | float shadowScale; // fraction of light cutoff for spot lights
77 | float fogStart; // fog start = stat.extent * vis.map.fogstart
78 | float fogEnd; // fog end = stat.extent * vis.map.fogend
79 | float fogRGBA[4]; // fog rgba
80 | int shadowSize; // size of shadow map texture
81 | int offWidth; // width of offscreen buffer
82 | int offHeight; // height of offscreen buffer
83 | int offSamples; // number of offscreen buffer multisamples
84 |
85 | // parameters specified at creation
86 | int fontScale; // font scale
87 | int auxWidth[mjNAUX]; // auxiliary buffer width
88 | int auxHeight[mjNAUX]; // auxiliary buffer height
89 | int auxSamples[mjNAUX]; // auxiliary buffer multisamples
90 |
91 | // offscreen rendering objects
92 | unsigned int offFBO; // offscreen framebuffer object
93 | unsigned int offFBO_r; // offscreen framebuffer for resolving multisamples
94 | unsigned int offColor; // offscreen color buffer
95 | unsigned int offColor_r; // offscreen color buffer for resolving multisamples
96 | unsigned int offDepthStencil; // offscreen depth and stencil buffer
97 | unsigned int offDepthStencil_r; // offscreen depth and stencil buffer for resolving multisamples
98 |
99 | // shadow rendering objects
100 | unsigned int shadowFBO; // shadow map framebuffer object
101 | unsigned int shadowTex; // shadow map texture
102 |
103 | // auxiliary buffers
104 | unsigned int auxFBO[mjNAUX]; // auxiliary framebuffer object
105 | unsigned int auxFBO_r[mjNAUX]; // auxiliary framebuffer object for resolving
106 | unsigned int auxColor[mjNAUX]; // auxiliary color buffer
107 | unsigned int auxColor_r[mjNAUX];// auxiliary color buffer for resolving
108 |
109 | // texture objects and info
110 | int ntexture; // number of allocated textures
111 | int textureType[100]; // type of texture (mjtTexture) (ntexture)
112 | unsigned int texture[100]; // texture names
113 |
114 | // displaylist starting positions
115 | unsigned int basePlane; // all planes from model
116 | unsigned int baseMesh; // all meshes from model
117 | unsigned int baseHField; // all hfields from model
118 | unsigned int baseBuiltin; // all buildin geoms, with quality from model
119 | unsigned int baseFontNormal; // normal font
120 | unsigned int baseFontShadow; // shadow font
121 | unsigned int baseFontBig; // big font
122 |
123 | // displaylist ranges
124 | int rangePlane; // all planes from model
125 | int rangeMesh; // all meshes from model
126 | int rangeHField; // all hfields from model
127 | int rangeBuiltin; // all builtin geoms, with quality from model
128 | int rangeFont; // all characters in font
129 |
130 | // skin VBOs
131 | int nskin; // number of skins
132 | unsigned int* skinvertVBO; // skin vertex position VBOs (nskin)
133 | unsigned int* skinnormalVBO; // skin vertex normal VBOs (nskin)
134 | unsigned int* skintexcoordVBO; // skin vertex texture coordinate VBOs (nskin)
135 | unsigned int* skinfaceVBO; // skin face index VBOs (nskin)
136 |
137 | // character info
138 | int charWidth[127]; // character widths: normal and shadow
139 | int charWidthBig[127]; // chacarter widths: big
140 | int charHeight; // character heights: normal and shadow
141 | int charHeightBig; // character heights: big
142 |
143 | // capabilities
144 | int glInitialized; // is OpenGL initialized
145 | int windowAvailable; // is default/window framebuffer available
146 | int windowSamples; // number of samples for default/window framebuffer
147 | int windowStereo; // is stereo available for default/window framebuffer
148 | int windowDoublebuffer; // is default/window framebuffer double buffered
149 |
150 | // framebuffer
151 | int currentBuffer; // currently active framebuffer: mjFB_WINDOW or mjFB_OFFSCREEN
152 |
153 | // pixel output format
154 | int readPixelFormat; // default color pixel format for mjr_readPixels
155 | };
156 | typedef struct mjrContext_ mjrContext;
157 |
158 | #if defined(__cplusplus)
159 | }
160 | #endif
161 | #endif // MUJOCO_MJRENDER_H_
162 |
--------------------------------------------------------------------------------
/simulation/mujoco/include/mujoco/mjtnum.h:
--------------------------------------------------------------------------------
1 | // Copyright 2021 DeepMind Technologies Limited
2 | //
3 | // Licensed under the Apache License, Version 2.0 (the "License");
4 | // you may not use this file except in compliance with the License.
5 | // You may obtain a copy of the License at
6 | //
7 | // http://www.apache.org/licenses/LICENSE-2.0
8 | //
9 | // Unless required by applicable law or agreed to in writing, software
10 | // distributed under the License is distributed on an "AS IS" BASIS,
11 | // WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
12 | // See the License for the specific language governing permissions and
13 | // limitations under the License.
14 |
15 | #ifndef MUJOCO_INCLUDE_MJTNUM_H_
16 | #define MUJOCO_INCLUDE_MJTNUM_H_
17 |
18 | //---------------------------------- floating-point definitions ------------------------------------
19 |
20 | // compile-time configuration options
21 | #define mjUSEDOUBLE // single or double precision for mjtNum
22 |
23 |
24 | // floating point data type and minval
25 | #ifdef mjUSEDOUBLE
26 | typedef double mjtNum;
27 | #define mjMINVAL 1E-15 // minimum value in any denominator
28 | #else
29 | typedef float mjtNum;
30 | #define mjMINVAL 1E-15f
31 | #endif
32 |
33 |
34 | #endif // MUJOCO_INCLUDE_MJTNUM_H_
35 |
--------------------------------------------------------------------------------
/simulation/mujoco/include/mujoco/mjui.h:
--------------------------------------------------------------------------------
1 | // Copyright 2021 DeepMind Technologies Limited
2 | //
3 | // Licensed under the Apache License, Version 2.0 (the "License");
4 | // you may not use this file except in compliance with the License.
5 | // You may obtain a copy of the License at
6 | //
7 | // http://www.apache.org/licenses/LICENSE-2.0
8 | //
9 | // Unless required by applicable law or agreed to in writing, software
10 | // distributed under the License is distributed on an "AS IS" BASIS,
11 | // WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
12 | // See the License for the specific language governing permissions and
13 | // limitations under the License.
14 |
15 | #ifndef MUJOCO_MJUI_H_
16 | #define MUJOCO_MJUI_H_
17 |
18 | #include
19 |
20 | #define mjMAXUISECT 10 // maximum number of sections
21 | #define mjMAXUIITEM 80 // maximum number of items per section
22 | #define mjMAXUITEXT 300 // maximum number of chars in edittext and other
23 | #define mjMAXUINAME 40 // maximum number of chars in name
24 | #define mjMAXUIMULTI 35 // maximum number of radio/select items in group
25 | #define mjMAXUIEDIT 7 // maximum number of elements in edit list
26 | #define mjMAXUIRECT 25 // maximum number of rectangles
27 |
28 | #define mjSEPCLOSED 1000 // closed state of adjustable separator
29 |
30 |
31 | // key codes matching GLFW (user must remap for other frameworks)
32 | #define mjKEY_ESCAPE 256
33 | #define mjKEY_ENTER 257
34 | #define mjKEY_TAB 258
35 | #define mjKEY_BACKSPACE 259
36 | #define mjKEY_INSERT 260
37 | #define mjKEY_DELETE 261
38 | #define mjKEY_RIGHT 262
39 | #define mjKEY_LEFT 263
40 | #define mjKEY_DOWN 264
41 | #define mjKEY_UP 265
42 | #define mjKEY_PAGE_UP 266
43 | #define mjKEY_PAGE_DOWN 267
44 | #define mjKEY_HOME 268
45 | #define mjKEY_END 269
46 | #define mjKEY_F1 290
47 | #define mjKEY_F2 291
48 | #define mjKEY_F3 292
49 | #define mjKEY_F4 293
50 | #define mjKEY_F5 294
51 | #define mjKEY_F6 295
52 | #define mjKEY_F7 296
53 | #define mjKEY_F8 297
54 | #define mjKEY_F9 298
55 | #define mjKEY_F10 299
56 | #define mjKEY_F11 300
57 | #define mjKEY_F12 301
58 |
59 |
60 | //---------------------------------- primitive types (mjt) -----------------------------------------
61 |
62 | typedef enum mjtButton_ { // mouse button
63 | mjBUTTON_NONE = 0, // no button
64 | mjBUTTON_LEFT, // left button
65 | mjBUTTON_RIGHT, // right button
66 | mjBUTTON_MIDDLE // middle button
67 | } mjtButton;
68 |
69 |
70 | typedef enum mjtEvent_ { // mouse and keyboard event type
71 | mjEVENT_NONE = 0, // no event
72 | mjEVENT_MOVE, // mouse move
73 | mjEVENT_PRESS, // mouse button press
74 | mjEVENT_RELEASE, // mouse button release
75 | mjEVENT_SCROLL, // scroll
76 | mjEVENT_KEY, // key press
77 | mjEVENT_RESIZE, // resize
78 | mjEVENT_REDRAW, // redraw
79 | mjEVENT_FILESDROP // files drop
80 | } mjtEvent;
81 |
82 |
83 | typedef enum mjtItem_ { // UI item type
84 | mjITEM_END = -2, // end of definition list (not an item)
85 | mjITEM_SECTION = -1, // section (not an item)
86 | mjITEM_SEPARATOR = 0, // separator
87 | mjITEM_STATIC, // static text
88 | mjITEM_BUTTON, // button
89 |
90 | // the rest have data pointer
91 | mjITEM_CHECKINT, // check box, int value
92 | mjITEM_CHECKBYTE, // check box, mjtByte value
93 | mjITEM_RADIO, // radio group
94 | mjITEM_RADIOLINE, // radio group, single line
95 | mjITEM_SELECT, // selection box
96 | mjITEM_SLIDERINT, // slider, int value
97 | mjITEM_SLIDERNUM, // slider, mjtNum value
98 | mjITEM_EDITINT, // editable array, int values
99 | mjITEM_EDITNUM, // editable array, mjtNum values
100 | mjITEM_EDITTXT, // editable text
101 |
102 | mjNITEM // number of item types
103 | } mjtItem;
104 |
105 |
106 | // predicate function: set enable/disable based on item category
107 | typedef int (*mjfItemEnable)(int category, void* data);
108 |
109 |
110 | //---------------------------------- mjuiState -----------------------------------------------------
111 |
112 | struct mjuiState_ { // mouse and keyboard state
113 | // constants set by user
114 | int nrect; // number of rectangles used
115 | mjrRect rect[mjMAXUIRECT]; // rectangles (index 0: entire window)
116 | void* userdata; // pointer to user data (for callbacks)
117 |
118 | // event type
119 | int type; // (type mjtEvent)
120 |
121 | // mouse buttons
122 | int left; // is left button down
123 | int right; // is right button down
124 | int middle; // is middle button down
125 | int doubleclick; // is last press a double click
126 | int button; // which button was pressed (mjtButton)
127 | double buttontime; // time of last button press
128 |
129 | // mouse position
130 | double x; // x position
131 | double y; // y position
132 | double dx; // x displacement
133 | double dy; // y displacement
134 | double sx; // x scroll
135 | double sy; // y scroll
136 |
137 | // keyboard
138 | int control; // is control down
139 | int shift; // is shift down
140 | int alt; // is alt down
141 | int key; // which key was pressed
142 | double keytime; // time of last key press
143 |
144 | // rectangle ownership and dragging
145 | int mouserect; // which rectangle contains mouse
146 | int dragrect; // which rectangle is dragged with mouse
147 | int dragbutton; // which button started drag (mjtButton)
148 |
149 | // files dropping (only valid when type == mjEVENT_FILESDROP)
150 | int dropcount; // number of files dropped
151 | const char** droppaths; // paths to files dropped
152 | };
153 | typedef struct mjuiState_ mjuiState;
154 |
155 |
156 | //---------------------------------- mjuiThemeSpacing ----------------------------------------------
157 |
158 | struct mjuiThemeSpacing_ { // UI visualization theme spacing
159 | int total; // total width
160 | int scroll; // scrollbar width
161 | int label; // label width
162 | int section; // section gap
163 | int itemside; // item side gap
164 | int itemmid; // item middle gap
165 | int itemver; // item vertical gap
166 | int texthor; // text horizontal gap
167 | int textver; // text vertical gap
168 | int linescroll; // number of pixels to scroll
169 | int samples; // number of multisamples
170 | };
171 | typedef struct mjuiThemeSpacing_ mjuiThemeSpacing;
172 |
173 |
174 | //---------------------------------- mjuiThemeColor ------------------------------------------------
175 |
176 | struct mjuiThemeColor_ { // UI visualization theme color
177 | float master[3]; // master background
178 | float thumb[3]; // scrollbar thumb
179 | float secttitle[3]; // section title
180 | float sectfont[3]; // section font
181 | float sectsymbol[3]; // section symbol
182 | float sectpane[3]; // section pane
183 | float shortcut[3]; // shortcut background
184 | float fontactive[3]; // font active
185 | float fontinactive[3]; // font inactive
186 | float decorinactive[3]; // decor inactive
187 | float decorinactive2[3]; // inactive slider color 2
188 | float button[3]; // button
189 | float check[3]; // check
190 | float radio[3]; // radio
191 | float select[3]; // select
192 | float select2[3]; // select pane
193 | float slider[3]; // slider
194 | float slider2[3]; // slider color 2
195 | float edit[3]; // edit
196 | float edit2[3]; // edit invalid
197 | float cursor[3]; // edit cursor
198 | };
199 | typedef struct mjuiThemeColor_ mjuiThemeColor;
200 |
201 |
202 | //---------------------------------- mjuiItem ------------------------------------------------------
203 |
204 | struct mjuiItemSingle_ { // check and button-related
205 | int modifier; // 0: none, 1: control, 2: shift; 4: alt
206 | int shortcut; // shortcut key; 0: undefined
207 | };
208 |
209 |
210 | struct mjuiItemMulti_ { // static, radio and select-related
211 | int nelem; // number of elements in group
212 | char name[mjMAXUIMULTI][mjMAXUINAME]; // element names
213 | };
214 |
215 |
216 | struct mjuiItemSlider_ { // slider-related
217 | double range[2]; // slider range
218 | double divisions; // number of range divisions
219 | };
220 |
221 |
222 | struct mjuiItemEdit_ { // edit-related
223 | int nelem; // number of elements in list
224 | double range[mjMAXUIEDIT][2]; // element range (min>=max: ignore)
225 | };
226 |
227 |
228 | struct mjuiItem_ { // UI item
229 | // common properties
230 | int type; // type (mjtItem)
231 | char name[mjMAXUINAME]; // name
232 | int state; // 0: disable, 1: enable, 2+: use predicate
233 | void *pdata; // data pointer (type-specific)
234 | int sectionid; // id of section containing item
235 | int itemid; // id of item within section
236 |
237 | // type-specific properties
238 | union {
239 | struct mjuiItemSingle_ single; // check and button
240 | struct mjuiItemMulti_ multi; // static, radio and select
241 | struct mjuiItemSlider_ slider; // slider
242 | struct mjuiItemEdit_ edit; // edit
243 | };
244 |
245 | // internal
246 | mjrRect rect; // rectangle occupied by item
247 | };
248 | typedef struct mjuiItem_ mjuiItem;
249 |
250 |
251 | //---------------------------------- mjuiSection ---------------------------------------------------
252 |
253 | struct mjuiSection_ { // UI section
254 | // properties
255 | char name[mjMAXUINAME]; // name
256 | int state; // 0: closed, 1: open
257 | int modifier; // 0: none, 1: control, 2: shift; 4: alt
258 | int shortcut; // shortcut key; 0: undefined
259 | int nitem; // number of items in use
260 | mjuiItem item[mjMAXUIITEM]; // preallocated array of items
261 |
262 | // internal
263 | mjrRect rtitle; // rectangle occupied by title
264 | mjrRect rcontent; // rectangle occupied by content
265 | };
266 | typedef struct mjuiSection_ mjuiSection;
267 |
268 |
269 | //---------------------------------- mjUI ----------------------------------------------------------
270 |
271 | struct mjUI_ { // entire UI
272 | // constants set by user
273 | mjuiThemeSpacing spacing; // UI theme spacing
274 | mjuiThemeColor color; // UI theme color
275 | mjfItemEnable predicate; // callback to set item state programmatically
276 | void* userdata; // pointer to user data (passed to predicate)
277 | int rectid; // index of this ui rectangle in mjuiState
278 | int auxid; // aux buffer index of this ui
279 | int radiocol; // number of radio columns (0 defaults to 2)
280 |
281 | // UI sizes (framebuffer units)
282 | int width; // width
283 | int height; // current heigth
284 | int maxheight; // height when all sections open
285 | int scroll; // scroll from top of UI
286 |
287 | // mouse focus
288 | int mousesect; // 0: none, -1: scroll, otherwise 1+section
289 | int mouseitem; // item within section
290 | int mousehelp; // help button down: print shortcuts
291 |
292 | // keyboard focus and edit
293 | int editsect; // 0: none, otherwise 1+section
294 | int edititem; // item within section
295 | int editcursor; // cursor position
296 | int editscroll; // horizontal scroll
297 | char edittext[mjMAXUITEXT]; // current text
298 | mjuiItem* editchanged; // pointer to changed edit in last mjui_event
299 |
300 | // sections
301 | int nsect; // number of sections in use
302 | mjuiSection sect[mjMAXUISECT]; // preallocated array of sections
303 | };
304 | typedef struct mjUI_ mjUI;
305 |
306 |
307 | //---------------------------------- mjuiDef -------------------------------------------------------
308 |
309 | struct mjuiDef_ { // table passed to mjui_add()
310 | int type; // type (mjtItem); -1: section
311 | char name[mjMAXUINAME]; // name
312 | int state; // state
313 | void* pdata; // pointer to data
314 | char other[mjMAXUITEXT]; // string with type-specific properties
315 | };
316 | typedef struct mjuiDef_ mjuiDef;
317 |
318 | #endif // MUJOCO_MJUI_H_
319 |
--------------------------------------------------------------------------------
/simulation/mujoco/include/mujoco/mjvisualize.h:
--------------------------------------------------------------------------------
1 | // Copyright 2021 DeepMind Technologies Limited
2 | //
3 | // Licensed under the Apache License, Version 2.0 (the "License");
4 | // you may not use this file except in compliance with the License.
5 | // You may obtain a copy of the License at
6 | //
7 | // http://www.apache.org/licenses/LICENSE-2.0
8 | //
9 | // Unless required by applicable law or agreed to in writing, software
10 | // distributed under the License is distributed on an "AS IS" BASIS,
11 | // WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
12 | // See the License for the specific language governing permissions and
13 | // limitations under the License.
14 |
15 | #ifndef MUJOCO_MJVISUALIZE_H_
16 | #define MUJOCO_MJVISUALIZE_H_
17 |
18 | #include
19 | #include
20 |
21 | #define mjNGROUP 6 // number of geom, site, joint, skin groups with visflags
22 | #define mjMAXLIGHT 100 // maximum number of lights in a scene
23 | #define mjMAXOVERLAY 500 // maximum number of characters in overlay text
24 | #define mjMAXLINE 100 // maximum number of lines per plot
25 | #define mjMAXLINEPNT 1000 // maximum number points per line
26 | #define mjMAXPLANEGRID 200 // maximum number of grid divisions for plane
27 |
28 |
29 | //---------------------------------- primitive types (mjt) -----------------------------------------
30 |
31 | typedef enum mjtCatBit_ { // bitflags for mjvGeom category
32 | mjCAT_STATIC = 1, // model elements in body 0
33 | mjCAT_DYNAMIC = 2, // model elements in all other bodies
34 | mjCAT_DECOR = 4, // decorative geoms
35 | mjCAT_ALL = 7 // select all categories
36 | } mjtCatBit;
37 |
38 |
39 | typedef enum mjtMouse_ { // mouse interaction mode
40 | mjMOUSE_NONE = 0, // no action
41 | mjMOUSE_ROTATE_V, // rotate, vertical plane
42 | mjMOUSE_ROTATE_H, // rotate, horizontal plane
43 | mjMOUSE_MOVE_V, // move, vertical plane
44 | mjMOUSE_MOVE_H, // move, horizontal plane
45 | mjMOUSE_ZOOM, // zoom
46 | mjMOUSE_SELECT // selection
47 | } mjtMouse;
48 |
49 |
50 | typedef enum mjtPertBit_ { // mouse perturbations
51 | mjPERT_TRANSLATE = 1, // translation
52 | mjPERT_ROTATE = 2 // rotation
53 | } mjtPertBit;
54 |
55 |
56 | typedef enum mjtCamera_ { // abstract camera type
57 | mjCAMERA_FREE = 0, // free camera
58 | mjCAMERA_TRACKING, // tracking camera; uses trackbodyid
59 | mjCAMERA_FIXED, // fixed camera; uses fixedcamid
60 | mjCAMERA_USER // user is responsible for setting OpenGL camera
61 | } mjtCamera;
62 |
63 |
64 | typedef enum mjtLabel_ { // object labeling
65 | mjLABEL_NONE = 0, // nothing
66 | mjLABEL_BODY, // body labels
67 | mjLABEL_JOINT, // joint labels
68 | mjLABEL_GEOM, // geom labels
69 | mjLABEL_SITE, // site labels
70 | mjLABEL_CAMERA, // camera labels
71 | mjLABEL_LIGHT, // light labels
72 | mjLABEL_TENDON, // tendon labels
73 | mjLABEL_ACTUATOR, // actuator labels
74 | mjLABEL_CONSTRAINT, // constraint labels
75 | mjLABEL_SKIN, // skin labels
76 | mjLABEL_SELECTION, // selected object
77 | mjLABEL_SELPNT, // coordinates of selection point
78 | mjLABEL_CONTACTFORCE, // magnitude of contact force
79 |
80 | mjNLABEL // number of label types
81 | } mjtLabel;
82 |
83 |
84 | typedef enum mjtFrame_ { // frame visualization
85 | mjFRAME_NONE = 0, // no frames
86 | mjFRAME_BODY, // body frames
87 | mjFRAME_GEOM, // geom frames
88 | mjFRAME_SITE, // site frames
89 | mjFRAME_CAMERA, // camera frames
90 | mjFRAME_LIGHT, // light frames
91 | mjFRAME_CONTACT, // contact frames
92 | mjFRAME_WORLD, // world frame
93 |
94 | mjNFRAME // number of visualization frames
95 | } mjtFrame;
96 |
97 |
98 | typedef enum mjtVisFlag_ { // flags enabling model element visualization
99 | mjVIS_CONVEXHULL = 0, // mesh convex hull
100 | mjVIS_TEXTURE, // textures
101 | mjVIS_JOINT, // joints
102 | mjVIS_CAMERA, // cameras
103 | mjVIS_ACTUATOR, // actuators
104 | mjVIS_ACTIVATION, // activations
105 | mjVIS_LIGHT, // lights
106 | mjVIS_TENDON, // tendons
107 | mjVIS_RANGEFINDER, // rangefinder sensors
108 | mjVIS_CONSTRAINT, // point constraints
109 | mjVIS_INERTIA, // equivalent inertia boxes
110 | mjVIS_SCLINERTIA, // scale equivalent inertia boxes with mass
111 | mjVIS_PERTFORCE, // perturbation force
112 | mjVIS_PERTOBJ, // perturbation object
113 | mjVIS_CONTACTPOINT, // contact points
114 | mjVIS_CONTACTFORCE, // contact force
115 | mjVIS_CONTACTSPLIT, // split contact force into normal and tanget
116 | mjVIS_TRANSPARENT, // make dynamic geoms more transparent
117 | mjVIS_AUTOCONNECT, // auto connect joints and body coms
118 | mjVIS_COM, // center of mass
119 | mjVIS_SELECT, // selection point
120 | mjVIS_STATIC, // static bodies
121 | mjVIS_SKIN, // skin
122 |
123 | mjNVISFLAG // number of visualization flags
124 | } mjtVisFlag;
125 |
126 |
127 | typedef enum mjtRndFlag_ { // flags enabling rendering effects
128 | mjRND_SHADOW = 0, // shadows
129 | mjRND_WIREFRAME, // wireframe
130 | mjRND_REFLECTION, // reflections
131 | mjRND_ADDITIVE, // additive transparency
132 | mjRND_SKYBOX, // skybox
133 | mjRND_FOG, // fog
134 | mjRND_HAZE, // haze
135 | mjRND_SEGMENT, // segmentation with random color
136 | mjRND_IDCOLOR, // segmentation with segid+1 color
137 | mjRND_CULL_FACE, // cull backward faces
138 |
139 | mjNRNDFLAG // number of rendering flags
140 | } mjtRndFlag;
141 |
142 |
143 | typedef enum mjtStereo_ { // type of stereo rendering
144 | mjSTEREO_NONE = 0, // no stereo; use left eye only
145 | mjSTEREO_QUADBUFFERED, // quad buffered; revert to side-by-side if no hardware support
146 | mjSTEREO_SIDEBYSIDE // side-by-side
147 | } mjtStereo;
148 |
149 |
150 | //---------------------------------- mjvPerturb ----------------------------------------------------
151 |
152 | struct mjvPerturb_ { // object selection and perturbation
153 | int select; // selected body id; non-positive: none
154 | int skinselect; // selected skin id; negative: none
155 | int active; // perturbation bitmask (mjtPertBit)
156 | int active2; // secondary perturbation bitmask (mjtPertBit)
157 | mjtNum refpos[3]; // reference position for selected object
158 | mjtNum refquat[4]; // reference orientation for selected object
159 | mjtNum reflocalpos[3]; // reference selection point in object coordinates
160 | mjtNum localpos[3]; // selection point in object coordinates
161 | mjtNum scale; // relative mouse motion-to-space scaling (set by initPerturb)
162 | };
163 | typedef struct mjvPerturb_ mjvPerturb;
164 |
165 |
166 | //---------------------------------- mjvCamera -----------------------------------------------------
167 |
168 | struct mjvCamera_ { // abstract camera
169 | // type and ids
170 | int type; // camera type (mjtCamera)
171 | int fixedcamid; // fixed camera id
172 | int trackbodyid; // body id to track
173 |
174 | // abstract camera pose specification
175 | mjtNum lookat[3]; // lookat point
176 | mjtNum distance; // distance to lookat point or tracked body
177 | mjtNum azimuth; // camera azimuth (deg)
178 | mjtNum elevation; // camera elevation (deg)
179 | };
180 | typedef struct mjvCamera_ mjvCamera;
181 |
182 |
183 | //---------------------------------- mjvGLCamera ---------------------------------------------------
184 |
185 | struct mjvGLCamera_ { // OpenGL camera
186 | // camera frame
187 | float pos[3]; // position
188 | float forward[3]; // forward direction
189 | float up[3]; // up direction
190 |
191 | // camera projection
192 | float frustum_center; // hor. center (left,right set to match aspect)
193 | float frustum_bottom; // bottom
194 | float frustum_top; // top
195 | float frustum_near; // near
196 | float frustum_far; // far
197 | };
198 | typedef struct mjvGLCamera_ mjvGLCamera;
199 |
200 |
201 | //---------------------------------- mjvGeom -------------------------------------------------------
202 |
203 | struct mjvGeom_ { // abstract geom
204 | // type info
205 | int type; // geom type (mjtGeom)
206 | int dataid; // mesh, hfield or plane id; -1: none
207 | int objtype; // mujoco object type; mjOBJ_UNKNOWN for decor
208 | int objid; // mujoco object id; -1 for decor
209 | int category; // visual category
210 | int texid; // texture id; -1: no texture
211 | int texuniform; // uniform cube mapping
212 | int texcoord; // mesh geom has texture coordinates
213 | int segid; // segmentation id; -1: not shown
214 |
215 | // OpenGL info
216 | float texrepeat[2]; // texture repetition for 2D mapping
217 | float size[3]; // size parameters
218 | float pos[3]; // Cartesian position
219 | float mat[9]; // Cartesian orientation
220 | float rgba[4]; // color and transparency
221 | float emission; // emission coef
222 | float specular; // specular coef
223 | float shininess; // shininess coef
224 | float reflectance; // reflectance coef
225 | char label[100]; // text label
226 |
227 | // transparency rendering (set internally)
228 | float camdist; // distance to camera (used by sorter)
229 | float modelrbound; // geom rbound from model, 0 if not model geom
230 | mjtByte transparent; // treat geom as transparent
231 | };
232 | typedef struct mjvGeom_ mjvGeom;
233 |
234 |
235 | //---------------------------------- mjvLight ------------------------------------------------------
236 |
237 | struct mjvLight_ { // OpenGL light
238 | float pos[3]; // position rel. to body frame
239 | float dir[3]; // direction rel. to body frame
240 | float attenuation[3]; // OpenGL attenuation (quadratic model)
241 | float cutoff; // OpenGL cutoff
242 | float exponent; // OpenGL exponent
243 | float ambient[3]; // ambient rgb (alpha=1)
244 | float diffuse[3]; // diffuse rgb (alpha=1)
245 | float specular[3]; // specular rgb (alpha=1)
246 | mjtByte headlight; // headlight
247 | mjtByte directional; // directional light
248 | mjtByte castshadow; // does light cast shadows
249 | };
250 | typedef struct mjvLight_ mjvLight;
251 |
252 |
253 | //---------------------------------- mjvOption -----------------------------------------------------
254 |
255 | struct mjvOption_ { // abstract visualization options
256 | int label; // what objects to label (mjtLabel)
257 | int frame; // which frame to show (mjtFrame)
258 | mjtByte geomgroup[mjNGROUP]; // geom visualization by group
259 | mjtByte sitegroup[mjNGROUP]; // site visualization by group
260 | mjtByte jointgroup[mjNGROUP]; // joint visualization by group
261 | mjtByte tendongroup[mjNGROUP]; // tendon visualization by group
262 | mjtByte actuatorgroup[mjNGROUP]; // actuator visualization by group
263 | mjtByte skingroup[mjNGROUP]; // skin visualization by group
264 | mjtByte flags[mjNVISFLAG]; // visualization flags (indexed by mjtVisFlag)
265 | };
266 | typedef struct mjvOption_ mjvOption;
267 |
268 |
269 | //---------------------------------- mjvScene ------------------------------------------------------
270 |
271 | struct mjvScene_ { // abstract scene passed to OpenGL renderer
272 | // abstract geoms
273 | int maxgeom; // size of allocated geom buffer
274 | int ngeom; // number of geoms currently in buffer
275 | mjvGeom* geoms; // buffer for geoms (ngeom)
276 | int* geomorder; // buffer for ordering geoms by distance to camera (ngeom)
277 |
278 | // skin data
279 | int nskin; // number of skins
280 | int* skinfacenum; // number of faces in skin (nskin)
281 | int* skinvertadr; // address of skin vertices (nskin)
282 | int* skinvertnum; // number of vertices in skin (nskin)
283 | float* skinvert; // skin vertex data (nskin)
284 | float* skinnormal; // skin normal data (nskin)
285 |
286 | // OpenGL lights
287 | int nlight; // number of lights currently in buffer
288 | mjvLight lights[mjMAXLIGHT]; // buffer for lights (nlight)
289 |
290 | // OpenGL cameras
291 | mjvGLCamera camera[2]; // left and right camera
292 |
293 | // OpenGL model transformation
294 | mjtByte enabletransform; // enable model transformation
295 | float translate[3]; // model translation
296 | float rotate[4]; // model quaternion rotation
297 | float scale; // model scaling
298 |
299 | // OpenGL rendering effects
300 | int stereo; // stereoscopic rendering (mjtStereo)
301 | mjtByte flags[mjNRNDFLAG]; // rendering flags (indexed by mjtRndFlag)
302 |
303 | // framing
304 | int framewidth; // frame pixel width; 0: disable framing
305 | float framergb[3]; // frame color
306 | };
307 | typedef struct mjvScene_ mjvScene;
308 |
309 |
310 | //---------------------------------- mjvFigure -----------------------------------------------------
311 |
312 | struct mjvFigure_ { // abstract 2D figure passed to OpenGL renderer
313 | // enable flags
314 | int flg_legend; // show legend
315 | int flg_ticklabel[2]; // show grid tick labels (x,y)
316 | int flg_extend; // automatically extend axis ranges to fit data
317 | int flg_barplot; // isolated line segments (i.e. GL_LINES)
318 | int flg_selection; // vertical selection line
319 | int flg_symmetric; // symmetric y-axis
320 |
321 | // style settings
322 | float linewidth; // line width
323 | float gridwidth; // grid line width
324 | int gridsize[2]; // number of grid points in (x,y)
325 | float gridrgb[3]; // grid line rgb
326 | float figurergba[4]; // figure color and alpha
327 | float panergba[4]; // pane color and alpha
328 | float legendrgba[4]; // legend color and alpha
329 | float textrgb[3]; // text color
330 | float linergb[mjMAXLINE][3]; // line colors
331 | float range[2][2]; // axis ranges; (min>=max) automatic
332 | char xformat[20]; // x-tick label format for sprintf
333 | char yformat[20]; // y-tick label format for sprintf
334 | char minwidth[20]; // string used to determine min y-tick width
335 |
336 | // text labels
337 | char title[1000]; // figure title; subplots separated with 2+ spaces
338 | char xlabel[100]; // x-axis label
339 | char linename[mjMAXLINE][100]; // line names for legend
340 |
341 | // dynamic settings
342 | int legendoffset; // number of lines to offset legend
343 | int subplot; // selected subplot (for title rendering)
344 | int highlight[2]; // if point is in legend rect, highlight line
345 | int highlightid; // if id>=0 and no point, highlight id
346 | float selection; // selection line x-value
347 |
348 | // line data
349 | int linepnt[mjMAXLINE]; // number of points in line; (0) disable
350 | float linedata[mjMAXLINE][2*mjMAXLINEPNT]; // line data (x,y)
351 |
352 | // output from renderer
353 | int xaxispixel[2]; // range of x-axis in pixels
354 | int yaxispixel[2]; // range of y-axis in pixels
355 | float xaxisdata[2]; // range of x-axis in data units
356 | float yaxisdata[2]; // range of y-axis in data units
357 | };
358 | typedef struct mjvFigure_ mjvFigure;
359 |
360 | #endif // MUJOCO_MJVISUALIZE_H_
361 |
--------------------------------------------------------------------------------
/simulation/mujoco/include/platform_ui_adapter.h:
--------------------------------------------------------------------------------
1 | // Copyright 2023 DeepMind Technologies Limited
2 | //
3 | // Licensed under the Apache License, Version 2.0 (the "License");
4 | // you may not use this file except in compliance with the License.
5 | // You may obtain a copy of the License at
6 | //
7 | // http://www.apache.org/licenses/LICENSE-2.0
8 | //
9 | // Unless required by applicable law or agreed to in writing, software
10 | // distributed under the License is distributed on an "AS IS" BASIS,
11 | // WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
12 | // See the License for the specific language governing permissions and
13 | // limitations under the License.
14 |
15 | #ifndef MUJOCO_SIMULATE_PLATFORM_UI_ADAPTER_H_
16 | #define MUJOCO_SIMULATE_PLATFORM_UI_ADAPTER_H_
17 |
18 | #include
19 |
20 | #include
21 |
22 | namespace mujoco {
23 | class PlatformUIAdapter {
24 | public:
25 | virtual ~PlatformUIAdapter();
26 |
27 | inline mjuiState& state() { return state_; }
28 | inline const mjuiState& state() const { return state_; }
29 |
30 | inline mjrContext& mjr_context() { return con_; }
31 | inline const mjrContext& mjr_context() const { return con_; }
32 |
33 | inline void SetEventCallback(void (*event_callback)(mjuiState*)) {
34 | event_callback_ = event_callback;
35 | }
36 |
37 | inline void SetLayoutCallback(void (*layout_callback)(mjuiState*)) {
38 | layout_callback_ = layout_callback;
39 | }
40 |
41 | // Optionally overrideable function to (re)create an mjrContext for an mjModel
42 | virtual bool RefreshMjrContext(const mjModel* m, int fontscale);
43 |
44 | // Pure virtual functions to be implemented by individual adapters
45 | virtual std::pair GetCursorPosition() const = 0;
46 | virtual double GetDisplayPixelsPerInch() const = 0;
47 | virtual std::pair GetFramebufferSize() const = 0;
48 | virtual std::pair GetWindowSize() const = 0;
49 | virtual bool IsGPUAccelerated() const = 0;
50 | virtual void PollEvents() = 0;
51 | virtual void SetClipboardString(const char* text) = 0;
52 | virtual void SetVSync(bool enabled) = 0;
53 | virtual void SetWindowTitle(const char* title) = 0;
54 | virtual bool ShouldCloseWindow() const = 0;
55 | virtual void SwapBuffers() = 0;
56 | virtual void ToggleFullscreen() = 0;
57 |
58 | virtual bool IsLeftMouseButtonPressed() const = 0;
59 | virtual bool IsMiddleMouseButtonPressed() const = 0;
60 | virtual bool IsRightMouseButtonPressed() const = 0;
61 |
62 | virtual bool IsAltKeyPressed() const = 0;
63 | virtual bool IsCtrlKeyPressed() const = 0;
64 | virtual bool IsShiftKeyPressed() const = 0;
65 |
66 | virtual bool IsMouseButtonDownEvent(int act) const = 0;
67 | virtual bool IsKeyDownEvent(int act) const = 0;
68 |
69 | virtual int TranslateKeyCode(int key) const = 0;
70 | virtual mjtButton TranslateMouseButton(int button) const = 0;
71 |
72 | protected:
73 | PlatformUIAdapter();
74 |
75 | // Event handlers
76 | void OnFilesDrop(int count, const char** paths);
77 | void OnKey(int key, int scancode, int act);
78 | void OnMouseButton(int button, int act);
79 | void OnMouseMove(double x, double y);
80 | void OnScroll(double xoffset, double yoffset);
81 | void OnWindowRefresh();
82 | void OnWindowResize(int width, int height);
83 |
84 | mjuiState state_;
85 | void (*event_callback_)(mjuiState*);
86 | void (*layout_callback_)(mjuiState*);
87 |
88 | mjrContext con_;
89 | const mjModel* last_model_ = nullptr;
90 | int last_fontscale_ = -1;
91 |
92 | private:
93 | void UpdateMjuiState();
94 | };
95 | } // namespace mujoco
96 |
97 | #endif // MUJOCO_SIMULATE_PLATFORM_UI_ADAPTER_H_
98 |
--------------------------------------------------------------------------------
/simulation/mujoco/include/simulate.h:
--------------------------------------------------------------------------------
1 | // Copyright 2021 DeepMind Technologies Limited
2 | //
3 | // Licensed under the Apache License, Version 2.0 (the "License");
4 | // you may not use this file except in compliance with the License.
5 | // You may obtain a copy of the License at
6 | //
7 | // http://www.apache.org/licenses/LICENSE-2.0
8 | //
9 | // Unless required by applicable law or agreed to in writing, software
10 | // distributed under the License is distributed on an "AS IS" BASIS,
11 | // WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
12 | // See the License for the specific language governing permissions and
13 | // limitations under the License.
14 |
15 | #ifndef MUJOCO_SIMULATE_SIMULATE_H_
16 | #define MUJOCO_SIMULATE_SIMULATE_H_
17 |
18 | #include
19 | #include
20 | #include
21 | #include
22 | #include
23 | #include
24 | #include
25 |
26 | #include
27 | #include "platform_ui_adapter.h"
28 |
29 | namespace mujoco {
30 |
31 | //-------------------------------- global -----------------------------------------------
32 |
33 | // Simulate states not contained in MuJoCo structures
34 | class Simulate {
35 | public:
36 | using Clock = std::chrono::steady_clock;
37 | static_assert(std::ratio_less_equal_v);
38 |
39 | // create object and initialize the simulate ui
40 | Simulate(std::unique_ptr platform_ui_adapter);
41 |
42 | // Apply UI pose perturbations to model and data
43 | void applyposepertubations(int flg_paused);
44 |
45 | // Apply UI force perturbations to model and data
46 | void applyforceperturbations();
47 |
48 | // Request that the Simulate UI thread render a new model
49 | // optionally delete the old model and data when done
50 | void load(const char* file, mjModel* m, mjData* d);
51 |
52 | // functions below are used by the renderthread
53 | // load mjb or xml model that has been requested by load()
54 | void loadmodel();
55 |
56 | // prepare to render
57 | void prepare();
58 |
59 | // render the ui to the window
60 | void render();
61 |
62 | // loop to render the UI (must be called from main thread because of MacOS)
63 | void renderloop();
64 |
65 | // constants
66 | static constexpr int kMaxFilenameLength = 1000;
67 |
68 | // model and data to be visualized
69 | mjModel* mnew = nullptr;
70 | mjData* dnew = nullptr;
71 |
72 | mjModel* m = nullptr;
73 | mjData* d = nullptr;
74 | std::mutex mtx;
75 | std::condition_variable cond_loadrequest;
76 |
77 | // options
78 | int spacing = 0;
79 | int color = 3;
80 | int font = 0;
81 | int ui0_enable = 0;
82 | int ui1_enable = 0;
83 | int help = 0;
84 | int info = 0;
85 | int profiler = 0;
86 | int sensor = 0;
87 | int fullscreen = 0;
88 | int vsync = 1;
89 | int busywait = 0;
90 |
91 | // keyframe index
92 | int key = 0;
93 |
94 | // simulation
95 | int run = 1;
96 |
97 | // atomics for cross-thread messages
98 | std::atomic_int exitrequest = false;
99 | std::atomic_int droploadrequest = false;
100 | std::atomic_int screenshotrequest = false;
101 | std::atomic_int uiloadrequest = 0;
102 |
103 | // loadrequest
104 | // 2: render thread asked to update its model
105 | // 1: showing "loading" label, about to load
106 | // 0: model loaded or no load requested.
107 | int loadrequest = 0;
108 |
109 | // strings
110 | char loadError[kMaxFilenameLength] = "";
111 | char dropfilename[kMaxFilenameLength] = "";
112 | char filename[kMaxFilenameLength] = "";
113 | char previous_filename[kMaxFilenameLength] = "";
114 |
115 | // time synchronization
116 | int realTimeIndex;
117 | bool speedChanged = true;
118 | float measuredSlowdown = 1.0;
119 | // logarithmically spaced realtime slow-down coefficients (percent)
120 | static constexpr float percentRealTime[] = {
121 | 100, 80, 66, 50, 40, 33, 25, 20, 16, 13,
122 | 10, 8, 6.6, 5.0, 4, 3.3, 2.5, 2, 1.6, 1.3,
123 | 1, .8, .66, .5, .4, .33, .25, .2, .16, .13,
124 | .1
125 | };
126 |
127 | // control noise
128 | double ctrlnoisestd = 0.0;
129 | double ctrlnoiserate = 0.0;
130 |
131 | // watch
132 | char field[mjMAXUITEXT] = "qpos";
133 | int index = 0;
134 |
135 | // physics: need sync
136 | int disable[mjNDISABLE] = {0};
137 | int enable[mjNENABLE] = {0};
138 |
139 | // rendering: need sync
140 | int camera = 0;
141 |
142 | // abstract visualization
143 | mjvScene scn = {};
144 | mjvCamera cam = {};
145 | mjvOption vopt = {};
146 | mjvPerturb pert = {};
147 | mjvFigure figconstraint = {};
148 | mjvFigure figcost = {};
149 | mjvFigure figtimer = {};
150 | mjvFigure figsize = {};
151 | mjvFigure figsensor = {};
152 |
153 | // OpenGL rendering and UI
154 | int refreshRate = 60;
155 | int windowpos[2] = {0};
156 | int windowsize[2] = {0};
157 | std::unique_ptr platform_ui;
158 | mjuiState& uistate;
159 | mjUI ui0 = {};
160 | mjUI ui1 = {};
161 |
162 | // Constant arrays needed for the option section of UI and the UI interface
163 | // TODO setting the size here is not ideal
164 | const mjuiDef defOption[14] = {
165 | {mjITEM_SECTION, "Option", 1, nullptr, "AO"},
166 | {mjITEM_SELECT, "Spacing", 1, &this->spacing, "Tight\nWide"},
167 | {mjITEM_SELECT, "Color", 1, &this->color, "Default\nOrange\nWhite\nBlack"},
168 | {mjITEM_SELECT, "Font", 1, &this->font, "50 %\n100 %\n150 %\n200 %\n250 %\n300 %"},
169 | {mjITEM_CHECKINT, "Left UI (Tab)", 1, &this->ui0_enable, " #258"},
170 | {mjITEM_CHECKINT, "Right UI", 1, &this->ui1_enable, "S#258"},
171 | {mjITEM_CHECKINT, "Help", 2, &this->help, " #290"},
172 | {mjITEM_CHECKINT, "Info", 2, &this->info, " #291"},
173 | {mjITEM_CHECKINT, "Profiler", 2, &this->profiler, " #292"},
174 | {mjITEM_CHECKINT, "Sensor", 2, &this->sensor, " #293"},
175 | #ifdef __APPLE__
176 | {mjITEM_CHECKINT, "Fullscreen", 0, &this->fullscreen, " #294"},
177 | #else
178 | {mjITEM_CHECKINT, "Fullscreen", 1, &this->fullscreen, " #294"},
179 | #endif
180 | {mjITEM_CHECKINT, "Vertical Sync", 1, &this->vsync, ""},
181 | {mjITEM_CHECKINT, "Busy Wait", 1, &this->busywait, ""},
182 | {mjITEM_END}
183 | };
184 |
185 |
186 | // simulation section of UI
187 | const mjuiDef defSimulation[12] = {
188 | {mjITEM_SECTION, "Simulation", 1, nullptr, "AS"},
189 | {mjITEM_RADIO, "", 2, &this->run, "Pause\nRun"},
190 | {mjITEM_BUTTON, "Reset", 2, nullptr, " #259"},
191 | {mjITEM_BUTTON, "Reload", 2, nullptr, "CL"},
192 | {mjITEM_BUTTON, "Align", 2, nullptr, "CA"},
193 | {mjITEM_BUTTON, "Copy pose", 2, nullptr, "CC"},
194 | {mjITEM_SLIDERINT, "Key", 3, &this->key, "0 0"},
195 | {mjITEM_BUTTON, "Load key", 3},
196 | {mjITEM_BUTTON, "Save key", 3},
197 | {mjITEM_SLIDERNUM, "Noise scale", 2, &this->ctrlnoisestd, "0 2"},
198 | {mjITEM_SLIDERNUM, "Noise rate", 2, &this->ctrlnoiserate, "0 2"},
199 | {mjITEM_END}
200 | };
201 |
202 |
203 | // watch section of UI
204 | const mjuiDef defWatch[5] = {
205 | {mjITEM_SECTION, "Watch", 0, nullptr, "AW"},
206 | {mjITEM_EDITTXT, "Field", 2, this->field, "qpos"},
207 | {mjITEM_EDITINT, "Index", 2, &this->index, "1"},
208 | {mjITEM_STATIC, "Value", 2, nullptr, " "},
209 | {mjITEM_END}
210 | };
211 |
212 | // info strings
213 | char info_title[Simulate::kMaxFilenameLength] = {0};
214 | char info_content[Simulate::kMaxFilenameLength] = {0};
215 | };
216 | } // namespace mujoco
217 |
218 | #endif
219 |
--------------------------------------------------------------------------------
/simulation/mujoco/launch/simulation_launch.py:
--------------------------------------------------------------------------------
1 | import os
2 | from ament_index_python.packages import get_package_share_path
3 | from launch import LaunchDescription
4 | from launch.actions import DeclareLaunchArgument
5 | from launch.substitutions import LaunchConfiguration
6 | from launch_ros.actions import Node
7 |
8 |
9 | def generate_launch_description():
10 | xml_file_name = "model/xml/digit.xml"
11 | xml_file = os.path.join(get_package_share_path("description"), xml_file_name)
12 |
13 | return LaunchDescription(
14 | [
15 | Node(
16 | package="mujoco",
17 | executable="simulation",
18 | name="simulation_mujoco",
19 | output="screen",
20 | parameters=[
21 | {"simulation/model_file": xml_file},
22 | ],
23 | emulate_tty=True,
24 | arguments=[("__log_level:=debug")],
25 | ),
26 | ]
27 | )
28 |
--------------------------------------------------------------------------------
/simulation/mujoco/lib/libmujoco.so:
--------------------------------------------------------------------------------
1 | libmujoco.so.2.3.2
--------------------------------------------------------------------------------
/simulation/mujoco/lib/libmujoco.so.2.3.2:
--------------------------------------------------------------------------------
https://raw.githubusercontent.com/MindSpaceInc/Digit-MuJoCo-ROS2/1a263e07331cb22cf86b80ee2435febdb3be3bf2/simulation/mujoco/lib/libmujoco.so.2.3.2
--------------------------------------------------------------------------------
/simulation/mujoco/package.xml:
--------------------------------------------------------------------------------
1 |
2 |
3 |
4 | mujoco
5 | 0.0.0
6 | TODO: Package description
7 | DeepBreak
8 | BSD
9 |
10 | ament_cmake
11 |
12 | ament_lint_auto
13 | ament_lint_common
14 |
15 | rclcpp
16 | communication
17 | geometry_msgs
18 | tf2
19 | tf2_ros
20 | nav_msgs
21 | sensor_msgs
22 | cv_bridge
23 | ros2launch
24 |
25 |
26 | ament_cmake
27 |
28 |
29 |
--------------------------------------------------------------------------------
/simulation/mujoco/src/MuJoCoMessageHandler.cpp:
--------------------------------------------------------------------------------
1 | #include "MuJoCoMessageHandler.h"
2 |
3 | #include "cv_bridge/cv_bridge.h"
4 | #include "sensor_msgs/image_encodings.hpp"
5 |
6 | namespace deepbreak {
7 |
8 | MuJoCoMessageHandler::MuJoCoMessageHandler(mj::Simulate *sim)
9 | : Node("MuJoCoMessageHandler"), sim_(sim), name_prefix("simulation/") {
10 | model_param_name = name_prefix + "model_file";
11 | this->declare_parameter(model_param_name, "");
12 |
13 | reset_service_ = this->create_service(
14 | name_prefix + "sim_reset",
15 | std::bind(&MuJoCoMessageHandler::reset_callback, this,
16 | std::placeholders::_1, std::placeholders::_2));
17 |
18 | auto qos = rclcpp::QoS(rclcpp::KeepLast(1), rmw_qos_profile_sensor_data);
19 | imu_publisher_ = this->create_publisher(
20 | name_prefix + "imu_data", qos);
21 | joint_state_publisher_ = this->create_publisher(
22 | name_prefix + "joint_states", qos);
23 | odom_publisher_ = this->create_publisher(
24 | name_prefix + "odom", qos);
25 |
26 |
27 | timers_.emplace_back(this->create_wall_timer(
28 | 2.5ms, std::bind(&MuJoCoMessageHandler::imu_callback, this)));
29 | timers_.emplace_back(this->create_wall_timer(
30 | 1ms, std::bind(&MuJoCoMessageHandler::joint_callback, this)));
31 | timers_.emplace_back(this->create_wall_timer(
32 | 20ms, std::bind(&MuJoCoMessageHandler::odom_callback, this)));
33 | timers_.emplace_back(this->create_wall_timer(
34 | 100ms, std::bind(&MuJoCoMessageHandler::drop_old_message, this)));
35 |
36 | actuator_cmd_subscription_ =
37 | this->create_subscription(
38 | name_prefix + "actuators_cmds", qos,
39 | std::bind(&MuJoCoMessageHandler::actuator_cmd_callback, this,
40 | std::placeholders::_1));
41 |
42 | param_subscriber_ = std::make_shared(this);
43 | cb_handle_ = param_subscriber_->add_parameter_callback(
44 | model_param_name, std::bind(&MuJoCoMessageHandler::parameter_callback,
45 | this, std::placeholders::_1));
46 |
47 | actuator_cmds_ptr_ = std::make_shared();
48 |
49 | RCLCPP_INFO(this->get_logger(), "Start MuJoCoMessageHandler ...");
50 |
51 | std::string model_file = this->get_parameter(model_param_name)
52 | .get_parameter_value()
53 | .get();
54 | mju::strcpy_arr(sim_->filename, model_file.c_str());
55 | sim_->uiloadrequest.fetch_add(1);
56 | }
57 |
58 | MuJoCoMessageHandler::~MuJoCoMessageHandler() {
59 | RCLCPP_INFO(this->get_logger(), "close node ...");
60 | }
61 |
62 | void MuJoCoMessageHandler::reset_callback(
63 | const std::shared_ptr request,
64 | std::shared_ptr response) {
65 | while (sim_->d == nullptr && rclcpp::ok()) {
66 | std::this_thread::sleep_for(std::chrono::seconds(1));
67 | }
68 | if (sim_->d != nullptr) {
69 | if (request->header.frame_id != std::string(&sim_->m->names[0])) {
70 | RCLCPP_ERROR(this->get_logger(), "reset request is not for %s",
71 | &sim_->m->names[0]);
72 | response->is_success = false;
73 | } else {
74 | sim_->mtx.lock();
75 | mj_resetData(sim_->m, sim_->d);
76 | sim_->d->qpos[0] = request->base_pose.position.x;
77 | sim_->d->qpos[1] = request->base_pose.position.y;
78 | sim_->d->qpos[2] = request->base_pose.position.z;
79 | sim_->d->qpos[3] = request->base_pose.orientation.w;
80 | sim_->d->qpos[4] = request->base_pose.orientation.x;
81 | sim_->d->qpos[5] = request->base_pose.orientation.y;
82 | sim_->d->qpos[6] = request->base_pose.orientation.z;
83 |
84 | for (int i = 0; i < request->joint_state.position.size(); i++) {
85 | int joint_id = mj_name2id(sim_->m, mjOBJ_JOINT,
86 | request->joint_state.name[i].c_str());
87 | if (joint_id > -1) {
88 | sim_->d->qpos[sim_->m->jnt_qposadr[joint_id]] =
89 | request->joint_state.position[i];
90 | } else {
91 | RCLCPP_WARN(this->get_logger(), "Request joint %s does not exist",
92 | request->joint_state.name[i].c_str());
93 | }
94 | }
95 | for (size_t k = 0; k < actuator_cmds_ptr_->actuators_name.size(); k++) {
96 | actuator_cmds_ptr_->kp[k] = 0;
97 | actuator_cmds_ptr_->pos[k] = 0;
98 | actuator_cmds_ptr_->kd[k] = 0;
99 | actuator_cmds_ptr_->vel[k] = 0;
100 | actuator_cmds_ptr_->torque[k] = 0.0;
101 | }
102 | sim_->mtx.unlock();
103 | response->is_success = true;
104 | RCLCPP_INFO(this->get_logger(), "reset robot state...");
105 | RCLCPP_INFO(this->get_logger(), "robot total mass: %f",
106 | sim_->m->body_subtreemass[0]);
107 | }
108 | } else {
109 | response->is_success = false;
110 | }
111 | }
112 |
113 | void MuJoCoMessageHandler::imu_callback() {
114 | if (sim_->d != nullptr) {
115 | auto message = sensor_msgs::msg::Imu();
116 | message.header.frame_id = &sim_->m->names[0];
117 | message.header.stamp = rclcpp::Clock().now();
118 | const std::lock_guard lock(sim_->mtx);
119 |
120 | for (int i = 0; i < sim_->m->nsensor; i++) {
121 | if (sim_->m->sensor_type[i] == mjtSensor::mjSENS_ACCELEROMETER) {
122 | message.linear_acceleration.x =
123 | sim_->d->sensordata[sim_->m->sensor_adr[i]];
124 | message.linear_acceleration.y =
125 | sim_->d->sensordata[sim_->m->sensor_adr[i] + 1];
126 | message.linear_acceleration.z =
127 | sim_->d->sensordata[sim_->m->sensor_adr[i] + 2];
128 | } else if (sim_->m->sensor_type[i] == mjtSensor::mjSENS_FRAMEQUAT) {
129 | message.orientation.w = sim_->d->sensordata[sim_->m->sensor_adr[i]];
130 | message.orientation.x = sim_->d->sensordata[sim_->m->sensor_adr[i] + 1];
131 | message.orientation.y = sim_->d->sensordata[sim_->m->sensor_adr[i] + 2];
132 | message.orientation.z = sim_->d->sensordata[sim_->m->sensor_adr[i] + 3];
133 | } else if (sim_->m->sensor_type[i] == mjtSensor::mjSENS_GYRO) {
134 | message.angular_velocity.x =
135 | sim_->d->sensordata[sim_->m->sensor_adr[i]];
136 | message.angular_velocity.y =
137 | sim_->d->sensordata[sim_->m->sensor_adr[i] + 1];
138 | message.angular_velocity.z =
139 | sim_->d->sensordata[sim_->m->sensor_adr[i] + 2];
140 | }
141 | }
142 | imu_publisher_->publish(message);
143 | }
144 | }
145 |
146 | void MuJoCoMessageHandler::odom_callback() {
147 | const std::lock_guard lock(sim_->mtx);
148 | if (sim_->d != nullptr) {
149 | auto message = nav_msgs::msg::Odometry();
150 | message.header.frame_id = &sim_->m->names[0];
151 | message.header.stamp = rclcpp::Clock().now();
152 | message.pose.pose.position.x = sim_->d->qpos[0];
153 | message.pose.pose.position.y = sim_->d->qpos[1];
154 | message.pose.pose.position.z = sim_->d->qpos[2];
155 | message.pose.pose.orientation.w = sim_->d->qpos[3];
156 | message.pose.pose.orientation.x = sim_->d->qpos[4];
157 | message.pose.pose.orientation.y = sim_->d->qpos[5];
158 | message.pose.pose.orientation.z = sim_->d->qpos[6];
159 | message.twist.twist.linear.x = sim_->d->qvel[0];
160 | message.twist.twist.linear.y = sim_->d->qvel[1];
161 | message.twist.twist.linear.z = sim_->d->qvel[2];
162 | message.twist.twist.angular.x = sim_->d->qvel[3];
163 | message.twist.twist.angular.y = sim_->d->qvel[4];
164 | message.twist.twist.angular.z = sim_->d->qvel[5];
165 | odom_publisher_->publish(message);
166 | }
167 | }
168 |
169 | void MuJoCoMessageHandler::joint_callback() {
170 | const std::lock_guard lock(sim_->mtx);
171 |
172 | if (sim_->d != nullptr) {
173 | sensor_msgs::msg::JointState jointState;
174 | jointState.header.frame_id = &sim_->m->names[0];
175 | jointState.header.stamp = rclcpp::Clock().now();
176 | for (int i = 0; i < sim_->m->njnt; i++) {
177 | if (sim_->m->jnt_type[i] == mjtJoint::mjJNT_HINGE) {
178 | std::string jnt_name(mj_id2name(sim_->m, mjtObj::mjOBJ_JOINT, i));
179 | jointState.name.emplace_back(jnt_name);
180 | jointState.position.push_back(sim_->d->qpos[sim_->m->jnt_qposadr[i]]);
181 | jointState.velocity.push_back(sim_->d->qvel[sim_->m->jnt_dofadr[i]]);
182 | jointState.effort.push_back(
183 | sim_->d->qfrc_actuator[sim_->m->jnt_dofadr[i]]);
184 | }
185 | }
186 | joint_state_publisher_->publish(jointState);
187 | }
188 | }
189 |
190 | void MuJoCoMessageHandler::actuator_cmd_callback(
191 | const communication::msg::ActuatorCmds::SharedPtr msg) const {
192 | if (sim_->d != nullptr) {
193 | actuator_cmds_ptr_->time = this->now().seconds();
194 | actuator_cmds_ptr_->actuators_name.resize(msg->actuators_name.size());
195 | actuator_cmds_ptr_->kp.resize(msg->kp.size());
196 | actuator_cmds_ptr_->pos.resize(msg->pos.size());
197 | actuator_cmds_ptr_->kd.resize(msg->kd.size());
198 | actuator_cmds_ptr_->vel.resize(msg->vel.size());
199 | actuator_cmds_ptr_->torque.resize(msg->torque.size());
200 | for (size_t k = 0; k < msg->actuators_name.size(); k++) {
201 | actuator_cmds_ptr_->actuators_name[k] = msg->actuators_name[k];
202 | actuator_cmds_ptr_->kp[k] = msg->kp[k];
203 | actuator_cmds_ptr_->pos[k] = msg->pos[k];
204 | actuator_cmds_ptr_->kd[k] = msg->kd[k];
205 | actuator_cmds_ptr_->vel[k] = msg->vel[k];
206 | actuator_cmds_ptr_->torque[k] = msg->torque[k];
207 | }
208 | // RCLCPP_INFO(this->get_logger(), "subscribe actuator cmds");
209 | }
210 | }
211 |
212 | void MuJoCoMessageHandler::parameter_callback(const rclcpp::Parameter &) {
213 | std::string model_file = this->get_parameter(model_param_name)
214 | .get_parameter_value()
215 | .get();
216 | RCLCPP_INFO(this->get_logger(), "load model from: %s", model_file.c_str());
217 | mju::strcpy_arr(sim_->filename, model_file.c_str());
218 | sim_->uiloadrequest.fetch_add(1);
219 | }
220 |
221 | void MuJoCoMessageHandler::drop_old_message() {
222 | if (abs(actuator_cmds_ptr_->time - this->now().seconds()) > 0.2) {
223 | for (size_t k = 0; k < actuator_cmds_ptr_->actuators_name.size(); k++) {
224 | actuator_cmds_ptr_->kp[k] = 0.0;
225 | actuator_cmds_ptr_->pos[k] = 0.0;
226 | actuator_cmds_ptr_->kd[k] = 1.0;
227 | actuator_cmds_ptr_->vel[k] = 0.0;
228 | actuator_cmds_ptr_->torque[k] = 0.0;
229 | }
230 | }
231 | }
232 |
233 | std::shared_ptr
234 | MuJoCoMessageHandler::get_actuator_cmds_ptr() {
235 | return actuator_cmds_ptr_;
236 | }
237 |
238 | } // namespace deepbreak
239 |
--------------------------------------------------------------------------------
/simulation/mujoco/src/glfw_adapter.cc:
--------------------------------------------------------------------------------
1 | // Copyright 2023 DeepMind Technologies Limited
2 | //
3 | // Licensed under the Apache License, Version 2.0 (the "License");
4 | // you may not use this file except in compliance with the License.
5 | // You may obtain a copy of the License at
6 | //
7 | // http://www.apache.org/licenses/LICENSE-2.0
8 | //
9 | // Unless required by applicable law or agreed to in writing, software
10 | // distributed under the License is distributed on an "AS IS" BASIS,
11 | // WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
12 | // See the License for the specific language governing permissions and
13 | // limitations under the License.
14 |
15 | #include "glfw_adapter.h"
16 |
17 | #include
18 | #include
19 |
20 | #include
21 | #include
22 | #include
23 | #include "glfw_dispatch.h"
24 |
25 | namespace mujoco {
26 | namespace {
27 | int MaybeGlfwInit() {
28 | static const int is_initialized = []() {
29 | auto success = Glfw().glfwInit();
30 | if (success == GLFW_TRUE) {
31 | std::atexit(Glfw().glfwTerminate);
32 | }
33 | return success;
34 | }();
35 | return is_initialized;
36 | }
37 |
38 | GlfwAdapter& GlfwAdapterFromWindow(GLFWwindow* window) {
39 | return *static_cast(Glfw().glfwGetWindowUserPointer(window));
40 | }
41 | } // namespace
42 |
43 | GlfwAdapter::GlfwAdapter() {
44 | if (MaybeGlfwInit() != GLFW_TRUE) {
45 | mju_error("could not initialize GLFW");
46 | }
47 |
48 | // multisampling
49 | Glfw().glfwWindowHint(GLFW_SAMPLES, 4);
50 | Glfw().glfwWindowHint(GLFW_VISIBLE, 1);
51 |
52 | // get video mode and save
53 | vidmode_ = *Glfw().glfwGetVideoMode(Glfw().glfwGetPrimaryMonitor());
54 |
55 | // create window
56 | window_ = Glfw().glfwCreateWindow((2 * vidmode_.width) / 3,
57 | (2 * vidmode_.height) / 3,
58 | "MuJoCo", nullptr, nullptr);
59 | if (!window_) {
60 | mju_error("could not create window");
61 | }
62 |
63 | // save window position and size
64 | Glfw().glfwGetWindowPos(window_, &window_pos_.first, &window_pos_.second);
65 | Glfw().glfwGetWindowSize(window_, &window_size_.first, &window_size_.second);
66 |
67 | // set callbacks
68 | Glfw().glfwSetWindowUserPointer(window_, this);
69 | Glfw().glfwSetDropCallback(
70 | window_, +[](GLFWwindow* window, int count, const char** paths) {
71 | GlfwAdapterFromWindow(window).OnFilesDrop(count, paths);
72 | });
73 | Glfw().glfwSetKeyCallback(
74 | window_, +[](GLFWwindow* window, int key, int scancode, int act, int mods) {
75 | GlfwAdapterFromWindow(window).OnKey(key, scancode, act);
76 | });
77 | Glfw().glfwSetMouseButtonCallback(
78 | window_, +[](GLFWwindow* window, int button, int act, int mods) {
79 | GlfwAdapterFromWindow(window).OnMouseButton(button, act);
80 | });
81 | Glfw().glfwSetCursorPosCallback(
82 | window_, +[](GLFWwindow* window, double x, double y) {
83 | GlfwAdapterFromWindow(window).OnMouseMove(x, y);
84 | });
85 | Glfw().glfwSetScrollCallback(
86 | window_, +[](GLFWwindow* window, double xoffset, double yoffset) {
87 | GlfwAdapterFromWindow(window).OnScroll(xoffset, yoffset);
88 | });
89 | Glfw().glfwSetWindowRefreshCallback(
90 | window_, +[](GLFWwindow* window) {
91 | GlfwAdapterFromWindow(window).OnWindowRefresh();
92 | });
93 | Glfw().glfwSetWindowSizeCallback(
94 | window_, +[](GLFWwindow* window, int width, int height) {
95 | GlfwAdapterFromWindow(window).OnWindowResize(width, height);
96 | });
97 |
98 | // make context current
99 | Glfw().glfwMakeContextCurrent(window_);
100 | }
101 |
102 | GlfwAdapter::~GlfwAdapter() {
103 | Glfw().glfwMakeContextCurrent(nullptr);
104 | Glfw().glfwDestroyWindow(window_);
105 | }
106 |
107 | std::pair GlfwAdapter::GetCursorPosition() const {
108 | double x, y;
109 | Glfw().glfwGetCursorPos(window_, &x, &y);
110 | return {x, y};
111 | }
112 |
113 | double GlfwAdapter::GetDisplayPixelsPerInch() const {
114 | int width_mm, height_mm;
115 | Glfw().glfwGetMonitorPhysicalSize(
116 | Glfw().glfwGetPrimaryMonitor(), &width_mm, &height_mm);
117 | return 25.4 * vidmode_.width / width_mm;
118 | }
119 |
120 | std::pair GlfwAdapter::GetFramebufferSize() const {
121 | int width, height;
122 | Glfw().glfwGetFramebufferSize(window_, &width, &height);
123 | return {width, height};
124 | }
125 |
126 | std::pair GlfwAdapter::GetWindowSize() const {
127 | int width, height;
128 | Glfw().glfwGetWindowSize(window_, &width, &height);
129 | return {width, height};
130 | }
131 |
132 | bool GlfwAdapter::IsGPUAccelerated() const {
133 | return true;
134 | }
135 |
136 | void GlfwAdapter::PollEvents() {
137 | Glfw().glfwPollEvents();
138 | }
139 |
140 | void GlfwAdapter::SetClipboardString(const char* text) {
141 | Glfw().glfwSetClipboardString(window_, text);
142 | }
143 |
144 | void GlfwAdapter::SetVSync(bool enabled){
145 | Glfw().glfwSwapInterval(enabled);
146 | }
147 |
148 | void GlfwAdapter::SetWindowTitle(const char* title) {
149 | Glfw().glfwSetWindowTitle(window_, title);
150 | }
151 |
152 | bool GlfwAdapter::ShouldCloseWindow() const {
153 | return Glfw().glfwWindowShouldClose(window_);
154 | }
155 |
156 | void GlfwAdapter::SwapBuffers() {
157 | Glfw().glfwSwapBuffers(window_);
158 | }
159 |
160 | void GlfwAdapter::ToggleFullscreen() {
161 | // currently full screen: switch to windowed
162 | if (Glfw().glfwGetWindowMonitor(window_)) {
163 | // restore window from saved data
164 | Glfw().glfwSetWindowMonitor(window_, nullptr, window_pos_.first, window_pos_.second,
165 | window_size_.first, window_size_.second, 0);
166 | }
167 |
168 | // currently windowed: switch to full screen
169 | else {
170 | // save window data
171 | Glfw().glfwGetWindowPos(window_, &window_pos_.first, &window_pos_.second);
172 | Glfw().glfwGetWindowSize(window_, &window_size_.first,
173 | &window_size_.second);
174 |
175 | // switch
176 | Glfw().glfwSetWindowMonitor(window_, Glfw().glfwGetPrimaryMonitor(), 0,
177 | 0, vidmode_.width, vidmode_.height,
178 | vidmode_.refreshRate);
179 | }
180 | }
181 |
182 | bool GlfwAdapter::IsLeftMouseButtonPressed() const {
183 | return Glfw().glfwGetMouseButton(window_, GLFW_MOUSE_BUTTON_LEFT) == GLFW_PRESS;
184 | }
185 |
186 | bool GlfwAdapter::IsMiddleMouseButtonPressed() const {
187 | return Glfw().glfwGetMouseButton(window_, GLFW_MOUSE_BUTTON_MIDDLE) == GLFW_PRESS;
188 | }
189 |
190 | bool GlfwAdapter::IsRightMouseButtonPressed() const {
191 | return Glfw().glfwGetMouseButton(window_, GLFW_MOUSE_BUTTON_RIGHT) == GLFW_PRESS;
192 | }
193 |
194 | bool GlfwAdapter::IsAltKeyPressed() const {
195 | return Glfw().glfwGetKey(window_, GLFW_KEY_LEFT_ALT) == GLFW_PRESS ||
196 | Glfw().glfwGetKey(window_, GLFW_KEY_RIGHT_ALT) == GLFW_PRESS;
197 | }
198 |
199 | bool GlfwAdapter::IsCtrlKeyPressed() const {
200 | return Glfw().glfwGetKey(window_, GLFW_KEY_LEFT_CONTROL) == GLFW_PRESS ||
201 | Glfw().glfwGetKey(window_, GLFW_KEY_RIGHT_CONTROL) == GLFW_PRESS;
202 | }
203 |
204 | bool GlfwAdapter::IsShiftKeyPressed() const {
205 | return Glfw().glfwGetKey(window_, GLFW_KEY_LEFT_SHIFT) == GLFW_PRESS ||
206 | Glfw().glfwGetKey(window_, GLFW_KEY_RIGHT_SHIFT) == GLFW_PRESS;
207 | }
208 |
209 | bool GlfwAdapter::IsMouseButtonDownEvent(int act) const {
210 | return act == GLFW_PRESS;
211 | }
212 |
213 | bool GlfwAdapter::IsKeyDownEvent(int act) const { return act == GLFW_PRESS; }
214 |
215 | int GlfwAdapter::TranslateKeyCode(int key) const { return key; }
216 |
217 | mjtButton GlfwAdapter::TranslateMouseButton(int button) const {
218 | if (button == GLFW_MOUSE_BUTTON_LEFT) {
219 | return mjBUTTON_LEFT;
220 | } else if (button == GLFW_MOUSE_BUTTON_RIGHT) {
221 | return mjBUTTON_RIGHT;
222 | } else if (button == GLFW_MOUSE_BUTTON_MIDDLE) {
223 | return mjBUTTON_MIDDLE;
224 | }
225 | return mjBUTTON_NONE;
226 | }
227 | } // namespace mujoco
228 |
--------------------------------------------------------------------------------
/simulation/mujoco/src/glfw_dispatch.cc:
--------------------------------------------------------------------------------
1 | // Copyright 2022 DeepMind Technologies Limited
2 | //
3 | // Licensed under the Apache License, Version 2.0 (the "License");
4 | // you may not use this file except in compliance with the License.
5 | // You may obtain a copy of the License at
6 | //
7 | // http://www.apache.org/licenses/LICENSE-2.0
8 | //
9 | // Unless required by applicable law or agreed to in writing, software
10 | // distributed under the License is distributed on an "AS IS" BASIS,
11 | // WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
12 | // See the License for the specific language governing permissions and
13 | // limitations under the License.
14 |
15 | #include "glfw_dispatch.h"
16 |
17 | #ifdef mjGLFW_DYNAMIC_SYMBOLS
18 | #ifdef _MSC_VER
19 | #include
20 | #include
21 | #else
22 | #include
23 | #endif
24 | #endif
25 |
26 | #include
27 | #include
28 |
29 | namespace mujoco {
30 |
31 | // return dispatch table for glfw functions
32 | const struct Glfw& Glfw(void* dlhandle) {
33 | {
34 | // set static init_dlhandle
35 | static const void* init_dlhandle = dlhandle;
36 |
37 | // check that not already initialized
38 | if (dlhandle && dlhandle != init_dlhandle) {
39 | std::cerr << "dlhandle is specified when GLFW dispatch table is already "
40 | "initialized\n";
41 | abort();
42 | }
43 | }
44 |
45 | // make and intialize dispatch table
46 | static const struct Glfw glfw = [&]() { // create and call constructor
47 | // allocate
48 | struct Glfw glfw;
49 |
50 | // load glfw dynamically
51 | #ifdef mjGLFW_DYNAMIC_SYMBOLS
52 | #ifdef _MSC_VER
53 | if (!dlhandle) dlhandle = LoadLibraryA("glfw3.dll");
54 | if (!dlhandle) {
55 | std::cerr << "cannot obtain a shared object handle\n";
56 | abort();
57 | }
58 | #define mjGLFW_RESOLVE_SYMBOL(func) \
59 | glfw.func = reinterpret_cast( \
60 | GetProcAddress(reinterpret_cast(dlhandle), #func))
61 | #else
62 | if (!dlhandle) dlhandle = dlopen("nullptr", RTLD_GLOBAL | RTLD_NOW);
63 | if (!dlhandle) {
64 | std::cerr << "cannot obtain a shared object handle\n";
65 | abort();
66 | }
67 | #define mjGLFW_RESOLVE_SYMBOL(func) \
68 | glfw.func = reinterpret_cast(dlsym(dlhandle, #func))
69 | #endif
70 | #else
71 | #define mjGLFW_RESOLVE_SYMBOL(func) glfw.func = &::func
72 | #endif
73 |
74 | // set pointers in dispatch table
75 | #define mjGLFW_INITIALIZE_SYMBOL(func) \
76 | if (!(mjGLFW_RESOLVE_SYMBOL(func))) { \
77 | std::cerr << "cannot dlsym " #func "\n"; \
78 | abort(); \
79 | }
80 |
81 | // go/keep-sorted start
82 | mjGLFW_INITIALIZE_SYMBOL(glfwCreateWindow);
83 | mjGLFW_INITIALIZE_SYMBOL(glfwDestroyWindow);
84 | mjGLFW_INITIALIZE_SYMBOL(glfwGetCursorPos);
85 | mjGLFW_INITIALIZE_SYMBOL(glfwGetFramebufferSize);
86 | mjGLFW_INITIALIZE_SYMBOL(glfwGetKey);
87 | mjGLFW_INITIALIZE_SYMBOL(glfwGetMonitorPhysicalSize);
88 | mjGLFW_INITIALIZE_SYMBOL(glfwGetMouseButton);
89 | mjGLFW_INITIALIZE_SYMBOL(glfwGetPrimaryMonitor);
90 | mjGLFW_INITIALIZE_SYMBOL(glfwGetTime);
91 | mjGLFW_INITIALIZE_SYMBOL(glfwGetVideoMode);
92 | mjGLFW_INITIALIZE_SYMBOL(glfwGetWindowMonitor);
93 | mjGLFW_INITIALIZE_SYMBOL(glfwGetWindowPos);
94 | mjGLFW_INITIALIZE_SYMBOL(glfwGetWindowSize);
95 | mjGLFW_INITIALIZE_SYMBOL(glfwGetWindowUserPointer);
96 | mjGLFW_INITIALIZE_SYMBOL(glfwInit);
97 | mjGLFW_INITIALIZE_SYMBOL(glfwMakeContextCurrent);
98 | mjGLFW_INITIALIZE_SYMBOL(glfwPollEvents);
99 | mjGLFW_INITIALIZE_SYMBOL(glfwSetClipboardString);
100 | mjGLFW_INITIALIZE_SYMBOL(glfwSetCursorPosCallback);
101 | mjGLFW_INITIALIZE_SYMBOL(glfwSetDropCallback);
102 | mjGLFW_INITIALIZE_SYMBOL(glfwSetKeyCallback);
103 | mjGLFW_INITIALIZE_SYMBOL(glfwSetMouseButtonCallback);
104 | mjGLFW_INITIALIZE_SYMBOL(glfwSetScrollCallback);
105 | mjGLFW_INITIALIZE_SYMBOL(glfwSetWindowMonitor);
106 | mjGLFW_INITIALIZE_SYMBOL(glfwSetWindowRefreshCallback);
107 | mjGLFW_INITIALIZE_SYMBOL(glfwSetWindowSizeCallback);
108 | mjGLFW_INITIALIZE_SYMBOL(glfwSetWindowTitle);
109 | mjGLFW_INITIALIZE_SYMBOL(glfwSetWindowUserPointer);
110 | mjGLFW_INITIALIZE_SYMBOL(glfwSwapBuffers);
111 | mjGLFW_INITIALIZE_SYMBOL(glfwSwapInterval);
112 | mjGLFW_INITIALIZE_SYMBOL(glfwTerminate);
113 | mjGLFW_INITIALIZE_SYMBOL(glfwWindowHint);
114 | mjGLFW_INITIALIZE_SYMBOL(glfwWindowShouldClose);
115 | // go/keep-sorted end
116 |
117 | #undef mjGLFW_INITIALIZE_SYMBOL
118 |
119 | return glfw;
120 | }();
121 | return glfw;
122 | }
123 | } // namespace mujoco
124 |
--------------------------------------------------------------------------------
/simulation/mujoco/src/platform_ui_adapter.cc:
--------------------------------------------------------------------------------
1 | // Copyright 2023 DeepMind Technologies Limited
2 | //
3 | // Licensed under the Apache License, Version 2.0 (the "License");
4 | // you may not use this file except in compliance with the License.
5 | // You may obtain a copy of the License at
6 | //
7 | // http://www.apache.org/licenses/LICENSE-2.0
8 | //
9 | // Unless required by applicable law or agreed to in writing, software
10 | // distributed under the License is distributed on an "AS IS" BASIS,
11 | // WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
12 | // See the License for the specific language governing permissions and
13 | // limitations under the License.
14 |
15 | #include "platform_ui_adapter.h"
16 |
17 | #include
18 |
19 | namespace mujoco {
20 | PlatformUIAdapter::PlatformUIAdapter() {
21 | mjr_defaultContext(&con_);
22 | }
23 |
24 | PlatformUIAdapter::~PlatformUIAdapter() {
25 | mjr_freeContext(&con_);
26 | }
27 |
28 | bool PlatformUIAdapter::RefreshMjrContext(const mjModel* m, int fontscale) {
29 | if (m != last_model_ || fontscale != last_fontscale_) {
30 | mjr_makeContext(m, &con_, fontscale);
31 | last_model_ = m;
32 | last_fontscale_ = fontscale;
33 | return true;
34 | }
35 | return false;
36 | }
37 |
38 | void PlatformUIAdapter::OnFilesDrop(int count, const char** paths) {
39 | state_.type = mjEVENT_FILESDROP;
40 | state_.dropcount = count;
41 | state_.droppaths = paths;
42 |
43 | // application-specific processing
44 | if (event_callback_) {
45 | event_callback_(&state_);
46 | }
47 |
48 | // remove paths pointer from mjuiState since we don't own it
49 | state_.dropcount = 0;
50 | state_.droppaths = nullptr;
51 | }
52 |
53 | void PlatformUIAdapter::OnKey(int key, int scancode, int act) {
54 | // translate API-specific key code
55 | int mj_key = TranslateKeyCode(key);
56 |
57 | // release: nothing to do
58 | if (!IsKeyDownEvent(act)) {
59 | return;
60 | }
61 |
62 | // update state
63 | UpdateMjuiState();
64 |
65 | // set key info
66 | state_.type = mjEVENT_KEY;
67 | state_.key = mj_key;
68 | state_.keytime = std::chrono::duration(
69 | std::chrono::steady_clock::now().time_since_epoch()).count();
70 |
71 | // application-specific processing
72 | if (event_callback_) {
73 | event_callback_(&state_);
74 | }
75 | }
76 |
77 | void PlatformUIAdapter::OnMouseButton(int button, int act) {
78 | // translate API-specific mouse button code
79 | mjtButton mj_button = TranslateMouseButton(button);
80 |
81 | // update state
82 | UpdateMjuiState();
83 |
84 | // swap left and right if Alt
85 | if (state_.alt) {
86 | if (mj_button == mjBUTTON_LEFT) {
87 | mj_button = mjBUTTON_RIGHT;
88 | } else if (mj_button == mjBUTTON_RIGHT) {
89 | mj_button = mjBUTTON_LEFT;
90 | }
91 | }
92 |
93 | // press
94 | if (IsMouseButtonDownEvent(act)) {
95 | double now = std::chrono::duration(
96 | std::chrono::steady_clock::now().time_since_epoch()).count();
97 |
98 | // detect doubleclick: 250 ms
99 | if (mj_button == state_.button && now - state_.buttontime < 0.25) {
100 | state_.doubleclick = 1;
101 | } else {
102 | state_.doubleclick = 0;
103 | }
104 |
105 | // set info
106 | state_.type = mjEVENT_PRESS;
107 | state_.button = mj_button;
108 | state_.buttontime = now;
109 |
110 | // start dragging
111 | if (state_.mouserect) {
112 | state_.dragbutton = state_.button;
113 | state_.dragrect = state_.mouserect;
114 | }
115 | }
116 |
117 | // release
118 | else {
119 | state_.type = mjEVENT_RELEASE;
120 | }
121 |
122 | // application-specific processing
123 | if (event_callback_) {
124 | event_callback_(&state_);
125 | }
126 |
127 | // stop dragging after application processing
128 | if (state_.type == mjEVENT_RELEASE) {
129 | state_.dragrect = 0;
130 | state_.dragbutton = 0;
131 | }
132 | }
133 |
134 | void PlatformUIAdapter::OnMouseMove(double x, double y) {
135 | // no buttons down: nothing to do
136 | if (!state_.left && !state_.right && !state_.middle) {
137 | return;
138 | }
139 |
140 | // update state
141 | UpdateMjuiState();
142 |
143 | // set move info
144 | state_.type = mjEVENT_MOVE;
145 |
146 | // application-specific processing
147 | if (event_callback_) {
148 | event_callback_(&state_);
149 | }
150 | }
151 |
152 | void PlatformUIAdapter::OnScroll(double xoffset, double yoffset) {
153 | // update state
154 | UpdateMjuiState();
155 |
156 | // set scroll info, scale by buffer-to-window ratio
157 | const double buffer_window_ratio =
158 | static_cast(GetFramebufferSize().first) / GetWindowSize().first;
159 | state_.type = mjEVENT_SCROLL;
160 | state_.sx = xoffset * buffer_window_ratio;
161 | state_.sy = yoffset * buffer_window_ratio;
162 |
163 | // application-specific processing
164 | if (event_callback_) {
165 | event_callback_(&state_);
166 | }
167 | }
168 |
169 | void PlatformUIAdapter::OnWindowRefresh() {
170 | state_.type = mjEVENT_REDRAW;
171 |
172 | // application-specific processing
173 | if (event_callback_) {
174 | event_callback_(&state_);
175 | }
176 | }
177 |
178 | void PlatformUIAdapter::OnWindowResize(int width, int height) {
179 | auto [buf_width, buf_height] = GetFramebufferSize();
180 | state_.rect[0].width = buf_width;
181 | state_.rect[0].height = buf_height;
182 | if (state_.nrect < 1) state_.nrect = 1;
183 |
184 | // update window layout
185 | if (layout_callback_) {
186 | layout_callback_(&state_);
187 | }
188 |
189 | // update state
190 | UpdateMjuiState();
191 |
192 | // set resize info
193 | state_.type = mjEVENT_RESIZE;
194 |
195 | // stop dragging
196 | state_.dragbutton = 0;
197 | state_.dragrect = 0;
198 |
199 | // application-specific processing
200 | if (event_callback_) {
201 | event_callback_(&state_);
202 | }
203 | }
204 |
205 | void PlatformUIAdapter::UpdateMjuiState() {
206 | // mouse buttons
207 | state_.left = IsLeftMouseButtonPressed();
208 | state_.right = IsRightMouseButtonPressed();
209 | state_.middle = IsMiddleMouseButtonPressed();
210 |
211 | // keyboard modifiers
212 | state_.control = IsCtrlKeyPressed();
213 | state_.shift = IsShiftKeyPressed();
214 | state_.alt = IsAltKeyPressed();
215 |
216 | // swap left and right if Alt
217 | if (state_.alt) {
218 | int tmp = state_.left;
219 | state_.left = state_.right;
220 | state_.right = tmp;
221 | }
222 |
223 | // get mouse position, scale by buffer-to-window ratio
224 | auto [x, y] = GetCursorPosition();
225 | const double buffer_window_ratio =
226 | static_cast(GetFramebufferSize().first) / GetWindowSize().first;
227 | x *= buffer_window_ratio;
228 | y *= buffer_window_ratio;
229 |
230 | // invert y to match OpenGL convention
231 | y = state_.rect[0].height - y;
232 |
233 | // save
234 | state_.dx = x - state_.x;
235 | state_.dy = y - state_.y;
236 | state_.x = x;
237 | state_.y = y;
238 |
239 | // find mouse rectangle
240 | state_.mouserect = mjr_findRect(mju_round(x), mju_round(y), state_.nrect-1, state_.rect+1) + 1;
241 | }
242 | } // namespace mujoco
243 |
--------------------------------------------------------------------------------