|
| 1 | +# Copyright 2023 The dm_control Authors. |
| 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 | + |
| 16 | +"""Make torque actuators for the dog model.""" |
| 17 | + |
1 | 18 | import collections |
2 | | -from dm_control import mjcf |
| 19 | + |
3 | 20 |
|
4 | 21 | def add_motors(physics, model, lumbar_joints, cervical_joints, caudal_joints): |
5 | | - #physics = mjcf.Physics.from_mjcf_model(model) |
6 | | - # Fixed Tendons: |
7 | | - spinal_joints = collections.OrderedDict() |
8 | | - spinal_joints['lumbar_'] = lumbar_joints |
9 | | - spinal_joints['cervical_'] = cervical_joints |
10 | | - spinal_joints['caudal_'] = caudal_joints |
11 | | - tendons = [] |
12 | | - for region in spinal_joints.keys(): |
13 | | - for direction in ['extend', 'bend', 'twist']: |
14 | | - joints = [ |
15 | | - joint for joint in spinal_joints[region] if direction in joint.name |
16 | | - ] |
17 | | - if joints: |
18 | | - tendon = model.tendon.add( |
19 | | - 'fixed', name=region + direction, dclass=joints[0].dclass) |
20 | | - tendons.append(tendon) |
21 | | - joint_inertia = physics.bind(joints).M0 |
22 | | - coefs = joint_inertia ** .25 |
23 | | - coefs /= coefs.sum() |
24 | | - coefs *= len(joints) |
25 | | - for i, joint in enumerate(joints): |
26 | | - tendon.add('joint', joint=joint, coef=coefs[i]) |
27 | | - |
28 | | - # Actuators: |
29 | | - all_spinal_joints = [ |
30 | | - joint for region in spinal_joints.values() for joint in region # pylint: disable=g-complex-comprehension |
31 | | - ] |
32 | | - root_joint = model.find('joint', 'root') |
33 | | - actuated_joints = [ |
34 | | - joint for joint in model.find_all('joint') |
35 | | - if joint not in all_spinal_joints and joint is not root_joint |
36 | | - ] |
37 | | - for tendon in tendons: |
38 | | - gain = 0. |
39 | | - for joint in tendon.joint: |
40 | | - # joint.joint.user = physics.bind(joint.joint).damping |
41 | | - def_joint = model.default.find('default', joint.joint.dclass) |
42 | | - j_gain = def_joint.general.gainprm or def_joint.parent.general.gainprm |
43 | | - gain += j_gain[0] * joint.coef |
44 | | - gain /= len(tendon.joint) |
45 | | - |
46 | | - model.actuator.add( |
47 | | - 'general', tendon=tendon, name=tendon.name, dclass=tendon.dclass) |
48 | | - |
49 | | - for joint in actuated_joints: |
50 | | - model.actuator.add( |
51 | | - 'general', joint=joint, name=joint.name, dclass=joint.dclass) |
52 | | - |
53 | | - return actuated_joints |
| 22 | + """Add torque motors in model. |
| 23 | +
|
| 24 | + Args: |
| 25 | + physics: an instance of physics for the most updated model. |
| 26 | + model: model in which we want to add motors. |
| 27 | + lumbar_joints: a list of joints objects. |
| 28 | + cervical_joints: a list of joints objects. |
| 29 | + caudal_joints: a list of joints objects. |
| 30 | + """ |
| 31 | + # Fixed Tendons: |
| 32 | + spinal_joints = collections.OrderedDict() |
| 33 | + spinal_joints['lumbar_'] = lumbar_joints |
| 34 | + spinal_joints['cervical_'] = cervical_joints |
| 35 | + spinal_joints['caudal_'] = caudal_joints |
| 36 | + tendons = [] |
| 37 | + for region in spinal_joints.keys(): |
| 38 | + for direction in ['extend', 'bend', 'twist']: |
| 39 | + joints = [ |
| 40 | + joint for joint in spinal_joints[region] if direction in joint.name |
| 41 | + ] |
| 42 | + if joints: |
| 43 | + tendon = model.tendon.add( |
| 44 | + 'fixed', name=region + direction, dclass=joints[0].dclass) |
| 45 | + tendons.append(tendon) |
| 46 | + joint_inertia = physics.bind(joints).M0 |
| 47 | + coefs = joint_inertia ** .25 |
| 48 | + coefs /= coefs.sum() |
| 49 | + coefs *= len(joints) |
| 50 | + for i, joint in enumerate(joints): |
| 51 | + tendon.add('joint', joint=joint, coef=coefs[i]) |
| 52 | + |
| 53 | + # Actuators: |
| 54 | + all_spinal_joints = [ |
| 55 | + joint for region in spinal_joints.values() for joint in region |
| 56 | + # pylint: disable=g-complex-comprehension |
| 57 | + ] |
| 58 | + root_joint = model.find('joint', 'root') |
| 59 | + actuated_joints = [ |
| 60 | + joint for joint in model.find_all('joint') |
| 61 | + if joint not in all_spinal_joints and joint is not root_joint |
| 62 | + ] |
| 63 | + for tendon in tendons: |
| 64 | + gain = 0. |
| 65 | + for joint in tendon.joint: |
| 66 | + # joint.joint.user = physics.bind(joint.joint).damping |
| 67 | + def_joint = model.default.find('default', joint.joint.dclass) |
| 68 | + j_gain = def_joint.general.gainprm or def_joint.parent.general.gainprm |
| 69 | + gain += j_gain[0] * joint.coef |
| 70 | + gain /= len(tendon.joint) |
| 71 | + |
| 72 | + model.actuator.add( |
| 73 | + 'general', tendon=tendon, name=tendon.name, dclass=tendon.dclass) |
| 74 | + |
| 75 | + for joint in actuated_joints: |
| 76 | + model.actuator.add( |
| 77 | + 'general', joint=joint, name=joint.name, dclass=joint.dclass) |
| 78 | + |
| 79 | + return actuated_joints |
0 commit comments