def __exit__(self, exc_type, exc_val, exc_tb):
global TASK_TREE
global CURRENT_TASK_TREE_NODE
TASK_TREE = self.suspended_tree
CURRENT_TASK_TREE_NODE = self.suspended_current_node
BulletWorld.current_bullet_world.restore_state(self.world_state, self.objects2attached)
pybullet.removeAllUserDebugItems()
def compute_grasp(self, bb, depth):
"""
Computes the center world coordinate and width size of a grasp defined
by its bounding box. bb is a (4,2) np array with the corners of the
grasp
"""
center_pixels = np.mean(bb, axis=0).astype(np.int)
center_world = self.world_from_camera(center_pixels[1],
center_pixels[0], depth)
corners_world = np.zeros((3, 4))
# Width line
p0 = self.world_from_camera(bb[0, 1], bb[0, 0], depth)
p1 = self.world_from_camera(bb[1, 1], bb[1, 0], depth)
p2 = self.world_from_camera(bb[2, 1], bb[2, 0], depth)
p3 = self.world_from_camera(bb[3, 1], bb[3, 0], depth)
width = np.linalg.norm(p1 - p0)
if self.debug:
p.removeAllUserDebugItems()
p.addUserDebugLine(self.pos, center_world)
p.addUserDebugLine(p0, p1, [0, 0, 0])
p.addUserDebugLine(p1, p2, [0, 1, 0])
p.addUserDebugLine(p2, p3, [0, 0, 0])
p.addUserDebugLine(p3, p0, [0, 1, 0])
return center_world, width
def reset(self):
# Reset all joint using normal distribution
for j in self.joint_list:
p.resetJointState(self.robot_id, j,
np.random.uniform(low=-np.pi/4, high=np.pi/4))
# Set random target and put it in observations
self.target_position = np.array([
np.random.uniform(0.219 - 0.069*self.delta,
0.219 + 0.069*self.delta),
np.random.uniform(0.020 - 0.153*self.delta,
0.020 + 0.153*self.delta),
np.random.uniform(0.128 - 0.072*self.delta,
0.128 + 0.072*self.delta),
])
self.observation[-3:] = self.target_position
# Show target as a crosshair
p.removeAllUserDebugItems()
p.addUserDebugLine(self.target_position - [0, 0, 0.01],
self.target_position + [0, 0, 0.01],
[0, 0, 0], 2)
p.addUserDebugLine(self.target_position - [0, 0.01, 0],
self.target_position + [0, 0.01, 0],
[0, 0, 0], 2)
# Return observation
self._update_observation()
return self.observation
def check_grip(cubeID, handID):
# TODO: make direction of motion consistant (up and away from origin?)
# TODO: modify to take in hand and cube position/orientation + load into environment w/gravity before shaking
"""
check grip by adding in gravity
"""
print("checking strength of current grip")
mag = 1
pos, oren = p.getBasePositionAndOrientation(handID)
# pos, oren = p.getBasePositionAndOrientation(cubeID)
time_limit = .5
finish_time = time() + time_limit
p.addUserDebugText("Grav Check!", [-.07, .07, .07], textColorRGB=[0, 0, 1], textSize=1)
while time() < finish_time:
p.stepSimulation()
# add in "gravity"
p.applyExternalForce(cubeID, linkIndex=-1, forceObj=[0, 0, -mag], posObj=pos, flags=p.WORLD_FRAME)
contact = p.getContactPoints(cubeID, handID) # see if hand is still holding obj after gravity is applied
print("contacts", contact)
if len(contact) > 0:
p.removeAllUserDebugItems()
p.addUserDebugText("Grav Check Passed!", [-.07, .07, .07], textColorRGB=[0, 1, 0], textSize=1)
sleep(.3)
print("Good Grip to Add")
return get_robot_config(handID)
else:
p.removeAllUserDebugItems()
p.addUserDebugText("Grav Check Failed!", [-.07, .07, .07], textColorRGB=[1, 0, 0], textSize=1)
sleep(.3)
return None
def state(self):
'''
State is an array of data
'''
# state.append( self.vehicle_pos[0] ) # X
# state.append( self.vehicle_pos[1] ) # Y
# quat = data[1]
# ori = p.getEulerFromQuaternion( quat )
# print( quat[0], quat[1], quat[2], quat[3], sep="\n" )
# find [ x y ] direction vector from quaternion
qw, qx, qy, qz = self.vehicle_orientation
# x = 2 * ( qx*qz + qw*qy )
# y = 2 * ( qy*qz - qw*qx )
# z = 1 - 2 * ( qx*qx + qy*qy )
x = 2 * (qx * qy - qw * qz)
y = 1 - 2 * (qx * qx + qz * qz)
z = 2 * (qy * qz + qw * qx)
# z = 2 * ( qx*qz - qw*qy )
# y = 2 * ( qy*qx + qw*qz )
# x = 1 - 2 * ( qz*qz + qy*qy )
vehhdg = math.atan2(z, -y) # yaw ( around Z )
state = []
state.append(vehhdg / math.pi) # convert -1 to 1
# print( "Hdg:", x, y, z, math.degrees( math.atan2(z,-y) ), math.degrees( ori[2] ) )
# state.append( self.speed )
state.append(self.steer)
ang = -math.pi
for ball in self.balls:
ballpos = ball.pos
dx = ballpos[0] - self.vehicle_pos[0]
dy = ballpos[1] - self.vehicle_pos[1]
ang = math.atan2(dy, dx)
state.append(ang / math.pi)
in_front = abs(ang) < math.pi / 2.0
dist = clamp(ball.distance_to_vehicle, 0, 20) / 20.0
state.append(dist if in_front else -dist)
if self.mode == 'human' and (self.frame & 31 == 0):
if self.text:
p.removeAllUserDebugItems()
txt = "Sp {:.3f} St {:.3f} He {:.3f} Be {:.3f} Sc {:.3f}".format(
self.speed * self.max_speed,
self.steer * self.max_steer * 57.296, vehhdg * 57.296,
ang * 57.296, self.score)
self.text = p.addUserDebugText(txt, [-5, -5, 1], [0, 0, 0])
return state
def render_line(self):
if not self.animate:
return
p.removeAllUserDebugItems()
self.target_debug_line = p.addUserDebugLine([self.target, 0, 0],
[self.target, 0, 0.5],
lineWidth=6,
lineColorRGB=[1, 0, 0])
def remove_all_debug(physicsClientId):
"""Remove all debug parameters.
Args:
physicsClientId (int): unique id for physics server.
Returns:
None
"""
pb.removeAllUserDebugItems(physicsClientId)
def evaluate_model(params, env, policy, regressor):
mse_errors = []
for i in range(params["eval_episodes"]):
obs = env.reset()
h = None
mse_episode_errors = []
for j in range(params["max_steps"]):
# Get action from policy
action, _states = policy.predict(obs, deterministic=True)
action = action + np.random.randn(env.act_dim)
with T.no_grad():
# Predict next step
if "lstm" in regressor.__class__.__name__.lower():
obs_pred, h = regressor.forward_step(
T.tensor(np.concatenate((obs, action)),
dtype=T.float32).unsqueeze(0).unsqueeze(0), h)
obs_pred = obs_pred[0]
else:
obs_pred = regressor.forward(
T.tensor(np.concatenate((obs, action)),
dtype=T.float32).unsqueeze(0))
# Step env and get next obs GT
obs, reward, done, info = env.step(action)
# Calculate error and plot info
obs_err = np.mean(np.square(obs - obs_pred.numpy()))
mse_episode_errors.append(obs_err)
p.removeAllUserDebugItems()
clp_err = np.clip(obs_err, 0, 1)
p.addUserDebugText(
"Mean err: % 3.3f" % (obs_err), [-1, 0, 2],
textColorRGB=[clp_err, 1 - clp_err, 1 - clp_err],
textSize=2)
time.sleep(0.01)
if done:
print("Done, breaking")
break
mse_errors.append(mse_episode_errors)
print("Eval episode: {}/{}, mean_mse = {}, min_mse = {}, max_mse = {}".
format(i, params["eval_episodes"], np.mean(mse_episode_errors),
np.min(mse_episode_errors), np.max(mse_episode_errors)))
mean_mse = np.mean(mse_errors)
min_mse = np.min(mse_errors)
max_mse = np.max(mse_errors)
print(
"Evaluation complete, Global mean_mse = {}, Global min_mse = {}, Global max_mse = {}"
.format(mean_mse, min_mse, max_mse))
return mean_mse, min_mse, max_mse
def _reset_pos(self):
p.resetBasePositionAndOrientation(self.robotId, self.robotStartPos, self.robotStartOrientation)
for i in range(len(self.joints.items())):
p.resetJointState(self.robotId, self.joints[i][0], 0)
p.removeAllUserDebugItems()
randomOffsetPos = [
random.random()*10,
random.random()*10,
0
]
def draw_camera_pos(self):
pb.removeAllUserDebugItems()
start = self.camera_pos
end_x = start + np.dot(self.camera_rot, np.array([0.1, 0, 0, 1.0
]))[0:3]
pb.addUserDebugLine(start, end_x, [1, 0, 0], 5)
end_y = start + np.dot(self.camera_rot, np.array([0, 0.1, 0, 1.0
]))[0:3]
pb.addUserDebugLine(start, end_y, [0, 1, 0], 5)
end_z = start + np.dot(self.camera_rot, np.array([0, 0, 0.1, 1.0
]))[0:3]
pb.addUserDebugLine(start, end_z, [0, 0, 1], 5)
def step(self, ctrl):
p.setJointMotorControl2(self.cartpole,
0,
p.TORQUE_CONTROL,
force=ctrl * 20)
p.stepSimulation()
self.step_ctr += 1
# x, x_dot, theta, theta_dot
obs = self.get_obs()
x, x_dot, theta, theta_dot = obs
x_sphere = x - np.sin(p.getJointState(self.cartpole, 1)[0])
target_pen = np.clip(
np.abs(x_sphere - self.target) * 3.0 * (1 - abs(theta)), -2, 2)
vel_pen = (np.square(x_dot) * 0.1 +
np.square(theta_dot) * 0.5) * (1 - abs(theta))
r = 1 - target_pen - vel_pen - np.square(ctrl[0]) * 0.005
p.removeAllUserDebugItems()
p.addUserDebugLine(
(x, 0, 0),
(x_sphere, 0, -np.cos(p.getJointState(self.cartpole, 1)[0])))
#p.addUserDebugText("sphere mass: {0:.3f}".format(self.mass), [0, 0, 2])
#p.addUserDebugText("sphere x: {0:.3f}".format(x_sphere), [0, 0, 2])
#p.addUserDebugText("cart pen: {0:.3f}".format(cart_pen), [0, 0, 2])
#p.addUserDebugText("x: {0:.3f}".format(x), [0, 0, 2])
#p.addUserDebugText("x_target: {0:.3f}".format(self.target), [0, 0, 2.2])
#p.addUserDebugText("cart_pen: {0:.3f}".format(cart_pen), [0, 0, 2.4])
done = self.step_ctr > self.max_steps
# Change target
if np.random.rand() < self.target_change_prob:
self.target = np.clip(
np.random.rand() * 2 * self.target_var - self.target_var, -2,
2)
p.resetBasePositionAndOrientation(self.target_vis,
[self.target, 0, -1],
[0, 0, 0, 1])
if self.latent_input:
obs = np.concatenate((obs, [self.get_latent_label()]))
if self.action_input:
obs = np.concatenate((obs, ctrl))
obs = np.concatenate((obs, [self.target]))
return obs, r, done, {}
def ur5_camera(robotID):
pos = p.getLinkState(robotID, linkIndex=7, computeForwardKinematics=1)[-2]
ort = p.getLinkState(robotID, linkIndex=7, computeForwardKinematics=1)[-1]
rot_mat = np.array(p.getMatrixFromQuaternion(ort))
rot_mat = np.reshape(rot_mat, [3, 3])
dir = rot_mat.dot([1, 0, 0])
up_vector = rot_mat.dot([0, 0, 1])
s = 0.01
view_matrix = p.computeViewMatrix(pos, pos + s * dir, up_vector)
p.addUserDebugText(text=".",
textPosition=pos + s * dir,
textColorRGB=[1, 0, 0],
textSize=10)
projection_matrix = p.computeProjectionMatrixFOV(fov, aspect, near, far)
f_len = projection_matrix[0]
_, _, rgbImg, depthImg_buffer, segImg = p.getCameraImage(
width,
height,
view_matrix,
projection_matrix,
renderer=p.ER_BULLET_HARDWARE_OPENGL)
depthImg_buf = np.reshape(depthImg_buffer, [width, height])
depthImg = far * near / (far - (far - near) * depthImg_buf)
f_x = width / (2 * tan(fov * pi / 360))
f_y = height / (2 * tan(fov * pi / 360))
for i in range(10000):
if (not (i % 10000)):
rgbImg_array = np.array(rgbImg)
im = Image.fromarray(rgbImg_array)
im = im.convert("RGB")
im.save(
"/home/nalin/Desktop/prem/fruit_plucking2/apple_detection1/detection/images/{}.jpg"
.format(i))
# = = Store data for segmentation = =
# segImg_array = np.array(segImg)+1
# length = len(np.unique(segImg_array))
# unique_values = np.unique(segImg_array)
# print('unique = ', unique_values)
# print('len = ',length)
# print('shape = ', segImg_array.shape)
# print('trunk pix index',unique_values[1])
# segImg_array[segImg_array == unique_values[1]] = unique_values[0] # color of trunk is repleced by background
# img = Image.fromarray((segImg_array)*255/length)
# img = img.convert("RGB")
# img.save("Address/{}.png".format(time.time()))
# print('Image saved')
#time.sleep(0.75)
p.removeAllUserDebugItems()
return (depthImg, f_x, f_y)
def reset(self):
fov = 20. + self._cameraRandom * np.random.uniform(-2, 2)
aspect = self._width / self._height
near = 0.01
far = 10
self._proj_matrix = p.computeProjectionMatrixFOV(
fov, aspect, near, far)
# counter and flag
self.goal_rotation_angle = 0
self._env_step = 0
self.terminated = 0
self.finger_angle = 0.3
self._success = False
p.removeAllUserDebugItems()
self.out_of_range = False
self._collision_box = False
self.drop_down = False
self._rank_before = 0
self.inverse_rank = 0
p.resetSimulation()
p.setPhysicsEngineParameter(numSolverIterations=300)
p.setTimeStep(self._timeStep)
self._planeUid = p.loadURDF(os.path.join(self._urdfRoot, "plane.urdf"),
[0, 0, -1])
self._tableUid = p.loadURDF(
os.path.join(self._urdfRoot, "table/table.urdf"),
[0.5000000, 0.00000, -.820000],
p.getQuaternionFromEuler(np.radians([0, 0, 90.])))
p.setGravity(0, 0, -10)
self._kuka = kuka.Kuka(urdfRootPath=self._urdfRoot,
timeStep=self._timeStep)
self._envStepCounter = 0
p.stepSimulation()
# Choose the objects in the bin.
urdfList = self._get_random_object(self._numObjects, test=self._isTest)
self._objectUids = self._randomly_place_objects(urdfList)
self._init_obj_high = self._obj_high()
self._rank_before = self._rank_1()
# self._domain_random()
return self._get_observation()
def ray_check(self, smarticles):
'''
updates position of smarticles and checks if any are hit by the light rays
'''
p.removeAllUserDebugItems()
results = self.draw_rays()
smart_ids = [x.id for x in smarticles]
for smart in smarticles:
smart.update_position()
smart.set_plank(0)
for ray in results:
if ray[0] >= smart_ids[0] and ray[1] == -1:
index = smart_ids.index(ray[0])
if smarticles[index].light_plank(ray[3], self.yaw):
p.addUserDebugLine(self.x, ray[3], self.ray_hit_color)
def removeDebugItems(*args):
'''
remove one or multiple debug items
:param args: int, list, tuple
id of the items to be removed
'''
if len(args) == 0:
pybullet.removeAllUserDebugItems()
else:
for arg in args:
if isinstance(arg, int):
pybullet.removeUserDebugItem(arg)
elif isinstance(arg, list) or isinstance(arg, tuple):
for item in arg:
SimEnv.removeDebugItems(item)
def main(args):
NOISE=0.00005
# get a bunch of random blocks
# if args.use_vision:
if True:
blocks = load_blocks(fname=args.blocks_file,
num_blocks=args.num_blocks)
else:
blocks = get_adversarial_blocks(num_blocks=args.num_blocks)
agent = PandaAgent(blocks, NOISE,
use_platform=False, teleport=False,
use_action_server=args.use_action_server,
use_vision=args.use_vision,
real=args.real)
if args.show_frames:
agent.step_simulation(T=1, vis_frames=True, lifeTime=0.)
input('Start building?')
p.removeAllUserDebugItems()
for tx in range(0, args.num_towers):
# Build a random tower out of blocks.
n_blocks = np.random.randint(2, args.num_blocks + 1)
tower_blocks = np.random.choice(blocks, n_blocks, replace=False)
tower = sample_random_tower(tower_blocks)
#tower = build_tower(tower_blocks, constructable=True, max_attempts=50000)
# and execute the resulting plan.
print(f"Starting tower {tx}")
if args.use_action_server:
agent.simulate_tower_parallel(tower,
real=args.real,
base_xy=(0.5, -0.3),
vis=True,
T=2500)
else:
success, stable = agent.simulate_tower(tower,
real=args.real,
base_xy=(0.5, -0.3),
vis=True,
T=2500,
save_tower=args.save_tower)
if not success:
print('Planner failed.')
print(f"Finished tower {tx}")
def run_generate_scene(self):
body_id = self.bodies.next()
pos, ori = self._get_pos_and_ori(body_id)
eulers = p.getEulerFromQuaternion(ori)
sliders = [
p.addUserDebugParameter('x', -2, 2, pos[0]),
p.addUserDebugParameter('y', -2, 2, pos[1]),
p.addUserDebugParameter('z', -2, 2, pos[2]),
p.addUserDebugParameter('roll', -180, 180, np.rad2deg(eulers[0])),
p.addUserDebugParameter('pitch', -180, 180, np.rad2deg(eulers[1])),
p.addUserDebugParameter('yaw', -180, 180, np.rad2deg(eulers[2]))
]
qKey = ord('q')
pKey = ord(' ')
nextKey = 65309L
simulate = False
stop = False
while True:
keys = p.getKeyboardEvents()
if qKey in keys and keys[qKey] & p.KEY_WAS_TRIGGERED:
stop = True
break
if nextKey in keys and keys[nextKey] & p.KEY_WAS_TRIGGERED:
break
if pKey in keys and keys[pKey] & p.KEY_WAS_TRIGGERED:
pos, ori = self._get_pos_and_ori(body_id)
simulate = not simulate
self.step()
if not simulate:
targets_values = [
p.readUserDebugParameter(s_id) for s_id in sliders
]
p.resetBasePositionAndOrientation(
body_id, targets_values[:3],
p.getQuaternionFromEuler(np.deg2rad(targets_values[3:6])))
p.removeAllUserDebugItems()
for slider in sliders:
p.removeUserDebugItem(slider)
return stop
def debug_run(self):
p.removeAllUserDebugItems()
p.addUserDebugParameter("x", -2, 2, 0)
p.addUserDebugParameter("y", -2, 2, 0)
p.addUserDebugParameter("z", -5, 0, 0)
p.addUserDebugParameter("roll", -1.5, 1.5, 0)
p.addUserDebugParameter("pitch", -1.5, 1.5, 0)
p.addUserDebugParameter("yaw", -1.5, 1.5, 0)
p.addUserDebugParameter("width", 0, 0.1, 0)
old_values = [0] * 7
while True:
self.step()
values = [p.readUserDebugParameter(id) for id in range(7)]
values[6] /= 2.
values.append(-values[6])
p.setJointMotorControlArray(bodyUniqueId=self.gid,
jointIndices=range(8),
controlMode=p.POSITION_CONTROL,
targetPositions=values)
def step(self, ctrl):
p.setJointMotorControl2(self.cartpole,
0,
p.TORQUE_CONTROL,
force=ctrl * 20)
p.stepSimulation()
self.step_ctr += 1
# x, x_dot, theta, theta_dot
obs = self.get_obs()
x, x_dot, theta, theta_dot = obs
x_sphere = x - np.sin(p.getJointState(self.cartpole, 1)[0])
target_pen = np.clip(
np.abs(x_sphere - self.target) * 1.0 * (1 - abs(theta)), -2, 2)
vel_pen = (np.square(x_dot) * 0.1 +
np.square(theta_dot) * 0.5) * (1 - abs(theta))
r_rich = 1 - target_pen - vel_pen - np.square(ctrl[0]) * 0.005
r = r_rich
p.removeAllUserDebugItems()
#p.addUserDebugText("sphere mass: {0:.3f}".format(self.mass), [0, 0, 2])
#p.addUserDebugText("sphere x: {0:.3f}".format(x_sphere), [0, 0, 2])
#p.addUserDebugText("cart pen: {0:.3f}".format(cart_pen), [0, 0, 2])
p.addUserDebugText("x: {0:.3f}".format(target_pen), [0, 0, 2])
#p.addUserDebugText("x_target: {0:.3f}".format(self.target), [0, 0, 2.2])
#p.addUserDebugText("cart_pen: {0:.3f}".format(cart_pen), [0, 0, 2.4])
done = self.step_ctr > self.max_steps
#time.sleep(0.01)
if self.latent_input:
obs = np.concatenate((obs, [self.get_latent_label()]))
if self.action_input:
obs = np.concatenate((obs, ctrl))
return obs, r, done, {}
def check_grip(oID, rID):
"""
check grip by adding in gravity
"""
#print("checking strength of current grip")
mag = 1
pos, oren = p.getBasePositionAndOrientation(rID)
time_limit = .5
finish_time = time() + time_limit
p.addUserDebugText("Grav Check!", [-.07, .07, .07],
textColorRGB=[0, 0, 1],
textSize=1)
while time() < finish_time:
p.stepSimulation()
p.applyExternalForce(oID,
linkIndex=-1,
forceObj=[0, 0, -mag],
posObj=pos,
flags=p.WORLD_FRAME)
contact = p.getContactPoints(
oID, rID) # see if hand is still holding obj after gravity is applied
if len(contact) > 0:
p.removeAllUserDebugItems()
p.addUserDebugText("Grav Check Passed!", [-.07, .07, .07],
textColorRGB=[0, 1, 0],
textSize=1)
print("Grav Check Passed")
sleep(.2)
return get_robot_config(rID, oID)
else:
p.removeAllUserDebugItems()
p.addUserDebugText("Grav Check Failed!", [-.07, .07, .07],
textColorRGB=[1, 0, 0],
textSize=1)
print("Grav Check Failed")
sleep(.2)
return None
def pbvs(self,
arm,
goal_pos,
goal_rot,
kv=1.3,
kw=0.8,
eps_pos=0.005,
eps_rot=0.05,
vs_rate=20,
plot_pose=False,
print_err=False,
perturb_jacobian=False,
perturb_Jac_joint_mu=0.1,
perturb_Jac_joint_sigma=0.1,
perturb_orientation=False,
mu_R=0.4,
sigma_R=0.4,
perturb_motion_R_mu=0.0,
perturb_motion_R_sigma=0.0,
perturb_motion_t_mu=0.0,
perturb_motion_t_sigma=0.0,
plot_result=False,
pf_mode=False):
# Initialize variables
self.perturb_Jac_joint_mu = perturb_Jac_joint_mu
self.perturb_Jac_joint_sigma = perturb_Jac_joint_sigma
self.perturb_R_mu = mu_R
self.perturb_R_sigma = sigma_R
if arm == "right":
tool = self.right_tool
elif arm == "left":
tool = self.left_tool
else:
print('''arm incorrect, input "left" or "right" ''')
return
# Initialize pos_plot_id for later use
if pf_mode:
cur_pos, cur_rot = SE32_trans_rot(self.cur_pose_est)
else:
cur_pos, cur_rot = SE32_trans_rot(
self.Trc.inverse() * get_pose(self.robot, tool, SE3=True))
if plot_pose:
pos_plot_id = self.draw_pose_cam(trans_rot2SE3(cur_pos, cur_rot))
# record end effector pose and time stamp at each iteration
if plot_result:
start = time.time()
times = []
eef_pos = []
eef_rot = []
if pf_mode:
motion = sp.SE3()
##### Control loop starts #####
while True:
# If using particle filter and the hog computation has finished, exit pbvs control
# and assign total estimated motion in eef frame
if pf_mode and self.shared_pf_fin.value == 1:
# eef Motion estimated from pose estimate of last iteration and current fk
motion_truth = (self.Trc *
self.cur_pose_est).inverse() * get_pose(
self.robot, tool, SE3=True)
# print("motion truth:", motion_truth)
dR = sp.SO3.exp(
np.random.normal(perturb_motion_R_mu,
perturb_motion_R_sigma, 3)).matrix()
dt = np.random.normal(perturb_motion_t_mu,
perturb_motion_t_sigma, 3)
# self.draw_pose_cam(motion)
self.motion = motion_truth * sp.SE3(dR, dt)
return
# get current eef pose in camera frame
if pf_mode:
cur_pos, cur_rot = SE32_trans_rot(self.cur_pose_est)
else:
cur_pos, cur_rot = SE32_trans_rot(
self.Trc.inverse() * get_pose(self.robot, tool, SE3=True))
if plot_pose:
self.draw_pose_cam(trans_rot2SE3(cur_pos, cur_rot),
uids=pos_plot_id)
if plot_result:
eef_pos.append(cur_pos)
eef_rot.append(cur_rot)
times.append(time.time() - start)
cur_pos_inv, cur_rot_inv = p.invertTransform(cur_pos, cur_rot)
# Pose of goal in camera frame
pos_cg, rot_cg = p.multiplyTransforms(cur_pos_inv, cur_rot_inv,
goal_pos, goal_rot)
# Evaluate current translational and rotational error
err_pos = np.linalg.norm(pos_cg)
err_rot = np.linalg.norm(p.getAxisAngleFromQuaternion(rot_cg)[1])
if err_pos < eps_pos and err_rot < eps_rot:
p.removeAllUserDebugItems()
break
else:
if print_err:
print("Error to goal, position: {:2f}, orientation: {:2f}".
format(err_pos, err_rot))
Rsc = np.array(p.getMatrixFromQuaternion(cur_rot)).reshape(3, 3)
# Perturb Rsc in SO(3) by a random variable in tangent space so(3)
if perturb_orientation:
dR = sp.SO3.exp(
np.random.normal(self.perturb_R_mu, self.perturb_R_sigma,
3))
Rsc = Rsc.dot(dR.matrix())
twist_local = np.hstack(
(np.array(pos_cg) * kv,
np.array(quat2se3(rot_cg)) * kw)).reshape(6, 1)
local2global = np.block([[Rsc, np.zeros((3, 3))],
[np.zeros((3, 3)), Rsc]])
twist_global = local2global.dot(twist_local)
start_loop = time.time()
self.cartesian_vel_control(arm,
np.asarray(twist_global),
1 / vs_rate,
perturb_jacobian=perturb_jacobian)
if pf_mode:
delta_t = time.time() - start_loop
motion = motion * sp.SE3.exp(twist_local * delta_t)
# self.draw_pose_cam(motion)
self.goal_reached = True
print("PBVS goal achieved!")
if plot_result:
eef_pos = np.array(eef_pos)
eef_rot_rpy = np.array(
[p.getEulerFromQuaternion(quat) for quat in eef_rot])
fig, axs = plt.subplots(3, 2)
sub_titles = [['x', 'roll'], ['y', 'pitch'], ['z', 'yaw']]
fig.suptitle(
"Position Based Visual Servo End Effector Pose - time plot")
for i in range(3):
axs[i, 0].plot(times, eef_pos[:, i] * 100)
axs[i, 0].plot(times, goal_pos[i] * np.ones(len(times)) * 100)
axs[i, 0].legend([sub_titles[i][0], 'goal'])
axs[i, 0].set_xlabel('Time(s)')
axs[i, 0].set_ylabel('cm')
goal_rpy = p.getEulerFromQuaternion(goal_rot)
for i in range(3):
axs[i, 1].plot(times, eef_rot_rpy[:, i] * 180 / np.pi)
axs[i, 1].plot(times,
goal_rpy[i] * np.ones(len(times)) * 180 / np.pi)
axs[i, 1].legend([sub_titles[i][1], 'goal'])
axs[i, 1].set_xlabel('Time(s)')
axs[i, 1].set_ylabel('deg')
for ax in axs.flat:
ax.set(xlabel='time')
plt.show()
def remove_all_markers():
p.removeAllUserDebugItems()
events = p.getVREvents(allAnalogAxes=1)
for e in (events):
if (e[CONTROLLER_ID]==controllerId ):
for a in range(10):
print("analog axis"+str(a)+"="+str(e[8][a]))
if (e[BUTTONS][33]&p.VR_BUTTON_WAS_TRIGGERED):
prevPosition[e[CONTROLLER_ID]] = e[POSITION]
if (e[BUTTONS][32]&p.VR_BUTTON_WAS_TRIGGERED):
widths[e[CONTROLLER_ID]]=widths[e[0]]+1
if (widths[e[CONTROLLER_ID]]>20):
widths[e[CONTROLLER_ID]] = 1
if (e[BUTTONS][1]&p.VR_BUTTON_WAS_TRIGGERED):
p.resetSimulation()
#p.setGravity(0,0,-10)
p.removeAllUserDebugItems()
p.loadURDF("plane.urdf")
if (e[BUTTONS][33]==p.VR_BUTTON_IS_DOWN):
pt = prevPosition[e[CONTROLLER_ID]]
#print(prevPosition[e[0]])
print("e[POSITION]")
print(e[POSITION])
print("pt")
print(pt)
diff = [pt[0]-e[POSITION][0],pt[1]-e[POSITION][1],pt[2]-e[POSITION][2]]
lenSqr = diff[0]*diff[0]+diff[1]*diff[1]+diff[2]*diff[2]
ptDistThreshold = 0.01
if (lenSqr>(ptDistThreshold*ptDistThreshold)):
p.addUserDebugLine(e[POSITION],prevPosition[e[CONTROLLER_ID]],colors[e[CONTROLLER_ID]],widths[e[CONTROLLER_ID]])
#p.loadURDF("cube_small.urdf",e[1])
def clear_visualization(self):
"""Clear all visualization items."""
pybullet.removeAllUserDebugItems()
def reset(self):
p.removeAllUserDebugItems()
self.last = None
(0.9238795325112867, -3.313870358775352e-17, 0.3826834323650899,
-3.313870358775352e-17)),
((0.10223707190067913, -1.252043028573645e-17, 0.10223707190067911),
(8.971000920213853e-17, 0.9238795325112867, -9.706101561122023e-18,
-0.3826834323650899)))
rot_vec = [0, 0, 1]
i = 1
iter = pi / 4
for pose in poses:
pos = pose[0]
quat = pose[1]
print("original")
p.removeAllUserDebugItems()
p.addUserDebugText("Original", [-.07, .07, .07],
textColorRGB=[0, 1, 0],
textSize=1)
p.resetBasePositionAndOrientation(rID, pos, quat)
add_debug_lines(rID)
relax(rID)
grasp(rID)
sleep(.1)
for i in range(0, 8):
p.stepSimulation()
print("rotate wrist for new grasp")
quat_w = quat[3]
quat_x = quat[0]
def clean_up(rID):
"""
remove robot + all associated debug feedback
"""
p.removeBody(rID)
p.removeAllUserDebugItems()
# [0, 0, 1],
# )
# time.sleep(1.0 / fps)
# keys = pybullet.getKeyboardEvents()
# if qKey in keys and keys[qKey] & pybullet.KEY_WAS_TRIGGERED:
# print("QUIT")
# doneAll = True
# elif rKey in keys and keys[rKey] & pybullet.KEY_WAS_TRIGGERED:
# done = True
# elif eKey in keys and keys[eKey] & pybullet.KEY_WAS_TRIGGERED:
# pause = not pause
print(" Hidup selama {} steps".format(env.cur_timestep))
timestep_data.append(env.cur_timestep)
drift_data.append(np.mean(drift))
pybullet.removeAllUserDebugItems()
if (i == 10):
doneAll = True
print(" Mean drift untuk {} derajat : {}".format(degree, np.mean(drift_data)))
print(" Mean timestep untuk {} derajat: {}".format(degree, np.mean(timestep_data)))
experiment_data[degree] = {
"drift": copy.deepcopy(drift_data),
"timestep": copy.deepcopy(timestep_data),
}
env.close()
ray.shutdown()
with open('Log/data_{}_{}.json'.format(experiment_id[-19:], checkpoint_num), 'w') as fp:
json.dump(experiment_data, fp)
def remove_all_debug():
p.removeAllUserDebugItems(physicsClientId=CLIENT)
def calc_hipbone_to_mouth_height(self):
# Let user get familiar with the HMD
p.resetSimulation(physicsClientId=self.id)
p.setOriginCameraPositionAndOrientation(deviceTypeFilter=p.VR_DEVICE_HMD, physicsClientId=self.id)
# Load all models off screen and then move them into place
self.plane = p.loadURDF(os.path.join(os.path.join(os.path.dirname(os.path.realpath(__file__)), 'assets'), 'plane', 'plane.urdf'), physicsClientId=self.id)
HMD_distance = 0.0
while True:
p.removeAllUserDebugItems(physicsClientId=self.id)
event = p.setOriginCameraPositionAndOrientation(deviceTypeFilter=p.VR_DEVICE_HMD, physicsClientId=self.id)
(roll, pitch, yaw) = p.getEulerFromQuaternion([event[3], event[4], event[5], event[6]], physicsClientId=self.id)
roll_diff = np.around(np.rad2deg(roll) - 90, decimals=2)
pitch_diff = np.around(np.rad2deg(pitch), decimals=2)
yaw_diff = np.around(np.rad2deg(yaw), decimals=2)
event = np.array(event)
event[0] = 0.0
event[1] = 0.0
roll_rgb, rgb_mode = self.check_degree(roll_diff)
if rgb_mode == "+":
# "UP"
text1 = p.addUserDebugText(text="X: " + str(roll_diff), textPosition=[-event[0]+0.1, -event[1]-3, event[2]-0.4], textColorRGB=roll_rgb, textSize=0.1, textOrientation=p.getQuaternionFromEuler([np.pi/2.0, 0, np.pi], physicsClientId=self.id), physicsClientId=self.id)
line1 = p.addUserDebugLine(lineFromXYZ=[-event[0], -event[1]-3, event[2]-0.3], lineToXYZ=[-event[0], -event[1]-3, event[2]], lineColorRGB=roll_rgb, lineWidth=1.5, physicsClientId=self.id)
line1_part1 = p.addUserDebugLine(lineFromXYZ=[-event[0], -event[1]-3, event[2]], lineToXYZ=[-event[0]-0.1, -event[1]-3, event[2]-0.1], lineColorRGB=roll_rgb, lineWidth=1.5, physicsClientId=self.id)
line1_part2 = p.addUserDebugLine(lineFromXYZ=[-event[0], -event[1]-3, event[2]], lineToXYZ=[-event[0]+0.1, -event[1]-3, event[2]-0.1], lineColorRGB=roll_rgb, lineWidth=1.5, physicsClientId=self.id)
else:
# "DOWN"
text1 = p.addUserDebugText(text="X: " + str(roll_diff), textPosition=[-event[0]+0.1, -event[1]-3, event[2]+0.35], textColorRGB=roll_rgb, textSize=0.1, textOrientation=p.getQuaternionFromEuler([np.pi/2.0, 0, np.pi], physicsClientId=self.id), physicsClientId=self.id)
line1 = p.addUserDebugLine(lineFromXYZ=[-event[0], -event[1]-3, event[2]+0.3], lineToXYZ=[-event[0], -event[1]-3, event[2]], lineColorRGB=roll_rgb, lineWidth=1.5, physicsClientId=self.id)
line1_part1 = p.addUserDebugLine(lineFromXYZ=[-event[0], -event[1]-3, event[2]], lineToXYZ=[-event[0]-0.1, -event[1]-3, event[2]+0.1], lineColorRGB=roll_rgb, lineWidth=1.5, physicsClientId=self.id)
line1_part2 = p.addUserDebugLine(lineFromXYZ=[-event[0], -event[1]-3, event[2]], lineToXYZ=[-event[0]+0.1, -event[1]-3, event[2]+0.1], lineColorRGB=roll_rgb, lineWidth=1.5, physicsClientId=self.id)
pitch_rgb, pitch_mode = self.check_degree(pitch_diff)
if pitch_mode == "+":
text2 = p.addUserDebugText(text="Y: " + str(abs(pitch_diff)), textPosition=[-event[0]-0.2, -event[1]-3, event[2]+0.2], textColorRGB=pitch_rgb, textSize=0.1, textOrientation=p.getQuaternionFromEuler([np.pi/2.0, 0, np.pi], physicsClientId=self.id), physicsClientId=self.id)
else:
text2 = p.addUserDebugText(text="Y: " + str(abs(pitch_diff)), textPosition=[-event[0]+0.4, -event[1]-3, event[2]+0.2], textColorRGB=pitch_rgb, textSize=0.1, textOrientation=p.getQuaternionFromEuler([np.pi/2.0, 0, np.pi], physicsClientId=self.id), physicsClientId=self.id)
yaw_rgb, yaw_mode = self.check_degree(yaw_diff)
if yaw_mode == "+":
# "LEFT"
text3 = p.addUserDebugText(text="Z: " + str(abs(yaw_diff)), textPosition=[-event[0]-0.35, -event[1]-3, event[2]-0.02], textColorRGB=yaw_rgb, textSize=0.1, textOrientation=p.getQuaternionFromEuler([np.pi/2.0, 0, np.pi], physicsClientId=self.id), physicsClientId=self.id)
line3 = p.addUserDebugLine(lineFromXYZ=[-event[0], -event[1]-3, event[2]], lineToXYZ=[-event[0]-0.3, -event[1]-3, event[2]], lineColorRGB=yaw_rgb, lineWidth=1.5, physicsClientId=self.id)
line3_part1 = p.addUserDebugLine(lineFromXYZ=[-event[0], -event[1]-3, event[2]], lineToXYZ=[-event[0]-0.1, -event[1]-3, event[2]-0.1], lineColorRGB=yaw_rgb, lineWidth=1.5, physicsClientId=self.id)
line3_part2 = p.addUserDebugLine(lineFromXYZ=[-event[0], -event[1]-3, event[2]], lineToXYZ=[-event[0]-0.1, -event[1]-3, event[2]+0.1], lineColorRGB=yaw_rgb, lineWidth=1.5, physicsClientId=self.id)
else:
# "RIGHT"
text3 = p.addUserDebugText(text="Z: " + str(abs(yaw_diff)), textPosition=[-event[0]+0.6, -event[1]-3, event[2]-0.02], textColorRGB=yaw_rgb, textSize=0.1, textOrientation=p.getQuaternionFromEuler([np.pi/2.0, 0, np.pi], physicsClientId=self.id), physicsClientId=self.id)
line3 = p.addUserDebugLine(lineFromXYZ=[-event[0]+0.3, -event[1]-3, event[2]], lineToXYZ=[-event[0], -event[1]-3, event[2]], lineColorRGB=yaw_rgb, lineWidth=1.5, physicsClientId=self.id)
line3_part1 = p.addUserDebugLine(lineFromXYZ=[-event[0], -event[1]-3, event[2]], lineToXYZ=[-event[0]+0.1, -event[1]-3, event[2]-0.1], lineColorRGB=yaw_rgb, lineWidth=1.5, physicsClientId=self.id)
line3_part2 = p.addUserDebugLine(lineFromXYZ=[-event[0], -event[1]-3, event[2]], lineToXYZ=[-event[0]+0.1, -event[1]-3, event[2]+0.1], lineColorRGB=yaw_rgb, lineWidth=1.5, physicsClientId=self.id)
p.removeAllUserDebugItems(physicsClientId=self.id)
if np.absolute(roll_diff) < 5.00 and np.absolute(pitch_diff) < 5.00 and np.absolute(yaw_diff) < 5.00:
HMD_distance = event[2]
break
if self.task in ['scratch_itch', 'feeding', 'drinking']:
seat_to_hmd_height = HMD_distance - 0.51
elif self.task in ['bed_bathing']:
seat_to_hmd_height = HMD_distance - 0.54
hipbone_to_mouth_height = seat_to_hmd_height - (0.068 + 0.1335*self.config('radius_scale', 'human_female') if self.gender == 'male' else 0.058 + 0.127 * self.config('radius_scale', 'human_female'))
return hipbone_to_mouth_height
def axis_at(x, y, z): # draw x/y/z grid lines crossing at x,y,z p.removeAllUserDebugItems() p.addUserDebugLine((-10,y,z), (10,y,z), (1,1,1)) p.addUserDebugLine((x,-10,z), (x,10,z), (1,1,1)) p.addUserDebugLine((x,y,-10), (x,y,10), (1,1,1))
