def pybullet_path_checking(start, end):
'''
:param start: starting coordinate in real world
:return: True:no collision with object in path
False: collide with object in path
'''
pos = end
dz = 2
dense_of_ray = 1 #describ how dense the ray for collision detection
_, _, _, hit_position, _ = p.rayTest(
start, [start[0], start[1], start[2] - dz])[0]
height = start[2] - hit_position[2]
start_pos_array = [[
start[0], start[1], hit_position[2] + i * height / dense_of_ray
] for i in range(1, dense_of_ray + 1)]
_, _, _, end_hit_position, _ = p.rayTest(pos,
[pos[0], pos[1], pos[2] - dz])[0]
end_height = pos[2] - end_hit_position[2]
end_pos_array = [[
pos[0], pos[1], end_hit_position[2] + i * end_height / dense_of_ray
] for i in range(1, dense_of_ray + 1)]
result = p.rayTestBatch(start_pos_array, end_pos_array)
for r in result:
if r[1] == -1:
#no collision
continue
else:
return False
return True
def activateNailgun(nailGunID,
object1ID,
parentRefCOM,
childRefCOM,
childFrameOrn=[0, 0, 0]):
#check to see if they're touching
gunContact = p.getContactPoints(nailGunID, object1ID)
if not len(gunContact):
print('No Contact')
return False
#get rid of the nasty contact points-just use COM of nailgun + some z component to get pos, and just use straight up as orn
#use ray tracing to determine locations of constrain
gunPos, _ = p.getBasePositionAndOrientation(nailGunID)
rayPos = np.array(gunPos) + np.array([0, 0, 0.165])
rayOrn = np.array([0, 0, 1])
rayDist = 0.25
#rayOrn = np.array([0,0,1])
#Check and see if the ray intersects both objects
rayTest = p.rayTest(rayPos, rayPos + rayDist * rayOrn)[0]
hitID = rayTest[0]
hitPos = np.array(rayTest[3])
#Add constrain
constraintID = p.createConstraint(object1ID,
-1,
hitID,
-1,
p.JOINT_FIXED,
hitPos,
parentRefCOM,
childRefCOM,
childFrameOrientation=childFrameOrn)
print("Finished")
print(p.getConstraintInfo(constraintID))
return constraintID
def read_sensors(body_idx, directions=[], range=2, draw_rays=True):
# measure the distance along rays cast into the directions specified in the array dirs from body with index
# body_idx. The ray's length can be modified with the range argument. If directions is empty, six rays are cast
# into the x-, x+, y-, y+, z-, z+ directions from the body's center of mass in local body coordinates
if not directions:
directions = [[-1, 0, 0], [1, 0, 0], [0, -1, 0], [0, 1, 0], [0, 0, -1],
[0, 0, 1]]
sensor_data = []
pos, orn = p.getBasePositionAndOrientation(body_idx)
for direction in directions:
local_dir = p.rotateVector(orn, direction)
ray_to = [
pos[0] + range * local_dir[0], pos[1] + range * local_dir[1],
pos[2] + range * local_dir[2]
]
ray_info = p.rayTest(pos, ray_to)
sensor_data.append(ray_info[0][2] * range)
if draw_rays:
if ray_info[0][0] > 0:
draw_to = ray_info[0][3]
else:
draw_to = ray_to
p.addUserDebugLine(pos, draw_to, [0, 1, 0], 1, 0.1)
return sensor_data
def get_hit(self, x, y):
"""
Shoot a ray through pixel location (x, y) and returns the position and
normal that this ray hits
:param x: image pixel x coordinate
:param y: image pixel y coordinate
"""
camera_pose = np.array([self.px, self.py, self.pz])
self.renderer.set_camera(camera_pose,
camera_pose + self.view_direction, self.up)
position_cam = np.array([
(x - self.renderer.width / 2) / float(self.renderer.width / 2) *
np.tan(self.renderer.horizontal_fov / 2.0 / 180.0 * np.pi),
-(y - self.renderer.height / 2) / float(self.renderer.height / 2) *
np.tan(self.renderer.vertical_fov / 2.0 / 180.0 * np.pi), -1, 1
])
position_cam[:3] *= 5
position_world = np.linalg.inv(self.renderer.V).dot(position_cam)
position_eye = camera_pose
res = p.rayTest(position_eye, position_world[:3])
hit_pos = None
hit_normal = None
if len(res) > 0 and res[0][0] != -1:
object_id, link_id, _, hit_pos, hit_normal = res[0]
return hit_pos, hit_normal
def on_mouse_button(self, window, button, action, mods):
if mods == 0:
x, y = glfw.get_cursor_pos(window)
x, y = x / WIDTH, y / HEIGHT
if action == glfw.PRESS:
if button == glfw.MOUSE_BUTTON_LEFT:
self.is_drag = True
self.drag_prepos = vec2(x, y)
elif button == glfw.MOUSE_BUTTON_RIGHT:
pass
elif action == glfw.RELEASE:
if button == glfw.MOUSE_BUTTON_LEFT:
self.is_drag = False
self.drag_prepos = vec2(x, y)
elif button == glfw.MOUSE_BUTTON_RIGHT:
ray_from = self.camera_pos
camdir = (self.perspective * self.view *
vec4(ray_from, 1.0)).xyz
ray_to = self.camera_pos + camdir * 1000.0
ray = Ray(self.gl, ray_from, ray_to)
self.debug_scene.append(ray)
rayquery = pb.rayTest([*self.camera_pos], [*ray_to])[0]
objid, linkid, fract, pos, norm = rayquery
if objid > 0:
print("OBJID FOUND", objid, linkid, fract, pos, norm)
def ray_collision(ray):
# TODO: be careful to disable gravity and set static masses for everything
step_simulation() # Needed for some reason
start, end = ray
result, = p.rayTest(start, end, physicsClientId=CLIENT)
# TODO: assign hit_position to be the end?
return RayResult(*result)
def apply_push_force(self, x, y, force):
"""
Apply pushing force to a 3D point. Given a pixel location (x, y),
compute the 3D location of that point, and then apply a virtual force
of a given magnitude towards the negative surface normal at that point
:param x: image pixel x coordinate
:param y: image pixel y coordinate
:param force: force magnitude
"""
camera_pose = np.array([self.px, self.py, self.pz])
self.renderer.set_camera(camera_pose,
camera_pose + self.view_direction, self.up)
position_cam = np.array([
(x - self.renderer.width / 2) / float(self.renderer.width / 2) *
np.tan(self.renderer.horizontal_fov / 2.0 / 180.0 * np.pi),
-(y - self.renderer.height / 2) / float(self.renderer.height / 2) *
np.tan(self.renderer.vertical_fov / 2.0 / 180.0 * np.pi), -1, 1
])
position_cam[:3] *= 5
position_world = np.linalg.inv(self.renderer.V).dot(position_cam)
position_eye = camera_pose
res = p.rayTest(position_eye, position_world[:3])
if len(res) > 0 and res[0][0] != -1:
# there is hit
object_id, link_id, _, hit_pos, hit_normal = res[0]
p.changeDynamics(object_id,
-1,
activationState=p.ACTIVATION_STATE_WAKE_UP)
p.applyExternalForce(object_id, link_id,
-np.array(hit_normal) * force, hit_pos,
p.WORLD_FRAME)
def single_test(urdf_file, isDIRECT=True):
p.connect(p.DIRECT if isDIRECT else p.GUI)
p.setAdditionalSearchPath(pybullet_data.getDataPath())
p.loadURDF(urdf_file, [3, 3, 1])
numRays = 1024
rayLen = 13
info_lst = []
rayFrom_lst = []
rayTo_lst = []
for i in range(numRays):
rayFrom = [0, 0, 1]
rayTo = [
rayLen * math.sin(2. * math.pi * float(i) / numRays),
rayLen * math.cos(2. * math.pi * float(i) / numRays), 1
]
info = p.rayTest(rayFrom, rayTo)
info_lst.append(info[0])
p.addUserDebugLine(rayFrom, rayTo, [1, 0, 0])
rayFrom_lst.append(rayFrom)
rayTo_lst.append(rayTo)
info_lst_batch = p.rayTestBatch(rayFrom_lst, rayTo_lst)
collide_predicate = lambda info: info[0] != -1
info_filtered = list(filter(collide_predicate, info_lst))
info_batch_filtered = list(filter(collide_predicate, info_lst_batch))
p.disconnect()
return info_filtered, info_batch_filtered
def get_present_pos_with_robotheight(pos):
'''
:param coord: (x,y,z) the coord in real world
:return: the new adjusted pos
'''
_, _, _, hit_position, _ = p.rayTest(pos, [pos[0], pos[1], pos[2] - 2])[0]
return (hit_position[0], hit_position[1], hit_position[2] + ROBOT_HEIGHT)
def rayTest(rayFromPosition: Vec3, rayToPosition: Vec3, object_id=-1) -> Tuple[float, Vec3, Vec3]:
if object_id != -1:
rayFromPosition = translation.vec3_local_to_world_id(object_id, rayFromPosition)
rayToPosition = translation.vec3_local_to_world_id(object_id, rayToPosition)
_, _, hit_fraction, hit_position, hit_normal = p.rayTest(rayFromPosition.as_nwu(), rayToPosition.as_nwu())[0]
return hit_fraction, Vec3.from_nwu(hit_position), Vec3.from_nwu(hit_normal)
def check_proximity(self):
ee_pos = np.array(p.getLinkState(self.robot, self.tool)[0])
tool_pos = np.array(p.getLinkState(self.body, 0)[0])
vec = (tool_pos - ee_pos) / np.linalg.norm((tool_pos - ee_pos))
ee_targ = ee_pos + vec
ray_data = p.rayTest(ee_pos, ee_targ)[0]
obj, link, ray_frac = ray_data[0], ray_data[1], ray_data[2]
return obj, link, ray_frac
def get_object_uid(x, y):
"""(x,y)にあるオブジェクトのuidを返す
"""
rayFrom, rayTo = getRayFromTo(x, y)
rayInfo = p.rayTest(rayFrom, rayTo)
hit = rayInfo[0]
object_uid = hit[0]
return object_uid
def create_constraint(self, x, y, fixed=False):
"""
Create a constraint between the constraint marker and the object
at pixel location (x, y). This is used for human users' mouse
interaction with the objects in the scenes.
:param x: image pixel x coordinate
:param y: image pixel y coordinate
:param fixed: whether to create a fixed joint. Otherwise, it's a point2point joint.
"""
camera_pose = np.array([self.px, self.py, self.pz])
self.renderer.set_camera(camera_pose,
camera_pose + self.view_direction, self.up)
position_cam = np.array([
(x - self.renderer.width / 2) / float(self.renderer.width / 2) *
np.tan(self.renderer.horizontal_fov / 2.0 / 180.0 * np.pi),
-(y - self.renderer.height / 2) / float(self.renderer.height / 2) *
np.tan(self.renderer.vertical_fov / 2.0 / 180.0 * np.pi), -1, 1
])
position_cam[:3] *= 5
position_world = np.linalg.inv(self.renderer.V).dot(position_cam)
position_eye = camera_pose
res = p.rayTest(position_eye, position_world[:3])
if len(res) > 0 and res[0][0] != -1:
object_id, link_id, _, hit_pos, hit_normal = res[0]
p.changeDynamics(object_id,
-1,
activationState=p.ACTIVATION_STATE_WAKE_UP)
link_pos, link_orn = None, None
if link_id == -1:
link_pos, link_orn = p.getBasePositionAndOrientation(object_id)
else:
link_state = p.getLinkState(object_id, link_id)
link_pos, link_orn = link_state[:2]
child_frame_pos, child_frame_orn = \
p.multiplyTransforms(*p.invertTransform(link_pos, link_orn),
hit_pos,
[0, 0, 0, 1])
self.constraint_marker.set_position(hit_pos)
self.constraint_marker2.set_position(hit_pos)
self.dist = np.linalg.norm(np.array(hit_pos) - camera_pose)
cid = p.createConstraint(
parentBodyUniqueId=self.constraint_marker.body_id,
parentLinkIndex=-1,
childBodyUniqueId=object_id,
childLinkIndex=link_id,
jointType=[p.JOINT_POINT2POINT, p.JOINT_FIXED][fixed],
jointAxis=(0, 0, 0),
parentFramePosition=(0, 0, 0),
childFramePosition=child_frame_pos,
childFrameOrientation=child_frame_orn,
)
p.changeConstraint(cid, maxForce=100)
self.cid.append(cid)
self.interaction_x, self.interaction_y = x, y
def ray_test(self, from_pos, to_pos):
hit = p.rayTest(rayFromPosition=from_pos,
rayToPosition=to_pos,
physicsClientId=self._pid)
intersection = Intersection._make(*hit)
del hit # NOTE force garbage collection of pybullet objects
if intersection.id >= 0: # if intersection, replace bid with id
intersection = intersection._replace(
id=self._bid_to_body[intersection.id].id)
return intersection
def test_raycast(self, start_position, end_position):
"""
"""
ray_start = start_position.to_array().flatten()
ray_end = end_position.to_array().flatten()
ray_dist = euclidean(ray_start, ray_end)
result = p.rayTest(ray_start, ray_end)
if result is not None:
distance = ray_dist * result[0][2]
sim_id = result[0][0]
hit_object_id = self.reverse_entity_id_map[sim_id]
hit_object = self.nodes_map[hit_object_id]
return True, distance, hit_object
return False, None, None
def raytest_scatter(x_start, b_min, b_max, N):
w = (b_max - b_min) / N
pts = []
mat = np.zeros((N, N))
for i in range(N):
for j in range(N):
y = b_min[0] + i * w[0]
z = b_min[1] + j * w[1]
pos = np.array([x_start, y, z])
direction = np.array([1., 0., -0.])
res = pybullet.rayTest(pos, pos + direction)[0]
pts.append(np.array(res[3]))
mat[i, j] = res[2]
return np.array(pts), mat
def __fun(self, pos1, pos2, pos3, thetha):
"""Helper function to get distances"""
r = 7
x = [0 for i in range(8)]
t = [0 for i in range(8)]
dist = np.zeros(8)
for i in range(8):
x[i] = p.rayTest([pos1, pos2, pos3], [
pos1 + r * math.cos(math.pi * i / 4 + thetha),
pos2 + r * math.sin(math.pi * i / 4 + thetha), pos3
])
#print(x[i])
x[i] = x[i][0]
x[i] = list(x[i])
#print(x[i])
t[i] = list(x[i][3])
#print(t[i])
# p.addUserDebugLine([pos1+0.2*math.cos(thetha+math.pi/2),pos2,pos3],[pos1+r*math.cos(thetha+math.pi*i/4),pos2+r*math.sin(thetha+math.pi*i/4),pos3],[1,0,1])
if (x[i][0] == -1 or 0):
#p.addUserDebugLine([pos1,pos2,pos3+0.7],[pos1+r*math.cos(0+math.pi*i/6+thetha),pos2+r*math.sin(0+math.pi*i/6+thetha),pos3],[1,0,1])
dist[i] = 7
#print(dist[i])
else:
#p.addUserDebugLine([pos1,pos2,pos3+0.7],t[i],[1,0,0])
dist[i] = np.sqrt(
pow(t[i][0] - pos1, 2) + pow(t[i][1] - pos2, 2) +
pow(t[i][2] - pos3, 2))
dist[0] -= 0.16
dist[1] += 0.09
dist[7] += 0.09
dist[2] += 0.12
dist[6] += 0.13
dist[3] += 0.19
dist[5] += 0.19
dist[4] += 0.20
# dist[5]+=0.09
distance = [round(i, 3) for i in dist]
return distance
def simulator_get_observation(self,
robot_pos=False,
robot_orn=False,
width=720,
height=720,
far=100.0,
near=0.01):
if robot_pos == False or robot_orn == False:
return -1
offset_pos = [0, 0, 0.035]
offset_orn = p.getQuaternionFromEuler([0, 0, 0])
view_pos, view_orn = p.multiplyTransforms(robot_pos, robot_orn,
offset_pos, offset_orn)
target_pos, _ = p.multiplyTransforms(view_pos, view_orn, [0, 0, 5],
offset_orn)
# print(p.rayTest(view_pos, target_pos))
objectid, _, _, hitposition, _ = p.rayTest(view_pos, target_pos)[0]
if objectid == -1:
return INF
else:
return math.dist(view_pos, hitposition)
def get_mouse_events(self):
events = p.getMouseEvents(physicsClientId=self.sim.id)
for event in events:
if event[4] == 3:
_, x, y, _, _ = event
camera_params = p.getDebugVisualizerCamera(
physicsClientId=self.sim.id)
width = camera_params[0]
height = camera_params[1]
aspect = width / height
pos_view = torch.tensor([
1.0, aspect * (1.0 - (x / width) * 2),
1.0 - (y / height) * 2
])
camera_forward = torch.tensor(camera_params[5])
camera_left = -torch.tensor(camera_params[6])
camera_left /= camera_left.norm()
camera_up = torch.tensor(camera_params[7])
camera_up /= camera_up.norm()
R = torch.stack([camera_forward, camera_left, camera_up],
dim=1)
camera_target = torch.tensor(camera_params[-1])
target_dist = camera_params[-2]
camera_pos_world = camera_target - target_dist * camera_forward
pos_world = R @ pos_view + camera_pos_world
vec = pos_world - camera_pos_world
vec = vec / vec.norm()
ray = p.rayTest(rayFromPosition=camera_pos_world.tolist(),
rayToPosition=(100.0 * vec +
camera_pos_world).tolist(),
physicsClientId=self.sim.id)
hit_pos = torch.tensor(ray[0][3])
results = {}
results['camera_pos'] = camera_pos_world
results['target_pos'] = torch.tensor(ray[0][3])
results['target_obj'] = self.object_dict.get(ray[0][0], None)
results['target_normal'] = torch.tensor(ray[0][4])
return results
def get_reading(self):
'''
Find distance to nearest object in line-of-sight between [min_range, max_range] of th sensor.
Returns np.infty if none, otherwise returns distance in simulator units.
'''
from_pos = np.array(
p.multiplyTransforms(self.get_position(), self.get_orientation(),
[self._min_range, 0, 0], [0, 0, 0, 1])[0])
to_pos = np.array(
p.multiplyTransforms(self.get_position(), self.get_orientation(),
[self._max_range, 0, 0], [0, 0, 0, 1])[0])
# rayTest returns ((obj_id, link_index, percentage along ray of first intersection, hit position, hit normal vector))
hit = p.rayTest(from_pos, to_pos)[0]
if self._debug_mode is True:
sensor_direction = np.array(
p.multiplyTransforms(from_pos, self.get_orientation(),
[0.01, 0, 0], [0, 0, 0, 1])[0])
ray_color = [0, 0, 1]
if hit[0] == -1: ray_color = [0, 1, 0]
p.addUserDebugLine(from_pos,
to_pos,
ray_color,
lineWidth=3,
lifeTime=0.1)
self._sensor_debug_id = p.addUserDebugLine(
from_pos,
sensor_direction, [0, 0, 0],
lineWidth=10,
replaceItemUniqueId=self._sensor_debug_id)
if hit[0] == -1: return np.infty
return self._range * hit[
2] + self._min_range # hit[2] is the % along the ray (from min to max range away from the sensor)
def compute_p_W_and_n_W(self, phi_B):
"""
Given a planar angle in the object frame B, perform ray cast from the
origin of the B frame and find the corresponding surface normal and
ray impact position in the world frame W.
FIXME(wualbert): We don't care about z as we are in 2D.
This may cause problems later.
@param phi_B: The yaw of the contact point in the B frame.
@return: (p_W, n_W). p_W is the 3D contact point on the object.
n_W is the 3D outward-facing unit normal vector at the contact point.
Both of these are in the world frame.
"""
# Get the pose of the object frame origin in world frame
p_B_W, q_B_W = p.getBasePositionAndOrientation(self.body_id)
e_B_W = p.getEulerFromQuaternion(q_B_W)
# In 2D, only the yaw should be nonzero
np.testing.assert_allclose(e_B_W[:2], np.zeros(2), atol=1e-4)
# add phi_B
e_phi_W = e_B_W + np.asarray([0., 0., phi_B])
e_phi_W %= 2 * np.pi
q_phi_W = p.getQuaternionFromEuler(e_phi_W)
# Construct the unit ray vector
# Compute the ray to cast
ray_direction_W = np.dot(
np.array(p.getMatrixFromQuaternion(q_phi_W)).reshape(3, 3),
np.asarray([1., 0., 0.]))
# FIXME(wualbert): the ray shouldn't be of arbitrary length
ray_start_W = p_B_W + ray_direction_W * 10.
ray_end_W = p_B_W - ray_direction_W * 10.
ans = p.rayTest(ray_start_W, ray_end_W)
for a in ans:
if a[0] == self.body_id:
# FIXME(wualbert): a cleaner way to do this
# We only care about the normal of this particular object
return a[3:]
raise AssertionError
(-rangey / 2 + j) * meshScale[1] * 2, 1],
useMaximalCoordinates=True)
p.changeVisualShape(bodyUid, -1, textureUniqueId=texUid)
p.configureDebugVisualizer(p.COV_ENABLE_RENDERING, 1)
p.stopStateLogging(logId)
p.setGravity(0, 0, -10)
p.setRealTimeSimulation(1)
colors = [[1, 0, 0, 1], [0, 1, 0, 1], [0, 0, 1, 1], [1, 1, 1, 1]]
currentColor = 0
while (1):
p.getCameraImage(320, 200)
mouseEvents = p.getMouseEvents()
for e in mouseEvents:
if ((e[0] == 2) and (e[3] == 0) and (e[4] & p.KEY_WAS_TRIGGERED)):
mouseX = e[1]
mouseY = e[2]
rayFrom, rayTo = getRayFromTo(mouseX, mouseY)
rayInfo = p.rayTest(rayFrom, rayTo)
#p.addUserDebugLine(rayFrom,rayTo,[1,0,0],3)
for l in range(len(rayInfo)):
hit = rayInfo[l]
objectUid = hit[0]
if (objectUid >= 1):
#p.removeBody(objectUid)
p.changeVisualShape(objectUid, -1, rgbaColor=colors[currentColor])
currentColor += 1
if (currentColor >= len(colors)):
currentColor = 0
def main():
p.setRealTimeSimulation(1)
p.setGravity(0, 0, 0) # we don't use gravity yet
# debug colors only for show
colors = [[1, 0, 0, 1], [0, 1, 0, 1], [0, 0, 1, 1], [1, 1, 1, 1],
[0, 0, 0, 1]]
currentColor = 0
try:
print("open SpaceMouse")
print(spacenavigator.list_devices())
success = spacenavigator.open()
body = p.getBodyUniqueId(mantis_robot)
EndEffectorIndex = p.getNumJoints(body) - 1
print("")
print("EndEffectorIndex %i" % EndEffectorIndex)
# i = 0
# for i in range(0, p.getNumJoints(body)):
# n = p.getJointInfo(body,i)
# alpha robot orientierung
# orn = p.getQuaternionFromEuler([math.pi/2,0,math.pi/2])
# p.resetBasePositionAndOrientation(body, [0, 0, 2 ], orn)
tx = 0
ty = 5
tz = 3
tj = 0
tp = 0
tr = math.pi / 2
orn = p.getQuaternionFromEuler([tj, tp, tr])
jointT = p.calculateInverseKinematics(body,
EndEffectorIndex, [tx, ty, tz],
orn,
jointDamping=jd)
p.setJointMotorControlArray(body, [1, 2, 3, 4, 5, 6],
controlMode=p.POSITION_CONTROL,
targetPositions=jointT)
print("x:%f y:%f z:%f j:%f p:%f r:%f " % (tx, ty, tz, tj, tp, tr))
print(jointT)
p.resetBasePositionAndOrientation(target, [tx, ty, tz], orn)
loop = 0
hasPrevPose = 0
trailDuration = 15
distance = 5
yaw = 90
motorDir = [1, -1, 1, 1, 1, 1]
while (p.isConnected()):
time.sleep(1. / 240.) # set to 40 fps
p.stepSimulation()
# targetVelocity = p.readUserDebugParameter(targetVelocitySlider)
if success:
state = spacenavigator.read()
# print(state)
tx += state[1] * 0.01
ty += state[2] * 0.01
tz += state[3] * 0.01
# tj += state[4]*0.005
tp += state[5] * 0.005
tr += state[6] * -0.005
# print("x:%f y:%f z:%f j:%f p:%f r:%f " % (state[1],state[2],state[3],state[4],state[5],state[6]))
# orn = p.getQuaternionFromEuler([tj,tp,tr])
orn = p.getQuaternionFromEuler([tj, tp, tr])
jointT = p.calculateInverseKinematics(body, EndEffectorIndex,
[tx, ty, tz], orn)
p.setJointMotorControlArray(body, [1, 2, 3, 4, 5, 6],
controlMode=p.POSITION_CONTROL,
targetPositions=jointT)
# print("x:%f y:%f z:%f j:%f p:%f r:%f " % (tx,ty,tz,tj,tp,tr))
p.resetBasePositionAndOrientation(target, [tx, ty, tz], orn)
targetPos, targetOrn = p.getBasePositionAndOrientation(target)
# p.resetDebugVisualizerCamera(distance, yaw, 20, targetPos)
loop += 1
if loop > 10:
loop = 0
data_string = ""
for i in range(1, EndEffectorIndex):
jt = p.getJointState(body, i)
data_string += "motor%i:%i\r\n" % (
i, scale(motorDir[i - 1] * jt[0]))
transfer_socket.sendto(bytes(data_string, "utf-8"),
(transfer_ip, transfer_port))
ls = p.getLinkState(body, EndEffectorIndex)
targetPos, targetOrn = p.getBasePositionAndOrientation(target)
if (hasPrevPose):
p.addUserDebugLine(prevPose, targetPos, [0, 0, 0.3], 1,
trailDuration)
p.addUserDebugLine(prevPose1, ls[4], [1, 0, 0], 1,
trailDuration)
prevPose = targetPos
prevPose1 = ls[4]
hasPrevPose = 1
# get mouse events
mouseEvents = p.getMouseEvents()
for e in mouseEvents:
if ((e[0] == 2) and (e[3] == 0)
and (e[4] & p.KEY_WAS_TRIGGERED)):
mouseX = e[1]
mouseY = e[2]
rayFrom, rayTo = getRayFromTo(mouseX, mouseY)
rayInfo = p.rayTest(rayFrom, rayTo)
for l in range(len(rayInfo)):
hit = rayInfo[l]
objectUid = hit[0]
jointUid = hit[1]
if (objectUid >= 1):
# this is for debug click on an object to get id
# oject will change color this has no effect
# changing color real time seems to slow
print("obj %i joint %i" % (objectUid, jointUid))
# n = p.getJointInfo(objectUid,jointUid)
n = p.getJointState(objectUid, jointUid)
print(n)
p.changeVisualShape(objectUid,
jointUid,
rgbaColor=colors[currentColor])
currentColor += 1
if (currentColor >= len(colors)):
currentColor = 0
except KeyboardInterrupt:
print("KeyboardInterrupt has been caught.")
finally:
endProgramm()
def render_physics_gifs(main_urdf_file_and_offset):
step_per_sec = 100
s = Simulator(mode='headless',
image_width=512,
image_height=512,
physics_timestep=1 / float(step_per_sec))
root_dir = '/cvgl2/u/chengshu/ig_dataset_v5/objects'
obj_count = 0
for i, obj_class_dir in enumerate(sorted(os.listdir(root_dir))):
obj_class = obj_class_dir
# if obj_class not in SELECTED_CLASSES:
# continue
obj_class_dir = os.path.join(root_dir, obj_class_dir)
for obj_inst_dir in os.listdir(obj_class_dir):
# if obj_inst_dir != '14402':
# continue
imgs = []
scene = EmptyScene()
s.import_scene(scene, render_floor_plane=True)
obj_inst_name = obj_inst_dir
# urdf_path = obj_inst_name + '.urdf'
obj_inst_dir = os.path.join(obj_class_dir, obj_inst_dir)
urdf_path, offset = main_urdf_file_and_offset[obj_inst_dir]
# urdf_path = os.path.join(obj_inst_dir, urdf_path)
# print('urdf_path', urdf_path)
obj = ArticulatedObject(urdf_path)
s.import_object(obj)
push_visual_marker = VisualMarker(radius=0.1)
s.import_object(push_visual_marker)
push_visual_marker.set_position([100, 100, 0.0])
z = stable_z_on_aabb(obj.body_id, [[0, 0, 0], [0, 0, 0]])
# offset is translation from the bounding box center to the base link frame origin
# need to add another offset that is the translation from the base link frame origin to the inertial frame origin
# p.resetBasePositionAndOrientation() sets the inertial frame origin instead of the base link frame origin
# Assuming the bounding box center is at (0, 0, z) where z is half of the extent in z-direction
base_link_to_inertial = p.getDynamicsInfo(obj.body_id, -1)[3]
obj.set_position([
offset[0] + base_link_to_inertial[0],
offset[1] + base_link_to_inertial[1], z
])
center, extent = get_center_extent(obj.body_id)
# the bounding box center should be at (0, 0) on the xy plane and the bottom of the bounding box should touch the ground
if not (np.linalg.norm(center[:2]) < 1e-3
and np.abs(center[2] - extent[2] / 2.0) < 1e-3):
print('-' * 50)
print('WARNING:', urdf_path, 'xy error',
np.linalg.norm(center[:2]), 'z error',
np.abs(center[2] - extent[2] / 2.0))
height = extent[2]
max_half_extent = max(extent[0], extent[1]) / 2.0
px = max_half_extent * 2
py = max_half_extent * 2
pz = extent[2] * 1.2
camera_pose = np.array([px, py, pz])
# camera_pose = np.array([0.01, 0.01, 3])
s.renderer.set_camera(camera_pose, [0, 0, 0], [0, 0, 1])
# drop 1 second
for _ in range(step_per_sec):
s.step()
frame = s.renderer.render(modes=('rgb'))
imgs.append(
Image.fromarray(
(frame[0][:, :, :3] * 255).astype(np.uint8)))
ray_start = [max_half_extent * 1.5, max_half_extent * 1.5, 0]
ray_end = [-100.0, -100.0, 0]
unit_force = np.array([-1.0, -1.0, 0.0])
unit_force /= np.linalg.norm(unit_force)
force_mag = 100.0
ray_zs = [height * 0.5]
valid_ray_z = False
for trial in range(5):
for ray_z in ray_zs:
ray_start[2] = ray_z
ray_end[2] = ray_z
res = p.rayTest(ray_start, ray_end)
if res[0][0] == obj.body_id:
valid_ray_z = True
break
if valid_ray_z:
break
increment = 1 / (2**(trial + 1))
ray_zs = np.arange(increment / 2.0, 1.0, increment) * height
# push 4 seconds
for i in range(step_per_sec * 4):
res = p.rayTest(ray_start, ray_end)
object_id, link_id, _, hit_pos, hit_normal = res[0]
if object_id != obj.body_id:
break
push_visual_marker.set_position(hit_pos)
p.applyExternalForce(object_id, link_id,
unit_force * force_mag, hit_pos,
p.WORLD_FRAME)
s.step()
# print(hit_pos)
frame = s.renderer.render(modes=('rgb'))
rgb = frame[0][:, :, :3]
# add red border
border_width = 10
border_color = np.array([1.0, 0.0, 0.0])
rgb[:border_width, :, :] = border_color
rgb[-border_width:, :, :] = border_color
rgb[:, :border_width, :] = border_color
rgb[:, -border_width:, :] = border_color
imgs.append(Image.fromarray((rgb * 255).astype(np.uint8)))
gif_path = '{}/visualizations/{}_cm_physics_v3.gif'.format(
obj_inst_dir, obj_inst_name)
imgs = imgs[::4]
imgs[0].save(gif_path,
save_all=True,
append_images=imgs[1:],
optimize=True,
duration=40,
loop=0)
obj_count += 1
# print(obj_count, gif_path, len(imgs), valid_ray_z, ray_z)
print(obj_count, gif_path)
s.reload()
events = p.getVREvents()
for e in (events):
if e[CONTROLLER_ID] == uiControllerId:
p.resetBasePositionAndOrientation(uiCube, e[POSITION],
e[ORIENTATION])
pos = e[POSITION]
orn = e[ORIENTATION]
lineFrom = pos
mat = p.getMatrixFromQuaternion(orn)
dir = [mat[0], mat[3], mat[6]]
to = [
pos[0] + dir[0] * rayLen, pos[1] + dir[1] * rayLen,
pos[2] + dir[2] * rayLen
]
hit = p.rayTest(lineFrom, to)
oldRay = pointRay
color = [1, 1, 0]
width = 3
#pointRay = p.addUserDebugLine(lineFrom,to,color,width,lifeTime=1)
#if (oldRay>=0):
# p.removeUserDebugItem(oldRay)
if (hit):
#if (hit[0][0]>=0):
hitObjectUid = hit[0][0]
linkIndex = hit[0][1]
if (hitObjectUid >= 0):
objectInfo = str(hitObjectUid) + " Link Index=" + str(
linkIndex) + "\nBase Name:" + p.getBodyInfo(
hitObjectUid)[0].decode(
#keep the gripper centered/symmetric
b = p.getJointState(pr2_gripper, 2)[0]
p.setJointMotorControl2(pr2_gripper, 0, p.POSITION_CONTROL, targetPosition=b, force=3)
events = p.getVREvents()
for e in (events):
if e[CONTROLLER_ID] == uiControllerId:
p.resetBasePositionAndOrientation(uiCube, e[POSITION], e[ORIENTATION])
pos = e[POSITION]
orn = e[ORIENTATION]
lineFrom = pos
mat = p.getMatrixFromQuaternion(orn)
dir = [mat[0], mat[3], mat[6]]
to = [pos[0] + dir[0] * rayLen, pos[1] + dir[1] * rayLen, pos[2] + dir[2] * rayLen]
hit = p.rayTest(lineFrom, to)
oldRay = pointRay
color = [1, 1, 0]
width = 3
#pointRay = p.addUserDebugLine(lineFrom,to,color,width,lifeTime=1)
#if (oldRay>=0):
# p.removeUserDebugItem(oldRay)
if (hit):
#if (hit[0][0]>=0):
hitObjectUid = hit[0][0]
linkIndex = hit[0][1]
if (hitObjectUid >= 0):
objectInfo = str(hitObjectUid) + " Link Index=" + str(
linkIndex) + "\nBase Name:" + p.getBodyInfo(hitObjectUid)[0].decode(
) + "\nBody Info:" + p.getBodyInfo(hitObjectUid)[1].decode()
def place(self, robot_id: int):
# TODO(ycho): Consider exposing this parameter.
EPS = 1e-3
robot_pose = pb.getBasePositionAndOrientation(robot_id,
physicsClientId=self.sim_id)
old_pos = robot_pose[0]
old_rot = robot_pose[1]
old_z = old_pos[2]
floor_aabb = np.asarray(pb.getAABB(
self.env_ids_[0], -1, physicsClientId=self.sim_id), dtype=np.float32)
robot_aabb = debug_get_full_aabb(self.sim_id, robot_id)
robot_size = robot_aabb[1] - robot_aabb[0]
# NOTE(ycho): Shrink the sampled space by the robot radius.
pos_min = floor_aabb[0, :2] + 0.5 * robot_size[:2]
pos_max = floor_aabb[1, :2] - 0.5 * robot_size[:2]
for i in range(self.settings_.max_placement_iter):
logging.debug('Placement {}/{}'.format(i,
self.settings_.max_placement_iter))
# Sample X-Y position from floor AABB.
x, y = np.random.uniform(pos_min, pos_max)
# Cast ray from robot top -> floor.
ray_src = [x, y, floor_aabb[1, 2] +
robot_aabb[1, 2] - robot_aabb[0, 2]]
ray_dst = [x, y, floor_aabb[0, 2] - EPS]
ray_res = pb.rayTest(ray_src, ray_dst, physicsClientId=self.sim_id)
# If by some magic, multiple intersections happened,
# ignore this case.
if len(ray_res) != 1:
continue
ray_res = ray_res[0]
# The ray must hit env + floor.
if (ray_res[0] != self.env_ids_[0]) or (ray_res[1] != -1):
continue
# Complete the desired new position.
# new_z = floor_aabb[1, 2] + (old_z - robot_aabb[0, 2])
new_z = floor_aabb[1, 2] + (old_z - robot_aabb[0, 2])
new_pos = np.asarray([x, y, new_z + EPS], dtype=np.float32)
new_rot = old_rot
# NOTE(ycho): Alternatively, sample from a random SE2 orientation:
# new_rot = pb.getQuaternionFromEuler([0.0, 0.0, np.random.uniform(-np.pi, np.pi)])
# Reject the new position if it collides with existing objects.
if self.settings_.use_fast_aabb_in_placement:
# NOTE(ycho): This query is conservative, so the returned objects
# may not actually overlap with the robot. However,
# perhaps if the object is close enough to the robot that we should
new_aabb = robot_aabb + new_pos - old_pos
o = pb.getOverlappingObjects(new_aabb[0], new_aabb[1],
physicsClientId=self.sim_id)
if o is not None:
continue
pb.resetBasePositionAndOrientation(robot_id,
new_pos,
new_rot,
physicsClientId=self.sim_id)
break
else:
# Actually place the robot where it would go,
# and then check if it results in a collision.
# Try placing the robot here now ...
# NOTE(ycho): Since pybullet uses a default collision margin (0.04 I think?),
# even this may be a little bit more conservative than the actual collision.
pb.resetBasePositionAndOrientation(robot_id,
new_pos,
new_rot,
physicsClientId=self.sim_id)
col = False
for env_id in self.env_ids_:
cpts = pb.getClosestPoints(env_id, robot_id,
np.inf,
physicsClientId=self.sim_id)
for cpt in cpts:
if cpt[8] < 0:
col = True
break
# Early exit if collision found
if col:
break
# Continue searching if collision found
if col:
continue
# All is well! break.
break
1],
useMaximalCoordinates=True)
p.configureDebugVisualizer(p.COV_ENABLE_RENDERING, 1)
p.stopStateLogging(logId)
p.setGravity(0, 0, -10)
p.setRealTimeSimulation(1)
colors = [[1, 0, 0, 1], [0, 1, 0, 1], [0, 0, 1, 1], [1, 1, 1, 1]]
currentColor = 0
while (1):
mouseEvents = p.getMouseEvents()
for e in mouseEvents:
if ((e[0] == 2) and (e[3] == 0) and (e[4] & p.KEY_WAS_TRIGGERED)):
mouseX = e[1]
mouseY = e[2]
rayFrom, rayTo = getRayFromTo(mouseX, mouseY)
rayInfo = p.rayTest(rayFrom, rayTo)
#p.addUserDebugLine(rayFrom,rayTo,[1,0,0],3)
for l in range(len(rayInfo)):
hit = rayInfo[l]
objectUid = hit[0]
if (objectUid >= 1):
#p.removeBody(objectUid)
p.changeVisualShape(objectUid,
-1,
rgbaColor=colors[currentColor])
currentColor += 1
if (currentColor >= len(colors)):
currentColor = 0
CLIENT = p.connect(p.DIRECT) #or pybullet.DIRECT for non-graphical version
p.setAdditionalSearchPath(pybullet_data.getDataPath()) #used by loadURDF
p.setAdditionalSearchPath(pybullet_data.getDataPath()) #used by loadURDF
p.configureDebugVisualizer(p.COV_ENABLE_GUI, 0)
p.configureDebugVisualizer(p.COV_ENABLE_SHADOWS, 0)
model_urdf = rospack.get_path("eusurdf")
oven = p.loadURDF(model_urdf + "/models/cup/model.urdf")
#hoge = p.loadURDF("plane.urdf")
direction = np.array([1.0, 0.0, 0.0])
# umakuiku
set_point(oven, [0, 0.0, 0.0])
pos = np.array([-0.5, 0.0, 0.02])
result1 = p.rayTest(pos, pos + direction * 0.5)[0]
# umakuikanai girigiri
set_point(oven, [0, 0.0, 0.0])
pos = np.array([-0.05, 0.0, 0.02])
result2 = p.rayTest(pos, pos + direction * 0.5)[0]
col_pos1 = result1[3]
col_pos2 = result2[3]
def raytest_scatter(x_start, b_min, b_max, N):
w = (b_max - b_min) / N
pts = []
mat = np.zeros((N, N))
for i in range(N):
# init positions
gemPos, gemOrn = p.getBasePositionAndOrientation(gemId)
cubePos, cubeOrn = p.getBasePositionAndOrientation(boxId)
step_forward = 0.01
for i in range(DURATION):
p.stepSimulation()
time.sleep(1. / 240.)
gemPos, gemOrn = p.getBasePositionAndOrientation(gemId)
cubePos, cubeOrn = p.getBasePositionAndOrientation(boxId)
delta_pos = (step_forward * np.cos(i * 0.01),
step_forward * np.sin(i * 0.01), 0)
new_cube_pos = (cubePos[0] + delta_pos[0], cubePos[1] + delta_pos[1],
cubePos[2] + delta_pos[2])
p.resetBasePositionAndOrientation(boxId, new_cube_pos, cubeOrn)
oid, lk, frac, pos, norm = p.rayTest(cubePos, gemPos)[0]
# rt = p.rayTest(cubePos, gemPos)
# print("rayTest: %s" % rt[0][1])
print("rayTest: Norm: ")
print(norm)
p.applyExternalForce(objectUniqueId=boxId,
linkIndex=-1,
forceObj=pos,
posObj=gemPos,
flags=p.WORLD_FRAME)
print(cubePos, cubeOrn)
p.disconnect()
def main():
p.setRealTimeSimulation(1)
p.setGravity(0, 0, 0) # we don't use gravity yet
# debug colors only for show
colors = [[1, 0, 0, 1], [0, 1, 0, 1], [0, 0, 1, 1], [1, 1, 1, 1], [0, 0, 0, 1] ]
currentColor = 0
try:
body = p.getBodyUniqueId(mantis_robot)
EndEffectorIndex = p.getNumJoints(body) -1
print("EndEffectorIndex %i" % EndEffectorIndex)
tx = 0
ty = 5
tz = 3
tj = 0
tp = 0
tr = math.pi / 2
orn = p.getQuaternionFromEuler([tj,tp,tr])
jointT = p.calculateInverseKinematics(body, EndEffectorIndex, [tx, ty,tz], orn, jointDamping=jd)
p.setJointMotorControlArray(body,[1,2,3,4,5,6] , controlMode=p.POSITION_CONTROL , targetPositions=jointT )
print("x:%f y:%f z:%f j:%f p:%f r:%f " % (tx,ty,tz,tj,tp,tr))
print(jointT)
p.resetBasePositionAndOrientation(target, [tx, ty, tz ], orn)
loop = 0
hasPrevPose = 0
trailDuration = 15
targetSelected = False
distance = 5
yaw = 90
motorDir = [1,-1,1,1,1,1]
while (p.isConnected()):
time.sleep(1. / 240.) # set to 240 fps
p.stepSimulation()
# get target positon
targetPos, targetOrn = p.getBasePositionAndOrientation(target)
# do Inverse Kinematics
# jointT = p.calculateInverseKinematics(body, EndEffectorIndex, targetPos, targetOrn)
jointT = p.calculateInverseKinematics(body, EndEffectorIndex, targetPos, orn)
# set Robot Joints
p.setJointMotorControlArray(body,[1,2,3,4,5,6] , controlMode=p.POSITION_CONTROL , targetPositions=jointT )
# reduce the output speed to 1/10 of the simulation speed
# ~24 fps
loop +=1
if loop > 10:
loop = 0
# sends data to the robot over ethernet/UDP
# just a simple string with the motor number and the angle positon in 12 bit range (0-4096)
data_string = ""
for i in range ( 1, EndEffectorIndex):
jt = p.getJointState(body, i )
data_string += "motor%i:%i\r\n" % ( i, scale(motorDir[i-1] * jt[0]))
transfer_socket.sendto(bytes(data_string, "utf-8"), (transfer_ip, transfer_port))
# generates the trail for debug only
# optional
ls = p.getLinkState(body, EndEffectorIndex)
targetPos, targetOrn = p.getBasePositionAndOrientation(target)
if (hasPrevPose):
p.addUserDebugLine(prevPose, targetPos, [0, 0, 0.3], 1, trailDuration)
p.addUserDebugLine(prevPose1, ls[4], [1, 0, 0], 1, trailDuration)
prevPose = targetPos
prevPose1 = ls[4]
hasPrevPose = 1
# get mouse events
# changes color of the object clickt on and prints some info
mouseEvents = p.getMouseEvents()
for e in mouseEvents:
if ((e[0] == 2) and (e[3] == 0) and (e[4] & p.KEY_WAS_RELEASED) and (targetSelected == True) ):
# after mouse lost reset posion to prevent the target from floting around
targetSelected = False
targetPos, targetOrn = p.getBasePositionAndOrientation(target)
p.resetBasePositionAndOrientation(target, targetPos, targetOrn)
if ((e[0] == 2) and (e[3] == 0) and (e[4] & p.KEY_WAS_TRIGGERED)):
mouseX = e[1]
mouseY = e[2]
rayFrom, rayTo = getRayFromTo(mouseX, mouseY)
rayInfo = p.rayTest(rayFrom, rayTo)
for l in range(len(rayInfo)):
hit = rayInfo[l]
objectUid = hit[0]
jointUid = hit[1]
if( objectUid == target):
targetSelected = True
if (objectUid >= 1):
# this is for debug click on an object to get id
# oject will change color this has no effect
print("obj %i joint %i" % (objectUid , jointUid))
p.changeVisualShape(objectUid, jointUid, rgbaColor=colors[currentColor])
currentColor += 1
if (currentColor >= len(colors)):
currentColor = 0
except KeyboardInterrupt:
print("KeyboardInterrupt has been caught.")
finally:
endProgramm()
def main():
p.setRealTimeSimulation(1)
# p.setGravity(0, 0, -10) # we don't use gravity yet
atexit.register(endThreads)
for light in lights:
print("universe %d" % light.universe)
aOutput.startOut(p, lights)
mInput.startInput(lights, colliders)
wInput.startInput()
# debug colors only for show
colors = [[1, 0, 0, 1], [0, 1, 0, 1], [0, 0, 1, 1], [1, 1, 1, 1]]
currentColor = 0
try:
while (p.isConnected()):
time.sleep(1. / 40.) # set to 40 fps
# reset output timeout, hack for threads...
aOutput.alive = 0
mInput.alive = 0
wInput.alive = 0
# update the wheel feedback
hlWheel.setRotation(p, wInput.angle)
# this is the main part of the sim
p.stepSimulation()
for col in colliders:
col.simulate(p)
if col.enabled:
for light in lights:
light.doCollisionFilter(p, col)
# get mouse events
mouseEvents = p.getMouseEvents()
for e in mouseEvents:
if ((e[0] == 2) and (e[3] == 0)
and (e[4] & p.KEY_WAS_TRIGGERED)):
mouseX = e[1]
mouseY = e[2]
rayFrom, rayTo = getRayFromTo(mouseX, mouseY)
rayInfo = p.rayTest(rayFrom, rayTo)
for l in range(len(rayInfo)):
hit = rayInfo[l]
objectUid = hit[0]
jointUid = hit[1]
if (objectUid >= 1):
# this is for debug click on an object to get id
# oject will change color this has no effect
# changing color real time seems to slow
print("obj %i joint %i" % (objectUid, jointUid))
p.changeVisualShape(objectUid,
jointUid,
rgbaColor=colors[currentColor])
currentColor += 1
if (currentColor >= len(colors)):
currentColor = 0
except KeyboardInterrupt:
print("KeyboardInterrupt has been caught.")
finally:
endThreads()