def compute_reward(self):
baseOri = np.array(p.getBasePositionAndOrientation(self.robotId))
xposbefore = baseOri[0][0]
BaseAngVel = p.getBaseVelocity(self.robotId)
xvelbefore = BaseAngVel[0][0]
p.stepSimulation()
baseOri = np.array(p.getBasePositionAndOrientation(self.robotId))
xposafter = baseOri[0][0]
BaseAngVel = p.getBaseVelocity(self.robotId)
xvelafter = BaseAngVel[0][0]
# forward_reward = (xposafter - xposbefore)
forward_reward = 20 * (xvelbefore - xvelafter)
JointStates = p.getJointStates(self.robotId, self.movingJoints)
torques = np.array([np.array(joint[3]) for joint in JointStates])
ctrl_cost = 1.0 * np.square(torques).sum()
ContactPoints = p.getContactPoints(self.robotId, self.plane)
contact_cost = 5 * 1e-1 * len(ContactPoints)
# survive_reward = 1.0
survive_reward = 0.0
reward = forward_reward - ctrl_cost - contact_cost + survive_reward
# print("Reward ", reward , "Contact Cost ", contact_cost, "forward reward ",forward_reward, "Control Cost ", ctrl_cost)
# print("Reward ", reward)
reward = reward if reward > 0 else 0
p.addUserDebugLine(lineFromXYZ=(0, 0, 0),
lineToXYZ=(0.3, 0, 0),
lineWidth=5,
lineColorRGB=[0, 255, 0],
parentObjectUniqueId=self.robotId)
p.addUserDebugText("Rewards {}".format(reward), [0, 0, 0.3],
lifeTime=0.25,
textSize=2.5,
parentObjectUniqueId=self.robotId)
# print("Forward Reward", reward )
# print("Forward Reward", forward_reward, ctrl_cost, contact_cost , xvelbefore, xvelafter, xposbefore, xposafter )
return reward
def _load_robot_1(self):
robot = p.loadURDF(r'leg-1.SLDASM/urdf/leg-1.SLDASM.urdf',
self.robotPos_1,
useFixedBase=1,
flags=p.URDF_USE_IMPLICIT_CYLINDER)
for j in range(p.getNumJoints(robot)):
info = p.getJointInfo(robot, j)
# print(info)
joint_name = info[1].decode('utf8')
joint_type = info[2]
if joint_type == p.JOINT_REVOLUTE:
self.jointNameToID_1[joint_name] = info[0]
self.linkNameToID_1[info[12].decode('UTF-8')] = info[0]
self.revoluteID_1.append(j)
p.addUserDebugText(str(joint_name), [0, 0, 0],
parentObjectUniqueId=robot,
parentLinkIndex=j,
textColorRGB=[1, 0, 0])
return robot
def plot_pose(self,
pose,
axis_length=1.0,
text=None,
text_size=1.0,
text_color=[0, 0, 0]):
"""Plot a 6-DoF pose or a frame in the debugging visualizer.
Args:
pose: The pose to be plot.
axis_length: The length of the axes.
text: Text showing up next to the frame.
text_size: Size of the text.
text_color: Color of the text.
"""
if not isinstance(pose, Pose):
pose = Pose(pose)
origin = pose.position
x_end = origin + np.dot([axis_length, 0, 0], pose.matrix3.T)
y_end = origin + np.dot([0, axis_length, 0], pose.matrix3.T)
z_end = origin + np.dot([0, 0, axis_length], pose.matrix3.T)
pybullet.addUserDebugLine(origin,
x_end,
lineColorRGB=[1, 0, 0],
lineWidth=2)
pybullet.addUserDebugLine(origin,
y_end,
lineColorRGB=[0, 1, 0],
lineWidth=2)
pybullet.addUserDebugLine(origin,
z_end,
lineColorRGB=[0, 0, 1],
lineWidth=2)
if text is not None:
pybullet.addUserDebugText(text, origin, text_color, text_size)
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 add_text(self,
text,
pos=None,
poscam=[1, 1.25, -0.95],
parent=None,
name="text",
size=1.0,
lifetime=0.,
colour=[0., 0., 0.]):
# TODO: set pos to right in front of the camera if it is None
if pos is None:
camera_pos, R = self.get_camera_pos()
forward = R[:, 0]
left = R[:, 1]
up = R[:, 2]
pos = camera_pos + poscam[0] * forward + poscam[1] * left + poscam[
2] * up
else:
pos = totensor(pos)
colour = totensor(colour)
uid = self.debug_names.get(name, -1)
if parent is not None:
new_uid = p.addUserDebugText(text=text,
textPosition=pos.tolist(),
textColorRGB=colour.tolist(),
textSize=size,
lifeTime=lifetime,
parentObjectUniqueId=parent.uid,
replaceItemUniqueId=uid,
physicsClientId=self.sim.id)
else:
new_uid = p.addUserDebugText(text=text,
textPosition=pos.tolist(),
textColorRGB=colour.tolist(),
textSize=size,
lifeTime=lifetime,
replaceItemUniqueId=uid,
physicsClientId=self.sim.id)
self.debug_names[name] = new_uid
def _debug_links_text_init(self):
for dict_entry in self.body_name_2_joints:
idd = self.body_name_2_joints[dict_entry]['body_id']
i = 0
while True:
result = pybullet.getLinkState(idd, i)
if result is None:
break
pos = result[0]
iddd = pybullet.addUserDebugText('L(%d, %d)' % (idd, i), pos,
[0., 0., 0.])
self._debug_link_ids += [iddd]
i += 1
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 draw_link_frame(robot, link_idx, linewidth=5.0, text=None):
# This only works when the link has an visual element defined in the urdf file
if text is not None:
p.addUserDebugText(text, [0, 0, 0.1],
textColorRGB=[1, 0, 0],
textSize=1.5,
parentObjectUniqueId=robot,
parentLinkIndex=link_idx)
p.addUserDebugLine([0, 0, 0], [0.1, 0, 0], [1, 0, 0],
linewidth,
parentObjectUniqueId=robot,
parentLinkIndex=link_idx)
p.addUserDebugLine([0, 0, 0], [0, 0.1, 0], [0, 1, 0],
linewidth,
parentObjectUniqueId=robot,
parentLinkIndex=link_idx)
p.addUserDebugLine([0, 0, 0], [0, 0, 0.1], [0, 0, 1],
linewidth,
parentObjectUniqueId=robot,
parentLinkIndex=link_idx)
def add_text(text,
position=(0, 0, 0),
color=BLACK,
lifetime=None,
parent=NULL_ID,
parent_link=BASE_LINK):
return p.addUserDebugText(
str(text),
textPosition=position,
textColorRGB=color[:3], # textSize=1,
lifeTime=get_lifetime(lifetime),
parentObjectUniqueId=parent,
parentLinkIndex=parent_link,
physicsClientId=CLIENT)
def show_text_over_head(self, text):
"""Show some text over robot's head"""
camera_pos, _, _, _ = self.get_camera_target_positions()
camera_pos = list(camera_pos)
camera_pos[1] -= 0.1
camera_pos[2] += 0.2
if self.text_id >= 0:
p.removeUserDebugItem(self.text_id)
self.text_id = -1
self.text_id = p.addUserDebugText(text,
camera_pos,
textColorRGB=[0, 0, 0.8],
textSize=1,
lifeTime=0)
def __init__(self, *, realtime_sim=False, simple_model=True):
self.REALTIME_SIM = realtime_sim
p.connect(p.GUI)
p.setAdditionalSearchPath(pybullet_data.getDataPath())
p.setGravity(0, 0, -9.8)
p.setRealTimeSimulation(self.REALTIME_SIM)
p.loadURDF("plane.urdf")
urdf_file = "urdf/simple.urdf" if simple_model else "urdf/whole.urdf"
startPos = [0, 0, 0.1]
startOrientation = p.getQuaternionFromEuler([0, 0, 0])
self._model = p.loadURDF(urdf_file, startPos, startOrientation)
self._jointsInfo = []
self._jointsUserDebugParam = []
for i in range(p.getNumJoints(self._model)):
info = p.getJointInfo(self._model, i)
self._jointsInfo.append(info)
print(i, info)
self.CONTROL_JOINTS_NUM = min(100, p.getNumJoints(self._model))
for i in range(self.CONTROL_JOINTS_NUM):
jointName = self._jointsInfo[i][1]
p.setJointMotorControl2(self._model,
i,
p.POSITION_CONTROL,
targetVelocity=0,
force=0)
self._jointsUserDebugParam.append(
p.addUserDebugParameter(
str(jointName), self._jointsInfo[i][8],
self._jointsInfo[i][9],
(self._jointsInfo[i][8] + self._jointsInfo[i][9]) / 2))
p.addUserDebugText(str(jointName), [0, 0, 0],
parentObjectUniqueId=self._model,
parentLinkIndex=i,
textColorRGB=[1, 0, 0])
pass
def MakeArena(x,
y,
z=0.5,
scale_x=0.5,
scale_y=1,
scale_z=0.5,
Inter_area_dist=1,
pickAreaHeight=0.9):
p.setAdditionalSearchPath(pybullet_data.getDataPath())
planeOrn = [0, 0, 0, 1]
planeId = p.loadURDF("plane.urdf", [0, 0, 0], planeOrn)
#boxId = p.loadSDF(os.path.join("kuka_iiwa/kuka_with_gripper2.sdf"))
d = 1
stool1 = p.loadURDF("cube_small1.urdf", [0, d, 0], globalScaling=8)
stool2 = p.loadURDF("cube_small1.urdf", [0, -d, 0], globalScaling=8)
stool3 = p.loadURDF("cube_small1.urdf", [d, 0, 0], globalScaling=8)
stool4 = p.loadURDF("cube_small1.urdf", [-d, 0, 0], globalScaling=8)
#ballId = p.loadSoftBody("ball.obj", simFileName = "ball.vtk", basePosition = [0.5,0.5,0.05], scale = 0.07, mass = 0.1, useNeoHookean = 1, NeoHookeanMu = 400, NeoHookeanLambda = 600, NeoHookeanDamping = 0.001, useSelfCollision = 1, frictionCoeff = .5, collisionMargin = 0.001)
text = p.addUserDebugText("LDPE,HDPE", [-0.2, 0.9, 0.5], [1, 0, 0])
text = p.addUserDebugText("HDPE,LDPE,PET", [1, 0, 0.5], [1, 0, 0])
text = p.addUserDebugText("PVC,HDPE", [-1.3, -0.4, 0.5], [1, 0, 0])
text = p.addUserDebugText("POLYSTYRENE", [-0.2, -1.4, 0.5], [1, 0, 0])
car = p.loadURDF("bottle.urdf", [0.5, -0.5, 0], globalScaling=0.02)
def draw_text(self, name, text, location_index, left_offset=1.):
if name not in self._debug_ids:
self._debug_ids[name] = -1
uid = self._debug_ids[name]
move_down = location_index * 0.15
height_scale = self._camera_height * 0.7
self._debug_ids[name] = p.addUserDebugText(
str(text), [
self._camera_pos[0] + left_offset * height_scale,
self._camera_pos[1] + (1 - move_down) * height_scale, 0.1
],
textColorRGB=[0.5, 0.1, 0.1],
textSize=2,
replaceItemUniqueId=uid)
def add_text(text,
position=(0, 0, 0),
color=(0, 0, 0),
lifetime=None,
parent=-1,
parent_link=BASE_LINK,
text_size=1):
"""a copy from pybullet.util to enable text size"""
return p.addUserDebugText(str(text),
textPosition=position,
textColorRGB=color,
textSize=text_size,
lifeTime=0,
parentObjectUniqueId=parent,
parentLinkIndex=parent_link,
physicsClientId=CLIENT)
def create_pose_marker(position=np.array([0, 0, 0]),
orientation=np.array([0, 0, 0, 1]),
text='',
xColor=np.array([1, 0, 0]),
yColor=np.array([0, 1, 0]),
zColor=np.array([0, 0, 1]),
textColor=np.array([0, 0, 0]),
lineLength=0.1,
lineWidth=1,
textSize=1,
textPosition=np.array([0, 0, 0.1]),
textOrientation=None,
lifeTime=0,
parentObjectUniqueId=-1,
parentLinkIndex=-1):
"""Create a pose marker
Create a pose marker that identifies a position and orientation in space
with 3 colored lines.
"""
global remove_user_item_indices
pts = np.array([[0, 0, 0], [lineLength, 0, 0], [0, lineLength, 0],
[0, 0, lineLength]])
rotIdentity = np.array([0, 0, 0, 1])
po, _ = p.multiplyTransforms(position, orientation, pts[0, :], rotIdentity)
px, _ = p.multiplyTransforms(position, orientation, pts[1, :], rotIdentity)
py, _ = p.multiplyTransforms(position, orientation, pts[2, :], rotIdentity)
pz, _ = p.multiplyTransforms(position, orientation, pts[3, :], rotIdentity)
idx = p.addUserDebugLine(po, px, xColor, lineWidth, lifeTime,
parentObjectUniqueId, parentLinkIndex)
remove_user_item_indices.append(idx)
idx = p.addUserDebugLine(po, py, yColor, lineWidth, lifeTime,
parentObjectUniqueId, parentLinkIndex)
remove_user_item_indices.append(idx)
idx = p.addUserDebugLine(po, pz, zColor, lineWidth, lifeTime,
parentObjectUniqueId, parentLinkIndex)
remove_user_item_indices.append(idx)
if textOrientation is None:
textOrientation = orientation
idx = p.addUserDebugText(text, [0, 0, 0.1],
textColorRGB=textColor,
textSize=textSize,
parentObjectUniqueId=parentObjectUniqueId,
parentLinkIndex=parentLinkIndex)
remove_user_item_indices.append(idx)
def update(self):
infos = self.vive.getTrackersInfos()
for id in self.vive.trackers:
info = infos['trackers'][id]
m = info['pose_matrix']
position = np.array(m.T[3])[0][:3]
# if info['device_type'] == 'controller' and self.offset is None and np.linalg.norm(position)>0.5:
# self.offset = position.copy()
# print('Defining')
# print(self.offset)
# if self.offset is not None:
# position -= self.offset
orientation = m[:3, :3]
euler = convert_to_euler(orientation)
orientation = p.getQuaternionFromEuler(euler)
# fov, aspect, nearplane, farplane = 60, 1.0, 0.01, 100
# projection_matrix = p.computeProjectionMatrixFOV(fov, aspect, nearplane, farplane)
# tp = position.copy()
# tp[2] += 0.1
# view_matrix = p.computeViewMatrix(tp, tp+[-math.sin(euler[2]),math.cos(euler[2]),0], [0, 0, 1])
# img = p.getCameraImage(1000, 1000, view_matrix, projection_matrix)
# c = position.copy()
# p.resetDebugVisualizerCamera(0.2, cameraYaw=euler[2]*180/math.pi, cameraPitch=-120+euler[0]*180.0/math.pi, ,cameraTargetPosition=c)
# print(position)
p.resetBasePositionAndOrientation(self.trackers[id], position,
orientation)
for id in self.vive.references:
m = infos['references_corrected'][id]
position = np.array(m.T[3])[0][:3]
orientation = m[:3, :3]
euler = convert_to_euler(orientation)
orientation = p.getQuaternionFromEuler(euler)
if id not in self.texts:
self.texts[id] = p.addUserDebugText(id, position)
p.resetBasePositionAndOrientation(self.references[id], position,
orientation)
def visualize_trajectory(self, show_only_dot=False):
# The visualization or the forward kinematics overestimates the Z axis for some unknown reason
for i, configuration in enumerate(self.trajectory):
end_effector_world_position = self.robot.solve_forward_kinematics(
configuration)[0][0]
if i == 0:
text_string, color = "Start", [0.0, 1.0, 0.0]
elif i == self.N - 1:
text_string, color = "Goal", [0.0, 1.0, 0.0]
else:
text_string, color = str(i), [1.0, 0.0, 0.0]
if show_only_dot:
text_string = "*"
self.trajectory_visualization_ids.append(
p.addUserDebugText(text=text_string,
textPosition=end_effector_world_position,
textColorRGB=color,
textSize=0.75))
def update(self, action):
for frame in self.markers:
xyz, rot = self.parent.get_transform(self.markers[frame][0])
T_world_parent = self.trans_from_xyz_quat(xyz, rot)
T_net = self.T_offset.dot(T_world_parent)
self.markers[frame][1] = [
p.addUserDebugLine(
T_world_parent[:3, 3],
T_world_parent[:3, 3] +
T_world_parent[:3, :3].dot(vec) * self.scale,
vec,
lineWidth=2,
replaceItemUniqueId=line)
for vec, line in zip(np.eye(3), self.markers[frame][1])
]
if self.annotate:
self.markers[frame][2] = p.addUserDebugText(
frame,
T_net[:3, 3],
replaceItemUniqueId=self.markers[frame][2])
def loadDebugRefFrames(self):
p.addUserDebugText("rightpad", [0, 0, 0.05],
textColorRGB=[1, 0, 0],
textSize=1.,
parentObjectUniqueId=self.gripperUid,
parentLinkIndex=self.rightFingerPadIndex)
p.addUserDebugLine([0, 0, 0], [0.1, 0, 0], [1, 0, 0],
parentObjectUniqueId=self.gripperUid,
parentLinkIndex=self.rightFingerPadIndex)
p.addUserDebugLine([0, 0, 0], [0, 0.1, 0], [0, 1, 0],
parentObjectUniqueId=self.gripperUid,
parentLinkIndex=self.rightFingerPadIndex)
p.addUserDebugLine([0, 0, 0], [0, 0, 0.1], [0, 0, 1],
parentObjectUniqueId=self.gripperUid,
parentLinkIndex=self.rightFingerPadIndex)
p.addUserDebugText("leftpad", [0, 0, 0.05],
textColorRGB=[1, 0, 0],
textSize=1.,
parentObjectUniqueId=self.gripperUid,
parentLinkIndex=self.leftFingerPadIndex)
p.addUserDebugLine([0, 0, 0], [0.1, 0, 0], [1, 0, 0],
parentObjectUniqueId=self.gripperUid,
parentLinkIndex=self.leftFingerPadIndex)
p.addUserDebugLine([0, 0, 0], [0, 0.1, 0], [0, 1, 0],
parentObjectUniqueId=self.gripperUid,
parentLinkIndex=self.leftFingerPadIndex)
p.addUserDebugLine([0, 0, 0], [0, 0, 0.1], [0, 0, 1],
parentObjectUniqueId=self.gripperUid,
parentLinkIndex=self.leftFingerPadIndex)
p.addUserDebugText("wrist", [0, 0, 0.05],
textColorRGB=[1, 0, 0],
textSize=1.,
parentObjectUniqueId=self.gripperUid,
parentLinkIndex=self.wristWIndex)
p.addUserDebugLine([0, 0, 0], [0.1, 0, 0], [1, 0, 0],
parentObjectUniqueId=self.gripperUid,
parentLinkIndex=self.wristWIndex)
p.addUserDebugLine([0, 0, 0], [0, 0.1, 0], [0, 1, 0],
parentObjectUniqueId=self.gripperUid,
parentLinkIndex=self.wristWIndex)
p.addUserDebugLine([0, 0, 0], [0, 0, 0.1], [0, 0, 1],
parentObjectUniqueId=self.gripperUid,
parentLinkIndex=self.wristWIndex)
def step(self, action):
self.time = self.time + self.timeStep
applied_Voltage = p.readUserDebugParameter(self.voltageId)
frictionForce = p.readUserDebugParameter(self.frictionId)
jointTorque = p.readUserDebugParameter(self.torqueId)
# Calculate motor output torque and current
pos, vel, _, _ = p.getJointState(self.wheel, 1)
motor_torque, motor_current = self.motor_1.torque_from_voltage(
TimestampInput(applied_Voltage, self.time), vel)
# apply a joint torque from motor
p.setJointMotorControl2(self.wheel,
1,
p.TORQUE_CONTROL,
force=motor_torque)
# set the joint friction
p.setJointMotorControl2(self.wheel,
1,
p.VELOCITY_CONTROL,
targetVelocity=0,
force=frictionForce)
# Display
p.addUserDebugText("Motor Torque: " + str(motor_torque), [1.2, 0, 0.1],
textColorRGB=[0, 0, 0],
textSize=1,
replaceItemUniqueId=self.display_joint_torque)
p.addUserDebugText("Motor Current: " + str(motor_current),
[1.2, 0, 0.3],
textColorRGB=[0, 0, 0],
textSize=1,
replaceItemUniqueId=self.display_joint_current)
p.addUserDebugText("Time: " + str(self.time), [1.2, 0, 0.6],
textColorRGB=[0, 0, 0],
textSize=1,
replaceItemUniqueId=self.display_time)
p.stepSimulation()
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
erpId = p.addUserDebugParameter("erp",0,1,0.2)
kpMotorId = p.addUserDebugParameter("kpMotor",0,1,.2)
forceMotorId = p.addUserDebugParameter("forceMotor",0,2000,1000)
jointTypes = ["JOINT_REVOLUTE","JOINT_PRISMATIC",
"JOINT_SPHERICAL","JOINT_PLANAR","JOINT_FIXED"]
startLocations=[[0,0,2],[0,0,0],[0,0,-2],[0,0,-4]]
p.addUserDebugText("Stable PD", [startLocations[0][0],startLocations[0][1]+1,startLocations[0][2]], [0,0,0])
p.addUserDebugText("Spherical Drive", [startLocations[1][0],startLocations[1][1]+1,startLocations[1][2]], [0,0,0])
p.addUserDebugText("Explicit PD", [startLocations[2][0],startLocations[2][1]+1,startLocations[2][2]], [0,0,0])
p.addUserDebugText("Kinematic", [startLocations[3][0],startLocations[3][1]+1,startLocations[3][2]], [0,0,0])
humanoid = p.loadURDF("humanoid/humanoid.urdf", startLocations[0],globalScaling=0.25, useFixedBase=False, flags=p.URDF_MAINTAIN_LINK_ORDER)
humanoid2 = p.loadURDF("humanoid/humanoid.urdf", startLocations[1],globalScaling=0.25, useFixedBase=False, flags=p.URDF_MAINTAIN_LINK_ORDER)
humanoid3 = p.loadURDF("humanoid/humanoid.urdf", startLocations[2],globalScaling=0.25, useFixedBase=False, flags=p.URDF_MAINTAIN_LINK_ORDER)
humanoid4 = p.loadURDF("humanoid/humanoid.urdf", startLocations[3],globalScaling=0.25, useFixedBase=False, flags=p.URDF_MAINTAIN_LINK_ORDER)
humanoid_fix = p.createConstraint(humanoid,-1,-1,-1,p.JOINT_FIXED,[0,0,0],[0,0,0],startLocations[0],[0,0,0,1])
humanoid2_fix = p.createConstraint(humanoid2,-1,-1,-1,p.JOINT_FIXED,[0,0,0],[0,0,0],startLocations[1],[0,0,0,1])
humanoid3_fix = p.createConstraint(humanoid3,-1,-1,-1,p.JOINT_FIXED,[0,0,0],[0,0,0],startLocations[2],[0,0,0,1])
humanoid3_fix = p.createConstraint(humanoid4,-1,-1,-1,p.JOINT_FIXED,[0,0,0],[0,0,0],startLocations[3],[0,0,0,1])
kukaEndEffectorIndex = 6
kukaGripperIndex = 7
leftfing1Id, leftfing2Id = 8, 10
rightfing1Id, rightfing2Id = 11, 13
for b_id in block_ids:
p.changeDynamics(b_id, -1, lateralFriction=3, frictionAnchor=1)
p.changeDynamics(kukaId, leftfing2Id, lateralFriction=3, frictionAnchor=1)
p.changeDynamics(kukaId, rightfing2Id, lateralFriction=3, frictionAnchor=1)
objectNum = p.getNumBodies()
for i in range(p.getNumJoints(kukaId)):
show_coords(kukaId, parentLinkIndex=i, frameLength=0.08)
p.addUserDebugText("link:{}".format(i),
textPosition=[0.01, 0, 0],
textColorRGB=[1, 0, 0],
textSize=1,
parentObjectUniqueId=kukaId,
parentLinkIndex=i)
# pprint.pprint(p.getJointInfo(kukaId, i)[3])
# print(p.getJointInfo(kukaId, i)[3])
for b_id in block_ids:
show_coords(b_id, frameLength=0.05)
#lower limits for null space
ll = [-.967, -2, -2.96, 0.19, -2.96, -2.09, -3.05]
#upper limits for null space
ul = [.967, 2, 2.96, 2.29, 2.96, 2.09, 3.05]
#joint ranges for null space
jr = [5.8, 4, 5.8, 4, 5.8, 4, 6]
#restposes for null space
((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]
quat_y = quat[1]
quat_z = quat[2]
p.connect(p.GUI)
p.loadURDF("plane.urdf", [0, 0, -0.25])
minitaur = p.loadURDF("quadruped/minitaur_single_motor.urdf", useFixedBase=True)
print(p.getNumJoints(minitaur))
p.resetDebugVisualizerCamera(cameraDistance=1,
cameraYaw=23.2,
cameraPitch=-6.6,
cameraTargetPosition=[-0.064, .621, -0.2])
motorJointId = 1
p.setJointMotorControl2(minitaur, motorJointId, p.VELOCITY_CONTROL, targetVelocity=100000, force=0)
p.resetJointState(minitaur, motorJointId, targetValue=0, targetVelocity=1)
angularDampingSlider = p.addUserDebugParameter("angularDamping", 0, 1, 0)
jointFrictionForceSlider = p.addUserDebugParameter("jointFrictionForce", 0, 0.1, 0)
textId = p.addUserDebugText("jointVelocity=0", [0, 0, -0.2])
p.setRealTimeSimulation(1)
while (1):
frictionForce = p.readUserDebugParameter(jointFrictionForceSlider)
angularDamping = p.readUserDebugParameter(angularDampingSlider)
p.setJointMotorControl2(minitaur,
motorJointId,
p.VELOCITY_CONTROL,
targetVelocity=0,
force=frictionForce)
p.changeDynamics(minitaur, motorJointId, linearDamping=0, angularDamping=angularDamping)
time.sleep(0.01)
txt = "jointVelocity=" + str(p.getJointState(minitaur, motorJointId)[1])
prevTextId = textId
textId = p.addUserDebugText(txt, [0, 0, -0.2])
once = 1
if (once):
logId = -1 #p.startStateLogging(p.STATE_LOGGING_PROFILE_TIMINGS, "userDebugItems1.json")
print("logId userdebug")
print(logId)
for i in range(numLines):
spacing = 0.01
textPos = [.1 - (i + 1) * spacing, .1, 0.011]
text = "ABCDEFGH\nIJKLMNOPQRSTUVWXYZ\n01234567890abcdefg\n" * 10
lines[i] = p.addUserDebugText(text,
textColorRGB=[0, 0, 0],
textSize=0.01,
textPosition=textPos,
textOrientation=textOrn,
parentObjectUniqueId=uiCube,
parentLinkIndex=-1)
if (once):
once = 0
if (logId and logId > 0):
p.stopStateLogging(logId)
frameNr = 0
objectInfo = ""
pointRay = -1
rayLen = 100
while (1):
targetPosition=0,
force=0)
#print("joint",i,"=",p.getJointInfo(pole2,i))
timeStepId = p.addUserDebugParameter("timeStep", 0.001, 0.1, 0.01)
desiredPosCartId = p.addUserDebugParameter("desiredPosCart", -10, 10, 2)
desiredVelCartId = p.addUserDebugParameter("desiredVelCart", -10, 10, 0)
kpCartId = p.addUserDebugParameter("kpCart", 0, 500, 1300)
kdCartId = p.addUserDebugParameter("kdCart", 0, 300, 150)
maxForceCartId = p.addUserDebugParameter("maxForceCart", 0, 5000, 1000)
textColor = [1, 1, 1]
shift = 0.05
p.addUserDebugText("explicit PD", [shift, 0, .1],
textColor,
parentObjectUniqueId=pole,
parentLinkIndex=1)
p.addUserDebugText("explicit PD plugin", [shift, 0, -.1],
textColor,
parentObjectUniqueId=pole2,
parentLinkIndex=1)
p.addUserDebugText("stablePD", [shift, 0, .1],
textColor,
parentObjectUniqueId=pole4,
parentLinkIndex=1)
p.addUserDebugText("position constraint", [shift, 0, -.1],
textColor,
parentObjectUniqueId=pole3,
parentLinkIndex=1)
desiredPosPoleId = p.addUserDebugParameter("desiredPosPole", -10, 10, 0)
once = 1
if (once):
logId = -1 #p.startStateLogging(p.STATE_LOGGING_PROFILE_TIMINGS, "userDebugItems1.json")
print("logId userdebug")
print(logId)
for i in range(numLines):
spacing = 0.01
textPos = [.1 - (i + 1) * spacing, .1, 0.011]
text = "ABCDEFGH\nIJKLMNOPQRSTUVWXYZ\n01234567890abcdefg\n" * 10
lines[i] = p.addUserDebugText(text,
textColorRGB=[0, 0, 0],
textSize=0.01,
textPosition=textPos,
textOrientation=textOrn,
parentObjectUniqueId=uiCube,
parentLinkIndex=-1)
if (once):
once = 0
if (logId and logId > 0):
p.stopStateLogging(logId)
frameNr = 0
objectInfo = ""
pointRay = -1
rayLen = 100
while (1):
'''
#show this for 10 seconds
now = time.time()
while (time.time() < now+10):
p.stepSimulation()
'''
for jointIndex in range(p.getNumJoints(robot)):
joint = p.getJointInfo(robot, jointIndex)
if joint[2] == p.JOINT_FIXED:
continue
t = 0
p.removeAllUserDebugItems()
p.addUserDebugText(
"Joint: {} {}".format(jointIndex, joint[1].decode('utf8')),
[0., 0., 1.5], #[shift, 0, -.1],
textColor,
parentObjectUniqueId=robot,
parentLinkIndex=1)
max, min = joint[8:10]
mean = np.mean(joint[8:10])
amp = 0.5 * np.diff(joint[8:10])[0]
nsteps = 1000
dt = 1. / nsteps
for istep in range(nsteps):
joint_pos = mean + amp * np.sin(2. * np.pi * istep / nsteps)
if my_move_mode == MoveMode.POSITION:
# Non-dynamical movement:
p.resetJointState(robot, jointIndex, joint_pos)
elif my_move_mode == MoveMode.VELOCITY:
def test_model(env,
model=None,
implemented_combos=None,
arg_dict=None,
model_logdir=None,
deterministic=False):
if arg_dict.get("model_path") is None and model is None:
print(
"Path to the model using --model_path argument not specified. Testing random actions in selected environment."
)
test_env(env, arg_dict)
else:
try:
if arg_dict["algo"] == "multi":
model_args = implemented_combos[arg_dict["algo"]][
arg_dict["train_framework"]][1]
model = implemented_combos[arg_dict["algo"]][
arg_dict["train_framework"]][0].load(
arg_dict["model_path"], env=model_args[1].env)
else:
model = implemented_combos[arg_dict["algo"]][
arg_dict["train_framework"]][0].load(
arg_dict["model_path"])
except:
if (arg_dict["algo"] in implemented_combos.keys()) and (
arg_dict["train_framework"] not in list(
implemented_combos[arg_dict["algo"]].keys())):
err = "{} is only implemented with {}".format(
arg_dict["algo"],
list(implemented_combos[arg_dict["algo"]].keys())[0])
elif arg_dict["algo"] not in implemented_combos.keys():
err = "{} algorithm is not implemented.".format(
arg_dict["algo"])
else:
err = "invalid model_path argument"
raise Exception(err)
images = [] # Empty list for gif images
success_episodes_num = 0
distance_error_sum = 0
vel = arg_dict["max_velocity"]
force = arg_dict["max_force"]
steps_sum = 0
p.resetDebugVisualizerCamera(1.8, 200, -30, [-1.0, .1, 0.1])
#p.setRealTimeSimulation(1)
#p.setTimeStep(0.01)
for e in range(arg_dict["eval_episodes"]):
done = False
obs = env.reset()
is_successful = 0
distance_error = 0
step_sum = 0
while not done:
steps_sum += 1
action, _state = model.predict(obs, deterministic=deterministic)
obs, reward, done, info = env.step(action)
is_successful = not info['f']
distance_error = info['d']
#p.addUserDebugText(f"Step:{steps_sum}",
# [-.5, .0, 0.3], textSize=1.0, lifeTime=0.5, textColorRGB=[0.0, 0.0, 1])
p.addUserDebugText(
f"Action (Gripper):{matrix(np.around(np.array(action),5))}",
[.8, .5, 0.05],
textSize=1.0,
lifeTime=0.5,
textColorRGB=[1, 0, 0])
p.addUserDebugText(
f"Endeff:{matrix(np.around(np.array(info['o']['additional_obs']['endeff_xyz']),5))}",
[.8, .5, 0.15],
textSize=1.0,
lifeTime=0.5,
textColorRGB=[0.0, 1, 0.0])
#p.addUserDebugText(f"Object:{matrix(np.around(np.array(info['o']['actual_state']),5))}",
# [.8, .5, 0.15], textSize=1.0, lifeTime=0.5, textColorRGB=[0.0, 0.0, 1])
p.addUserDebugText(f"Network:{env.env.reward.current_network}",
[.8, .5, 0.25],
textSize=1.0,
lifeTime=0.5,
textColorRGB=[0.0, 0.0, 1])
p.addUserDebugText(f"Subtask:{env.env.task.current_task}",
[.8, .5, 0.35],
textSize=1.0,
lifeTime=0.5,
textColorRGB=[0.4, 0.2, 1])
p.addUserDebugText(f"Episode:{e}", [.8, .5, 0.45],
textSize=1.0,
lifeTime=0.5,
textColorRGB=[0.4, 0.2, .3])
p.addUserDebugText(f"Step:{steps_sum}", [.8, .5, 0.55],
textSize=1.0,
lifeTime=0.5,
textColorRGB=[0.2, 0.8, 1])
p.addUserDebugText(f"Velocity:{vel}", [.8, .5, 0.85],
textSize=1.0,
lifeTime=0.5,
textColorRGB=[0.6, 0.8, .3])
p.addUserDebugText(f"Force:{force}", [.8, .5, 0.95],
textSize=1.0,
lifeTime=0.5,
textColorRGB=[0.3, 0.2, .4])
if (arg_dict["record"] > 0) and (len(images) < 800):
render_info = env.render(mode="rgb_array",
camera_id=arg_dict["camera"])
image = render_info[arg_dict["camera"]]["image"]
images.append(image)
print(f"appending image: total size: {len(images)}]")
success_episodes_num += is_successful
distance_error_sum += distance_error
mean_distance_error = distance_error_sum / arg_dict["eval_episodes"]
mean_steps_num = steps_sum // arg_dict["eval_episodes"]
print("#---------Evaluation-Summary---------#")
print("{} of {} episodes ({} %) were successful".format(
success_episodes_num, arg_dict["eval_episodes"],
success_episodes_num / arg_dict["eval_episodes"] * 100))
print("Mean distance error is {:.2f}%".format(mean_distance_error * 100))
print("Mean number of steps {}".format(mean_steps_num))
print("#------------------------------------#")
model_name = arg_dict["algo"] + '_' + str(arg_dict["steps"])
file = open(os.path.join(model_logdir, "train_" + model_name + ".txt"),
'a')
file.write("\n")
file.write("#Evaluation results: \n")
file.write("#{} of {} episodes were successful \n".format(
success_episodes_num, arg_dict["eval_episodes"]))
file.write("#Mean distance error is {:.2f}% \n".format(
mean_distance_error * 100))
file.write("#Mean number of steps {}\n".format(mean_steps_num))
file.close()
if arg_dict["record"] == 1:
gif_path = os.path.join(model_logdir, "train_" + model_name + ".gif")
imageio.mimsave(
gif_path,
[np.array(img) for i, img in enumerate(images) if i % 2 == 0],
fps=15)
os.system('./utils/gifopt -O3 --lossy=5 -o {dest} {source}'.format(
source=gif_path, dest=gif_path))
print("Record saved to " + gif_path)
elif arg_dict["record"] == 2:
video_path = os.path.join(model_logdir, "train_" + model_name + ".avi")
height, width, layers = image.shape
out = cv2.VideoWriter(video_path, cv2.VideoWriter_fourcc(*'XVID'), 30,
(width, height))
for img in images:
out.write(cv2.cvtColor(img, cv2.COLOR_RGB2BGR))
out.release()
print("Record saved to " + video_path)
def test_env(env, arg_dict):
#arg_dict["gui"] == 1
debug_mode = True
spawn_objects = False
action_control = "keyboard" #"observation", "random", "keyboard" or "slider"
visualize_sampling = False
visualize_traj = True
env.render("human")
#env.reset()
joints = [
'Joint1', 'Joint2', 'Joint3', 'Joint4', 'Joint5', 'Joint6', 'Joint7',
'Joint 8', 'Joint 9', 'Joint10', 'Joint11', 'Joint12', 'Joint13',
'Joint14', 'Joint15', 'Joint16', 'Joint17', 'Joint 18', 'Joint 19'
]
jointparams = [
'Jnt1', 'Jnt2', 'Jnt3', 'Jnt4', 'Jnt5', 'Jnt6', 'Jnt7', 'Jnt 8',
'Jnt 9', 'Jnt10', 'Jnt11', 'Jnt12', 'Jnt13', 'Jnt14', 'Jnt15', 'Jnt16',
'Jnt17', 'Jnt 18', 'Jnt 19'
]
cube = [
'Cube1', 'Cube2', 'Cube3', 'Cube4', 'Cube5', 'Cube6', 'Cube7', 'Cube8',
'Cube9', 'Cube10', 'Cube11', 'Cube12', 'Cube13', 'Cube14', 'Cube15',
'Cube16', 'Cube17', 'Cube18', 'Cube19'
]
cubecount = 0
if debug_mode:
if arg_dict["gui"] == 0:
print("Add --gui 1 parameter to visualize environment")
p.configureDebugVisualizer(p.COV_ENABLE_GUI, 0)
p.resetDebugVisualizerCamera(1.7, 200, -20, [-0.1, .0, 0.05])
p.setAdditionalSearchPath(pybullet_data.getDataPath())
#newobject = p.loadURDF("cube.urdf", [3.1,3.7,0.1])
#p.changeDynamics(newobject, -1, lateralFriction=1.00)
#p.setRealTimeSimulation(1)
if action_control == "slider":
if "joints" in arg_dict["robot_action"]:
if 'gripper' in arg_dict["robot_action"]:
print("gripper is present")
for i in range(env.action_space.shape[0]):
if i < (env.action_space.shape[0] -
len(env.env.robot.gjoints_rest_poses)):
joints[i] = p.addUserDebugParameter(
joints[i], env.action_space.low[i],
env.action_space.high[i],
env.env.robot.init_joint_poses[i])
else:
joints[i] = p.addUserDebugParameter(
joints[i], env.action_space.low[i],
env.action_space.high[i], .02)
else:
for i in range(env.action_space.shape[0]):
joints[i] = p.addUserDebugParameter(
joints[i], env.action_space.low[i],
env.action_space.high[i],
env.env.robot.init_joint_poses[i])
elif "absolute" in arg_dict["robot_action"]:
if 'gripper' in arg_dict["robot_action"]:
print("gripper is present")
for i in range(env.action_space.shape[0]):
if i < (env.action_space.shape[0] -
len(env.env.robot.gjoints_rest_poses)):
joints[i] = p.addUserDebugParameter(
joints[i], -1, 1, arg_dict["robot_init"][i])
else:
joints[i] = p.addUserDebugParameter(
joints[i], -1, 1, .02)
else:
for i in range(env.action_space.shape[0]):
joints[i] = p.addUserDebugParameter(
joints[i], -1, 1, arg_dict["robot_init"][i])
elif "step" in arg_dict["robot_action"]:
if 'gripper' in arg_dict["robot_action"]:
print("gripper is present")
for i in range(env.action_space.shape[0]):
if i < (env.action_space.shape[0] -
len(env.env.robot.gjoints_rest_poses)):
joints[i] = p.addUserDebugParameter(
joints[i], -1, 1, 0)
else:
joints[i] = p.addUserDebugParameter(
joints[i], -1, 1, .02)
else:
for i in range(env.action_space.shape[0]):
joints[i] = p.addUserDebugParameter(
joints[i], -1, 1, 0)
#maxvelo = p.addUserDebugParameter("Max Velocity", 0.1, 50, env.env.robot.joints_max_velo[0])
#maxforce = p.addUserDebugParameter("Max Force", 0.1, 300, env.env.robot.joints_max_force[0])
lfriction = p.addUserDebugParameter("Lateral Friction", 0, 100, 0)
rfriction = p.addUserDebugParameter("Spinning Friction", 0, 100, 0)
ldamping = p.addUserDebugParameter("Linear Damping", 0, 100, 0)
adamping = p.addUserDebugParameter("Angular Damping", 0, 100, 0)
#action.append(jointparams[i])
if visualize_sampling:
visualize_sampling_area(arg_dict)
#visualgr = p.createVisualShape(shapeType=p.GEOM_SPHERE, radius=.005, rgbaColor=[0,0,1,.1])
if action_control == "random":
action = env.action_space.sample()
if action_control == "keyboard":
action = arg_dict["robot_init"]
if "gripper" in arg_dict["robot_action"]:
action.append(.1)
action.append(.1)
if action_control == "slider":
action = []
for i in range(env.action_space.shape[0]):
jointparams[i] = p.readUserDebugParameter(joints[i])
action.append(jointparams[i])
for e in range(10000):
env.reset()
#if spawn_objects:
# cube[e] = p.loadURDF(pkg_resources.resource_filename("myGym", os.path.join("envs", "objects/assembly/urdf/cube_holes.urdf")), [0, 0.5, .1])
#if visualize_traj:
# visualize_goal(info)
for t in range(arg_dict["max_episode_steps"]):
if action_control == "slider":
action = []
for i in range(env.action_space.shape[0]):
jointparams[i] = p.readUserDebugParameter(joints[i])
action.append(jointparams[i])
#env.env.robot.joints_max_velo[i] = p.readUserDebugParameter(maxvelo)
#env.env.robot.joints_max_force[i] = p.readUserDebugParameter(maxforce)
if action_control == "observation":
if arg_dict["robot_action"] == "joints":
action = info['o']["additional_obs"]["joints_angles"] #n
else:
action = info['o']["additional_obs"]["endeff_xyz"]
#action[0] +=.3
elif action_control == "keyboard":
keypress = p.getKeyboardEvents()
#print(action)
if 97 in keypress.keys() and keypress[97] == 1:
action[2] += .03
print(action)
if 122 in keypress.keys() and keypress[122] == 1:
action[2] -= .03
print(action)
if 65297 in keypress.keys() and keypress[65297] == 1:
action[1] -= .03
print(action)
if 65298 in keypress.keys() and keypress[65298] == 1:
action[1] += .03
print(action)
if 65295 in keypress.keys() and keypress[65295] == 1:
action[0] += .03
print(action)
if 65296 in keypress.keys() and keypress[65296] == 1:
action[0] -= .03
print(action)
if 120 in keypress.keys() and keypress[120] == 1:
action[3] -= .03
action[4] -= .03
print(action)
if 99 in keypress.keys() and keypress[99] == 1:
action[3] += .03
action[4] += .03
print(action)
if 100 in keypress.keys() and keypress[100] == 1:
cube[cubecount] = p.loadURDF(
pkg_resources.resource_filename(
"myGym",
os.path.join(
"envs",
"objects/assembly/urdf/cube_holes.urdf")),
[action[0], action[1], action[2] - 0.2])
change_dynamics(cube[cubecount], lfriction, rfriction,
ldamping, adamping)
cubecount += 1
if "step" in arg_dict["robot_action"]:
action[:3] = np.multiply(action[:3], 10)
#for i in range (env.action_space.shape[0]):
# env.env.robot.joints_max_velo[i] = p.readUserDebugParameter(maxvelo)
# env.env.robot.joints_max_force[i] = p.readUserDebugParameter(maxforce)
elif action_control == "random":
action = env.action_space.sample()
#print (f"Action:{action}")
observation, reward, done, info = env.step(action)
if visualize_traj:
#visualize_trajectories(info,action)
p.addUserDebugText(
f"Action (Gripper):{matrix(np.around(np.array(action),5))}",
[.8, .5, 0.2],
textSize=1.0,
lifeTime=0.5,
textColorRGB=[1, 0, 0])
p.addUserDebugText(
f"Endeff:{matrix(np.around(np.array(info['o']['additional_obs']['endeff_xyz']),5))}",
[.8, .5, 0.1],
textSize=1.0,
lifeTime=0.5,
textColorRGB=[0.0, 1, 0.0])
#p.addUserDebugText(f"Object:{matrix(np.around(np.array(info['o']['actual_state']),5))}",
# [.8, .5, 0.15], textSize=1.0, lifeTime=0.5, textColorRGB=[0.0, 0.0, 1])
p.addUserDebugText(f"Network:{env.env.reward.current_network}",
[.8, .5, 0.25],
textSize=1.0,
lifeTime=0.5,
textColorRGB=[0.0, 0.0, 1])
p.addUserDebugText(f"Subtask:{env.env.task.current_task}",
[.8, .5, 0.35],
textSize=1.0,
lifeTime=0.5,
textColorRGB=[0.4, 0.2, 1])
p.addUserDebugText(f"Episode:{e}", [.8, .5, 0.45],
textSize=1.0,
lifeTime=0.5,
textColorRGB=[0.4, 0.2, .3])
p.addUserDebugText(f"Step:{t}", [.8, .5, 0.55],
textSize=1.0,
lifeTime=0.5,
textColorRGB=[0.2, 0.8, 1])
#visualize_goal(info)
#if debug_mode:
#print("Reward is {}, observation is {}".format(reward, observation))
#if t>=1:
#action = matrix(np.around(np.array(action),5))
#oaction = env.env.robot.get_joints_states()
#oaction = matrix(np.around(np.array(oaction[0:action.shape[0]]),5))
#diff = matrix(np.around(np.array(action-oaction),5))
#print(env.env.robot.get_joints_states())
#print(f"Step:{t}")
#print (f"RAction:{action}")
#print(f"OAction:{oaction}")
#print(f"DAction:{diff}")
#p.addUserDebugText(f"DAction:{diff}",
# [1, 1, 0.1], textSize=1.0, lifeTime=0.05, textColorRGB=[0.6, 0.0, 0.6])
#time.sleep(.5)
#clear()
#if action_control == "slider":
# action=[]
if "step" in arg_dict["robot_action"]:
action[:3] = [0, 0, 0]
if arg_dict["visualize"]:
visualizations = [[], []]
env.render("human")
for camera_id in range(len(env.cameras)):
camera_render = env.render(mode="rgb_array",
camera_id=camera_id)
image = cv2.cvtColor(camera_render[camera_id]["image"],
cv2.COLOR_RGB2BGR)
depth = camera_render[camera_id]["depth"]
image = cv2.copyMakeBorder(image,
30,
10,
10,
20,
cv2.BORDER_CONSTANT,
value=[255, 255, 255])
cv2.putText(image, 'Camera {}'.format(camera_id), (10, 20),
cv2.FONT_HERSHEY_SIMPLEX, .5, (0, 0, 0), 1, 0)
visualizations[0].append(image)
depth = cv2.copyMakeBorder(depth,
30,
10,
10,
20,
cv2.BORDER_CONSTANT,
value=[255, 255, 255])
cv2.putText(depth, 'Camera {}'.format(camera_id), (10, 20),
cv2.FONT_HERSHEY_SIMPLEX, .5, (0, 0, 0), 1, 0)
visualizations[1].append(depth)
if len(visualizations[0]) % 2 != 0:
visualizations[0].append(
255 *
np.ones(visualizations[0][0].shape, dtype=np.uint8))
visualizations[1].append(
255 *
np.ones(visualizations[1][0].shape, dtype=np.float32))
fig_rgb = np.vstack((np.hstack((visualizations[0][0::2])),
np.hstack((visualizations[0][1::2]))))
fig_depth = np.vstack((np.hstack((visualizations[1][0::2])),
np.hstack((visualizations[1][1::2]))))
cv2.imshow('Camera RGB renders', fig_rgb)
cv2.imshow('Camera depthrenders', fig_depth)
cv2.waitKey(1)
if done:
print("Episode finished after {} timesteps".format(t + 1))
break
p.setAdditionalSearchPath(pybullet_data.getDataPath())
p.loadURDF("plane.urdf", [0, 0, -0.25])
minitaur = p.loadURDF("quadruped/minitaur_single_motor.urdf", useFixedBase=True)
print(p.getNumJoints(minitaur))
p.resetDebugVisualizerCamera(cameraDistance=1,
cameraYaw=23.2,
cameraPitch=-6.6,
cameraTargetPosition=[-0.064, .621, -0.2])
motorJointId = 1
p.setJointMotorControl2(minitaur, motorJointId, p.VELOCITY_CONTROL, targetVelocity=100000, force=0)
p.resetJointState(minitaur, motorJointId, targetValue=0, targetVelocity=1)
angularDampingSlider = p.addUserDebugParameter("angularDamping", 0, 1, 0)
jointFrictionForceSlider = p.addUserDebugParameter("jointFrictionForce", 0, 0.1, 0)
textId = p.addUserDebugText("jointVelocity=0", [0, 0, -0.2])
p.setRealTimeSimulation(1)
while (1):
frictionForce = p.readUserDebugParameter(jointFrictionForceSlider)
angularDamping = p.readUserDebugParameter(angularDampingSlider)
p.setJointMotorControl2(minitaur,
motorJointId,
p.VELOCITY_CONTROL,
targetVelocity=0,
force=frictionForce)
p.changeDynamics(minitaur, motorJointId, linearDamping=0, angularDamping=angularDamping)
time.sleep(0.01)
txt = "jointVelocity=" + str(p.getJointState(minitaur, motorJointId)[1])
prevTextId = textId
textId = p.addUserDebugText(txt, [0, 0, -0.2])
import pybullet as p
import time
cid = p.connect(p.SHARED_MEMORY)
if (cid<0):
p.connect(p.GUI)
p.loadURDF("plane.urdf")
kuka = p.loadURDF("kuka_iiwa/model.urdf")
p.addUserDebugText("tip", [0,0,0.1],textColorRGB=[1,0,0],textSize=1.5,parentObjectUniqueId=kuka, parentLinkIndex=6)
p.addUserDebugLine([0,0,0],[0.1,0,0],[1,0,0],parentObjectUniqueId=kuka, parentLinkIndex=6)
p.addUserDebugLine([0,0,0],[0,0.1,0],[0,1,0],parentObjectUniqueId=kuka, parentLinkIndex=6)
p.addUserDebugLine([0,0,0],[0,0,0.1],[0,0,1],parentObjectUniqueId=kuka, parentLinkIndex=6)
p.setRealTimeSimulation(0)
angle=0
while (True):
time.sleep(0.01)
p.resetJointState(kuka,2,angle)
p.resetJointState(kuka,3,angle)
angle+=0.01
#Once the video is recorded, you can extract all individual frames using ffmpeg
#mkdir frames
#ffmpeg -i test.mp4 "frames/out-%03d.png"
#by default, PyBullet runs at 240Hz
p.connect(p.GUI,
options="--width=1920 --height=1080 --mp4=\"test.mp4\" --mp4fps=240")
p.setAdditionalSearchPath(pybullet_data.getDataPath())
p.configureDebugVisualizer(p.COV_ENABLE_GUI, 0)
p.configureDebugVisualizer(p.COV_ENABLE_SINGLE_STEP_RENDERING, 1)
p.loadURDF("plane.urdf")
#in 3 seconds, the object travels about 0.5*g*t^2 meter ~ 45 meter.
r2d2 = p.loadURDF("r2d2.urdf", [0, 0, 45])
#disable linear damping
p.changeDynamics(r2d2, -1, linearDamping=0)
p.setGravity(0, 0, -10)
for i in range(3 * 240):
txt = "frame " + str(i)
item = p.addUserDebugText(txt, [0, 1, 0])
p.stepSimulation()
#synchronize the visualizer (rendering frames for the video mp4) with stepSimulation
p.configureDebugVisualizer(p.COV_ENABLE_SINGLE_STEP_RENDERING, 1)
#print("r2d2 vel=", p.getBaseVelocity(r2d2)[0][2])
p.removeUserDebugItem(item)
p.disconnect()
p.setJointMotorControl2(pole3, i, p.POSITION_CONTROL, targetPosition=0, force=0)
p.setJointMotorControl2(pole4, i, p.POSITION_CONTROL, targetPosition=0, force=0)
#print("joint",i,"=",p.getJointInfo(pole2,i))
timeStepId = p.addUserDebugParameter("timeStep", 0.001, 0.1, 0.01)
desiredPosCartId = p.addUserDebugParameter("desiredPosCart", -10, 10, 2)
desiredVelCartId = p.addUserDebugParameter("desiredVelCart", -10, 10, 0)
kpCartId = p.addUserDebugParameter("kpCart", 0, 500, 1300)
kdCartId = p.addUserDebugParameter("kdCart", 0, 300, 150)
maxForceCartId = p.addUserDebugParameter("maxForceCart", 0, 5000, 1000)
textColor = [1, 1, 1]
shift = 0.05
p.addUserDebugText("explicit PD", [shift, 0, .1],
textColor,
parentObjectUniqueId=pole,
parentLinkIndex=1)
p.addUserDebugText("explicit PD plugin", [shift, 0, -.1],
textColor,
parentObjectUniqueId=pole2,
parentLinkIndex=1)
p.addUserDebugText("stablePD", [shift, 0, .1],
textColor,
parentObjectUniqueId=pole4,
parentLinkIndex=1)
p.addUserDebugText("position constraint", [shift, 0, -.1],
textColor,
parentObjectUniqueId=pole3,
parentLinkIndex=1)
desiredPosPoleId = p.addUserDebugParameter("desiredPosPole", -10, 10, 0)