class RRTUtils(): def __init__(self): self.ik = IK() self.plan_env = gym.make('PandaReacher-v0', shelf=True) self.plan_env.reset() self.p = self.plan_env.robot._p self.drive_res = 100 def getConfig(self): config = self.p.getJointStates(0, range(7)) config = np.array([e[0] for e in config]) return config def setConfig(self, cfg): for j in range(7): self.p.resetJointState(0, j, cfg[j], 0) self.p.setJointMotorControlArray(bodyIndex=0, jointIndices=range(7), controlMode=pybullet.POSITION_CONTROL, targetPositions=cfg, positionGains=[0.5] * 7, velocityGains=[1.0] * 7) def drive_to_position(self, from_joint_config, to_joint_config): waypoints = np.linspace(from_joint_config, to_joint_config, self.drive_res) self.setConfig(from_joint_config) for waypoint in waypoints: self.p.setJointMotorControlArray( bodyIndex=0, jointIndices=range(7), controlMode=pybullet.POSITION_CONTROL, targetPositions=waypoint, positionGains=[0.5] * 7, velocityGains=[1.0] * 7) self.p.stepSimulation() end_config = self.getConfig() if not self.in_collision(end_config): return end_config for u in np.arange(0.01, 1, 0.01): target_config = (1 - u) * end_config + u * np.array(from_joint_config) if not self.in_collision(target_config): return target_config raise Exception('not returning', target_loc) def in_collision(self, config): self.setConfig(config) self.p.stepSimulation() contacts = self.p.getContactPoints() for c in contacts: _, bodyA, bodyB, linkA, linkB, posA, posB, norm, dist, nf, _, _, _, _ = c if dist < 0: return True return False def in_free(self, config): return not self.in_collision(config) def target_loc_to_joint_config(self, target_loc): flag, cfg, initial_cfg = self.ik.solve(target_loc) self.plan_env.reset(target_loc=target_loc) cfg = self.drive_to_position(initial_cfg, cfg) assert not self.in_collision(cfg), target_loc return cfg