(self)
| 153 | self.initialize() |
| 154 | |
| 155 | def initialize(self): |
| 156 | self.rest_config = self.atlas.getPositions() |
| 157 | |
| 158 | self.legs = [] |
| 159 | for i in range(self.atlas.getNumDofs()): |
| 160 | dof_name = self.atlas.getDof(i).getName() |
| 161 | if len(dof_name) > 6 and dof_name[1:6] == "_leg_": |
| 162 | self.legs.append(self.atlas.getDof(i).getIndexInSkeleton()) |
| 163 | |
| 164 | self.legs.append(self.atlas.getDof("rootJoint_rot_x").getIndexInSkeleton()) |
| 165 | self.legs.append(self.atlas.getDof("rootJoint_rot_y").getIndexInSkeleton()) |
| 166 | self.legs.append(self.atlas.getDof("rootJoint_pos_z").getIndexInSkeleton()) |
| 167 | |
| 168 | self.default_bounds = [] |
| 169 | self.default_target_tf = [] |
| 170 | self.constraint_active = [] |
| 171 | self.end_effector_index = [] |
| 172 | |
| 173 | for i in range(self.atlas.getNumEndEffectors()): |
| 174 | ee = self.atlas.getEndEffector(i) |
| 175 | ik = ee.getIK() |
| 176 | if ik: |
| 177 | self.default_bounds.append(ik.getErrorMethod().getBounds()) |
| 178 | self.default_target_tf.append(ik.getTarget().getRelativeTransform()) |
| 179 | self.constraint_active.append(False) |
| 180 | self.end_effector_index.append(ee.getIndexInSkeleton()) |
| 181 | |
| 182 | self.posture = self.atlas.getIK(True).getObjective() |
| 183 | self.balance = self.atlas.getIK(True).getProblem().getEqConstraint(1) |
| 184 | |
| 185 | self.optimization_key = ord('r') |
| 186 | self.move_components = [False] * TeleoperationWorld.NUM_MOVE |
| 187 | |
| 188 | def handle(self, ea, aa): |
| 189 | if self.atlas is None: |
no test coverage detected