(self)
| 221 | |
| 222 | # Prints out the result of executing the action |
| 223 | def reset(self): |
| 224 | self.moving = True |
| 225 | for obj in self.scene.get_known_object_names(): |
| 226 | if "inst" in obj: |
| 227 | print("removing ", obj) |
| 228 | self.scene.remove_world_object(obj) |
| 229 | reset_pos = [-0.08251470748374336, 0.19599197531792156, 0.841849711995378, -1.7190730213700678, -0.6383234506829698, 1.3664373589356658, 0.030431373252636857] |
| 230 | # reset_pos = [-0.0745181431487993, 0.4494850737236856, 0.8270193549708322, -1.5205520947895879, -0.8831002754171688, 1.3287292592525481, 0.00643488985824300] |
| 231 | # reset_pos = [-0.5861821080702139, 0.055900042107092915, -0.3281157743728023, -1.826453942497152, 0.7512797036630254, 1.6094823167995767, 2.091976654118447] |
| 232 | #reset_pos = [-0.5878410830476827, -0.28344825294979825, 1.1282720037594176, -1.874332442399111, -0.31171058199803037, 1.8769036887578303, -0.5659639980552924] |
| 233 | group = self.group |
| 234 | group.set_goal_tolerance(0.01) |
| 235 | group.set_planner_id("RRTConnect") |
| 236 | group.set_max_velocity_scaling_factor(0.4) |
| 237 | group.set_max_acceleration_scaling_factor(0.4) |
| 238 | # pose_goal.position.x = 0.4 |
| 239 | group.set_joint_value_target(reset_pos) |
| 240 | #self.go_to_pose_goal([-8.66025404e-01, -2.65143810e-17, 1.53080850e-17, -5.00000000e-01], [0.15 + 35 , -0.16160254, 0.47009619]) |
| 241 | #return |
| 242 | ## Now, we call the planner to compute the plan and execute it. |
| 243 | plan = group.go(wait=True) |
| 244 | # print("moved?") |
| 245 | # Calling `stop()` ensures that there is no residual movement |
| 246 | group.stop() |
| 247 | # It is always good to clear your targets after planning with poses. |
| 248 | # Note: there is no equivalent function for clear_joint_value_targets() |
| 249 | group.clear_pose_targets() |
| 250 | # sleeping until read |
| 251 | print("sleeping from reset") |
| 252 | time.sleep(2) |
| 253 | self.moving = False |
| 254 | |
| 255 | |
| 256 | def go_to_pose_goal(self, orientation, position): |
no test coverage detected