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
