from __future__ import division
import openravepy
import trajoptpy
import bulletsimpy
from lfd.rapprentice import animate_traj, ropesim
import numpy as np
from robot_world import RobotWorld
import sim_util
import settings
import importlib
[docs]class StaticSimulation(object):
def __init__(self, env=None):
if env is not None:
self.env = env
else:
self.env = openravepy.Environment()
self.env.StopSimulation()
self.sim_objs = []
self.robot = None
self.__viewer_cache = None
[docs] def add_objects(self, objs_to_add, consider_finger_collisions=True):
if consider_finger_collisions:
self._include_gripper_finger_collisions()
n_robots = len(self.env.GetRobots())
for obj_to_add in objs_to_add:
if obj_to_add.dynamic:
raise RuntimeError("Dynamic object can't be added to StaticSimulation")
else:
self.sim_objs.append(obj_to_add)
obj_to_add.add_to_env(self)
if len(self.env.GetRobots()) > n_robots:
if self.robot:
raise NotImplementedError("Behavior for adding more than one robot has not been defined")
self.robot = self.env.GetRobots()[-1]
if consider_finger_collisions:
self._exclude_gripper_finger_collisions()
[docs] def remove_objects(self, objs_to_remove, consider_finger_collisions=True):
if consider_finger_collisions:
self._include_gripper_finger_collisions()
for obj_to_remove in objs_to_remove:
if obj_to_remove.dynamic:
raise RuntimeError("Dynamic object can't be removed from StaticSimulation")
else:
self.sim_objs.remove(obj_to_remove)
obj_to_remove.remove_from_env()
if self.robot and self.robot not in self.env.GetRobots():
raise NotImplementedError("Behavior for removing robots has not been defined")
if consider_finger_collisions:
self._exclude_gripper_finger_collisions()
[docs] def get_state(self):
sim_objs_constructor_infos = [sim_obj._get_constructor_info() for sim_obj in self.sim_objs]
states = {}
for sim_obj in self.sim_objs:
state_key = "".join(sim_obj.names)
assert state_key not in states, "multiple sim_objs with same names"
states[state_key] = sim_obj.get_state()
states["dof_limits"] = np.asarray(self.robot.GetDOFLimits())
states["dof_values"] = self.robot.GetDOFValues()
sim_state = (sim_objs_constructor_infos, states)
return sim_state
[docs] def set_state(self, sim_state):
constr_infos, states = sim_state
cur_constr_infos = [sim_obj._get_constructor_info() for sim_obj in self.sim_objs]
constr_infos_to_remove = [constr_info for constr_info in cur_constr_infos if constr_info not in constr_infos]
constr_infos_to_add = [constr_info for constr_info in constr_infos if constr_info not in cur_constr_infos]
sim_objs_to_add = []
sim_objs_to_remove = []
for constr_info in constr_infos_to_remove:
sim_obj = self.sim_objs[cur_constr_infos.index(constr_info)]
sim_objs_to_remove.append(sim_obj)
for constr_info in constr_infos_to_add:
((class_name, class_module), args, kwargs) = constr_info
class_module = importlib.import_module(class_module)
c = getattr(class_module, class_name)
sim_objs_to_add.append(c(*args, **kwargs))
self.remove_objects(sim_objs_to_remove)
self.add_objects(sim_objs_to_add)
# the states should have one and only one state for every sim_obj and dof info
states_keys = ["".join(sim_obj.names) for sim_obj in self.sim_objs] + ["dof_limits", "dof_values"]
assert set(states_keys) == set(states.keys())
for sim_obj in self.sim_objs:
state_key = "".join(sim_obj.names)
sim_obj.set_state(states[state_key])
self.robot.SetDOFLimits(*states["dof_limits"])
self.robot.SetDOFValues(states["dof_values"])
@property
def viewer(self):
if not self.__viewer_cache and trajoptpy.ViewerExists(self.env):
self.__viewer_cache = trajoptpy.GetViewer(self.env)
return self.__viewer_cache
[docs] def create_viewer(self, window_prop=None, camera_matrix=None):
trajoptpy.GetViewer(self.env) # creates viewer
if window_prop is None:
window_prop = settings.WINDOW_PROP
self.viewer.SetWindowProp(*window_prop)
if camera_matrix is None:
camera_matrix = settings.CAMERA_MATRIX
self.viewer.SetCameraManipulatorMatrix(np.asarray(camera_matrix))
def _exclude_gripper_finger_collisions(self):
if not self.robot:
return
cc = trajoptpy.GetCollisionChecker(self.env)
for lr in 'lr':
for flr in 'lr':
finger_link_name = "%s_gripper_%s_finger_tip_link" % (lr, flr)
finger_link = self.robot.GetLink(finger_link_name)
for sim_obj in self.sim_objs:
for bt_obj in sim_obj.get_bullet_objects():
for link in bt_obj.GetKinBody().GetLinks():
cc.ExcludeCollisionPair(finger_link, link)
def _include_gripper_finger_collisions(self):
if not self.robot:
return
cc = trajoptpy.GetCollisionChecker(self.env)
for lr in 'lr':
for flr in 'lr':
finger_link_name = "%s_gripper_%s_finger_tip_link" % (lr, flr)
finger_link = self.robot.GetLink(finger_link_name)
for sim_obj in self.sim_objs:
for bt_obj in sim_obj.get_bullet_objects():
for link in bt_obj.GetKinBody().GetLinks():
cc.IncludeCollisionPair(finger_link, link)
@staticmethod
[docs] def simulation_state_equal(s0, s1):
if s0[0] != s1[0]:
return False
d0 = s0[1]
d1 = s1[1]
if not set(d0.keys()) == set(d1.keys()):
return False
for (k, v0) in d0.iteritems():
v1 = d1[k]
if not np.all(v0 == v1):
return False
return True
[docs]class DynamicSimulation(StaticSimulation):
def __init__(self, env=None):
super(DynamicSimulation, self).__init__(env=env)
self.dyn_sim_objs = []
self.bt_env = None
self.bt_robot = None
self.dyn_bt_objs = []
[docs] def add_objects(self, sim_objs):
static_sim_objs = [sim_obj for sim_obj in sim_objs if not sim_obj.dynamic]
dyn_sim_objs = [sim_obj for sim_obj in sim_objs if sim_obj.dynamic]
self._include_gripper_finger_collisions()
# add static objects
super(DynamicSimulation, self).add_objects(static_sim_objs, consider_finger_collisions=False)
# add dynamic objects
self._remove_bullet()
for sim_obj in dyn_sim_objs:
self.sim_objs.append(sim_obj)
self.dyn_sim_objs.append(sim_obj)
self._create_bullet()
self._exclude_gripper_finger_collisions()
[docs] def remove_objects(self, sim_objs):
static_sim_objs = [sim_obj for sim_obj in sim_objs if not sim_obj.dynamic]
dyn_sim_objs = [sim_obj for sim_obj in sim_objs if sim_obj.dynamic]
self._include_gripper_finger_collisions()
# remove static objects
super(DynamicSimulation, self).remove_objects(static_sim_objs, consider_finger_collisions=False)
# remove dynamic objects
self._remove_bullet()
for sim_obj in dyn_sim_objs:
self.sim_objs.remove(sim_obj)
self.dyn_sim_objs.remove(sim_obj)
self._create_bullet()
self._exclude_gripper_finger_collisions()
[docs] def set_state(self, sim_state):
"""
Defined such that execution1 and execution2 gives the same results
if execution1 == execution2 in the following code:
set_state(sim_state)
execution1()
set_state(sim_state)
execution2()
"""
self._include_gripper_finger_collisions()
self._remove_bullet()
self._create_bullet()
self._exclude_gripper_finger_collisions()
super(DynamicSimulation, self).set_state(sim_state)
self.update()
[docs] def update(self):
self.bt_robot.UpdateBullet()
self._update_rave()
[docs] def step(self):
self.bt_robot.UpdateBullet()
self.bt_env.Step(.01, 200, .005)
self._update_rave()
[docs] def get_translations(self):
"""return translation part of all links of all dynamic objects"""
return np.concatenate(
[np.asarray([link.GetTransform() for link in bt_obj.GetKinBody().GetLinks()])[:, :3, 3]
for bt_obj in self.dyn_bt_objs]
)
[docs] def settle(self, max_steps=100, tol=.001, step_viewer=1):
"""Keep stepping until the dynamic objects doesn't move, up to some tolerance"""
prev_trans = self.get_translations()
for i in range(max_steps):
self.bt_env.Step(.01, 200, .005)
self._update_rave()
if self.viewer and step_viewer != 0 and i % step_viewer == 0:
if self.viewer:
self.viewer.Step()
if i % 10 == 0 and i != 0:
curr_trans = self.get_translations()
diff = np.sqrt(((curr_trans - prev_trans)**2).sum(axis=1))
if diff.max() < tol:
break
prev_trans = curr_trans
self._update_rave()
if self.viewer and step_viewer != 0:
self.viewer.Step()
def _create_bullet(self):
# create bullet environment and dynamic objects in it
dyn_obj_names = []
for sim_obj in self.dyn_sim_objs:
if not sim_obj.add_after:
sim_obj.add_to_env(self)
dyn_obj_names.extend(sim_obj.names)
self.bt_env = bulletsimpy.BulletEnvironment(self.env, dyn_obj_names)
self.bt_env.SetGravity([0, 0, -9.8])
if self.robot:
self.bt_robot = self.bt_env.GetObjectByName(self.robot.GetName())
for sim_obj in self.dyn_sim_objs:
if sim_obj.add_after:
sim_obj.add_to_env(self)
for sim_obj in self.dyn_sim_objs:
self.dyn_bt_objs.extend(sim_obj.get_bullet_objects())
self._update_rave()
def _remove_bullet(self):
# remove bullet environment and dynamic objects in it
for sim_obj in self.dyn_sim_objs:
sim_obj.remove_from_env()
self.bt_env = None
self.bt_robot = None
self.dyn_bt_objs = []
def _update_rave(self):
for bt_obj in self.dyn_bt_objs:
bt_obj.UpdateRave()
self.env.UpdatePublishedBodies()
[docs]class DynamicSimulationRobotWorld(DynamicSimulation, RobotWorld):
def __init__(self, env=None, T_w_k=None, range_k=2.):
"""
T_w_k: world transform of the depth camera
range_k: length of the rays. 2 meters by default
"""
self.constraints = {"l": [], "r": []}
self.constraints_links = {"l": [], "r": []}
super(DynamicSimulationRobotWorld, self).__init__(env=env)
self.T_w_k = T_w_k
self.range_k = range_k
[docs] def observe_cloud(self):
if self.T_w_k is None:
if self.robot is None:
raise RuntimeError("Can't observe cloud when there is no robot")
else:
from lfd.rapprentice import berkeley_pr2
self.T_w_k = berkeley_pr2.get_kinect_transform(self.robot)
# camera's parameters
cx = 320.-.5
cy = 240.-.5
f = 525. # focal length
w = 640.
h = 480.
pixel_ij = np.array(np.meshgrid(np.arange(w), np.arange(h))).T.reshape((-1, 2)) # all pixel positions
rays_to = self.range_k * np.c_[(pixel_ij - np.array([cx, cy])) / f, np.ones(pixel_ij.shape[0])]
rays_from = np.zeros_like(rays_to)
# transform the rays from the camera frame to the world frame
rays_to = rays_to.dot(self.T_w_k[:3,:3].T) + self.T_w_k[:3,3]
rays_from = rays_from.dot(self.T_w_k[:3,:3].T) + self.T_w_k[:3,3]
cloud = []
for sim_obj in self.dyn_sim_objs:
for bt_obj in sim_obj.get_bullet_objects():
ray_collisions = self.bt_env.RayTest(rays_from, rays_to, bt_obj)
pts = np.empty((len(ray_collisions), 3))
for i, ray_collision in enumerate(ray_collisions):
pts[i, :] = ray_collision.pt
cloud.append(pts)
cloud = np.concatenate(cloud)
# hack to filter out point below the top of the table. TODO: fix this hack
table_sim_objs = [sim_obj for sim_obj in self.sim_objs if "table" in sim_obj.names]
assert len(table_sim_objs) == 1
table_sim_obj = table_sim_objs[0]
table_height = table_sim_obj.translation[2] + table_sim_obj.extents[2]
cloud = cloud[cloud[:, 2] > table_height, :]
return cloud
[docs] def open_gripper(self, lr, target_val=None, step_viewer=1, max_vel=.02):
self._remove_constraints(lr)
# generate gripper finger trajectory
joint_ind = self.robot.GetJoint("%s_gripper_l_finger_joint" % lr).GetDOFIndex()
start_val = self.robot.GetDOFValues([joint_ind])[0]
if target_val is None:
target_val = sim_util.get_binary_gripper_angle(True)
joint_traj = np.linspace(start_val, target_val, np.ceil(abs(target_val - start_val) / max_vel))
# execute gripper finger trajectory
for val in joint_traj:
self.robot.SetDOFValues([val], [joint_ind])
self.step()
if self.viewer and step_viewer:
self.viewer.Step()
[docs] def close_gripper(self, lr, step_viewer=1, max_vel=.02, close_dist_thresh=0.004, grab_dist_thresh=0.005):
print 'CLOSING GRIPPER'
# generate gripper finger trajectory
joint_ind = self.robot.GetJoint("%s_gripper_l_finger_joint" % lr).GetDOFIndex()
start_val = self.robot.GetDOFValues([joint_ind])[0]
print 'start_val: ', start_val
# execute gripper finger trajectory
dyn_bt_objs = [bt_obj for sim_obj in self.dyn_sim_objs for bt_obj in sim_obj.get_bullet_objects()]
next_val = start_val
while next_val:
flr2finger_pts_grid = self._get_finger_pts_grid(lr)
ray_froms, ray_tos = flr2finger_pts_grid['l'], flr2finger_pts_grid['r']
# stop closing if any ray hits a dynamic object within a distance of close_dist_thresh from both sides
next_vel = max_vel
for bt_obj in dyn_bt_objs:
from_to_ray_collisions = self.bt_env.RayTest(ray_froms, ray_tos, bt_obj)
to_from_ray_collisions = self.bt_env.RayTest(ray_tos, ray_froms, bt_obj)
rays_dists = np.inf * np.ones((len(ray_froms), 2))
for rc in from_to_ray_collisions:
ray_id = np.argmin(np.apply_along_axis(np.linalg.norm, 1, ray_froms - rc.rayFrom))
rays_dists[ray_id, 0] = np.linalg.norm(rc.pt - rc.rayFrom)
for rc in to_from_ray_collisions:
ray_id = np.argmin(np.apply_along_axis(np.linalg.norm, 1, ray_tos - rc.rayFrom))
rays_dists[ray_id, 1] = np.linalg.norm(rc.pt - rc.rayFrom)
colliding_rays_inds = np.logical_and(rays_dists[:, 0] != np.inf, rays_dists[:, 1] != np.inf)
if np.any(colliding_rays_inds):
rays_dists = rays_dists[colliding_rays_inds, :]
if np.any(np.logical_and(rays_dists[:, 0] < close_dist_thresh,
rays_dists[:, 1] < close_dist_thresh)):
next_vel = 0
else:
next_vel = np.minimum(next_vel, np.min(rays_dists.sum(axis=1)))
if next_vel == 0:
break
next_val = np.maximum(next_val - next_vel, 0)
self.robot.SetDOFValues([next_val], [joint_ind])
self.step()
if self.viewer and step_viewer:
self.viewer.Step()
handles = []
# add constraints at the points where a ray hits a dynamic link within a distance of grab_dist_thresh
for bt_obj in dyn_bt_objs:
from_to_ray_collisions = self.bt_env.RayTest(ray_froms, ray_tos, bt_obj)
to_from_ray_collisions = self.bt_env.RayTest(ray_tos, ray_froms, bt_obj)
ray_collisions = [rc for rcs in [from_to_ray_collisions, to_from_ray_collisions] for rc in rcs]
for rc in ray_collisions:
if np.linalg.norm(rc.pt - rc.rayFrom) < grab_dist_thresh:
link_tf = rc.link.GetTransform()
link_tf[:3, 3] = rc.pt
self._add_constraints(lr, rc.link, link_tf)
if self.viewer and step_viewer:
self.viewer.Step()
[docs] def execute_trajectory(self, full_traj, step_viewer=1, interactive=False,
max_cart_vel_trans_traj=.05, sim_callback=None):
# TODO: incorporate other parts of sim_full_traj_maybesim
if sim_callback is None:
sim_callback = lambda i: self.step()
traj, dof_inds = full_traj
# # clip finger joint angles to the binary gripper angles if necessary
# for lr in 'lr':
# joint_ind = self.robot.GetJoint("%s_gripper_l_finger_joint"%lr).GetDOFIndex()
# if joint_ind in dof_inds:
# ind = dof_inds.index(joint_ind)
# traj[:,ind] = np.minimum(traj[:,ind], get_binary_gripper_angle(True))
# traj[:,ind] = np.maximum(traj[:,ind], get_binary_gripper_angle(False))
# in simulation mode, we must make sure to gradually move to the new starting position
self.robot.SetActiveDOFs(dof_inds)
curr_vals = self.robot.GetActiveDOFValues()
transition_traj = np.r_[[curr_vals], [traj[0]]]
sim_util.unwrap_in_place(transition_traj, dof_inds=dof_inds)
transition_traj = ropesim.retime_traj(self.robot, dof_inds, transition_traj,
max_cart_vel=max_cart_vel_trans_traj)
animate_traj.animate_traj(transition_traj, self.robot, restore=False, pause=interactive,
callback=sim_callback, step_viewer=step_viewer if self.viewer else 0)
traj[0] = transition_traj[-1]
sim_util.unwrap_in_place(traj, dof_inds=dof_inds)
traj = ropesim.retime_traj(self.robot, dof_inds, traj) # make the trajectory slow enough for the simulation
animate_traj.animate_traj(traj, self.robot, restore=False, pause=interactive,
callback=sim_callback, step_viewer=step_viewer if self.viewer else 0)
if self.viewer and step_viewer:
self.viewer.Step()
return True
def _create_bullet(self):
for lr in 'lr':
if self.constraints[lr] or self.constraints_links[lr]:
raise RuntimeError("Bullet environment can't be removed while the robot is grasping an object")
super(DynamicSimulationRobotWorld, self)._create_bullet()
def _remove_bullet(self):
for lr in 'lr':
if self.constraints[lr] or self.constraints_links[lr]:
raise RuntimeError("Bullet environment can't be removed while the robot is grasping an object")
super(DynamicSimulationRobotWorld, self)._remove_bullet()
def _get_finger_pts_grid(self, lr, min_sample_dist=0.005):
sample_grid = None
flr2finger_pts_grid = {}
for finger_lr in 'lr':
world_from_finger = self.robot.GetLink("%s_gripper_%s_finger_tip_link" % (lr, finger_lr)).GetTransform()
finger_pts = world_from_finger[:3, 3] \
+ sim_util.get_finger_rel_pts(finger_lr).dot(world_from_finger[:3, :3].T)
pt0 = finger_pts[0 if finger_lr == 'l' else 3][None, :]
pt1 = finger_pts[1 if finger_lr == 'l' else 2][None, :]
pt3 = finger_pts[3 if finger_lr == 'l' else 0][None, :]
if sample_grid is None:
num_sample_01 = np.round(np.linalg.norm(pt1 - pt0)/min_sample_dist)
num_sample_03 = np.round(np.linalg.norm(pt3 - pt0)/min_sample_dist)
sample_grid = np.array(np.meshgrid(np.linspace(0, 1, num_sample_01),
np.linspace(0, 1, num_sample_03))).T.reshape((-1, 2))
flr2finger_pts_grid[finger_lr] = pt0 + sample_grid[:, 0][:, None].dot(pt1 - pt0) \
+ sample_grid[:, 1][:, None].dot(pt3 - pt0)
return flr2finger_pts_grid
def _remove_constraints(self, lr, grab_link=None):
"""
If grab_link is None, remove all constraints that attaches the lr gripper,
else remove all constraints that attaches between the lr gripper and grab_link
"""
num_links_removed = 0
for (cnt, link) in zip(self.constraints[lr], self.constraints_links[lr]):
if grab_link is None or link == grab_link:
self.bt_env.RemoveConstraint(cnt)
num_links_removed += 1
# TODO: provide option to color the contrained links and save color before overriding it
for link in self.constraints_links[lr]:
if grab_link is None or link == grab_link:
for geom in link.GetGeometries():
geom.SetDiffuseColor([1., 1., 1.])
if grab_link is None:
self.constraints[lr] = []
self.constraints_links[lr] = []
else:
if grab_link in self.constraints_links[lr]:
constraints_links_pairs = zip(*[(cnt, link) for (cnt, link)
in zip(self.constraints[lr], self.constraints_links[lr])
if link != grab_link])
if constraints_links_pairs:
constraints, constraints_links = constraints_links_pairs
self.constraints[lr] = list(constraints)
self.constraints_links[lr] = list(constraints_links)
else:
self.constraints[lr] = []
self.constraints_links[lr] = []
def _add_constraints(self, lr, grab_link, grab_tf=None):
if grab_tf is None:
grab_tf = grab_link.GetTransform()
# TODO: provide option to color the contrained links and save color before overriding it
for geom in grab_link.GetGeometries():
geom.SetDiffuseColor([1.,0.,0.])
for flr in 'lr':
robot_link = self.robot.GetLink("%s_gripper_%s_finger_tip_link" % (lr, flr))
cnt = self.bt_env.AddConstraint({
"type": "generic6dof",
"params": {
"link_a": robot_link,
"link_b": grab_link,
"frame_in_a": np.linalg.inv(robot_link.GetTransform()).dot(grab_tf),
"frame_in_b": np.linalg.inv(grab_link.GetTransform()).dot(grab_tf),
"use_linear_reference_frame_a": False,
"stop_erp": .8,
"stop_cfm": .1,
"disable_collision_between_linked_bodies": True,
}
})
self.constraints[lr].append(cnt)
self.constraints_links[lr].append(grab_link)
[docs]class DynamicRopeSimulationRobotWorld(DynamicSimulationRobotWorld):
[docs] def in_grasp_region(self,robot, lr, pt):
tol = .00
manip_name = {"l": "leftarm", "r": "rightarm"}[lr]
manip = robot.GetManipulator(manip_name)
l_finger = robot.GetLink("%s_gripper_l_finger_tip_link"%lr)
r_finger = robot.GetLink("%s_gripper_r_finger_tip_link"%lr)
def transform(hmat, p):
return hmat[:3,:3].dot(p) + hmat[:3,3]
def on_inner_side(pt, finger_lr):
finger = l_finger
closing_dir = np.cross(manip.GetLocalToolDirection(), [-1, 0, 0])
local_inner_pt = np.array([0.234402, -0.299, 0])/20.
if finger_lr == "r":
finger = r_finger
closing_dir *= -1
local_inner_pt[1] *= -1
inner_pt = transform(finger.GetTransform(), local_inner_pt)
return manip.GetTransform()[:3,:3].dot(closing_dir).dot(pt - inner_pt) > 0
# check that pt is behind the gripper tip
pt_local = transform(np.linalg.inv(manip.GetTransform()), pt)
if pt_local[2] > .03 + tol:
return False
# check that pt is within the finger width
if abs(pt_local[0]) > .01 + tol:
return False
# check that pt is between the fingers
if not on_inner_side(pt, "l") or not on_inner_side(pt, "r"):
return False
return True
[docs] def close_gripper(self, lr, step_viewer=1, max_vel=.02, close_dist_thresh=0.004, grab_dist_thresh=0.005):
print 'CLOSING GRIPPER'
# generate gripper finger trajectory
joint_ind = self.robot.GetJoint("%s_gripper_l_finger_joint"%lr).GetDOFIndex()
start_val = self.robot.GetDOFValues([joint_ind])[0]
print 'start_val: ', start_val
# execute gripper finger trajectory
dyn_bt_objs = [bt_obj for sim_obj in self.dyn_sim_objs for bt_obj in sim_obj.get_bullet_objects()]
next_val = start_val
while next_val:
flr2finger_pts_grid = self._get_finger_pts_grid(lr)
ray_froms, ray_tos = flr2finger_pts_grid['l'], flr2finger_pts_grid['r']
# stop closing if any ray hits a dynamic object within a distance of close_dist_thresh from both sides
next_vel = max_vel
for bt_obj in dyn_bt_objs:
from_to_ray_collisions = self.bt_env.RayTest(ray_froms, ray_tos, bt_obj)
to_from_ray_collisions = self.bt_env.RayTest(ray_tos, ray_froms, bt_obj)
rays_dists = np.inf * np.ones((len(ray_froms), 2))
for rc in from_to_ray_collisions:
ray_id = np.argmin(np.apply_along_axis(np.linalg.norm, 1, ray_froms - rc.rayFrom))
rays_dists[ray_id,0] = np.linalg.norm(rc.pt - rc.rayFrom)
for rc in to_from_ray_collisions:
ray_id = np.argmin(np.apply_along_axis(np.linalg.norm, 1, ray_tos - rc.rayFrom))
rays_dists[ray_id,1] = np.linalg.norm(rc.pt - rc.rayFrom)
colliding_rays_inds = np.logical_and(rays_dists[:,0] != np.inf, rays_dists[:,1] != np.inf)
if np.any(colliding_rays_inds):
rays_dists = rays_dists[colliding_rays_inds,:]
if np.any(np.logical_and(rays_dists[:,0] < close_dist_thresh, rays_dists[:,1] < close_dist_thresh)):
next_vel = 0
else:
next_vel = np.minimum(next_vel, np.min(rays_dists.sum(axis=1)))
if next_vel == 0:
break
next_val = np.maximum(next_val - next_vel, 0)
self.robot.SetDOFValues([next_val], [joint_ind])
self.step()
if self.viewer and step_viewer:
self.viewer.Step()
rope = [bt_obj for sim_obj in self.dyn_sim_objs for bt_obj in sim_obj.get_bullet_objects()][0]
nodes, ctl_pts = rope.GetNodes(), rope.GetControlPoints()
graspable_nodes = np.array([self.in_grasp_region(self.robot, lr, n) for n in nodes])
graspable_ctl_pts = np.array([self.in_grasp_region(self.robot, lr, n) for n in ctl_pts])
graspable_inds = np.flatnonzero(np.logical_or(graspable_nodes, np.logical_or(graspable_ctl_pts[:-1], graspable_ctl_pts[1:])))
print 'graspable inds for %s: %s' % (lr, str(graspable_inds))
if len(graspable_inds) == 0:
return False
robot_link = self.robot.GetLink("%s_gripper_l_finger_tip_link"%lr)
rope_links = rope.GetKinBody().GetLinks()
for i_node in graspable_inds:
i_cnt = i_node
for geom in rope_links[i_cnt].GetGeometries():
geom.SetDiffuseColor([1.,0.,0.])
link = rope_links[i_cnt]
self._add_constraints(lr, link, link.GetTransform())
if step_viewer and self.viewer:
self.viewer.Step()
return True
[docs] def release_rope(self, lr):
# print 'RELEASE: %s (%d constraints)' % (lr, len(self.constraints[lr]))
for c in self.constraints[lr]:
self.bt_env.RemoveConstraint(c)
rope_links = self.rope.GetKinBody().GetLinks()
for link in self.constraints_links[lr]:
for geom in link.GetGeometries():
geom.SetDiffuseColor([1.,1.,1.])
self.constraints[lr] = []
self.constraints_links[lr] = []