def getExtendedObservation(self):
self._observation = self._kuka.getObservation()
gripperState = p.getLinkState(self._kuka.kukaUid,self._kuka.kukaGripperIndex)
gripperPos = gripperState[0]
gripperOrn = gripperState[1]
blockPos,blockOrn = p.getBasePositionAndOrientation(self.blockUid)
invGripperPos,invGripperOrn = p.invertTransform(gripperPos,gripperOrn)
gripperMat = p.getMatrixFromQuaternion(gripperOrn)
dir0 = [gripperMat[0],gripperMat[3],gripperMat[6]]
dir1 = [gripperMat[1],gripperMat[4],gripperMat[7]]
dir2 = [gripperMat[2],gripperMat[5],gripperMat[8]]
gripperEul = p.getEulerFromQuaternion(gripperOrn)
#print("gripperEul")
#print(gripperEul)
blockPosInGripper,blockOrnInGripper = p.multiplyTransforms(invGripperPos,invGripperOrn,blockPos,blockOrn)
projectedBlockPos2D =[blockPosInGripper[0],blockPosInGripper[1]]
blockEulerInGripper = p.getEulerFromQuaternion(blockOrnInGripper)
#print("projectedBlockPos2D")
#print(projectedBlockPos2D)
#print("blockEulerInGripper")
#print(blockEulerInGripper)
#we return the relative x,y position and euler angle of block in gripper space
blockInGripperPosXYEulZ =[blockPosInGripper[0],blockPosInGripper[1],blockEulerInGripper[2]]
#p.addUserDebugLine(gripperPos,[gripperPos[0]+dir0[0],gripperPos[1]+dir0[1],gripperPos[2]+dir0[2]],[1,0,0],lifeTime=1)
#p.addUserDebugLine(gripperPos,[gripperPos[0]+dir1[0],gripperPos[1]+dir1[1],gripperPos[2]+dir1[2]],[0,1,0],lifeTime=1)
#p.addUserDebugLine(gripperPos,[gripperPos[0]+dir2[0],gripperPos[1]+dir2[1],gripperPos[2]+dir2[2]],[0,0,1],lifeTime=1)
self._observation.extend(list(blockInGripperPosXYEulZ))
return self._observation
def _reward(self):
#rewards is height of target object
blockPos,blockOrn=p.getBasePositionAndOrientation(self.blockUid)
closestPoints = p.getClosestPoints(self.blockUid,self._kuka.kukaUid,1000, -1, self._kuka.kukaEndEffectorIndex)
reward = -1000
numPt = len(closestPoints)
#print(numPt)
if (numPt>0):
#print("reward:")
reward = -closestPoints[0][8]*10
if (blockPos[2] >0.2):
reward = reward+10000
print("successfully grasped a block!!!")
#print("self._envStepCounter")
#print(self._envStepCounter)
#print("self._envStepCounter")
#print(self._envStepCounter)
#print("reward")
#print(reward)
#print("reward")
#print(reward)
return reward
def collect_observations(self, human):
#print("ordered_joint_indices")
#print(ordered_joint_indices)
jointStates = p.getJointStates(human,self.ordered_joint_indices)
j = np.array([self.current_relative_position(jointStates, human, *jtuple) for jtuple in self.ordered_joints]).flatten()
#print("j")
#print(j)
body_xyz, (qx, qy, qz, qw) = p.getBasePositionAndOrientation(human)
#print("body_xyz")
#print(body_xyz, qx,qy,qz,qw)
z = body_xyz[2]
self.distance = body_xyz[0]
if self.initial_z==None:
self.initial_z = z
(vx, vy, vz), _ = p.getBaseVelocity(human)
more = np.array([z-self.initial_z, 0.1*vx, 0.1*vy, 0.1*vz, qx, qy, qz, qw])
rcont = p.getContactPoints(human, -1, self.right_foot, -1)
#print("rcont")
#print(rcont)
lcont = p.getContactPoints(human, -1, self.left_foot, -1)
#print("lcont")
#print(lcont)
feet_contact = np.array([len(rcont)>0, len(lcont)>0])
return np.clip( np.concatenate([more] + [j] + [feet_contact]), -5, +5)
def dumpStateToFile(file): for i in range (p.getNumBodies()): pos,orn = p.getBasePositionAndOrientation(i) linVel,angVel = p.getBaseVelocity(i) txtPos = "pos="+str(pos)+"\n" txtOrn = "orn="+str(orn)+"\n" txtLinVel = "linVel"+str(linVel)+"\n" txtAngVel = "angVel"+str(angVel)+"\n" file.write(txtPos) file.write(txtOrn) file.write(txtLinVel) file.write(txtAngVel)
def getExtendedObservation(self):
self._observation = self._kuka.getObservation()
eeState = p.getLinkState(self._kuka.kukaUid,self._kuka.kukaEndEffectorIndex)
endEffectorPos = eeState[0]
endEffectorOrn = eeState[1]
blockPos,blockOrn = p.getBasePositionAndOrientation(self.blockUid)
invEEPos,invEEOrn = p.invertTransform(endEffectorPos,endEffectorOrn)
blockPosInEE,blockOrnInEE = p.multiplyTransforms(invEEPos,invEEOrn,blockPos,blockOrn)
blockEulerInEE = p.getEulerFromQuaternion(blockOrnInEE)
self._observation.extend(list(blockPosInEE))
self._observation.extend(list(blockEulerInEE))
return self._observation
def _reward(self):
"""Calculates the reward for the episode.
The reward is 1 if one of the objects is above height .2 at the end of the
episode.
"""
reward = 0
self._graspSuccess = 0
for uid in self._objectUids:
pos, _ = p.getBasePositionAndOrientation(
uid)
# If any block is above height, provide reward.
if pos[2] > 0.2:
self._graspSuccess += 1
reward = 1
break
return reward
def _termination(self):
#print (self._kuka.endEffectorPos[2])
state = p.getLinkState(self._kuka.kukaUid,self._kuka.kukaEndEffectorIndex)
actualEndEffectorPos = state[0]
#print("self._envStepCounter")
#print(self._envStepCounter)
if (self.terminated or self._envStepCounter>self._maxSteps):
self._observation = self.getExtendedObservation()
return True
maxDist = 0.005
closestPoints = p.getClosestPoints(self._kuka.trayUid, self._kuka.kukaUid,maxDist)
if (len(closestPoints)):#(actualEndEffectorPos[2] <= -0.43):
self.terminated = 1
#print("terminating, closing gripper, attempting grasp")
#start grasp and terminate
fingerAngle = 0.3
for i in range (100):
graspAction = [0,0,0.0001,0,fingerAngle]
self._kuka.applyAction(graspAction)
p.stepSimulation()
fingerAngle = fingerAngle-(0.3/100.)
if (fingerAngle<0):
fingerAngle=0
for i in range (1000):
graspAction = [0,0,0.001,0,fingerAngle]
self._kuka.applyAction(graspAction)
p.stepSimulation()
blockPos,blockOrn=p.getBasePositionAndOrientation(self.blockUid)
if (blockPos[2] > 0.23):
#print("BLOCKPOS!")
#print(blockPos[2])
break
state = p.getLinkState(self._kuka.kukaUid,self._kuka.kukaEndEffectorIndex)
actualEndEffectorPos = state[0]
if (actualEndEffectorPos[2]>0.5):
break
self._observation = self.getExtendedObservation()
return True
return False
lift_data.targetPosition = lift_position
lift_data.min_sync_time = 3
lift_pos, lift_vel, lift_acc = agent.gen_motion_list(lift_data)
ee_position = agent.get_tip_position(env)
obj_position = agent.get_obj_position(env.objectUid)
grasp_offset = [x - y for x, y in zip(ee_position, obj_position)]
for i in range(len(lift_pos)):
action = ['lift', lift_pos[i], init_tip_ori, fingers]
observation, reward, done = env.step(action)
fingers = observation[-2:]
# start rotating and recording
env.init_tip_pose = p.getLinkState(env.pandaUid, 11)[0]
env.init_tip_ori = p.getLinkState(env.pandaUid, 11)[1]
env.init_obj_pose = p.getBasePositionAndOrientation(env.objectUid)[0]
env.init_obj_ori = p.getBasePositionAndOrientation(env.objectUid)[1]
ee_position = agent.get_tip_position(env)
# enableCollision = 0
# p.setCollisionFilterPair(bodyUniqueIdA=env.pandaUid, bodyUniqueIdB=env.objectUid,
# linkIndexA=9, linkIndexB=-1, enableCollision=enableCollision)
# p.setCollisionFilterPair(bodyUniqueIdA=env.pandaUid, bodyUniqueIdB=env.objectUid,
# linkIndexA=10, linkIndexB=-1, enableCollision=enableCollision)
for j in range(6):
rotate_pos = [ee_position] * 120 # hz=240, lasting 0.5 second.
rotate_group_ori = []
pre_ori = agent.get_tip_orientation(env)
for i in range(len(rotate_pos)):
rotate_group_ori.append(p.getQuaternionFromEuler(
[0., -np.pi, np.pi / 2. + np.pi / 4 * i / len(rotate_pos) + np.pi / 4 * j]
)) # quaternion
def _step(self, action):
done = False
aspect = 1
camTargetPos = [0, 0, 0]
yaw = 40
pitch = 10.0
roll = 0
upAxisIndex = 2
camDistance = 4
pixelWidth = 320
pixelHeight = 240
nearPlane = 0.0001
farPlane = 0.022
lightDirection = [0, 1, 0]
lightColor = [1, 1, 1] #optional
agent_po = pb.getBasePositionAndOrientation(self.agent_mb)
x = agent_po[0][0]
y = agent_po[0][1]
z = agent_po[0][2]
if action == 0: #down
x += self.move
elif action == 1: #up
x -= self.move
elif action == 2: #left
y += self.move
elif action == 3: #right
y -= self.move
elif action == 4: #<
z += self.move
elif action == 5: #>
z -= self.move
elif action == 9:
#print(self.current_observation[:100])
#print(np.amax(self.current_observation))
print(np.unique(self.current_observation))
if self.is_touching():
print('I am touching')
else:
print('I am not touching')
pivot = [x, y, z]
orn = pb.getQuaternionFromEuler([0, 0, 0])
pb.changeConstraint(self.agent_cid,
pivot,
jointChildFrameOrientation=[0, 1, 0, 1],
maxForce=100)
viewMatrix = self.ahead_view()
projectionMatrix = pb.computeProjectionMatrixFOV(
self.fov, aspect, nearPlane, farPlane)
w, h, img_arr, depths, mask = pb.getCameraImage(
self.cameraImageHeight,
self.cameraImageHeight,
viewMatrix,
projectionMatrix,
lightDirection,
lightColor,
renderer=pb.ER_TINY_RENDERER)
info = [42]
pb.stepSimulation()
self.steps += 1
reward = 0
if self.is_touching():
touch_reward = 1000
new_obs = np.absolute(depths - 1.0)
# if you want binary representation of depth camera, uncomment
# the line below
#new_obs[new_obs > 0] = 1
self.current_observation = new_obs.flatten()
else:
touch_reward = 0
self.current_observation = np.zeros(self.cameraImageHeight *
self.cameraImageHeight)
agent_po = pb.getBasePositionAndOrientation(self.agent_mb)
obj_po = pb.getBasePositionAndOrientation(self.obj_to_classify)
distance = math.sqrt(
sum([(xi - yi)**2 for xi, yi in zip(obj_po[0], agent_po[0])
])) #TODO faster euclidean
if distance < self.prev_distance:
reward += 1 #* (self.max_steps - self.steps)
elif distance > self.prev_distance:
reward -= 1
reward -= distance
reward += touch_reward
self.prev_distance = distance
if self.steps >= self.max_steps or self.is_touching():
done = True
return self.current_observation, reward, done, info
def BeizerCurve(self, weights):
if self.bezier_timesteps == 0:
list_one = [
p.getBasePositionAndOrientation(self.box1)[0],
p.getLinkState(self.baxter, 29)[0]
]
list_two = [
p.getBasePositionAndOrientation(self.box2)[0],
p.getLinkState(self.baxter, 51)[0]
]
x_one = [p[0] for p in list_one]
y_one = [p[1] for p in list_one]
z_one = [p[2] for p in list_one]
x_one_mid = (x_one[0] + x_one[1]) / 2
y_one_mid = (y_one[0] + y_one[1]) / 2
z_one_mid = (z_one[0] + z_one[1]) / 2
x_one.append(x_one_mid)
y_one.append(y_one_mid)
z_one.append(z_one_mid)
self.bezier_right = [x_one, y_one, z_one]
x_two = [p[0] for p in list_two]
y_two = [p[1] for p in list_two]
z_two = [p[2] for p in list_two]
x_two_mid = (x_two[0] + x_two[1]) / 2
y_two_mid = (y_two[0] + y_two[1]) / 2
z_two_mid = (z_two[0] + z_two[1]) / 2
x_two.append(x_two_mid)
y_two.append(y_two_mid)
z_two.append(z_two_mid)
self.bezier_left = [x_two, y_two, z_two]
points_one, points_two = [], []
n, t_i = 3, np.linspace(0.0, 1.0, 6)[1:]
t = t_i[self.bezier_timesteps]
polynomial_array_one = np.array([
(comb(n, i) * (t**(i)) * (1 - t)**(n - i)) * w
for i, w in zip(range(0, 3), weights[:3])
])
polynomial_array_two = np.array([
(comb(n, i) * (t**(i)) * (1 - t)**(n - i)) * w
for i, w in zip(range(0, 3), weights[3:])
])
x_one_values = np.dot(self.bezier_right[0], polynomial_array_one)
y_one_values = np.dot(self.bezier_right[1], polynomial_array_one)
z_one_values = np.dot(self.bezier_right[2], polynomial_array_one)
x_two_values = np.dot(self.bezier_left[0], polynomial_array_two)
y_two_values = np.dot(self.bezier_left[1], polynomial_array_two)
z_two_values = np.dot(self.bezier_left[2], polynomial_array_two)
points_one.extend([x_one_values, y_one_values, z_one_values])
points_two.extend([x_two_values, y_two_values, z_two_values])
self.bezier_timesteps += 1
self.bezier_timesteps = self.bezier_timesteps % 5
return points_one, points_two
updateCam=1
time.sleep(timestep)
pts = p.getContactPoints()
#print("numPoints=",len(pts))
#for point in pts:
# print("Point:bodyA=", point[1],"bodyB=",point[2],"linkA=",point[3],"linkB=",point[4],"dist=",point[8],"force=",point[9])
states.append(p.saveState())
#observations.append(obs)
stateIndex = len(states)
#print("stateIndex =",stateIndex )
frame += 1
if (updateCam):
distance=5
yaw = 0
humanPos, humanOrn = p.getBasePositionAndOrientation(bike)
humanBaseVel = p.getBaseVelocity(bike)
#print("frame",frame, "humanPos=",humanPos, "humanVel=",humanBaseVel)
if (gui):
camInfo = p.getDebugVisualizerCamera()
curTargetPos = camInfo[11]
distance=camInfo[10]
yaw = camInfo[8]
pitch=camInfo[9]
targetPos = [0.95*curTargetPos[0]+0.05*humanPos[0],0.95*curTargetPos[1]+0.05*humanPos[1],curTargetPos[2]]
p.resetDebugVisualizerCamera(distance,yaw,pitch,targetPos);
def runSim(t, kp, ki, kd):
modArr, bodyId = startSim()
mods = modArr
mod1 = modArr[0]
mod2 = modArr[1]
mod3 = modArr[2]
mod4 = modArr[3]
k_i = ki
k_p = kp
k_d = kd
time_prev = time.time()
start_time = time.time()
store_data_time = time.time()
for i in range(4):
print(p.getBasePositionAndOrientation(modArr[i]))
#raise(ArithmeticError)
posArr1 = []
posArr2 = []
posArr3 = []
posArr4 = []
posArr5 = []
velArr1 = []
velArr2 = []
velArr3 = []
velArr4 = []
bodyPos = []
bodyPos.append(p.getBasePositionAndOrientation(bodyId)[0])
q = False
numIts = 0
#run for 10 seconds
endtime = 8
while numIts < 240 * endtime:
time.sleep(1. / 100.)
numIts += 1
print(numIts)
try:
bodyPos.append(p.getBasePositionAndOrientation(bodyId)[0])
# if time.time()-start_time < 5:
# k_p = 0.2
# k_d = 0
# k_i = 0
# else:
# k_p = kp
# k_i = ki
# k_d = kd
if time.time() - start_time > 1:
# Get velocity of each wheel in x direction
vel_x = []
for i in range(4):
temp = p.getBaseVelocity(mods[i])
temp = temp[0][0] # Get index of x velocity
vel_x.append(temp)
# print(vel_x)
# Get position of each wheel
pos_y = []
for i in range(4):
temp, _ = p.getBasePositionAndOrientation(mods[i])
pos_y.append(temp[1]) # Get index of y position
# temp = p.getBaseVelocity(mods[i])
# temp = temp[0][1] # Get index of y velocity
# pos_y.append(temp)
# print(pos_y)
# noise. Use noise of 3 for simulation
for i in range(4):
# p.applyExternalForce(mods[i],-1,[np.random.normal(0, 5),np.random.normal(0, 5),0],[0,0,0],p.LINK_FRAME)
pass
# Apply force: either random noise or control
if True: #time.time() - lastControlTime > 0.005: # Control every .1 second
# Velocity control for x axis
control_x = []
for i in range(4):
error = (vel_x[i] - goal_vel_x[i])
P = k_p * error
I = control_x_prev[i] + k_i * error * (time.time() -
time_prev)
D = k_d * (error - error_x_prev[i]) / (time.time() -
time_prev)
control_x.append(-P)
if control_x[i] > 100:
control_x[i] = 100
elif control_x[i] < -100:
control_x[i] = -100
control_x_prev[i] = control_x[i]
error_x_prev[i] = error
# print(control_x)
# Position control for y axis
control_y = []
for i in range(4):
error = pos_y[i] - goal_pos_y[i]
P = k_p * error * 10
I = control_y_prev[i] + k_i * 3 * error * (
time.time() - time_prev)
D = k_d * (error - error_y_prev[i]) / (time.time() -
time_prev)
control_y.append(-P - D)
# control_y.append(0)
control_y_prev[i] = control_y[i]
error_y_prev[i] = error
# print(control_y)
time_prev = time.time()
# Apply control as force
for i in range(2):
# p.applyExternalForce(mods[i],-1,[control_x[i],control_y[i],0],[0,0,0],p.LINK_FRAME)
p.applyExternalForce(
mods[i], -1,
np.array([control_x[i], control_y[i] / 1, 0]),
[0, -0.0, 0], p.LINK_FRAME)
p.applyExternalForce(
mods[i], -1,
np.array([control_x[i], control_y[i] / 1, 0]),
[0, 0.0, 0], p.LINK_FRAME)
for i in range(3, 4):
# p.applyExternalForce(mods[i],-1,[control_x[i],control_y[i],0],[0,0,0],p.LINK_FRAME)
p.applyExternalForce(
mods[i], -1,
np.array([control_x[i] * 0.1, control_y[i] / 1,
0]), [0, -0.0, 0], p.LINK_FRAME)
p.applyExternalForce(
mods[i], -1,
np.array([control_x[i] * 0.1, control_y[i] / 1,
0]), [0, 0.0, 0], p.LINK_FRAME)
# if i == 1 or i == 2:
# p.applyExternalForce(mods[i],-1,[control_x[i] - control_y[i]/5,0,0],[0,1/4,0],p.LINK_FRAME)
# p.applyExternalForce(mods[i],-1,[control_x[i] + control_y[i]/5,0,0],[0,-1/4,0],p.LINK_FRAME)
# else:
# p.applyExternalForce(mods[i],-1,[control_x[i] + control_y[i]/5,0,0],[0,-1/4,0],p.LINK_FRAME)
# p.applyExternalForce(mods[i],-1,[control_x[i] - control_y[i]/5,0,0],[0,1/4,0],p.LINK_FRAME)
pass
#lastControlTime = time.time()
# Store
pos1, ort = p.getBasePositionAndOrientation(mod1)
pos2, ort = p.getBasePositionAndOrientation(mod2)
pos3, ort = p.getBasePositionAndOrientation(mod3)
pos4, ort = p.getBasePositionAndOrientation(mod4)
pos5, ort = p.getBasePositionAndOrientation(bodyId)
posArr1.append(pos1)
posArr2.append(pos2)
posArr3.append(pos3)
posArr4.append(pos4)
posArr5.append(pos5)
velArr1.append(vel_x[0])
velArr2.append(vel_x[1])
velArr3.append(vel_x[2])
velArr4.append(vel_x[3])
p.stepSimulation()
except (KeyboardInterrupt):
return
posArr1 = np.array(posArr1)
posArr2 = np.array(posArr2)
posArr3 = np.array(posArr3)
posArr4 = np.array(posArr4)
posArr5 = np.array(posArr5)
# Master array for position
masterArr = np.zeros([posArr1.shape[0], posArr1.shape[1], 5])
masterArr[:, :, 0] = posArr1
masterArr[:, :, 1] = posArr2
masterArr[:, :, 2] = posArr3
masterArr[:, :, 3] = posArr4
masterArr[:, :, 4] = posArr5
# Master array for velocity in x direction
masterArr2 = np.zeros([len(velArr1), 4])
masterArr2[:, 0] = velArr1
masterArr2[:, 1] = velArr2
masterArr2[:, 2] = velArr3
masterArr2[:, 3] = velArr4
colors = ['red', 'green', 'purple', 'blue', 'black']
fig, (ax1, ax2) = plt.subplots(2)
for i in range(4):
ax1.plot(masterArr[:, 0, i],
masterArr[:, 1, i],
color=colors[i])
ax1.scatter(masterArr[0, 0, i],
masterArr[0, 1, i],
color='black')
for i in range(4):
ax2.plot(np.linspace(1, len(velArr1), num=len(velArr1)),
masterArr2[:, i],
color=colors[i])
ax1.set(xlabel='x', ylabel='y')
ax2.set(xlabel='time', ylabel='x velocity')
plt.savefig()
p.resetSimulation(p.RESET_USE_DEFORMABLE_WORLD)
return
posArr1 = np.array(posArr1)
posArr2 = np.array(posArr2)
posArr3 = np.array(posArr3)
posArr4 = np.array(posArr4)
posArr5 = np.array(posArr5)
# Master array for position
masterArr = np.zeros([posArr1.shape[0], posArr1.shape[1], 5])
masterArr[:, :, 0] = posArr1
masterArr[:, :, 1] = posArr2
masterArr[:, :, 2] = posArr3
masterArr[:, :, 3] = posArr4
masterArr[:, :, 4] = posArr5
# Master array for velocity in x direction
masterArr2 = np.zeros([len(velArr1), 4])
masterArr2[:, 0] = velArr1
masterArr2[:, 1] = velArr2
masterArr2[:, 2] = velArr3
masterArr2[:, 3] = velArr4
colors = ['red', 'green', 'purple', 'blue', 'black']
label = ['limb 1', 'limb 2', 'limb 3', 'limb 4', 'body']
fig, (ax1, ax2) = plt.subplots(2)
for i in range(5):
test = ax1.plot(masterArr[:, 0, i],
masterArr[:, 1, i],
color=colors[i],
label=label[i])
ax1.scatter(masterArr[0, 0, i], masterArr[0, 1, i], color='black')
ax1.legend()
ax1.set_ylim([-0.5, 0.5])
for i in range(4):
ax2.plot(np.linspace(0, t, num=len(velArr1)),
masterArr2[:, i],
color=colors[i],
label=label[i])
ax1.set(xlabel='x position (m)', ylabel='y position (m)')
ax2.set(xlabel='time (s)', ylabel='x velocity (m/s)')
ax2.legend()
title = str(kp) + "kp_" + str(ki) + "ki_" + str(kd) + "kd.png"
#plt.savefig(title)
fig2, ax3 = plt.subplots(1, 1)
N = len(bodyPos)
if N % 2 == 0:
Ndiv2 = int(N / 2)
blueMagenta = np.zeros([Ndiv2, 4])
magentaRed = np.zeros([Ndiv2, 4])
blueMagenta[:, 0] = np.linspace(0, 1, Ndiv2)
blueMagenta[:, 2] = 1
magentaRed[:, 2] = np.linspace(1, 0, Ndiv2)
magentaRed[:, 0] = 1
else:
N = N - 1
Ndiv2 = int(N / 2)
blueMagenta = np.zeros([Ndiv2, 4])
magentaRed = np.zeros([Ndiv2 + 1, 4])
blueMagenta[:, 0] = np.linspace(0, 1, Ndiv2)
blueMagenta[:, 2] = 1
magentaRed[:, 2] = np.linspace(1, 0, Ndiv2 + 1)
magentaRed[:, 0] = 1
N = N + 1
blueMagenta[:, 3] = 1
magentaRed[:, 3] = 1
twocolors = np.vstack((blueMagenta, magentaRed))
print(np.shape(twocolors))
cmap = ListedColormap(twocolors)
colors = np.array([[0, x / (len(bodyPos) - 1), 1]
for x in range(len(bodyPos) - 1)])
for i in range(len(bodyPos) - 1):
ax3.plot([bodyPos[i][0], bodyPos[i + 1][0]],
[bodyPos[i][1], bodyPos[i + 1][1]],
color=twocolors[i, :3],
linewidth=2)
ax3.set(xlabel='Body X Position', ylabel='Body Y Position')
fig2.colorbar(cm.ScalarMappable(cmap=cmap))
m.make_graded_limb_plot(masterArr, masterArr2, 0, endtime)
plt.show()
p.resetSimulation(p.RESET_USE_DEFORMABLE_WORLD)
print(np.sum(abs(masterArr[:, 1, 0] - 0.0833)))
print(np.sum(abs(masterArr[:, 1, 1] + 0.0833)))
print(np.sum(abs(masterArr[:, 1, 2] - 0.0833)))
print(np.sum(abs(masterArr[:, 1, 3] + 0.0833)))
avedev = np.sum(abs(masterArr[:, 1, 0] - 0.0833)) + np.sum(
abs(masterArr[:, 1, 1] + 0.0833)) + np.sum(
abs(masterArr[:, 1, 2] - 0.0833)) + np.sum(
abs(masterArr[:, 1, 3] + 0.0833))
avedev = avedev / 4
print(avedev)
print(np.sum(abs(masterArr[:, 1, 4])))
print((avedev - np.sum(abs(masterArr[:, 1, 4]))) / avedev)
return
def __init__(self):
"""
Constructor Function:for creating the baxter environment
Arguments:
None
Returns :
None
"""
try:
p.connect(p.GUI)
self.render_mode = p.ER_BULLET_HARDWARE_OPENGL
except:
p.connect(p.DIRECT)
self.render_mode = p.ER_TINY_RENDERER
p.setAdditionalSearchPath(pybullet_data.getDataPath())
p.loadURDF("plane.urdf", [0, 0, -1], useFixedBase=True)
p.setGravity(0, 0, -0.2)
self.baxter = p.loadURDF(
"data/baxter_common/baxter_description/urdf/toms_baxter.urdf",
[0, 0, 0],
useFixedBase=True)
self.moveVC(
self.baxter, [12, 34],
[-0.2, 0.2
]) # to prevent baxter hands from overlapping with the boxes
self.moveable_joints = [
5, 12, 13, 14, 15, 16, 18, 19, 27, 29, 34, 35, 36, 37, 38, 40, 41,
49, 51
] # only revolute joints are allowed to move
self.init_poses = [
np.round_(p.getJointState(self.baxter, j)[0], decimals=4)
for j in self.moveable_joints
]
time.sleep(0.1)
self.bezier_right = p.getLinkState(self.baxter, 29)[0]
self.bezier_left = p.getLinkState(self.baxter, 51)[0]
self.huskypos = [0, 0, 1]
self.table_pos = [0.7, 0, -0.9]
self.boxes_num = [0.1, 0.2, 0.3, 0.4,
0.5] # for different type of boxes
self.orn_t = p.getQuaternionFromEuler([0, 0, 1.58])
self.table = p.loadURDF("data/table/table.urdf", self.table_pos,
self.orn_t)
self.orn = p.getBasePositionAndOrientation(self.table)
box_1_num = np.random.choice(self.boxes_num) # choose one type box
self.h1 = 0.1 if box_1_num == 0.1 else 0.2
self.h1 = self.h1 + 0.08 # will be used to ensure touching and assigning task for box1
box_2_num = np.random.choice(self.boxes_num) # choose one type box
self.h2 = 0.1 if box_2_num == 0.1 else 0.2
self.h2 = self.h2 + 0.08 # will be used to ensure touching and assigning task for box2
self.box_t_poses = self.box_poses(
) # get random positions to load boxes
self.box1 = p.loadURDF(
"data/blocks/block_1_" + str(box_1_num) + ".urdf",
self.box_t_poses[0])
self.box2 = p.loadURDF(
"data/blocks/block_2_" + str(box_2_num) + ".urdf",
self.box_t_poses[1])
self.orn1 = p.getBasePositionAndOrientation(self.box1)
self.orn2 = p.getBasePositionAndOrientation(self.box2)
print("Position and Orientation of Table: ", self.orn1, self.orn2)
n1, n = 0, 1000
while (n1 < n):
p.setJointMotorControl2(self.baxter,
12,
p.VELOCITY_CONTROL,
targetVelocity=0,
force=0.2) # experimental
p.setJointMotorControl2(self.baxter,
34,
p.VELOCITY_CONTROL,
targetVelocity=0,
force=0.2) # experimental
p.stepSimulation()
n1 += 1
self.discount = 0.9
self.done = False
self.bezier_timesteps = 0
self.bezier_left = []
self.bezier_right = []
def go_to_pose(self,
target_pose,
obstacles=[],
attachments=[],
cart_traj=False,
use_policy=False,
maxForce=100):
total_traj = []
if self.pw.handonly:
p.changeConstraint(self.pw.cid,
target_pose[0],
target_pose[1],
maxForce=maxForce)
for i in range(80):
simulate_for_duration(self.dt_pose)
self.pw.steps_taken += self.dt_pose
if self.pw.steps_taken >= self.total_timeout:
return total_traj
ee_loc = p.getBasePositionAndOrientation(self.pw.robot)[0]
distance = np.linalg.norm(np.array(ee_loc) - target_pose[0])
if distance < 1e-3:
break
total_traj.append(ee_loc)
if self.pipe_attach is not None:
self.squeeze(force=self.squeeze_force)
else:
for i in range(50):
end_conf = inverse_kinematics_helper(self.pw.robot,
self.ee_link, target_pose)
if not use_policy:
motion_plan = plan_joint_motion(self.pw.robot,
get_movable_joints(
self.pw.robot),
end_conf,
obstacles=obstacles,
attachments=attachments)
if motion_plan is not None:
for conf in motion_plan:
self.go_to_conf(conf)
ee_loc = p.getLinkState(self.pw.robot, 8)
if cart_traj:
total_traj.append(ee_loc[0] + ee_loc[1])
else:
total_traj.append(conf)
if self.pipe_attach is not None:
self.squeeze(force=self.squeeze_force)
else:
ee_loc = p.getLinkState(self.pw.robot, 8)
next_loc = self.policy.predict(
np.array(ee_loc[0] + ee_loc[1]).reshape(1, 7))[0]
next_pos = next_loc[0:3]
next_quat = next_loc[3:]
next_conf = inverse_kinematics_helper(
self.pw.robot, self.ee_link, (next_pos, next_quat))
if cart_traj:
total_traj.append(next_loc)
else:
total_traj.append(next_conf)
self.go_to_conf(next_conf)
if self.pipe_attach is not None:
self.squeeze(force=self.squeeze_force)
ee_loc = p.getLinkState(self.pw.robot, 8)[0]
distance = np.linalg.norm(np.array(ee_loc) - target_pose[0])
if distance < 1e-3:
break
return total_traj
def put_on_ground(self, agent):
agent_pos, agent_ori = map(
np.array, pybullet.getBasePositionAndOrientation(agent))
agent_pos[-1] = 0.6
pybullet.resetBasePositionAndOrientation(agent, agent_pos, agent_ori)
obj = pybullet.loadURDF(os.path.join(pybullet_data.getDataPath(), "r2d2.urdf"))
posX = 0
posY = 3
posZ = 2
obj2 = pybullet.loadURDF(
os.path.join(pybullet_data.getDataPath(), "kuka_iiwa/model.urdf"), posX,
posY, posZ)
#query the number of joints of the object
numJoints = pybullet.getNumJoints(obj)
print(numJoints)
#set the gravity acceleration
pybullet.setGravity(0, 0, -9.8)
#step the simulation for 5 seconds
t_end = time.time() + 5
while time.time() < t_end:
pybullet.stepSimulation()
posAndOrn = pybullet.getBasePositionAndOrientation(obj)
print(posAndOrn)
print("finished")
#remove all objects
pybullet.resetSimulation()
#disconnect from the physics server
pybullet.disconnect()
def get_pos(self):
return np.array(p.getBasePositionAndOrientation(self.pw.pipe)[0])
# forceSlider = p.addUserDebugParameter("maxForce",-10,10,0)
start = time.time()
for i in range(frequency * 30):
motorPos = []
for r in range(1, 3):
for i in range(len(motors)):
pos = (math.pi / 2) * p.readUserDebugParameter(
debugParams[i + (len(motors) * (r - 1))])
motorPos.append(pos)
p.setJointMotorControl2(robots[r - 1],
motors[i],
p.POSITION_CONTROL,
targetPosition=pos)
# maxForce = p.readUserDebugParameter(forceSlider)
hits = p.getContactPoints(robots[0], robots[1], 14, 14)
if len(hits) > 0:
print("hit", "." * np.random.randint(1, 10))
p.stepSimulation()
time.sleep(1. / frequency)
print(time.time() - start)
pos, orn = p.getBasePositionAndOrientation(robot1)
print(pos, orn)
p.disconnect()
p.createSoftBodyAnchor(armId2, 1, mod2, -1)
p.createSoftBodyAnchor(armId2, 2, mod2, -1)
p.createSoftBodyAnchor(armId2, 3, mod2, -1)
p.createSoftBodyAnchor(armId3, 7, mod3, -1)
p.createSoftBodyAnchor(armId3, 8, mod3, -1)
p.createSoftBodyAnchor(armId3, 16, mod3, -1)
p.createSoftBodyAnchor(armId3, 17, mod3, -1)
p.createSoftBodyAnchor(armId4, 7, mod4, -1)
p.createSoftBodyAnchor(armId4, 8, mod4, -1)
p.createSoftBodyAnchor(armId4, 16, mod4, -1)
p.createSoftBodyAnchor(armId4, 17, mod4, -1)
numLinks = p.getMeshData(armId1)
pos, ort = p.getBasePositionAndOrientation(armId1)
# run simulation
useRealTimeSimulation = 0
if (useRealTimeSimulation):
p.setRealTimeSimulation(0)
posArr1 = []
posArr2 = []
posArr3 = []
posArr4 = []
posArr5 = []
velArr1 = []
velArr2 = []
velArr3 = []
def getBaseOrientation(self): position, orientation = p.getBasePositionAndOrientation(self.quadruped) return orientation
jointIndicesAll = [ chest, neck, rightHip, rightKnee, rightAnkle, rightShoulder, rightElbow, leftHip, leftKnee, leftAnkle, leftShoulder, leftElbow ] basePos,baseOrn = p.getBasePositionAndOrientation(humanoid) pose = [ basePos[0],basePos[1],basePos[2], baseOrn[0],baseOrn[1],baseOrn[2],baseOrn[3], chestRot[0],chestRot[1],chestRot[2],chestRot[3], neckRot[0],neckRot[1],neckRot[2],neckRot[3], rightHipRot[0],rightHipRot[1],rightHipRot[2],rightHipRot[3], rightKneeRot[0], rightAnkleRot[0],rightAnkleRot[1],rightAnkleRot[2],rightAnkleRot[3], rightShoulderRot[0],rightShoulderRot[1],rightShoulderRot[2],rightShoulderRot[3], rightElbowRot[0], leftHipRot[0],leftHipRot[1],leftHipRot[2],leftHipRot[3], leftKneeRot[0], leftAnkleRot[0],leftAnkleRot[1],leftAnkleRot[2],leftAnkleRot[3], leftShoulderRot[0],leftShoulderRot[1],leftShoulderRot[2],leftShoulderRot[3], leftElbowRot[0] ]
p.createSoftBodyAnchor(armId4, 7, mod4, -1)
p.createSoftBodyAnchor(armId4, 8, mod4, -1)
p.createSoftBodyAnchor(armId4, 16, mod4, -1)
p.createSoftBodyAnchor(armId4, 17, mod4, -1)
# run simulation
useRealTimeSimulation = 0
if (useRealTimeSimulation):
p.setRealTimeSimulation(0)
#initialize position array
posArr = np.zeros([1, 4, 3])
modArr = [mod1, mod2, mod3, mod4]
for i in range(4):
pos, _ = p.getBasePositionAndOrientation(modArr[i])
posArr[:, i, :] = pos
#just try going in a straight line
des = posArr[0]
tstep = 1 / 100.
def control_prop(modArray, curLocs, desiredLocs, c=1):
if len(curLocs.shape) == 3:
curLocs = curLocs[0]
for i in range(len(modArray)):
p.applyExternalForce(modArray[i], -1,
c * (desiredLocs[i] - curLocs[i]), [0, 0, 0],
p.LINK_FRAME)
timeStep = p.readUserDebugParameter(timeStepId)
p.setTimeStep(timeStep)
desiredPosCart = p.readUserDebugParameter(desiredPosCartId)
desiredVelCart = p.readUserDebugParameter(desiredVelCartId)
kpCart = p.readUserDebugParameter(kpCartId)
kdCart = p.readUserDebugParameter(kdCartId)
maxForceCart = p.readUserDebugParameter(maxForceCartId)
desiredPosPole = p.readUserDebugParameter(desiredPosPoleId)
desiredVelPole = p.readUserDebugParameter(desiredVelPoleId)
kpPole = p.readUserDebugParameter(kpPoleId)
kdPole = p.readUserDebugParameter(kdPoleId)
maxForcePole = p.readUserDebugParameter(maxForcePoleId)
basePos, baseOrn = p.getBasePositionAndOrientation(pole)
baseDof = 7
taus = exPD.computePD(pole, [0, 1], [
basePos[0], basePos[1], basePos[2], baseOrn[0], baseOrn[1], baseOrn[2], baseOrn[3],
desiredPosCart, desiredPosPole
], [0, 0, 0, 0, 0, 0, 0, desiredVelCart, desiredVelPole], [0, 0, 0, 0, 0, 0, 0, kpCart, kpPole],
[0, 0, 0, 0, 0, 0, 0, kdCart, kdPole],
[0, 0, 0, 0, 0, 0, 0, maxForceCart, maxForcePole], timeStep)
for j in [0, 1]:
p.setJointMotorControlMultiDof(pole,
j,
controlMode=p.TORQUE_CONTROL,
force=[taus[j + baseDof]])
#p.setJointMotorControlArray(pole, [0,1], controlMode=p.TORQUE_CONTROL, forces=taus)
def get_positions(modArray):
poses = np.zeros([1, len(modArray), 3])
for i in range(len(modArray)):
pos, _ = p.getBasePositionAndOrientation(modArray[i])
poses[:, i, :] = pos
return poses
steps += 1
p.setJointMotorControl2(
turtle_bot,
0,
p.VELOCITY_CONTROL,
targetVelocity=random.random() * -1.0,
force=60)
p.setJointMotorControl2(
turtle_bot,
1,
p.VELOCITY_CONTROL,
targetVelocity=random.random() * -2.0,
force=60)
p.stepSimulation()
if enable_camera:
position, orientation = p.getBasePositionAndOrientation(turtle_bot)
rgb = get_image(np.array(position), orientation)
if show_image:
if fig is None:
fig = plt.imshow(rgb)
else:
fig.set_data(rgb)
plt.pause(0.00001)
if (steps + 1) % interval == 0:
print("steps=%s" % interval +
" frame_rate=%s" % (interval / (time.time() - t0)))
t0 = time.time()
def get_position(self):
pos, _ = p.getBasePositionAndOrientation(self.car)
return pos
p.setTimeStep(delta_t)
p.setAdditionalSearchPath(pybullet_data.getDataPath()) #optionally
p.setGravity(0,0,-9.8)
p.setRealTimeSimulation(0)
planeId = p.loadURDF("plane.urdf")
#cubeStartPos = [0,0,0.27]
cubeStartPos = [0,0,0.28]
cubeStartOrientation = p.getQuaternionFromEuler([0,0,0])
baseId = p.loadURDF("/home/bb8/robot/git/robot_dog/urdf/quadruped.urdf",cubeStartPos, cubeStartOrientation)
mode = p.POSITION_CONTROL
numJoints = p.getNumJoints(baseId)
maxForce=1.96 # 1.96Nm = 20Kg.cm
if (verbose):
basePos, baseOrn = p.getBasePositionAndOrientation(baseId)
print(basePos,baseOrn)
print( "number of joints %s" % numJoints)
jointNameToId = {}
for i in range(numJoints):
jointInfo = p.getJointInfo(baseId,i)
jointNameToId[jointInfo[1].decode('UTF-8')] = jointInfo[0]
# p.setJointMotorControl2(baseId,i,controlMode=mode,force=maxForce)
if (verbose):
print ("Joint : ", jointInfo)
base_front_left_joint=jointNameToId['base_front_left_joint']
front_left_hip_to_thigh=jointNameToId['front_left_hip_to_thigh']
front_left_thigh_to_leg=jointNameToId['front_left_thigh_to_leg']
linearDamping=0,
angularDamping=0,
rollingFriction=0.1,
mass=golfball_physics.mass,
restitution=0.5)
cp = p.getContactPoints(planeId, ballId)
while 1:
if len(cp) == 0 or not stop_on_collision:
# get velocity and manually apply drag force
vel, _ = np.array(p.getBaseVelocity(ballId))
drag = get_drag_force(vel, world_physics, golfball_physics)
# for some reason we have to divide by a very odd value here to get similar results to plot_trajectory.py
vel += drag / 11.04
p.resetBaseVelocity(ballId, vel)
# check for collision
cp = p.getContactPoints(planeId, ballId)
# simulate!
p.stepSimulation()
# move camera to ball position
pos, _ = p.getBasePositionAndOrientation(ballId)
*_, yaw, pitch, dist, _ = p.getDebugVisualizerCamera()
p.resetDebugVisualizerCamera(cameraDistance=dist,
cameraYaw=yaw,
cameraPitch=pitch,
cameraTargetPosition=pos)
sleep(1 / 240.)
def step(self, actions):
"""
Environment Step Function
Arguments:
List of Actions(Points to go)
Returns:
Environment Information and Reward
"""
reward = 0.0
actions[actions < 0] = 0
for i in range(5):
pts = self.BeizerCurve(actions)
right_pos = pts[0]
left_pos = pts[1]
p1 = 0
while p1 < 5:
joint_angles0 = p.calculateInverseKinematics(
self.baxter, 29, right_pos)
joint_angles1 = p.calculateInverseKinematics(
self.baxter, 51, left_pos)
move_actions = list(joint_angles0)[0:10] + list(
joint_angles1)[10:]
self.moveVC(self.baxter, self.moveable_joints, move_actions)
p1 += 1
box1_pos = list(p.getBasePositionAndOrientation(self.box1)[0])
box2_pos = list(p.getBasePositionAndOrientation(self.box2)[0])
boxes_poses = [box1_pos, box2_pos]
hand1_pos = p.getLinkState(self.baxter, 29)[0]
hand2_pos = p.getLinkState(self.baxter, 51)[0]
d1 = np.amin([
self.vec_mag(hand1_pos, box1_pos),
self.vec_mag(hand2_pos, box1_pos)
])
d2 = np.amin([
self.vec_mag(hand1_pos, box2_pos),
self.vec_mag(hand2_pos, box2_pos)
])
reward_n = self.getReward(d1, d2)
b1 = 0.5
if (boxes_poses[0][2] < self.box_t_poses[0][2] - b1
or boxes_poses[1][2] < self.box_t_poses[1][2] - b1):
reward_n = reward_n - 10
episode_destroyed = 1
else:
episode_destroyed = 0
reward = reward_n + self.discount * reward
img, depth = self.getImage()
if (d1 < self.h1 and d2 < self.h2):
self.done = True
else:
self.done = False
state_dict = {
"hand_ends": [hand1_pos, hand2_pos],
"eye_view": img,
"depth": depth
}
return (state_dict, reward, self.done, {
"episode_destroyed": episode_destroyed
})
torsoId = i
while True:
value, action, _, states = actor_critic.act(Variable(current_obs,
volatile=True),
Variable(states,
volatile=True),
Variable(masks, volatile=True),
deterministic=True)
states = states.data
cpu_actions = np.atleast_2d(action.data.squeeze(1).cpu().numpy())
# Obser reward and next obs
obs, reward, done, _ = env.step(cpu_actions)
masks.fill_(0.0 if done else 1.0)
if current_obs.dim() == 4:
current_obs *= masks.unsqueeze(2).unsqueeze(2)
else:
current_obs *= masks
update_current_obs(obs)
if args.env_name.find('Bullet') > -1:
if torsoId > -1:
distance = 5
yaw = 0
humanPos, humanOrn = p.getBasePositionAndOrientation(torsoId)
p.resetDebugVisualizerCamera(distance, yaw, -20, humanPos)
render_func('human')
bc.configureDebugVisualizer(bc.COV_ENABLE_Y_AXIS_UP, 1)
bc.setGravity(0, -9.8, 0)
motion = MotionCaptureData()
motionPath = json_path
motion.Load(motionPath)
print("numFrames = ", motion.NumFrames())
simTimeId = bc.addUserDebugParameter("simTime", 0, motion.NumFrames() - 1.1, 0)
y2zOrn = bc.getQuaternionFromEuler([-1.57, 0, 0])
bc.loadURDF("plane.urdf", [0, -0.04, 0], y2zOrn)
humanoid = Humanoid(bc, motion, [0, 0, 0]) #这是初始位置的坐标
print(p.getBasePositionAndOrientation(humanoid._humanoid))
simTime = 0
keyFrameDuration = motion.KeyFrameDuraction()
print("keyFrameDuration=", keyFrameDuration)
for utNum in range(motion.NumFrames()):
bc.stepSimulation()
humanoid.RenderReference(utNum * keyFrameDuration)
if draw_gt:
draw_ground_truth(coord_seq=ground_truth,
frame=utNum,
duration=keyFrameDuration,
shift=[-1.0, 0.0, 1.0])
time.sleep(0.001)
stage = 0
if (PID_CONTROL):
# calculate the speed of villain in unit/footstep
#########
#if h1==0:
h1 = p.getLinkState(cheetah, 0)
runCheetah()
#if h2==0:
h2 = p.getLinkState(cheetah, 0)
h = mag([
h1[0][0] - h2[0][0], h1[0][1] - h2[0][1], h1[0][2] - h2[0][2]
])
##############
rear_bump = p.getLinkState(husky, 9)
front_bump = p.getLinkState(husky, 8)
target_pos = p.getBasePositionAndOrientation(target)
###############
h3 = p.getLinkState(cheetah, 0)
enemy_dist = mag(
[h3[0][0] - target_pos[0][0], h3[0][0] - target_pos[0][0], 0])
############# I f eenemy comes in a distance of 4.5 unit then the car will automatically start and will come before the enemy how much far it is###########
if enemy_dist < 6:
rear_bump = p.getLinkState(husky, 9)
front_bump = p.getLinkState(husky, 8)
v1 = front_bump[0][0] - rear_bump[0][0]
v2 = front_bump[0][1] - rear_bump[0][1]
v3 = front_bump[0][2] - rear_bump[0][2]
frontVec = np.array(list((v1, v2, v3)))
t1 = target_pos[0][0] - rear_bump[0][0]
t2 = target_pos[0][1] - rear_bump[0][1]
t3 = 0
#load URDF, given a relative or absolute file+path
obj = pybullet.loadURDF(os.path.join(pybullet_data.getDataPath(), "r2d2.urdf"))
posX = 0
posY = 3
posZ = 2
obj2 = pybullet.loadURDF(os.path.join(pybullet_data.getDataPath(), "kuka_iiwa/model.urdf"), posX,
posY, posZ)
#query the number of joints of the object
numJoints = pybullet.getNumJoints(obj)
print(numJoints)
#set the gravity acceleration
pybullet.setGravity(0, 0, -9.8)
#step the simulation for 5 seconds
t_end = time.time() + 5
while time.time() < t_end:
pybullet.stepSimulation()
posAndOrn = pybullet.getBasePositionAndOrientation(obj)
print(posAndOrn)
print("finished")
#remove all objects
pybullet.resetSimulation()
#disconnect from the physics server
pybullet.disconnect()
import pybullet as p
import pybullet_data
from time import sleep
physicsClient = p.connect(p.GUI) #or p.DIRECT for non-graphical version
p.setAdditionalSearchPath(pybullet_data.getDataPath()) #used by loadURDF
p.setGravity(0, 0, -10)
planeId = p.loadURDF("plane.urdf")
cubeStartPos = [0, 0, 1]
cubeStartOrientation = p.getQuaternionFromEuler([0, 0, 0])
boxId = p.loadURDF("r2d2.urdf", cubeStartPos, cubeStartOrientation)
p.stepSimulation()
cubePos, cubeOrn = p.getBasePositionAndOrientation(boxId)
print(cubePos, cubeOrn)
sleep(20.0)
print('done sleeping')
p.disconnect()
objects = p.loadMJCF("mjcf/sphere.xml")
sphere = objects[0]
p.resetBasePositionAndOrientation(sphere, [0, 0, 1], [0, 0, 0, 1])
p.changeDynamics(sphere, -1, linearDamping=0.9)
p.changeVisualShape(sphere, -1, rgbaColor=[1, 0, 0, 1])
forward = 0
turn = 0
forwardVec = [2, 0, 0]
cameraDistance = 1
cameraYaw = 35
cameraPitch = -35
while (1):
spherePos, orn = p.getBasePositionAndOrientation(sphere)
cameraTargetPosition = spherePos
p.resetDebugVisualizerCamera(cameraDistance, cameraYaw, cameraPitch, cameraTargetPosition)
camInfo = p.getDebugVisualizerCamera()
camForward = camInfo[5]
keys = p.getKeyboardEvents()
for k, v in keys.items():
if (k == p.B3G_RIGHT_ARROW and (v & p.KEY_WAS_TRIGGERED)):
turn = -0.5
if (k == p.B3G_RIGHT_ARROW and (v & p.KEY_WAS_RELEASED)):
turn = 0
if (k == p.B3G_LEFT_ARROW and (v & p.KEY_WAS_TRIGGERED)):
turn = 0.5
def get_obj_orientation(self, obj_id):
return p.getBasePositionAndOrientation(obj_id)[1]
p.configureDebugVisualizer(p.COV_ENABLE_GUI,0)
p.configureDebugVisualizer(p.COV_ENABLE_RENDERING,1)#disable this to make it faster
p.configureDebugVisualizer(p.COV_ENABLE_TINY_RENDERER,0)
p.setPhysicsEngineParameter(enableConeFriction=1)
for i in range (num):
print("progress:",i,num)
x = velocity*math.sin(2.*3.1415*float(i)/num)
y = velocity*math.cos(2.*3.1415*float(i)/num)
print("velocity=",x,y)
sphere=p.loadURDF("sphere_small_zeroinertia.urdf", flags=p.URDF_USE_INERTIA_FROM_FILE, useMaximalCoordinates=useMaximalCoordinates)
p.changeDynamics(sphere,-1,lateralFriction=0.02)
#p.changeDynamics(sphere,-1,rollingFriction=10)
p.changeDynamics(sphere,-1,linearDamping=0)
p.changeDynamics(sphere,-1,angularDamping=0)
p.resetBaseVelocity(sphere,linearVelocity=[x,y,0])
prevPos = [0,0,0]
for i in range (2048):
p.stepSimulation()
pos = p.getBasePositionAndOrientation(sphere)[0]
if (i&64):
p.addUserDebugLine(prevPos,pos,[1,0,0],1)
prevPos = pos
p.configureDebugVisualizer(p.COV_ENABLE_RENDERING,1)
while (1):
time.sleep(0.01)
def getBasePosition(self): position, orientation = p.getBasePositionAndOrientation(self.quadruped) return position
def state_fields_of_pose_of(self, body_id, link_id=-1): # a method you will most probably need a lot to get pose and orientation if link_id == -1: (x, y, z), (a, b, c, d) = p.getBasePositionAndOrientation(body_id) else: (x, y, z), (a, b, c, d), _, _, _, _ = p.getLinkState(body_id, link_id) return np.array([x, y, z, a, b, c, d])
lateralFrictionId = p.addUserDebugParameter("lateral friction", 0, 1, 0.5)
spinningFrictionId = p.addUserDebugParameter("spinning friction", 0, 1, 0.03)
rollingFrictionId = p.addUserDebugParameter("rolling friction", 0, 1, 0.03)
plane = p.loadURDF("plane_with_restitution.urdf")
sphere = p.loadURDF("sphere_with_restitution.urdf", [0, 0, 2])
p.setRealTimeSimulation(1)
p.setGravity(0, 0, -10)
while (1):
restitution = p.readUserDebugParameter(restitutionId)
restitutionVelocityThreshold = p.readUserDebugParameter(
restitutionVelocityThresholdId)
p.setPhysicsEngineParameter(
restitutionVelocityThreshold=restitutionVelocityThreshold)
lateralFriction = p.readUserDebugParameter(lateralFrictionId)
spinningFriction = p.readUserDebugParameter(spinningFrictionId)
rollingFriction = p.readUserDebugParameter(rollingFrictionId)
p.changeDynamics(plane, -1, lateralFriction=1)
p.changeDynamics(sphere, -1, lateralFriction=lateralFriction)
p.changeDynamics(sphere, -1, spinningFriction=spinningFriction)
p.changeDynamics(sphere, -1, rollingFriction=rollingFriction)
p.changeDynamics(plane, -1, restitution=restitution)
p.changeDynamics(sphere, -1, restitution=restitution)
pos, orn = p.getBasePositionAndOrientation(sphere)
#print("pos=")
#print(pos)
time.sleep(0.01)
def get_orientation(self):
return p.getBasePositionAndOrientation(self.id)[1]
def distance(obj1, obj2):
"""Calculate the scalar Euclidean distance between two objects."""
pos1 = p.getBasePositionAndOrientation(obj1)[0]
pos2 = p.getBasePositionAndOrientation(obj2)[0]
return np.sqrt(np.sum(np.subtract(pos1, pos2)**2))
targetVel = targetVel + 1
targetVel1 = targetVel1 + 0.5
targetVel2 = targetVel2 - 0.5
targetVelRev = targetVelRev - 1
while (1):
br = 0
keys = p.getKeyboardEvents()
for k, v in keys.items():
if (k == ord('a') and (v & p.KEY_WAS_RELEASED)):
br = 1
break
if (br == 1):
break
if (k == ord('c') and (v & p.KEY_WAS_RELEASED)):
pos = p.getBasePositionAndOrientation(
car) #Getting the position and orientation of the car
cord = pos[0]
ori = p.getEulerFromQuaternion(pos[1])
unit = [
0.3 * math.cos(ori[2]), 0.3 * math.sin(ori[2]), 0.35
] #Calculating a unit vector in direction of the car. The anle depends only upon YAW(z) as car is in xy plane
cam = [
sum(x) for x in zip(cord, unit)
] #Fixing the camera a little forward from the center of the car
unit2 = [1 * math.cos(ori[2]), 1 * math.sin(ori[2]), 0.20]
see = [sum(x) for x in zip(cord, unit2)
] #Setting the looking direction in direction of the car
view_matrix = p.computeViewMatrix(
cam, see, [0, 0, 1]) #Calculating the position of the camera
pro_matrix = p.computeProjectionMatrixFOV(
fov, aspect, near,
def _compute_done(self):
cubePos, _ = p.getBasePositionAndOrientation(self.botId)
return cubePos[2] < 0.15 or self._envStepCounter >= 100000
import pybullet as p
from time import sleep
physicsClient = p.connect(p.GUI)
p.setGravity(0,0,-10)
planeId = p.loadURDF("plane.urdf")
cubeStartPos = [0,0,1]
cubeStartOrientation = p.getQuaternionFromEuler([0,0,0])
boxId = p.loadURDF("r2d2.urdf",cubeStartPos, cubeStartOrientation)
cubePos, cubeOrn = p.getBasePositionAndOrientation(boxId)
useRealTimeSimulation = 0
if (useRealTimeSimulation):
p.setRealTimeSimulation(1)
while 1:
if (useRealTimeSimulation):
p.setGravity(0,0,-10)
sleep(0.01) # Time in seconds.
else:
p.stepSimulation()
def _step(self, action):
done = False
#reward (float): amount of reward achieved by the previous action. The scale varies between environments, but the goal is always to increase your total reward.
#done (boolean): whether it's time to reset the environment again. Most (but not all) tasks are divided up into well-defined episodes, and done being True indicates the episode has terminated. (For example, perhaps the pole tipped too far, or you lost your last life.)
#observation (object): an environment-specific object representing your observation of the environment. For example, pixel data from a camera, joint angles and joint velocities of a robot, or the board state in a board game.
#info (dict): diagnostic information useful for debugging. It can sometimes be useful for learning (for example, it might contain the raw probabilities behind the environment's last state change). However, official evaluations of your agent are not allowed to use this for learning.
def convertSensor(finger_index):
if finger_index == self.indexId:
return random.uniform(-1, 1)
#return 0
else:
#return random.uniform(-1,1)
return 0
def convertAction(action):
#converted = (action-30)/10
#converted = (action-16)/10
if action == 6:
converted = -1
elif action == 25:
converted = 1
#print("action ",action)
#print("converted ",converted)
return converted
aspect = 1
camTargetPos = [0, 0, 0]
yaw = 40
pitch = 10.0
roll = 0
upAxisIndex = 2
camDistance = 4
nearPlane = 0.0001
farPlane = 0.022
lightDirection = [0, 1, 0]
lightColor = [1, 1, 1] #optional
fov = 50 #10 or 50
hand_po = pb.getBasePositionAndOrientation(self.agent)
ho = pb.getQuaternionFromEuler([0.0, 0.0, 0.0])
# So when trying to modify the physics of the engine, we run into some problems. If we leave
# angular damping at default (0.04) then the hand rotates when moving up and dow, due to torque.
# If we set angularDamping to 100.0 then the hand will bounce off into the background due to
# all the stored energy, when it makes contact with the object. The below set of parameters seem
# to have a reasonably consistent performance in keeping the hand level and not inducing unwanted
# behavior during contact.
pb.changeDynamics(self.agent,
linkIndex=-1,
spinningFriction=100.0,
angularDamping=35.0,
contactStiffness=0.0,
contactDamping=100)
if action == 65298 or action == 0: #down
pb.changeConstraint(
self.hand_cid,
(hand_po[0][0] + self.move, hand_po[0][1], hand_po[0][2]),
ho,
maxForce=50)
elif action == 65297 or action == 1: #up
pb.changeConstraint(
self.hand_cid,
(hand_po[0][0] - self.move, hand_po[0][1], hand_po[0][2]),
ho,
maxForce=50)
elif action == 65295 or action == 2: #left
pb.changeConstraint(
self.hand_cid,
(hand_po[0][0], hand_po[0][1] + self.move, hand_po[0][2]),
ho,
maxForce=50)
elif action == 65296 or action == 3: #right
pb.changeConstraint(
self.hand_cid,
(hand_po[0][0], hand_po[0][1] - self.move, hand_po[0][2]),
ho,
maxForce=50)
elif action == 44 or action == 4: #<
pb.changeConstraint(
self.hand_cid,
(hand_po[0][0], hand_po[0][1], hand_po[0][2] + self.move),
ho,
maxForce=50)
elif action == 46 or action == 5: #>
pb.changeConstraint(
self.hand_cid,
(hand_po[0][0], hand_po[0][1], hand_po[0][2] - self.move),
ho,
maxForce=50)
elif action >= 6 and action <= 7:
# keeps the hand from moving towards origin
pb.changeConstraint(self.hand_cid,
(hand_po[0][0], hand_po[0][1], hand_po[0][2]),
ho,
maxForce=50)
if action == 7:
action = 25 #bad kludge redo all this code
"""
self.pinkId = 0
self.middleId = 1
self.indexId = 2
self.thumbId = 3
self.ring_id = 4
pink = convertSensor(self.pinkId) #pinkId != indexId -> return random uniform
middle = convertSensor(self.middleId) # middleId != indexId -> return random uniform
thumb = convertSensor(self.thumbId) # thumbId != indexId -> return random uniform
ring = convertSensor(self.ring_id) # ring_id != indexId -> return random uniform
"""
index = convertAction(
action) # action = 6 or 25 due to kludge -> return -1 or 1
"""
pb.setJointMotorControl2(self.agent,7,pb.POSITION_CONTROL,self.pi/4.)
pb.setJointMotorControl2(self.agent,9,pb.POSITION_CONTROL,thumb+self.pi/10)
pb.setJointMotorControl2(self.agent,11,pb.POSITION_CONTROL,thumb)
pb.setJointMotorControl2(self.agent,13,pb.POSITION_CONTROL,thumb)
"""
# Getting positions of the index joints to use for moving to a relative position
joint17Pos = pb.getJointState(self.agent, 17)[0]
joint19Pos = pb.getJointState(self.agent, 19)[0]
joint21Pos = pb.getJointState(self.agent, 21)[0]
# need to keep the multiplier relatively small otherwise the joint will continue to move
# when you take other actions
finger_jump = 0.2
newJoint17Pos = joint17Pos + index * finger_jump
newJoint19Pos = joint19Pos + index * finger_jump
newJoint21Pos = joint21Pos + index * finger_jump
# following values found by experimentation
if newJoint17Pos <= -0.7:
newJoint17Pos = -0.7
elif newJoint17Pos >= 0.57:
newJoint17Pos = 0.57
if newJoint19Pos <= 0.13:
newJoint19Pos = 0.13
elif newJoint19Pos >= 0.42:
newJoint19Pos = 0.42
if newJoint21Pos <= -0.8:
newJoint21Pos = -0.8
elif newJoint21Pos >= 0.58:
newJoint21Pos = 0.58
pb.setJointMotorControl2(self.agent, 17, pb.POSITION_CONTROL,
newJoint17Pos)
pb.setJointMotorControl2(self.agent, 19, pb.POSITION_CONTROL,
newJoint19Pos)
pb.setJointMotorControl2(self.agent, 21, pb.POSITION_CONTROL,
newJoint21Pos)
"""
pb.setJointMotorControl2(self.agent,17,pb.POSITION_CONTROL,index)
pb.setJointMotorControl2(self.agent,19,pb.POSITION_CONTROL,index)
pb.setJointMotorControl2(self.agent,21,pb.POSITION_CONTROL,index)
pb.setJointMotorControl2(self.agent,24,pb.POSITION_CONTROL,middle)
pb.setJointMotorControl2(self.agent,26,pb.POSITION_CONTROL,middle)
pb.setJointMotorControl2(self.agent,28,pb.POSITION_CONTROL,middle)
pb.setJointMotorControl2(self.agent,40,pb.POSITION_CONTROL,pink)
pb.setJointMotorControl2(self.agent,42,pb.POSITION_CONTROL,pink)
pb.setJointMotorControl2(self.agent,44,pb.POSITION_CONTROL,pink)
ringpos = 0.5*(pink+middle)
pb.setJointMotorControl2(self.agent,32,pb.POSITION_CONTROL,ringpos)
pb.setJointMotorControl2(self.agent,34,pb.POSITION_CONTROL,ringpos)
pb.setJointMotorControl2(self.agent,36,pb.POSITION_CONTROL,ringpos)
"""
if self.downCameraOn: viewMatrix = down_view()
else: viewMatrix = self.ahead_view()
projectionMatrix = pb.computeProjectionMatrixFOV(
fov, aspect, nearPlane, farPlane)
w, h, img_arr, depths, mask = pb.getCameraImage(
self.touch_width,
self.touch_height,
viewMatrix,
projectionMatrix,
lightDirection,
lightColor,
renderer=pb.ER_TINY_RENDERER)
#w,h,img_arr,depths,mask = pb.getCameraImage(200,200, viewMatrix,projectionMatrix, lightDirection,lightColor,renderer=pb.ER_BULLET_HARDWARE_OPENGL)
new_obs = np.absolute(depths - 1.0)
new_obs[new_obs > 0] = 1
self.current_observation = new_obs.flatten()
info = [42] #answer to life,TODO use real values
pb.stepSimulation()
self.steps += 1
#reward if moving towards the object or touching the object
reward = 0
max_steps = 1000
if self.is_touching():
touch_reward = 10
if 'debug' in self.options and self.options['debug'] == True:
print("TOUCHING!!!!")
else:
touch_reward = 0
obj_po = pb.getBasePositionAndOrientation(self.obj_to_classify)
distance = math.sqrt(
sum([(xi - yi)**2 for xi, yi in zip(obj_po[0], hand_po[0])
])) #TODO faster euclidean
#distance = np.linalg.norm(obj_po[0],hand_po[0])
#print("distance:",distance)
if distance < self.prev_distance:
reward += 1 * (max_steps - self.steps)
elif distance > self.prev_distance:
reward -= 10
reward -= distance
reward += touch_reward
self.prev_distance = distance
if 'debug' in self.options and self.options['debug'] == True:
print("touch reward ", touch_reward)
print("action ", action)
print("reward ", reward)
print("distance ", distance)
if self.steps >= max_steps or self.is_touching():
done = True
return self.current_observation, reward, done, info
import pybullet as p
p.connect(p.GUI)
cube = p.loadURDF("cube.urdf")
frequency = 240
timeStep = 1. / frequency
p.setGravity(0, 0, -9.8)
p.changeDynamics(cube, -1, linearDamping=0, angularDamping=0)
p.setPhysicsEngineParameter(fixedTimeStep=timeStep)
for i in range(frequency):
p.stepSimulation()
pos, orn = p.getBasePositionAndOrientation(cube)
print(pos)
# Add Search Path
pybullet.setAdditionalSearchPath(pybullet_data.getDataPath())
# Serves as ground in our simulation
plainID = pybullet.loadURDF("plane.urdf") # Present in pybullet_data
# Load robot
robot = pybullet.loadURDF("/home/sp/repos/PyBulletProject/kuka_experimental/kuka_lbr_iiwa_support/urdf/lbr_iiwa_14_r820.urdf")
#robot = pybullet.loadURDF("/home/sp/repos/PyBulletProject/lbr_iiwa_14_r820.urdf")
# Get info about robot
pybullet.getNumJoints(robot)
# Orientation is a quaternion
position, orientation = pybullet.getBasePositionAndOrientation(robot)
pybullet.getJointInfo(robot, 7)
joint_positions = [j[0] for j in pybullet.getJointStates(robot, range(7))]
print(joint_positions)
world_position, world_orientation = pybullet.getLinkState(robot, 2)[:2]
print(world_position)
# Simulate gravity
pybullet.setGravity(0, 0, -9.81)
pybullet.setRealTimeSimulation(0) # no realtime simulation