def _randomly_place_objects(self, urdfList):
"""Randomly places the objects in the bin.
Args:
urdfList: The list of urdf files to place in the bin.
Returns:
The list of object unique ID's.
"""
# Randomize positions of each object urdf.
objectUids = []
for urdf_name in urdfList:
xpos = 0.4 +self._blockRandom*random.random()
ypos = self._blockRandom*(random.random()-.5)
angle = np.pi/2 + self._blockRandom * np.pi * random.random()
orn = p.getQuaternionFromEuler([0, 0, angle])
urdf_path = os.path.join(self._urdfRoot, urdf_name)
uid = p.loadURDF(urdf_path, [xpos, ypos, .15],
[orn[0], orn[1], orn[2], orn[3]])
objectUids.append(uid)
# Let each object fall to the tray individual, to prevent object
# intersection.
for _ in range(500):
p.stepSimulation()
return objectUids
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>1000):
self._observation = self.getExtendedObservation()
return True
if (actualEndEffectorPos[2] <= 0.10):
self.terminated = 1
#print("closing gripper, attempting grasp")
#start grasp and terminate
fingerAngle = 0.3
for i in range (1000):
graspAction = [0,0,0.001,0,fingerAngle]
self._kuka.applyAction(graspAction)
p.stepSimulation()
fingerAngle = fingerAngle-(0.3/100.)
if (fingerAngle<0):
fingerAngle=0
self._observation = self.getExtendedObservation()
return True
return False
def step2(self, action):
for i in range(self._actionRepeat):
self._kuka.applyAction(action)
p.stepSimulation()
if self._termination():
break
self._envStepCounter += 1
if self._renders:
time.sleep(self._timeStep)
self._observation = self.getExtendedObservation()
#print("self._envStepCounter")
#print(self._envStepCounter)
done = self._termination()
npaction = np.array([action[3]]) #only penalize rotation until learning works well [action[0],action[1],action[3]])
actionCost = np.linalg.norm(npaction)*10.
#print("actionCost")
#print(actionCost)
reward = self._reward()-actionCost
#print("reward")
#print(reward)
#print("len=%r" % len(self._observation))
return np.array(self._observation), reward, done, {}
def buildJointNameToIdDict(self):
nJoints = p.getNumJoints(self.quadruped)
self.jointNameToId = {}
for i in range(nJoints):
jointInfo = p.getJointInfo(self.quadruped, i)
self.jointNameToId[jointInfo[1].decode('UTF-8')] = jointInfo[0]
self.resetPose()
for i in range(100):
p.stepSimulation()
def applyAction(self, actions):
forces = [0.] * len(self.motors)
for m in range(len(self.motors)):
forces[m] = self.motor_power[m]*actions[m]*0.082
p.setJointMotorControlArray(self.human, self.motors,controlMode=p.TORQUE_CONTROL, forces=forces)
p.stepSimulation()
time.sleep(0.01)
distance=5
yaw = 0
def test(args):
p.connect(p.GUI)
p.setAdditionalSearchPath(pybullet_data.getDataPath())
fileName = os.path.join("mjcf", args.mjcf)
print("fileName")
print(fileName)
p.loadMJCF(fileName)
while (1):
p.stepSimulation()
p.getCameraImage(320,240)
time.sleep(0.01)
def setupWorld():
p.resetSimulation()
p.loadURDF("planeMesh.urdf")
kukaId = p.loadURDF("kuka_iiwa/model_free_base.urdf",[0,0,10])
for i in range (p.getNumJoints(kukaId)):
p.setJointMotorControl2(kukaId,i,p.POSITION_CONTROL,force=0)
for i in range (numObjects):
cube = p.loadURDF("cube_small.urdf",[0,i*0.02,(i+1)*0.2])
#p.changeDynamics(cube,-1,mass=100)
p.stepSimulation()
p.setGravity(0,0,-10)
def step(self,action):
p.stepSimulation()
start = time.time()
yaw = next(self.iter)
viewMatrix = p.computeViewMatrixFromYawPitchRoll(camTargetPos, camDistance, yaw, pitch, roll, upAxisIndex)
aspect = pixelWidth / pixelHeight;
projectionMatrix = p.computeProjectionMatrixFOV(fov, aspect, nearPlane, farPlane);
img_arr = p.getCameraImage(pixelWidth, pixelHeight, viewMatrix,
projectionMatrix, shadow=1,lightDirection=[1,1,1],
renderer=p.ER_BULLET_HARDWARE_OPENGL)
#renderer=pybullet.ER_TINY_RENDERER)
self._observation = img_arr[2]
return np.array(self._observation), 0, 0, {}
def reset(self):
self.quadruped = p.loadURDF("quadruped/quadruped.urdf",0,0,.3)
self.kp = 1
self.kd = 0.1
self.maxForce = 100
nJoints = p.getNumJoints(self.quadruped)
self.jointNameToId = {}
for i in range(nJoints):
jointInfo = p.getJointInfo(self.quadruped, i)
self.jointNameToId[jointInfo[1].decode('UTF-8')] = jointInfo[0]
self.resetPose()
for i in range(100):
p.stepSimulation()
def test(num_runs=300, shadow=1, log=True, plot=False):
if log:
logId = pybullet.startStateLogging(pybullet.STATE_LOGGING_PROFILE_TIMINGS, "renderTimings")
if plot:
plt.ion()
img = np.random.rand(200, 320)
#img = [tandard_normal((50,100))
image = plt.imshow(img,interpolation='none',animated=True,label="blah")
ax = plt.gca()
times = np.zeros(num_runs)
yaw_gen = itertools.cycle(range(0,360,10))
for i, yaw in zip(range(num_runs),yaw_gen):
pybullet.stepSimulation()
start = time.time()
viewMatrix = pybullet.computeViewMatrixFromYawPitchRoll(camTargetPos, camDistance, yaw, pitch, roll, upAxisIndex)
aspect = pixelWidth / pixelHeight;
projectionMatrix = pybullet.computeProjectionMatrixFOV(fov, aspect, nearPlane, farPlane);
img_arr = pybullet.getCameraImage(pixelWidth, pixelHeight, viewMatrix,
projectionMatrix, shadow=shadow,lightDirection=[1,1,1],
renderer=pybullet.ER_BULLET_HARDWARE_OPENGL)
#renderer=pybullet.ER_TINY_RENDERER)
stop = time.time()
duration = (stop - start)
if (duration):
fps = 1./duration
#print("fps=",fps)
else:
fps=0
#print("fps=",fps)
#print("duraction=",duration)
#print("fps=",fps)
times[i] = fps
if plot:
rgb = img_arr[2]
image.set_data(rgb)#np_img_arr)
ax.plot([0])
#plt.draw()
#plt.show()
plt.pause(0.01)
mean_time = float(np.mean(times))
print("mean: {0} for {1} runs".format(mean_time, num_runs))
print("")
if log:
pybullet.stopStateLogging(logId)
return mean_time
def _step(self, action):
self._humanoid.applyAction(action)
for i in range(self._actionRepeat):
p.stepSimulation()
if self._renders:
time.sleep(self._timeStep)
self._observation = self.getExtendedObservation()
if self._termination():
break
self._envStepCounter += 1
reward = self._reward()
done = self._termination()
#print("len=%r" % len(self._observation))
return np.array(self._observation), reward, done, {}
def _step(self, action):
p.stepSimulation()
# time.sleep(self.timeStep)
self.state = p.getJointState(self.cartpole, 1)[0:2] + p.getJointState(self.cartpole, 0)[0:2]
theta, theta_dot, x, x_dot = self.state
force = action
p.setJointMotorControl2(self.cartpole, 0, p.VELOCITY_CONTROL, targetVelocity=(action + self.state[3]))
done = x < -self.x_threshold \
or x > self.x_threshold \
or theta < -self.theta_threshold_radians \
or theta > self.theta_threshold_radians
reward = 1.0
return np.array(self.state), reward, done, {}
def testJacobian(self):
import pybullet as p
clid = p.connect(p.SHARED_MEMORY)
if (clid < 0):
p.connect(p.DIRECT)
time_step = 0.001
gravity_constant = -9.81
urdfs = [
"TwoJointRobot_w_fixedJoints.urdf", "TwoJointRobot_w_fixedJoints.urdf",
"kuka_iiwa/model.urdf", "kuka_lwr/kuka.urdf"
]
for urdf in urdfs:
p.resetSimulation()
p.setTimeStep(time_step)
p.setGravity(0.0, 0.0, gravity_constant)
robotId = p.loadURDF(urdf, useFixedBase=True)
p.resetBasePositionAndOrientation(robotId, [0, 0, 0], [0, 0, 0, 1])
numJoints = p.getNumJoints(robotId)
endEffectorIndex = numJoints - 1
# Set a joint target for the position control and step the sim.
self.setJointPosition(robotId, [0.1 * (i % 3) for i in range(numJoints)])
p.stepSimulation()
# Get the joint and link state directly from Bullet.
mpos, mvel, mtorq = self.getMotorJointStates(robotId)
result = p.getLinkState(robotId,
endEffectorIndex,
computeLinkVelocity=1,
computeForwardKinematics=1)
link_trn, link_rot, com_trn, com_rot, frame_pos, frame_rot, link_vt, link_vr = result
# Get the Jacobians for the CoM of the end-effector link.
# Note that in this example com_rot = identity, and we would need to use com_rot.T * com_trn.
# The localPosition is always defined in terms of the link frame coordinates.
zero_vec = [0.0] * len(mpos)
jac_t, jac_r = p.calculateJacobian(robotId, endEffectorIndex, com_trn, mpos, zero_vec,
zero_vec)
assert (allclose(dot(jac_t, mvel), link_vt))
assert (allclose(dot(jac_r, mvel), link_vr))
p.disconnect()
def evaluate_params(evaluateFunc,
params,
objectiveParams,
urdfRoot='',
timeStep=0.01,
maxNumSteps=10000,
sleepTime=0):
print('start evaluation')
beforeTime = time.time()
p.resetSimulation()
p.setTimeStep(timeStep)
p.loadURDF("%s/plane.urdf" % urdfRoot)
p.setGravity(0, 0, -10)
global minitaur
minitaur = Minitaur(urdfRoot)
start_position = current_position()
last_position = None # for tracing line
total_energy = 0
for i in range(maxNumSteps):
torques = minitaur.getMotorTorques()
velocities = minitaur.getMotorVelocities()
total_energy += np.dot(np.fabs(torques), np.fabs(velocities)) * timeStep
joint_values = evaluate_func_map[evaluateFunc](i, params)
minitaur.applyAction(joint_values)
p.stepSimulation()
if (is_fallen()):
break
if i % 100 == 0:
sys.stdout.write('.')
sys.stdout.flush()
time.sleep(sleepTime)
print(' ')
alpha = objectiveParams[0]
final_distance = np.linalg.norm(start_position - current_position())
finalReturn = final_distance - alpha * total_energy
elapsedTime = time.time() - beforeTime
print("trial for ", params, " final_distance", final_distance, "total_energy", total_energy,
"finalReturn", finalReturn, "elapsed_time", elapsedTime)
return finalReturn
def setupWorld(self):
numObjects = 50
maximalCoordinates = False
p.resetSimulation()
p.setPhysicsEngineParameter(deterministicOverlappingPairs=1)
p.loadURDF("planeMesh.urdf", useMaximalCoordinates=maximalCoordinates)
kukaId = p.loadURDF("kuka_iiwa/model_free_base.urdf", [0, 0, 10],
useMaximalCoordinates=maximalCoordinates)
for i in range(p.getNumJoints(kukaId)):
p.setJointMotorControl2(kukaId, i, p.POSITION_CONTROL, force=0)
for i in range(numObjects):
cube = p.loadURDF("cube_small.urdf", [0, i * 0.02, (i + 1) * 0.2])
#p.changeDynamics(cube,-1,mass=100)
p.stepSimulation()
p.setGravity(0, 0, -10)
def _step(self, action):
force = self.force_mag if action==1 else -self.force_mag
p.setJointMotorControl2(self.cartpole, 0, p.TORQUE_CONTROL, force=force)
p.stepSimulation()
self.state = p.getJointState(self.cartpole, 1)[0:2] + p.getJointState(self.cartpole, 0)[0:2]
theta, theta_dot, x, x_dot = self.state
done = x < -self.x_threshold \
or x > self.x_threshold \
or theta < -self.theta_threshold_radians \
or theta > self.theta_threshold_radians
done = bool(done)
reward = 1.0
#print("state=",self.state)
return np.array(self.state), reward, done, {}
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
def _step(self, action):
p.stepSimulation()
# time.sleep(self.timeStep)
self.state = p.getJointState(self.cartpole, 1)[0:2] + p.getJointState(self.cartpole, 0)[0:2]
theta, theta_dot, x, x_dot = self.state
dv = 0.1
deltav = [-10.*dv,-5.*dv, -2.*dv, -0.1*dv, 0, 0.1*dv, 2.*dv,5.*dv, 10.*dv][action]
p.setJointMotorControl2(self.cartpole, 0, p.VELOCITY_CONTROL, targetVelocity=(deltav + self.state[3]))
done = x < -self.x_threshold \
or x > self.x_threshold \
or theta < -self.theta_threshold_radians \
or theta > self.theta_threshold_radians
reward = 1.0
return np.array(self.state), reward, done, {}
def test_rolling_friction(self):
import pybullet as p
p.connect(p.DIRECT)
p.loadURDF("plane.urdf")
sphere = p.loadURDF("sphere2.urdf", [0, 0, 1])
p.resetBaseVelocity(sphere, linearVelocity=[1, 0, 0])
p.changeDynamics(sphere, -1, linearDamping=0, angularDamping=0)
#p.changeDynamics(sphere,-1,rollingFriction=0)
p.setGravity(0, 0, -10)
for i in range(1000):
p.stepSimulation()
vel = p.getBaseVelocity(sphere)
self.assertLess(vel[0][0], 1e-10)
self.assertLess(vel[0][1], 1e-10)
self.assertLess(vel[0][2], 1e-10)
self.assertLess(vel[1][0], 1e-10)
self.assertLess(vel[1][1], 1e-10)
self.assertLess(vel[1][2], 1e-10)
p.disconnect()
def _reset(self):
p.resetSimulation()
#p.setPhysicsEngineParameter(numSolverIterations=300)
p.setTimeStep(self._timeStep)
p.loadURDF(os.path.join(self._urdfRoot,"plane.urdf"))
dist = 5 +2.*random.random()
ang = 2.*3.1415925438*random.random()
ballx = dist * math.sin(ang)
bally = dist * math.cos(ang)
ballz = 1
p.setGravity(0,0,-10)
self._humanoid = simpleHumanoid.SimpleHumanoid(urdfRootPath=self._urdfRoot, timeStep=self._timeStep)
self._envStepCounter = 0
p.stepSimulation()
self._observation = self.getExtendedObservation()
return np.array(self._observation)
def step2(self, action):
for i in range(self._actionRepeat):
self._kuka.applyAction(action)
p.stepSimulation()
if self._termination():
break
#self._observation = self.getExtendedObservation()
self._envStepCounter += 1
self._observation = self.getExtendedObservation()
if self._renders:
time.sleep(self._timeStep)
#print("self._envStepCounter")
#print(self._envStepCounter)
done = self._termination()
reward = self._reward()
#print("len=%r" % len(self._observation))
return np.array(self._observation), reward, done, {}
def _reset(self):
self.terminated = 0
p.resetSimulation()
p.setPhysicsEngineParameter(numSolverIterations=150)
p.setTimeStep(self._timeStep)
p.loadURDF(os.path.join(self._urdfRoot,"plane.urdf"),[0,0,-1])
p.loadURDF(os.path.join(self._urdfRoot,"table/table.urdf"), 0.5000000,0.00000,-.820000,0.000000,0.000000,0.0,1.0)
xpos = 0.5 +0.2*random.random()
ypos = 0 +0.25*random.random()
ang = 3.1415925438*random.random()
orn = p.getQuaternionFromEuler([0,0,ang])
self.blockUid =p.loadURDF(os.path.join(self._urdfRoot,"block.urdf"), xpos,ypos,-0.1,orn[0],orn[1],orn[2],orn[3])
p.setGravity(0,0,-10)
self._kuka = kuka.Kuka(urdfRootPath=self._urdfRoot, timeStep=self._timeStep)
self._envStepCounter = 0
p.stepSimulation()
self._observation = self.getExtendedObservation()
return np.array(self._observation)
def _reset(self):
"""Environment reset called at the beginning of an episode.
"""
# Set the camera settings.
look = [0.23, 0.2, 0.54]
distance = 1.
pitch = -56 + self._cameraRandom*np.random.uniform(-3, 3)
yaw = 245 + self._cameraRandom*np.random.uniform(-3, 3)
roll = 0
self._view_matrix = p.computeViewMatrixFromYawPitchRoll(
look, distance, yaw, pitch, roll, 2)
fov = 20. + self._cameraRandom*np.random.uniform(-2, 2)
aspect = self._width / self._height
near = 0.01
far = 10
self._proj_matrix = p.computeProjectionMatrixFOV(
fov, aspect, near, far)
self._attempted_grasp = False
self._env_step = 0
self.terminated = 0
p.resetSimulation()
p.setPhysicsEngineParameter(numSolverIterations=150)
p.setTimeStep(self._timeStep)
p.loadURDF(os.path.join(self._urdfRoot,"plane.urdf"),[0,0,-1])
p.loadURDF(os.path.join(self._urdfRoot,"table/table.urdf"), 0.5000000,0.00000,-.820000,0.000000,0.000000,0.0,1.0)
p.setGravity(0,0,-10)
self._kuka = kuka.Kuka(urdfRootPath=self._urdfRoot, timeStep=self._timeStep)
self._envStepCounter = 0
p.stepSimulation()
# Choose the objects in the bin.
urdfList = self._get_random_object(
self._numObjects, self._isTest)
self._objectUids = self._randomly_place_objects(urdfList)
self._observation = self._get_observation()
return np.array(self._observation)
def main(unused_args):
timeStep = 0.01
c = p.connect(p.SHARED_MEMORY)
if (c<0):
c = p.connect(p.GUI)
p.resetSimulation()
p.setTimeStep(timeStep)
p.loadURDF("plane.urdf")
p.setGravity(0,0,-10)
minitaur = Minitaur()
amplitude = 0.24795664427
speed = 0.2860877729434
for i in range(1000):
a1 = math.sin(i*speed)*amplitude+1.57
a2 = math.sin(i*speed+3.14)*amplitude+1.57
joint_values = [a1, -1.57, a1, -1.57, a2, -1.57, a2, -1.57]
minitaur.applyAction(joint_values)
p.stepSimulation()
# print(minitaur.getBasePosition())
time.sleep(timeStep)
final_distance = np.linalg.norm(np.asarray(minitaur.getBasePosition()))
print(final_distance)
def _step_continuous(self, action):
"""Applies a continuous velocity-control action.
Args:
action: 5-vector parameterizing XYZ offset, vertical angle offset
(radians), and grasp angle (radians).
Returns:
observation: Next observation.
reward: Float of the per-step reward as a result of taking the action.
done: Bool of whether or not the episode has ended.
debug: Dictionary of extra information provided by environment.
"""
# Perform commanded action.
self._env_step += 1
self._kuka.applyAction(action)
for _ in range(self._actionRepeat):
p.stepSimulation()
if self._renders:
time.sleep(self._timeStep)
if self._termination():
break
# If we are close to the bin, attempt grasp.
state = p.getLinkState(self._kuka.kukaUid,
self._kuka.kukaEndEffectorIndex)
end_effector_pos = state[0]
if end_effector_pos[2] <= 0.1:
finger_angle = 0.3
for _ in range(500):
grasp_action = [0, 0, 0, 0, finger_angle]
self._kuka.applyAction(grasp_action)
p.stepSimulation()
#if self._renders:
# time.sleep(self._timeStep)
finger_angle -= 0.3/100.
if finger_angle < 0:
finger_angle = 0
for _ in range(500):
grasp_action = [0, 0, 0.001, 0, finger_angle]
self._kuka.applyAction(grasp_action)
p.stepSimulation()
if self._renders:
time.sleep(self._timeStep)
finger_angle -= 0.3/100.
if finger_angle < 0:
finger_angle = 0
self._attempted_grasp = True
observation = self._get_observation()
done = self._termination()
reward = self._reward()
debug = {
'grasp_success': self._graspSuccess
}
return observation, reward, done, debug
parser = urdfEditor.UrdfEditor()
parser.initializeFromBulletBody(mb,physicsClientId=org)
parser.saveUrdf("test.urdf")
if (1):
print("\ncreatingMultiBody...................n")
obUid = parser.createMultiBody(physicsClientId=gui)
parser2 = urdfEditor.UrdfEditor()
print("\n###########################################\n")
parser2.initializeFromBulletBody(obUid,physicsClientId=gui)
parser2.saveUrdf("test2.urdf")
for i in range (p.getNumJoints(obUid, physicsClientId=gui)):
p.setJointMotorControl2(obUid,i,p.VELOCITY_CONTROL,targetVelocity=0,force=1,physicsClientId=gui)
print(p.getJointInfo(obUid,i,physicsClientId=gui))
parser=0
p.setRealTimeSimulation(1,physicsClientId=gui)
while (p.getConnectionInfo(physicsClientId=gui)["isConnected"]):
p.stepSimulation(physicsClientId=gui)
time.sleep(0.01)
def _step_continuous(self, action):
"""Applies a continuous velocity-control action.
Args:
action: 5-vector parameterizing XYZ offset, vertical angle offset
(radians), and grasp angle (radians).
Returns:
observation: Next observation.
reward: Float of the per-step reward as a result of taking the action.
done: Bool of whether or not the episode has ended.
debug: Dictionary of extra information provided by environment.
"""
# Perform commanded action.
self._env_step += 1
self._kuka.applyAction(action)
for _ in range(self._actionRepeat):
p.stepSimulation()
time.sleep(0.00001)
state = p.getLinkState(self._kuka.kukaUid, self._kuka.kukaEndEffectorIndex)
end_effector_pos = state[0]
if self._termination():
break
# If we are close to the bin, attempt grasp.
state = p.getLinkState(self._kuka.kukaUid, self._kuka.kukaEndEffectorIndex)
end_effector_pos = state[0]
if end_effector_pos[2] <= 0.1:#0.1 for prev gripper
time.sleep(1)
print("attempting grasp")
print(end_effector_pos)
motor_fing = np.pi/4
hinge_fing = -np.pi/6
#grab config
grab = np.array([motor_fing,hinge_fing,-motor_fing,hinge_fing])
#grab = np.array([0,0,0,0])
for _ in range(500):
grasp_action = [0, 0, 0, 0, grab]
self._kuka.applyAction(grasp_action)
p.stepSimulation()
#if self._renders:
#time.sleep(self._timeStep)
#finger_angle -= 0.3 / 100.
#if finger_angle < 0:
#finger_angle = 0
for _ in range(100000):
grasp_action = [0, 0, 0.001, 0, grab]
self._kuka.applyAction(grasp_action)
p.stepSimulation()
state = p.getLinkState(self._kuka.kukaUid, self._kuka.kukaEndEffectorIndex)
end_effector_pos = state[0]
if end_effector_pos[2]>0.8:
break
time.sleep(self._timeStep)
#finger_angle -= 0.3 / 100.
#if finger_angle < 0:
#finger_angle = 0
for _ in range(1000):
grasp_action = [0, 0.01, 0.005, 0, grab]
self._kuka.applyAction(grasp_action)
p.stepSimulation()
time.sleep(self._timeStep)
#finger_angle -= 0.3 / 100.
#finger_angle=0.3
for _ in range(10000):
grasp_action = [0, -0.01, -0.005, 0, grab]
self._kuka.applyAction(grasp_action)
p.stepSimulation()
time.sleep(self._timeStep)
for _ in range(1000):
grasp_action = [0, 0.01, -0.001, 0, [0,0,0,0]]
self._kuka.applyAction(grasp_action)
p.stepSimulation()
time.sleep(self._timeStep)
self._attempted_grasp = True
observation = self._get_observation()
done = self._termination()
reward = self._reward()
for body in self._removal:
p.removeBody(body)
self._objectUids.remove(body)
self._removal=[]
for _ in range(100):
grasp_action=[0,0,0,0,np.zeros((4))]
self._kuka.applyAction(grasp_action)
debug = {'grasp_success': self._graspSuccess}
return observation, reward, done, debug
print(obj_vel)
print("obj_acc")
print(obj_acc)
torque = bullet.calculateInverseDynamics(id_robot, obj_pos, obj_vel, obj_acc)
q_tor[0][i] = torque[0]
q_tor[1][i] = torque[1]
if (verbose):
print("torque=")
print(torque)
# Set the Joint Torques:
bullet.setJointMotorControlArray(id_robot, id_revolute_joints, bullet.TORQUE_CONTROL, forces=[torque[0], torque[1]])
# Step Simulation
bullet.stepSimulation()
# Plot the Position, Velocity and Acceleration:
if plot:
figure = plt.figure(figsize=[15, 4.5])
figure.subplots_adjust(left=0.05, bottom=0.11, right=0.97, top=0.9, wspace=0.4, hspace=0.55)
ax_pos = figure.add_subplot(141)
ax_pos.set_title("Joint Position")
ax_pos.plot(t, q_pos_desired[0], '--r', lw=4, label='Desired q0')
ax_pos.plot(t, q_pos_desired[1], '--b', lw=4, label='Desired q1')
ax_pos.plot(t, q_pos[0], '-r', lw=1, label='Measured q0')
ax_pos.plot(t, q_pos[1], '-b', lw=1, label='Measured q1')
ax_pos.set_ylim(-1., 1.)
ax_pos.legend()
def reset(self):
if (self.test_phase):
global test_steps,test_done
if (test_steps != 0 ):
self.save_data_test()
test_steps = 0
p.resetSimulation()
p.setPhysicsEngineParameter(numSolverIterations=150)
p.setTimeStep(self._timeStep)
self._envStepCounter = 0
self.ep_counter = self.ep_counter + 1
p.loadURDF(os.path.join(pybullet_data.getDataPath(),"plane.urdf"), useFixedBase= True)
# Load robot
self._panda = pandaEnv(self._urdfRoot, timeStep=self._timeStep, basePosition =[0,0,0.625],
useInverseKinematics= self._useIK, action_space = self.action_dim, includeVelObs = self.includeVelObs)
# Load table and object for simulation
tableId = p.loadURDF(os.path.join(self._urdfRoot, "franka_description/table.urdf"), useFixedBase=True)
table_info = p.getVisualShapeData(tableId,-1)[0]
self._h_table =table_info[5][2] + table_info[3][2]
#limit panda workspace to table plane
self._panda.workspace_lim[2][0] = self._h_table
# Randomize start position of object and target.
#we take the target point
if (self.fixedPositionObj):
if(self.object_position==0):
#we have completely fixed position
self.obj_pose = [0.6,0.1,0.64]
self.target_pose = [0.4,0.45,0.64]
self._objID = p.loadURDF( os.path.join(self._urdfRoot,"franka_description/cube_small.urdf"), basePosition = self.obj_pose)
self._targetID = p.loadURDF(os.path.join(self._urdfRoot, "franka_description/domino/domino.urdf"), basePosition= self.target_pose)
elif(self.object_position==1):
if (self.keep_fixed_position):
if (self.ep_counter == 100):
self.obj_pose = [np.random.uniform(0.5,0.6),np.random.uniform(0,0.1),0.64]
self.target_pose = [np.random.uniform(0.4,0.5),np.random.uniform(0.45,0.55),0.64]
self.ep_counter = -1
else:
self.obj_pose = [np.random.uniform(0.5,0.6),np.random.uniform(0,0.1),0.64]
self.target_pose = self.target_pose = [0.4,0.45,0.64]
#self.target_pose = [np.random.uniform(0.4,0.5),np.random.uniform(0.45,0.55),0.64]
self._objID = p.loadURDF( os.path.join(self._urdfRoot,"franka_description/cube_small.urdf"), basePosition = self.obj_pose)
self._targetID = p.loadURDF(os.path.join(self._urdfRoot, "franka_description/domino/domino.urdf"), basePosition= self.target_pose)
elif(self.object_position==2):
self.obj_pose = [np.random.uniform(0.4,0.6),np.random.uniform(0,0.2),0.64]
self.target_pose = [np.random.uniform(0.3,0.5),np.random.uniform(0.35,0.55),0.64]
self._objID = p.loadURDF( os.path.join(self._urdfRoot,"franka_description/cube_small.urdf"), basePosition = self.obj_pose)
self._targetID = p.loadURDF(os.path.join(self._urdfRoot, "franka_description/domino/domino.urdf"), basePosition= self.target_pose)
elif(self.object_position==3):
print("")
else:
self.obj_pose, self.target_pose = self._sample_pose()
self._objID = p.loadURDF( os.path.join(self._urdfRoot,"franka_description/cube_small.urdf"), basePosition= self.obj_pose)
#useful to see where is the taget point
self._targetID = p.loadURDF(os.path.join(self._urdfRoot, "franka_description/domino/domino.urdf"), basePosition= self.target_pose)
self._debugGUI()
p.setGravity(0,0,-9.8)
# Let the world run for a bit
for _ in range(10):
p.stepSimulation()
#we take the dimension of the observation
return self.getExtendedObservation()
def move_bot(botId, goal_pos, goal_dir, steps=500, reset=False):
goalJoints = p.calculateInverseKinematics(
botId,
9,
goal_pos,
targetOrientation=goal_dir,
lowerLimits=joint_ll,
upperLimits=joint_ul,
jointRanges=joint_jr,
restPoses=[0, 0, 0, 1, -0.1, 0, 0, 0.1, 0],
jointDamping=[
0.001, 0.001, 0.001, 0.001, 0.001, 0.001, 0.001, 0.001, 0.001,
0.001
])
numJoints = p.getNumJoints(botId)
def set_joints(reset_joints):
for ji in range(numJoints):
jointInfo = p.getJointInfo(botId, ji)
qIndex = jointInfo[3]
if qIndex > -1:
x = goalJoints[qIndex - 7]
if qIndex - 7 < len(joint_ll) and (x < joint_ll[qIndex - 7] or
x > joint_ul[qIndex - 7]):
print('LIMIT VIOLATION!!', x, joint_ll[qIndex - 7],
joint_ul[qIndex - 7], jointInfo[1])
#exit()
if reset_joints:
p.resetJointState(botId, ji, x, 0)
p.setJointMotorControl2(botId,
ji,
p.POSITION_CONTROL,
targetPosition=x,
targetVelocity=0,
force=500,
positionGain=0.03,
velocityGain=1)
def check_joints(goalJoints):
jointStates = []
jointNames = []
if goalJoints is not None:
numJoints = p.getNumJoints(botId)
for ji in range(numJoints):
jointInfo = p.getJointInfo(botId, ji)
qIndex = jointInfo[3]
if qIndex > -1:
x = goalJoints[qIndex - 7]
jointPos = p.getJointState(botId, ji)[0]
jointNames.append(jointInfo[1])
jointStates.append(jointPos)
return jointStates, jointNames
set_joints(False)
stats = [[] for _ in range(len(goalJoints))]
for i in range(steps):
if reset:
print('setting joints')
set_joints(True)
p.stepSimulation()
if i % 10 == 0:
pos, names = check_joints(goalJoints)
for pi, px in enumerate(pos):
stats[pi].append(px)
import matplotlib.pyplot as plt
fig, ax = plt.subplots(1, 10)
for i, (g, s) in enumerate(zip(goalJoints, stats)):
ax[i].set_title(str(names[i]))
ax[i].plot(s, label='actual')
ax[i].plot([0, 100], [g, g], label='target')
if i < len(joint_ul):
ax[i].plot([0, 100], [joint_ll[i], joint_ll[i]], label='lower')
ax[i].plot([0, 100], [joint_ul[i], joint_ul[i]], label='upper')
fig.set_size_inches(30, 2)
fig.savefig('joints.png')
import pybullet as p
import time
import pybullet_data
physics_client = p.connect(p.GUI)
p.setAdditionalSearchPath(pybullet_data.getDataPath())
p.setGravity(0, 0, -9.8)
robotStartPos = [0, 0, 0.15]
robotStartOrientation = p.getQuaternionFromEuler([0, 0, 0])
planeID = p.loadURDF("plane.urdf")
robotID = p.loadURDF("mobile.urdf", robotStartPos, robotStartOrientation)
for i in range(10000):
p.stepSimulation()
time.sleep(1. / 240.)
cubePos, cubeOrn = p.getBasePositionAndOrientation(robotID)
print(cubePos, cubeOrn)
p.disconnect()
def reset(self):
self.terminated = False
self.n_contacts = 0
self.n_steps_outside = 0
p.resetSimulation()
p.setPhysicsEngineParameter(numSolverIterations=150)
p.setTimeStep(self._timestep)
p.loadURDF(os.path.join(self._urdf_root, "plane.urdf"), [0, 0, -1])
self.table_uid = p.loadURDF(
os.path.join(self._urdf_root, "table/table.urdf"), 0.5000000,
0.00000, -.820000, 0.000000, 0.000000, 0.0, 1.0)
# Initialize button position
x_pos = 0.5
y_pos = 0
if self._random_target:
x_pos += 0.15 * self.np_random.uniform(-1, 1)
y_pos += 0.3 * self.np_random.uniform(-1, 1)
self.button_uid = p.loadURDF("/urdf/simple_button.urdf",
[x_pos, y_pos, Z_TABLE])
self.button_pos = np.array([x_pos, y_pos, Z_TABLE])
p.setGravity(0, 0, -10)
self._kuka = kuka.Kuka(urdf_root_path=self._urdf_root,
timestep=self._timestep,
use_inverse_kinematics=(not self.action_joints),
small_constraints=(not self._random_target))
self._env_step_counter = 0
# Close the gripper and wait for the arm to be in rest position
for _ in range(500):
if self.action_joints:
self._kuka.applyAction(
list(np.array(self._kuka.joint_positions)[:7]) + [0, 0])
else:
self._kuka.applyAction([0, 0, 0, 0, 0])
p.stepSimulation()
# Randomize init arm pos: take 5 random actions
for _ in range(N_RANDOM_ACTIONS_AT_INIT):
if self._is_discrete:
action = [0, 0, 0, 0, 0]
sign = 1 if self.np_random.rand() > 0.5 else -1
action_idx = self.np_random.randint(3) # dx, dy or dz
action[action_idx] += sign * DELTA_V
else:
if self.action_joints:
joints = np.array(self._kuka.joint_positions)[:7]
joints += DELTA_THETA * self.np_random.normal(joints.shape)
action = list(joints) + [0, 0]
else:
action = np.zeros(5)
rand_direction = self.np_random.normal((3, ))
# L2 normalize, so that the random direction is not too high or too low
rand_direction /= np.linalg.norm(rand_direction, 2)
action[:3] += DELTA_V_CONTINUOUS * rand_direction
self._kuka.applyAction(list(action))
p.stepSimulation()
self._observation = self.getExtendedObservation()
self.button_pos = np.array(
p.getLinkState(self.button_uid, BUTTON_LINK_IDX)[0])
self.button_pos[
2] += BUTTON_DISTANCE_HEIGHT # Set the target position on the top of the button
if self.saver is not None:
self.saver.reset(self._observation, self.getTargetPos(),
self.getGroundTruth())
if self.srl_model != "raw_pixels":
return self.getSRLState(self._observation)
return np.array(self._observation)
def testSaveRestoreState(self):
numSteps = 500
numSteps2 = 30
verbose = 0
self.setupWorld()
for i in range(numSteps):
p.stepSimulation()
p.saveBullet("state.bullet")
if verbose:
p.setInternalSimFlags(1)
p.stepSimulation()
if verbose:
p.setInternalSimFlags(0)
print("contact points=")
for q in p.getContactPoints():
print(q)
for i in range(numSteps2):
p.stepSimulation()
file = open("saveFile.txt", "w")
self.dumpStateToFile(file)
file.close()
#################################
self.setupWorld()
#both restore from file or from in-memory state should work
p.restoreState(fileName="state.bullet")
stateId = p.saveState()
if verbose:
p.setInternalSimFlags(1)
p.stepSimulation()
if verbose:
p.setInternalSimFlags(0)
print("contact points restored=")
for q in p.getContactPoints():
print(q)
for i in range(numSteps2):
p.stepSimulation()
file = open("restoreFile.txt", "w")
self.dumpStateToFile(file)
file.close()
p.restoreState(stateId)
if verbose:
p.setInternalSimFlags(1)
p.stepSimulation()
if verbose:
p.setInternalSimFlags(0)
print("contact points restored=")
for q in p.getContactPoints():
print(q)
for i in range(numSteps2):
p.stepSimulation()
file = open("restoreFile2.txt", "w")
self.dumpStateToFile(file)
file.close()
file1 = open("saveFile.txt", "r")
file2 = open("restoreFile.txt", "r")
self.compareFiles(file1, file2)
file1.close()
file2.close()
file1 = open("saveFile.txt", "r")
file2 = open("restoreFile2.txt", "r")
self.compareFiles(file1, file2)
file1.close()
file2.close()
def playReferenceMotion(self, motionFile):
motionFile = os.path.join(os.path.dirname(__file__), motionFile)
with open(motionFile, "r") as motion_file:
motion = json.load(motion_file)
'''
Joint indices should correspond to the following:
0 - root Type: FIXED
1 - chest Type: SPHERICAL
2 - neck Type: SPHERICAL
3 - right_hip Type: SPHERICAL
4 - right_knee Type: REVOLUTE
5 - right_ankle Type: SPHERICAL
6 - right_shoulder Type: SPHERICAL
7 - right_elbow Type: REVOLUTE
8 - right_wrist Type: FIXED
9 - left_hip Type: SPHERICAL
10 - left_knee Type: REVOLUTE
11 - left_ankle Type: SPHERICAL
12 - left_shoulder Type: SPHERICAL
13 - left_elbow Type: REVOLUTE
14 - left_wrist Type: FIXED
'''
JointFrameMapIndices = [
0, #root
[9, 10, 11, 8], #chest
[13, 14, 15, 12], #neck
[17, 18, 19, 16], #rHip
20, #rKnee
[22, 23, 24, 21], #rAnkle
[26, 27, 28, 25], #rShoulder
29, #rElbow
0, #rWrist
[31, 32, 33, 30], #lHip
34, #lKnee
[36, 37, 38, 35], #lAnkle
[40, 41, 42, 39], #lShoulder
43, #lElbow
0, #lWrist
]
for frame in motion['Frames']:
basePos1 = [frame[i] for i in [1, 2, 3]]
baseOrn1 = frame[5:8] + [frame[4]]
# transform the position and orientation to have z-axis upward
y2zPos = [0, 0, 0.0]
y2zOrn = p.getQuaternionFromEuler([1.57, 0, 0])
basePos, baseOrn = p.multiplyTransforms(y2zPos, y2zOrn, basePos1, baseOrn1)
# set the agent's root position and orientation
p.resetBasePositionAndOrientation(
self.humanoidAgent,
posObj=basePos,
ornObj=baseOrn
)
# Set joint positions
for joint in self.revoluteJoints:
p.resetJointState(
self.humanoidAgent,
jointIndex=joint,
targetValue=frame[JointFrameMapIndices[joint]]
)
for joint in self.sphericalJoints:
p.resetJointStateMultiDof(
self.humanoidAgent,
jointIndex=joint,
targetValue=[frame[i] for i in JointFrameMapIndices[joint]]
)
for i in range(16):
p.stepSimulation()
time.sleep(0.1/240)
def step(self, ctrl_raw):
# Add new action to queue
self.act_queue.append(ctrl_raw)
self.act_queue.pop(0)
# Simulate the delayed action
if self.randomized_params["output_transport_delay"] > 0:
ctrl_raw_unqueued = self.act_queue[-self.config["act_input"]:]
ctrl_delayed = self.act_queue[
-1 - self.randomized_params["output_transport_delay"]]
else:
ctrl_raw_unqueued = self.act_queue
ctrl_delayed = self.act_queue[-1]
# Scale the action appropriately (neural network gives [-1,1], pid controller [0,1])
if self.config["controller_source"] == "nn":
ctrl_processed = np.clip(ctrl_delayed, -1, 1) * 0.5 + 0.5
else:
ctrl_processed = ctrl_delayed
# Take into account motor delay
self.update_motor_vel(ctrl_processed)
# Apply motor forces
for i in range(4):
motor_force_w_noise = np.clip(
self.current_motor_velocity_vec[i] *
self.randomized_params["motor_power_variance_vector"][i], 0, 1)
motor_force_scaled = motor_force_w_noise * self.randomized_params[
"motor_force_multiplier"]
p.applyExternalForce(self.robot,
linkIndex=i * 2 + 1,
forceObj=[0, 0, motor_force_scaled],
posObj=[0, 0, 0],
flags=p.LINK_FRAME,
physicsClientId=self.client_ID)
p.applyExternalTorque(
self.robot,
linkIndex=i * 2 + 1,
torqueObj=[
0, 0, self.current_motor_velocity_vec[i] *
self.reactive_torque_dir_vec[i] *
self.config["propeller_parasitic_torque_coeff"]
],
flags=p.LINK_FRAME,
physicsClientId=self.client_ID)
self.apply_external_disturbances()
p.stepSimulation(physicsClientId=self.client_ID)
if self.config["animate"]:
time.sleep(self.config["sim_timestep"])
self.step_ctr += 1
torso_pos, torso_quat, torso_euler, torso_vel, torso_angular_vel = self.get_obs(
)
roll, pitch, yaw = torso_euler
pos_delta = np.array(torso_pos) - np.array(self.config["target_pos"])
crashed = (abs(pos_delta) > 6.0).any() or (
(torso_pos[2] < 0.3) and (abs(roll) > 2.5 or abs(pitch) > 2.5))
if self.prev_act is not None:
action_penalty = np.mean(
np.square(np.array(ctrl_processed) - np.array(self.prev_act))
) * self.config["pen_act_coeff"]
else:
action_penalty = 0
# Calculate true reward
pen_position = np.mean(np.square(pos_delta)) * (
self.config["pen_position_coeff"] +
0 * self.step_ctr / float(self.config["max_steps"]))
pen_rpy = np.mean(np.square(
np.array(torso_euler))) * self.config["pen_rpy_coeff"]
pen_vel = np.mean(np.square(torso_vel)) * self.config["pen_vel_coeff"]
pen_rotvel = np.mean(
np.square(torso_angular_vel)) * self.config["pen_ang_vel_coeff"]
r_true = -action_penalty - pen_position - pen_rpy - pen_rotvel - pen_vel
#print(action_penalty, pen_position, pen_rpy, pen_rotvel, pen_vel)
# Calculate proxy reward (for learning purposes)
pen_position_proxy = np.mean(
np.square(pos_delta)) * self.config["pen_position_coeff"]
pen_yaw_proxy = np.mean(np.square(yaw)) * self.config["pen_yaw_coeff"]
pen_vel_proxy = np.mean(np.square(torso_vel)) * np.maximum(
1. - 3 * pen_position_proxy, 0) * self.config["pen_vel_coeff"]
r = -pen_position_proxy - pen_yaw_proxy - action_penalty - pen_vel_proxy
r = np.clip(r_true, -2, 2)
if self.step_ctr == 1:
r = 0
self.rew_queue.append([r])
self.rew_queue.pop(0)
if self.randomized_params["input_transport_delay"] > 0:
r_unqueued = self.rew_queue[-self.config["rew_input"]:]
else:
r_unqueued = self.rew_queue
done = (self.step_ctr > self.config["max_steps"])
compiled_obs = pos_delta, torso_quat, torso_vel, torso_angular_vel
compiled_obs_flat = [
item for sublist in compiled_obs for item in sublist
]
self.obs_queue.append(compiled_obs_flat)
self.obs_queue.pop(0)
if self.randomized_params["input_transport_delay"] > 0:
obs_raw_unqueued = self.obs_queue[-self.config["obs_input"]:]
else:
obs_raw_unqueued = self.obs_queue
aux_obs = []
for i in range(len(obs_raw_unqueued)):
t_obs = []
t_obs.extend(obs_raw_unqueued[i])
if self.config["act_input"] > 0:
t_obs.extend(ctrl_raw_unqueued[i])
if self.config["rew_input"] > 0:
t_obs.extend(r_unqueued[i])
if self.config["step_counter"] > 0:
def step_ctr_to_obs(step_ctr):
return (float(step_ctr) / self.config["max_steps"]) * 2 - 1
t_obs.extend(
[step_ctr_to_obs(np.maximum(self.step_ctr - i - 1, 0))])
aux_obs.extend(t_obs)
if self.config["latent_input"]:
aux_obs.extend(self.randomized_params_list_norm)
obs = np.array(aux_obs).astype(np.float32)
obs_dict = {
"pos_delta": pos_delta,
"torso_quat": torso_quat,
"torso_vel": torso_vel,
"torso_angular_vel": torso_angular_vel,
"reward": r_true,
"action": ctrl_processed
}
self.prev_act = ctrl_processed
return obs, r, done, {
"obs_dict": obs_dict,
"randomized_params": self.randomized_params,
"true_rew": r_true
}
robot = Robot()
robot_viz = RobotViz(sim)
robot_viz.load([0.0, 0.0, robot.height, 0.0])
path = Path2D(trajectory)
path_viz = PathViz(sim)
path_viz.load_path(path)
trajectory_end = False
current_tf = robot_viz.current_tf()
while not trajectory_end:
goal_tf = path.get_current_target()
delta_tf = get_control_cmd(pbtf2pathtf(current_tf), goal_tf)
new_tf = robot.move(current_tf, delta_tf)
robot_viz.visualize_base(*new_tf)
p.stepSimulation(sim.pClient)
current_tf = robot_viz.current_tf()
is_success, trajectory_end = path.try_step(pbtf2pathtf(current_tf))
if is_success:
lx, ly, ltheta = pbtf2pathtf(current_tf)
gx, gy, gtheta = path.get_current_target()
path_viz.draw_line(fx=lx, fy=ly, tx=gx, ty=gy)
# TODO add visualization that target is reached
# time.sleep(1/30)
time.sleep(1 / 10)
# TRAJECTORY is OVER - rotate robot
for i in range(20):
current_tf = robot_viz.current_tf()
delta_tf = 0.0, 0.0, 0.5
new_tf = robot.move(current_tf, delta_tf)
def run(self, runtime, goal_position=[]):
if (not goal_position == []) and (not self.goal_position
== goal_position):
# if self.goalBodyID is not None:
# pybullet.removeBody(self.goalBodyID, physicsClientId=self.physicsClient)
self.goalBodyID = pybullet.createMultiBody(
baseMass=0.0,
baseInertialFramePosition=[0, 0, 0],
baseVisualShapeIndex=self.visualGoalShapeId,
basePosition=goal_position,
useMaximalCoordinates=True,
physicsClientId=self.physicsClient)
# print ("Goal information: ", self.goalBodyID)
self.__base_collision = False
self.__heightExceed = False
self.goal_position = goal_position
assert not self.__controller == None, "Controller not set"
self.__states = [self.getState()] #load with init state
self.__rotations = [self.getRotation()]
self.__commands = []
old_time = 0
first = True
self.__command = object
controlStep = 0
self.flip = False
for i in range(int(runtime / self.__simStep)):
if i * self.__simStep - old_time > self.__controlStep or first:
state = self.getState()
self.__command = self.__controller.nextCommand(i *
self.__simStep)
if not first:
self.__states.append(state)
self.__rotations.append(self.getRotation())
self.__commands.append(self.__command)
first = False
old_time = i * self.__simStep
controlStep += 1
self.set_commands(self.__command)
pybullet.stepSimulation(physicsClientId=self.physicsClient)
if pybullet.getConnectionInfo(
self.physicsClient)['connectionMethod'] == pybullet.GUI:
time.sleep(self.__simStep / float(self.__vspeed))
# Flipfing behavior
if self.getZOrientation() < 0.0:
self.flip = True
#Base floor collision
if len(
pybullet.getContactPoints(
self.__planeId,
self.__hexapodId,
-1,
-1,
physicsClientId=self.physicsClient)) > 0:
self.__base_collision = True
#Jumping behavior when CM crosses 2.2
if self.getState()[2] > 2.2:
self.__heightExceed = True
def move_pos(
self,
absolute_pos=None,
relative_pos=None,
absolute_global_euler=None, # preferred
relative_global_euler=None, # preferred
relative_local_euler=None, # not using
absolute_global_quat=None, # preferred
relative_azi=None, # for arm
# relative_quat=None, # never use relative quat
numSteps=50,
maxJointVel=0.20,
timeStep=0,
relativePos=True,
checkContact=False,
objId=None,
posGain=20,
velGain=5):
# Get trajectory
eePosNow, eeQuatNow = self._panda.get_ee()
# Determine target pos
if absolute_pos is not None:
targetPos = absolute_pos
elif relative_pos is not None:
targetPos = eePosNow + relative_pos
else:
targetPos = eePosNow
# Determine target orn
if absolute_global_euler is not None:
targetOrn = euler2quat(absolute_global_euler)
elif relative_global_euler is not None:
targetOrn = quatMult(euler2quat(relative_global_euler), eeQuatNow)
elif relative_local_euler is not None:
targetOrn = quatMult(eeQuatNow, euler2quat(relative_local_euler))
elif absolute_global_quat is not None:
targetOrn = absolute_global_quat
elif relative_azi is not None:
# Extrinsic yaw
targetOrn = quatMult(euler2quat([relative_azi[0], 0, 0]),
eeQuatNow)
# Intrinsic pitch
targetOrn = quatMult(targetOrn, euler2quat([0, relative_azi[1],
0]))
# elif relative_quat is not None:
# targetOrn = quatMult(eeQuatNow, relative_quat)
else:
targetOrn = array([1.0, 0., 0., 0.])
# Get trajectory
trajPos = self.traj_time_scaling(startPos=eePosNow,
endPos=targetPos,
numSteps=numSteps)
# Run steps
numSteps = len(trajPos)
for step in range(numSteps):
# Get joint velocities from error tracking control, takes 0.2ms
jointDot = self.traj_tracking_vel(targetPos=trajPos[step],
targetQuat=targetOrn,
posGain=posGain,
velGain=velGain)
# Send velocity commands to joints
for i in range(self._panda.numJointsArm):
p.setJointMotorControl2(self._pandaId,
i,
p.VELOCITY_CONTROL,
targetVelocity=jointDot[i],
force=self._panda.maxJointForce[i],
maxVelocity=maxJointVel)
# Keep gripper current velocity
p.setJointMotorControl2(self._pandaId,
self._panda.pandaLeftFingerJointIndex,
p.VELOCITY_CONTROL,
targetVelocity=self._panda.fingerCurVel,
force=self._panda.maxFingerForce,
maxVelocity=0.05)
p.setJointMotorControl2(self._pandaId,
self._panda.pandaRightFingerJointIndex,
p.VELOCITY_CONTROL,
targetVelocity=self._panda.fingerCurVel,
force=self._panda.maxFingerForce,
maxVelocity=0.05)
# Quit if contact at either finger
if checkContact:
contact = self.check_contact(objId, both=False)
if contact:
return timeStep, False
# Step simulation, takes 1.5ms
p.stepSimulation()
timeStep += 1
return timeStep, True
def velocity(self, value):
Actuator.setVelocity(self.joint, value)
@property
def torque(self):
jointState = self.joint.getState()
return jointState.appliedTorque
@torque.setter
def torque(self, value):
Actuator.setTorque(self.joint, value)
if __name__ == "__main__":
import os
sid = pb.connect(pb.GUI)
cwd = os.getcwd()
pb.setAdditionalSearchPath(cwd + "../../data")
pb.setGravity(0, 0, -9.8, sid)
robot = BaxterRobot()
robot.resetJoints()
pb.setRealTimeSimulation(True)
for i in range(1000000):
pb.stepSimulation()
robot.controlActuators({"left_e0": 1.0}, controlType="torque")
print(robot.actuators.keys())
print(robot.joints.keys())
def step_simulation(steps=1, simulate=False):
for i in range(steps):
p.stepSimulation()
if simulate: time.sleep(SLEEP)
def test_foot_pressure_synchronization(self):
import pybullet as pb
fig, axs = plt.subplots(2)
self.walker.setPose(Transformation([0, 0, 0], [0, 0, 0, 1]))
self.walker.ready()
self.walker.wait(100)
self.walker.setGoal(Transformation([1, 0, 0], [0, 0, 0, 1]))
times = np.linspace(0, self.walker.soccerbot.robot_path.duration(), num=math.ceil(self.walker.soccerbot.robot_path.duration() / self.walker.soccerbot.robot_path.step_size) + 1)
lfp = np.zeros((4, 4, len(times)))
rfp = np.zeros((4, 4, len(times)))
i = 0
for t in times:
[lfp[:,:, i], rfp[:,:, i]] = self.walker.soccerbot.robot_path.footPosition(t)
i = i + 1
right = rfp[2, 3, :].ravel()
left = lfp[2, 3, :].ravel()
right = 0.1 - right
left = left - 0.1
axs[1].plot(times, left, label='Left')
axs[1].plot(times, right, label='Right')
axs[1].grid(b=True, which='both', axis='both')
axs[1].legend()
t = 0
scatter_pts_x = []
scatter_pts_y = []
scatter_pts_x_1 = []
scatter_pts_y_1 = []
while t <= self.walker.soccerbot.robot_path.duration():
if self.walker.soccerbot.current_step_time <= t <= self.walker.soccerbot.robot_path.duration():
self.walker.soccerbot.stepPath(t, verbose=True)
self.walker.soccerbot.apply_imu_feedback(self.walker.soccerbot.get_imu())
sensors = self.walker.soccerbot.get_foot_pressure_sensors(self.walker.ramp.plane)
for i in range(len(sensors)):
if sensors[i] == True:
scatter_pts_x.append(t)
scatter_pts_y.append(i)
if np.sum(sensors[0:4]) >= 2:
scatter_pts_x_1.append(t)
scatter_pts_y_1.append(-0.1)
if np.sum(sensors[4:8]) >= 2:
scatter_pts_x_1.append(t)
scatter_pts_y_1.append(0.1)
forces = self.walker.soccerbot.apply_foot_pressure_sensor_feedback(self.walker.ramp.plane)
pb.setJointMotorControlArray(bodyIndex=self.walker.soccerbot.body, controlMode=pb.POSITION_CONTROL,
jointIndices=list(range(0, 20, 1)),
targetPositions=self.walker.soccerbot.get_angles(),
forces=forces
)
self.walker.soccerbot.current_step_time = self.walker.soccerbot.current_step_time + self.walker.soccerbot.robot_path.step_size
pb.stepSimulation()
t = t + self.walker.PYBULLET_STEP
axs[0].scatter(scatter_pts_x, scatter_pts_y, s=3)
axs[1].scatter(scatter_pts_x_1, scatter_pts_y_1, s=3)
plt.show()
pitch = -10.0
roll = 0
upAxisIndex = 2
camDistance = 4
pixelWidth = 320
pixelHeight = 200
nearPlane = 0.01
farPlane = 100
fov = 60
main_start = time.time()
while (1):
for yaw in range(0, 360, 10):
pybullet.stepSimulation()
start = time.time()
viewMatrix = pybullet.computeViewMatrixFromYawPitchRoll(
camTargetPos, camDistance, yaw, pitch, roll, upAxisIndex)
aspect = pixelWidth / pixelHeight
projectionMatrix = pybullet.computeProjectionMatrixFOV(
fov, aspect, nearPlane, farPlane)
img_arr = pybullet.getCameraImage(
pixelWidth,
pixelHeight,
viewMatrix,
projectionMatrix,
shadow=1,
lightDirection=[1, 1, 1],
renderer=pybullet.ER_BULLET_HARDWARE_OPENGL)
def reset(self, init_joints=None, scene_file=None):
"""Environment reset"""
# Set the camera settings.
look = [
-0.35,
-0.58,
-0.88,
]
distance = 1.3
pitch = -41.0
yaw = 180
roll = 0
fov = 60.0 + self._cameraRandom * np.random.uniform(-2, 2)
self._view_matrix = p.computeViewMatrixFromYawPitchRoll(
look, distance, yaw, pitch, roll, 2)
aspect = float(self._window_width) / self._window_height
self.near = 0.1
self.far = 10
self._proj_matrix = p.computeProjectionMatrixFOV(
fov, aspect, self.near, self.far)
# Set table and plane
p.resetSimulation()
p.setTimeStep(self._timeStep)
# Intialize robot and objects
p.setGravity(0, 0, -9.81)
p.stepSimulation()
if init_joints is None:
self._panda = Panda(stepsize=self._timeStep,
base_shift=self._shift)
else:
for _ in range(1000):
p.stepSimulation()
self._panda = Panda(stepsize=self._timeStep,
init_joints=init_joints,
base_shift=self._shift)
plane_file = "data/objects/floor"
table_file = "data/objects/table/models"
self.plane_id = p.loadURDF(
os.path.join(plane_file, 'model_normalized.urdf'),
[0 - self._shift[0], 0 - self._shift[1], -0.82 - self._shift[2]])
self.table_id = p.loadURDF(
os.path.join(table_file, 'model_normalized.urdf'),
0.5 - self._shift[0],
0.0 - self._shift[1],
-0.82 - self._shift[2],
0.707,
0.0,
0.0,
0.707,
)
if not self._object_cached:
self._objectUids = self.cache_objects()
self.obj_path += [plane_file, table_file]
self._objectUids += [self.plane_id, self.table_id]
self._env_step = 0
return self._get_observation()
def _randomly_place_objects(self, urdfList, scale=1, poses=None):
"""
Randomize positions of each object urdf.
"""
xpos = 0.6 + 0.2 * (self._blockRandom * random.random() -
0.5) - self._shift[0]
ypos = 0.5 * self._blockRandom * (random.random() -
0.5) - self._shift[0]
orn = p.getQuaternionFromEuler([0, 0, 0]) #
p.resetBasePositionAndOrientation(
self._objectUids[self.target_idx],
[xpos, ypos, -0.44 - self._shift[2]],
[orn[0], orn[1], orn[2], orn[3]],
)
p.resetBaseVelocity(self._objectUids[self.target_idx], (0.0, 0.0, 0.0),
(0.0, 0.0, 0.0))
self.cached_objects[self.obj_path.index(urdfList[0])] = True
for _ in range(3000):
p.stepSimulation()
pos, _ = p.getLinkState(self._panda.pandaUid,
self._panda.pandaEndEffectorIndex)[:2]
pos = np.array([[pos[0], pos[1]], [xpos, ypos]])
k = 0
max_cnt = 50
obj_radius = [
0.05,
np.max(self.cache_object_extents[self.obj_path.index(urdfList[0])])
/ 2,
]
assigned_orn = [orn]
placed_indexes = [self.target_idx]
for i, name in enumerate(urdfList[1:]):
obj_idx = self.obj_path.index(name)
radius = np.max(self.cache_object_extents[obj_idx]) / 2
cnt = 0
if self.cached_objects[obj_idx]:
continue
while cnt < max_cnt:
cnt += 1
xpos_ = xpos - self._blockRandom * 1.0 * random.random()
ypos_ = ypos - self._blockRandom * 3 * (random.random() - 0.5
) # 0.5
xy = np.array([[xpos_, ypos_]])
if (self._check_safe_distance(xy, pos, obj_radius, radius)
and (xpos_ > 0.35 - self._shift[0]
and xpos_ < 0.65 - self._shift[0])
and (ypos_ < 0.20 - self._shift[1]
and ypos_ > -0.20 - self._shift[1])): # 0.15
break
if cnt == max_cnt:
continue # check 1
xpos_ = xpos_ + 0.05 # closer and closer to the target
angle = np.random.uniform(-np.pi, np.pi)
orn = p.getQuaternionFromEuler([0, 0, angle])
p.resetBasePositionAndOrientation(
self._objectUids[obj_idx],
[xpos, ypos_, -0.44 - self._shift[2]],
[orn[0], orn[1], orn[2], orn[3]],
) # xyzw
p.resetBaseVelocity(self._objectUids[obj_idx], (0.0, 0.0, 0.0),
(0.0, 0.0, 0.0))
for _ in range(3000):
p.stepSimulation()
_, new_orn = p.getBasePositionAndOrientation(
self._objectUids[obj_idx])
ang = (np.arccos(
2 * np.power(np.dot(tf_quat(orn), tf_quat(new_orn)), 2) - 1) *
180.0 / np.pi)
if ang > 20:
p.resetBasePositionAndOrientation(
self._objectUids[obj_idx],
[xpos, ypos, -10.0 - self._shift[2]],
[orn[0], orn[1], orn[2], orn[3]],
)
continue # check 2
self.cached_objects[obj_idx] = True
obj_radius.append(radius)
pos = np.concatenate([pos, xy], axis=0)
xpos = xpos_
placed_indexes.append(obj_idx)
assigned_orn.append(orn)
for _ in range(10000):
p.stepSimulation()
return True
def main():
# Open GUI and set pybullet_data in the path
p.connect(p.GUI)
#p.setAdditionalSearchPath(pybullet_data.getDataPath())
# Load plane contained in pybullet_data
p.loadURDF(os.path.join(pybullet_data.getDataPath(), "plane.urdf"))
# Set gravity for simulation
p.setGravity(0, 0, -9.8)
# load robot model
robotIds = p.loadSDF(
os.path.join(robot_data.getDataPath(), "iCub/icub_fixed_model.sdf"))
icubId = robotIds[0]
# set constraint between base_link and world
p.createConstraint(icubId, -1, -1, -1, p.JOINT_FIXED, [0, 0, 0], [0, 0, 0],
[
p.getBasePositionAndOrientation(icubId)[0][0],
p.getBasePositionAndOrientation(icubId)[0][1],
p.getBasePositionAndOrientation(icubId)[0][2] * 1.2
],
p.getBasePositionAndOrientation(icubId)[1])
##init_pos for standing
# without FT_sensors
init_pos = [0] * 15 + [
-29.4, 28.8, 0, 44.59, 0, 0, 0, 0.47, 0, 0, -29.4, 30.4, 0, 44.59, 0,
0, 0
]
# with FT_sensors
#init_pos = [0]*19 + [-29.4, 28.8, 0, 0, 44.59, 0, 0, 0, 0.47, 0, 0, -29.4, 30.4, 0, 0, 44.59, 0, 0, 0]
# all set to zero
#init_pos = [0]*p.getNumJoints(icubId)
# Load other objects
p.loadURDF(os.path.join(pybullet_data.getDataPath(), "table/table.urdf"),
[1.1, 0.0, 0.0])
p.loadURDF(os.path.join(pybullet_data.getDataPath(), "lego/lego.urdf"),
[0.5, 0.0, 0.8])
# add debug slider
jointIds = []
paramIds = []
joints_num = p.getNumJoints(icubId)
print("len init_pos ", len(init_pos))
print("len num joints", joints_num)
for j in range(joints_num):
info = p.getJointInfo(icubId, j)
jointName = info[1]
#jointType = info[2]
jointIds.append(j)
paramIds.append(
p.addUserDebugParameter(jointName.decode("utf-8"), info[8],
info[9], init_pos[j] / 180 * m.pi))
while True:
for i in range(joints_num):
p.setJointMotorControl2(icubId,
i,
p.POSITION_CONTROL,
targetPosition=p.readUserDebugParameter(i),
targetVelocity=0.0,
positionGain=0.25,
velocityGain=0.75,
force=50)
p.stepSimulation()
time.sleep(0.01)
def testSaveRestoreState(self):
numSteps = 500
numSteps2 = 30
verbose = 0
self.setupWorld()
for i in range(numSteps):
p.stepSimulation()
p.saveBullet("state.bullet")
if verbose:
p.setInternalSimFlags(1)
p.stepSimulation()
if verbose:
p.setInternalSimFlags(0)
print("contact points=")
for q in p.getContactPoints():
print(q)
for i in range(numSteps2):
p.stepSimulation()
file = open("saveFile.txt", "w")
self.dumpStateToFile(file)
file.close()
#################################
self.setupWorld()
#both restore from file or from in-memory state should work
p.restoreState(fileName="state.bullet")
stateId = p.saveState()
if verbose:
p.setInternalSimFlags(1)
p.stepSimulation()
if verbose:
p.setInternalSimFlags(0)
print("contact points restored=")
for q in p.getContactPoints():
print(q)
for i in range(numSteps2):
p.stepSimulation()
file = open("restoreFile.txt", "w")
self.dumpStateToFile(file)
file.close()
p.restoreState(stateId)
if verbose:
p.setInternalSimFlags(1)
p.stepSimulation()
if verbose:
p.setInternalSimFlags(0)
print("contact points restored=")
for q in p.getContactPoints():
print(q)
for i in range(numSteps2):
p.stepSimulation()
file = open("restoreFile2.txt", "w")
self.dumpStateToFile(file)
file.close()
file1 = open("saveFile.txt", "r")
file2 = open("restoreFile.txt", "r")
self.compareFiles(file1, file2)
file1.close()
file2.close()
file1 = open("saveFile.txt", "r")
file2 = open("restoreFile2.txt", "r")
self.compareFiles(file1, file2)
file1.close()
file2.close()
def run_simulation_null_y_COM(dt,
t_stance,
duration,
f,
A_lat,
th0_lat,
th0_abd,
thf_abd,
z_rotation,
girdle_friction,
body_friction,
last_link_friction,
leg_friction,
K_lateral,
k0_lateral,
Kp_lat,
Kp_r_abd,
Kp_l_abd,
Kp_flex,
Kv_lat,
Kv_r_abd,
Kv_l_abd,
Kv_flex,
keep_in_check=True,
graphic_mode=False,
plot_graph_joint=False,
plot_graph_COM=False,
video_logging=False,
video_path="./video/trash/trash.mp4"):
"""
Parameters
----------
dt : float
length of simulation time-step (seconds, tipically something between 1/240 and 1/480);
values too high can cause instability in the controller due to discretization\n
t_stance : float
duration of the stance phase (50% symmetric duty cycle)\n
duration : float
total duration of the simulation\n
f : float
walking frequency, titpically should be set to 1/(2*t_stance)\n
A_lat : float
amplitude of the traveling wave used for setting the starting position\n
th0_lat : float
offset of the traveling wave used for setting the starting position\n
th0_abd : float
starting value for the leg abduction trajectory\n
thf_abd : float
ending value for the leg abduction trajectory\n
z_rotation : float
initial z-rotation of the model reference frame\n
girdle_friction : float
value of the friction (w/ the floor) of the girdle link\n
body_friction : float
value of the friction (w/ the floor) of the spinal segments\n
last_link_friction : [type]
value of the friction (w/ the floor) of the last segment of the spine\n
leg_friction : float
value of the friction (w/ the floor) of the legs link (feet's friction, substantially)\n
K_lateral : float
gain of the closed-loop inverse kinematics\n
k0_lateral : float
k0 value of the closed-loop inverse kinematics, check crawler.py for a better description\n
Kp_lat : float
spinal lateral joints' proportional gain for the computed torque control\n
Kp_r_abd : float
right abduction joint's proportional gain for the computed torque control\n
Kp_l_abd : float
left abduction joint's proportional gain for the computed torque control\n
Kp_flex : float
leg flexion joints' proportional gain for the computed torque control\n
Kv_lat : float
spinal lateral joints' derivative gain for the computed torque control\n
Kv_r_abd : float
right abduction joint's derivative gain for the computed torque control\n
Kv_l_abd : float
left abduction joint's derivative gain for the computed torque control\n
Kv_flex : float
leg flexion joints' derivative gain for the computed torque control\n
keep_in_check : bool, optional
check if the simulation have an unexpected unrealistical behaviour
(should be properly set up inside this function definition), by default True\n
graphic_mode : bool, optional
show GUI of simulation, by default False. Need to be set to True for video logging\n
plot_graph_joint : bool, optional
plot joints' parameters, by default False\n
plot_graph_COM : bool, optional
plot COM's trajectory, by default False\n
video_logging : bool, optional
save a video of the simulation, by default False\n
video_path : str, optional
filepath for the video file, by default "./video/trash/trash.mp4"\n
Returns
-------
loss: float\n
Value of the loss function of the simulation
"""
# dt, t_stance, duration = time parameter, dt MUST be the same used to create the model
# A,f1,n,t2_off,delta_off,bias = parameters for the generation of the torques
# theta_rg0, theta_lg0, A_lat_0, theta_lat_0= parameters for setting the starting position
# girdle_friction, body_friction, leg_friction = lateral friction values
##########################################
####### PYBULLET SET-UP ##################
if graphic_mode:
physicsClient = p.connect(p.GUI)
else:
physicsClient = p.connect(p.DIRECT)
p.setTimeStep(dt)
p.setAdditionalSearchPath(pybullet_data.getDataPath())
p.setGravity(0, 0, -9.81)
### plane set-up
planeID = p.loadURDF("plane100.urdf", [0, 0, 0])
p.changeDynamics(
planeID,
linkIndex=-1,
lateralFriction=1,
spinningFriction=1,
rollingFriction=1,
#restitution = 0.5,
#contactStiffness = 0,
#contactDamping = 0
)
##########################################
####### MODEL SET-UP #####################
model = crw.Crawler(
urdf_path="/home/fra/Uni/Tesi/crawler/src",
dt_simulation=dt,
base_position=[0, 0, 0.05],
base_orientation=[0, 0, sin(z_rotation / 2),
cos(z_rotation / 2)],
mass_distribution=True)
# for i,elem in enumerate(model.links_state_array):
# print("Link %d "%i, elem["world_com_trn"])
# print(model.links_state_array)
# Set motion damping ("air friction")
for index in range(-1, model.num_joints):
p.changeDynamics(model.Id,
index,
linearDamping=0.0001,
angularDamping=0.0001)
# Girdle link dynamic properties
p.changeDynamics(
model.Id,
linkIndex=-1,
lateralFriction=girdle_friction,
spinningFriction=girdle_friction / 100,
rollingFriction=girdle_friction / 100,
restitution=0.1,
#contactStiffness = 0,
#contactDamping = 0
)
# Body dynamic properties
for i in range(1, model.num_joints - 4, 2):
p.changeDynamics(
model.Id,
linkIndex=i,
lateralFriction=body_friction,
spinningFriction=body_friction / 100,
rollingFriction=body_friction / 100,
restitution=0.1,
#contactStiffness = 0,
#contactDamping = 0
)
# # Last link dynamic properties
p.changeDynamics(
model.Id,
linkIndex=(model.control_indices[0][-1] + 1),
lateralFriction=last_link_friction,
spinningFriction=last_link_friction / 100000,
rollingFriction=last_link_friction / 100000,
restitution=0.1,
#contactStiffness = 0,
#contactDamping = 0
)
# Legs dynamic properties
for i in range(model.num_joints - 3, model.num_joints, 2):
p.changeDynamics(
model.Id,
linkIndex=i,
lateralFriction=leg_friction,
spinningFriction=leg_friction / 100,
rollingFriction=leg_friction / 100,
restitution=0.1,
#contactStiffness = 0,
#contactDamping = 0
)
# Dorsal joints "compliance"
model.turn_off_crawler()
for i in range(1, model.control_indices[0][-1] + 1, 2):
p.setJointMotorControl2(model.Id,
i,
controlMode=p.VELOCITY_CONTROL,
targetVelocity=0,
force=0.05)
# Starting position
model.set_bent_position(theta_rg=th0_abd,
theta_lg=-thf_abd,
A_lat=A_lat,
theta_lat_0=th0_lat)
##########################################
####### CONTROLLER PARAMETERS ############
Kp = model.generate_Kp(Kp_lat, Kp_r_abd, Kp_l_abd, Kp_flex)
Kv = model.generate_Kv(Kv_lat, Kv_r_abd, Kv_l_abd, Kv_flex)
model.set_low_pass_lateral_qa(fc=10)
model.set_low_pass_tau(fc=90)
##########################################
####### MISCELLANEOUS ####################
if graphic_mode:
p.configureDebugVisualizer(p.COV_ENABLE_GUI, 0)
p.resetDebugVisualizerCamera(cameraDistance=0.8,
cameraYaw=50,
cameraPitch=-60,
cameraTargetPosition=[0, 0, 0])
if (video_logging and graphic_mode):
video_writer = imageio.get_writer(video_path, fps=int(1 / dt))
##########################################
####### PERFORMANCE EVALUATION ###########
loss = 0.0
max_loss = 10000000.0 # value to set when something goes wrong, if constraints are "hard"
max_COM_height = 4 * model.body_sphere_radius
max_COM_speed = 10 #TODO: check this value
n_mean = 6
##########################################
####### TIME SERIES CREATION #############
steps = int(duration / dt)
steps_stance = int(t_stance / dt)
num_lat = len(model.control_indices[0])
f_walk = 1 / (2 * t_stance)
# Data array initialization
p_COM_time_array = np.zeros((steps, 3))
v_COM_time_array = np.zeros((steps, 3))
tau_time_array = np.zeros((steps, model.state.nqd))
joint_power_time_array = np.zeros((steps, model.state.nqd))
q_time_array = np.zeros((steps, model.state.nq))
qd_time_array = np.zeros((steps, model.state.nqd))
qdd_time_array = np.zeros((steps, model.state.nqd))
eq_time_array = np.zeros((steps, model.state.nq))
q_des_time_array = np.zeros((steps, model.state.nq))
qd_des_time_array = np.zeros((steps, model.state.nqd))
qdd_des_time_array = np.zeros((steps, model.state.nqd))
# Integrator for the work done by each joint
joint_power_integrator = crw.Integrator_Forward_Euler(
dt, np.zeros(model.state.nqd))
joint_energy_time_array = np.zeros((steps, model.state.nqd))
##########################################
####### RUN SIMULATION ###################
t = 0.0
# let the legs fall and leave the time to check the starting position
for i in range(100):
p.stepSimulation()
if graphic_mode:
time.sleep(dt)
# Initial Condition
model.state.update()
model.integrator_lateral_qa.reset(model.state.q[model.mask_act_shifted])
model.free_right_foot()
model.fix_left_foot()
model.fix_tail(second_last=False)
model.set_COM_y_ref()
qda_des_prev = np.array(([0] * len(model.control_indices[0])))
# walk and record data
for i in range(steps):
# UPDATE CONSTRAINTS
if not (i % steps_stance):
model.invert_feet()
print("step")
model.turn_off_crawler()
for tmp in range(1):
p.stepSimulation()
# UPDATE STATE
model.state.update()
p_COM_curr = np.array(model.COM_position_world())
v_COM_curr = np.array(model.COM_velocity_world())
# ADD FRAME TO VIDEO
if (graphic_mode and video_logging):
img = p.getCameraImage(800,
640,
renderer=p.ER_BULLET_HARDWARE_OPENGL)[2]
video_writer.append_data(img)
# ACTUATE MODEL AND STEP SIMULATION
qa_des, qda_des, qdda_des = model.solve_null_COM_y_speed_optimization_qdda(
qda_prev=qda_des_prev, K=K_lateral, k0=k0_lateral, filtered=True)
qda_des_prev = qda_des
q_des, qd_des, qdd_des, eq = model.generate_joints_trajectory(
theta0=th0_abd,
thetaf=thf_abd,
ti=t,
t_stance=t_stance,
qa_des=qa_des,
qda_des=qda_des,
qdda_des=qdda_des,
cos_abd=True)
tau_des, eq = model.solve_computed_torque_control(
q_des=q_des,
qd_des=qd_des,
qdd_des=qdd_des,
Kp=Kp,
Kv=Kv,
#rho=rho,
verbose=False)
tau_applied = model.apply_torques(tau_des=tau_des, filtered=True)
# UPDATE TIME-ARRAYS
if plot_graph_joint or plot_graph_COM:
q_time_array[i] = model.state.q.copy()
qd_time_array[i] = model.state.qd.copy()
qdd_time_array[i] = model.state.qdd.copy()
q_des_time_array[i] = q_des.copy()
qd_des_time_array[i] = qd_des.copy()
qdd_des_time_array[i] = qdd_des.copy()
p_COM_time_array[i] = p_COM_curr
v_COM_time_array[i] = v_COM_curr
eq_time_array[i] = q_des_time_array[i] - q_time_array[i]
tau_time_array[i] = tau_applied
for j, (v_j, tau_j) in enumerate(zip(model.state.qd, tau_applied)):
joint_power_time_array[i, j] = abs(v_j * tau_j)
joint_energy = joint_power_integrator.integrate(
joint_power_time_array[i])
joint_energy_time_array[i] = joint_energy
# print("eq: ", np.round(eq[model.mask_act],2), np.round(eq[model.mask_both_legs],2))
# print("tau: ", np.round(tau_applied[model.mask_act],2), np.round(tau_applied[model.mask_both_legs],2))
# print("qdd_des: ", np.round(qdd_des_time_array[i][model.mask_act],2), np.round(qdd_des_time_array[i][model.mask_both_legs],2))
#UPDATE LOSS - VARIATIONAL VERSION -d(x-displacement) + d(total energy expenditure)
# loss += (
# -100*(p_COM_time_array[i][0]-p_COM_time_array[i-1][0]) +
# 0.1*(joint_power_time_array[i]).dot(joint_power_time_array[i])
# )
# STEP SIMULATION
p.stepSimulation()
if graphic_mode:
time.sleep(dt)
t += dt
# CHECK SIMULATION to avoid that the model get schwifty
if keep_in_check:
# check COM height, model shouldn't fly or go through the ground
if (p_COM_curr[-1] > (max_COM_height)) or (p_COM_curr[-1] < 0.0):
print("\nLook at me I'm jumping\n")
loss += max_loss
break
if (np.linalg.norm(np.mean(v_COM_time_array[i - n_mean:i + 1], 0))
> max_COM_speed):
print("\nFaster than light. WOOOOOOOSH\n")
loss += max_loss
break
p.disconnect()
####### END SIMULATION ###################
if (video_logging and graphic_mode):
video_writer.close()
##########################################
####### PLOT DATA ########################
if plot_graph_joint:
fig_lat, axs_lat = plt.subplots(2, 3)
for i in range(0, num_lat):
#axs_lat[i//3,i%3].plot(qd_time_array[:,model.mask_act][:,i], color="xkcd:dark yellow", label="qd")
#axs_lat[i//3,i%3].plot(qdd_time_array[:,model.mask_act][:,i], color="xkcd:pale yellow", label="qdd")
#axs_lat[i//3,i%3].plot(joint_power_time_array[:,model.mask_act][:,i], color="xkcd:light salmon", label="Power")
#axs_lat[i//3,i%3].plot(joint_energy_time_array[:,model.mask_act][:,i], color="xkcd:dark salmon", label="Energy")
axs_lat[i // 3, i % 3].plot(tau_time_array[:, model.mask_act][:,
i],
color="xkcd:salmon",
label="Torque")
axs_lat[i // 3,
i % 3].plot(q_time_array[:, model.mask_act_shifted][:, i],
color="xkcd:yellow",
label="q")
axs_lat[i // 3,
i % 3].plot(q_des_time_array[:, model.mask_act_shifted][:,
i],
color="xkcd:dark teal",
label="q_des")
#axs_lat[i//3,i%3].plot(eq_time_array[:,model.mask_act_shifted][:,i], color="xkcd:red", label="error")
axs_lat[i // 3, i % 3].set_title("Lateral joint %d" % i)
handles_lat, labels_lat = axs_lat[0, 0].get_legend_handles_labels()
fig_lat.legend(handles_lat, labels_lat, loc='center right')
#
fig_girdle, axs_girdle = plt.subplots(2, 2)
for i in range(0, len(model.mask_both_legs)):
#axs_girdle[i//2,i%2].plot(qd_time_array[:,model.mask_both_legs][:,i], color="xkcd:dark yellow", label="qd")
#axs_girdle[i//2,i%2].plot(qdd_time_array[:,model.mask_both_legs][:,i], color="xkcd:pale yellow", label="qdd")
#axs_girdle[i//2,i%2].plot(joint_power_time_array[:,model.mask_both_legs][:,i], color="xkcd:light salmon", label="Power")
#axs_girdle[i//2,i%2].plot(joint_energy_time_array[:,model.mask_both_legs][:,i], color="xkcd:dark salmon", label="Energy")
axs_girdle[i // 2,
i % 2].plot(tau_time_array[:, model.mask_both_legs][:,
i],
color="xkcd:salmon",
label="Torque")
axs_girdle[i // 2, i % 2].plot(
q_time_array[:, model.mask_both_legs_shifted][:, i],
color="xkcd:yellow",
label="q")
axs_girdle[i // 2, i % 2].plot(
q_des_time_array[:, model.mask_both_legs_shifted][:, i],
color="xkcd:dark teal",
label="q_des")
axs_girdle[i // 2, i % 2].plot(
eq_time_array[:, model.mask_both_legs_shifted][:, i],
color="xkcd:red",
label="error")
axs_girdle[0, 0].set_title("Right abduction")
axs_girdle[0, 1].set_title("Right flexion")
axs_girdle[1, 0].set_title("Left abduction")
axs_girdle[1, 1].set_title("Left flexion")
handles_girdle, labels_girdle = axs_girdle[
0, 0].get_legend_handles_labels()
fig_girdle.legend(handles_girdle, labels_girdle, loc='center right')
#
plt.show()
#
if plot_graph_COM:
fig_COM = plt.figure()
axs_COM = fig_COM.add_subplot(111)
axs_COM.plot(p_COM_time_array[:, 0],
p_COM_time_array[:, 1],
color="xkcd:teal",
linewidth=4)
axs_COM.set_title("COM x-y trajectory")
axs_COM.set_xlim(-0.22, 0.01)
axs_COM.set_ylim(-0.16, 0.01)
axs_COM.set_aspect('equal')
# fig_COM_3D = plt.figure()
# axs_COM_3D = fig_COM_3D.add_subplot(111, projection='3d')
# axs_COM_3D.plot(p_COM_time_array[:,0], p_COM_time_array[:,1], p_COM_time_array[:,2],color="xkcd:teal")
# axs_COM_3D.set_title("COM 3D trajectory")
#
plt.show()
return loss
def update_physics(delay, quadcopterId, quadcopter_controller, config,
graph: datalogger):
while quadcopter_controller.sim:
#start = time.perf_counter()
#start = time.clock()
# the controllers are evaluated at a slower rate, only once in the
# control_subsample times the controller is evaluated
# if quadcopter_controller.sample == 0:
# quadcopter_controller.sample = control_subsample
# pos_meas,quaternion_meas = p.getBasePositionAndOrientation(quadcopterId)
# force_act1,force_act2,force_act3,force_act4,moment_yaw = quadcopter_controller.update_control(pos_meas,quaternion_meas)
# else:
# quadcopter_controller.sample -= 1
pos_meas, quaternion_meas = p.getBasePositionAndOrientation(
quadcopterId)
rotmat = quaternion2rotation_matrix(quaternion_meas)
front = np.array([pos_meas[0] + 0.1750, pos_meas[1], pos_meas[2]])
back = np.array([pos_meas[0] - 0.1750, pos_meas[1], pos_meas[2]])
left = np.array([pos_meas[0], pos_meas[1] + 0.1750, pos_meas[2]])
right = np.array([pos_meas[0], pos_meas[1] - 0.1750, pos_meas[2]])
front_meas = np.dot(rotmat.T, front.reshape(3, 1)).reshape(3)
back_meas = np.dot(rotmat.T, back.reshape(3, 1)).reshape(3)
left_meas = np.dot(rotmat.T, left.reshape(3, 1)).reshape(3)
right_meas = np.dot(rotmat.T, right.reshape(3, 1)).reshape(3)
force_act1, force_act2, force_act3, force_act4, moment_yaw = quadcopter_controller.update_control(
pos_meas, quaternion_meas, config)
radius = 0.25
ige_force1 = ige_force2 = ige_force3 = ige_force4 = 1.0
if (front_meas[2] < 5 * radius):
ige_force1 = groundEffect.groundEffect(front_meas[2], radius)
if (back_meas[2] < 5 * radius):
ige_force3 = groundEffect.groundEffect(back_meas[2], radius)
if (left_meas[2] < 5 * radius):
ige_force2 = groundEffect.groundEffect(left_meas[2], radius)
if (back_meas[2] < 5 * radius):
ige_force4 = groundEffect.groundEffect(right_meas[2], radius)
if graph.capture:
graph.addpoint(pos_meas[2], force_act1[2])
force_act1 = force_act1 / ige_force1
force_act2 = force_act2 / ige_force2
force_act3 = force_act3 / ige_force3
force_act4 = force_act4 / ige_force4
#capturing forces with altitude to plot
# apply forces/moments from controls etc:
# (do this each time, because forces and moments are reset to zero after a stepSimulation())
p.applyExternalForce(quadcopterId, -1, force_act1,
[config.arm_length, 0., 0.], p.LINK_FRAME)
p.applyExternalForce(quadcopterId, -1, force_act2,
[0., config.arm_length, 0.], p.LINK_FRAME)
p.applyExternalForce(quadcopterId, -1, force_act3,
[-config.arm_length, 0., 0.], p.LINK_FRAME)
p.applyExternalForce(quadcopterId, -1, force_act4,
[0., -config.arm_length, 0.], p.LINK_FRAME)
# for the yaw-control:
p.applyExternalTorque(quadcopterId, -1, [0., 0., moment_yaw],
p.LINK_FRAME)
# do simulation with pybullet:
p.stepSimulation()
def run():
physicsClient = p.connect(
p.GUI) # p.GUI for graphical, or p.DIRECT for non-graphical version
p.setAdditionalSearchPath(
pybullet_data.getDataPath()) # defines the path used by p.loadURDF
p.setGravity(0, 0, -10)
p.setPhysicsEngineParameter(enableConeFriction=1)
p.setRealTimeSimulation(
0
) # only if this is set to 0 and with explicit steps will the torque control work correctly
# load the ground plane
planeId = p.loadURDF("plane.urdf")
# add a box for blocked force
boxStartPos = [0, -1, 1.4]
boxStartOr = p.getQuaternionFromEuler([0, 0, 0])
boxId, boxConstraintId = load_constrained_urdf(
"urdfs/smallSquareBox.urdf",
boxStartPos,
boxStartOr,
physicsClient=physicsClient,
)
p.changeDynamics(boxId, -1, lateralFriction=2)
# load finger
finger = SMContinuumManipulator(manipulator_definition)
finger_startPos = [0, 1, 2.05]
finger_StartOr = p.getQuaternionFromEuler([-np.pi / 2, 0, np.pi])
finger.load_to_pybullet(
baseStartPos=finger_startPos,
baseStartOrn=finger_StartOr,
baseConstraint="static",
physicsClient=physicsClient,
)
p.changeDynamics(finger.bodyUniqueId, -1, lateralFriction=2)
p.changeDynamics(finger.bodyUniqueId, -1, restitution=1)
time_step = 0.001
p.setTimeStep(time_step)
n_steps = 20000
sim_time = 0.0
lam = 1.0
omega = 1.0
torque_fns = [
lambda t: 20 * np.sin(omega * t),
lambda t: 20 * np.sin(omega * t - 1 * np.pi),
] # - 0pi goes right, - pi goes left
real_time = time.time()
normal_forces = np.zeros((n_steps, ))
normal_forces_lastLink = np.zeros((n_steps, ))
time_plot = np.zeros((n_steps, ))
last_link = p.getNumJoints(
finger.bodyUniqueId) - 1 # last link of the finger
# wait for screen recording for demo
wait_for_rec = False
if wait_for_rec:
time.sleep(5)
print("6 more seconds")
time.sleep(6)
for i in range(n_steps):
print(torque_fns[0](sim_time))
finger.apply_actuationTorque(actuator_nr=0,
axis_nr=0,
actuation_torques=torque_fns[0](sim_time))
finger.apply_actuationTorque(actuator_nr=0,
axis_nr=1,
actuation_torques=0)
p.stepSimulation()
sim_time += time_step
# time it took to simulate
delta = time.time() - real_time
real_time = time.time()
# print(delta)
contactPoints = p.getContactPoints(boxId,
finger.bodyUniqueId,
physicsClientId=physicsClient)
contactPointsTip = p.getContactPoints(
bodyA=boxId,
bodyB=finger.bodyUniqueId,
linkIndexB=last_link,
physicsClientId=physicsClient,
)
# print(" ---------- normal force ---------")
time_plot[i] = sim_time
total_normal_force = 0
for contactPoint in contactPoints:
normal_force = contactPoint[9]
total_normal_force += normal_force
total_normal_force_lastLink = 0
print(len(contactPoints), len(contactPointsTip))
for contactPointTip in contactPointsTip:
normal_forceTip = contactPointTip[9]
print(normal_forceTip)
total_normal_force_lastLink += normal_forceTip
normal_forces[i] = total_normal_force
normal_forces_lastLink[i] = total_normal_force_lastLink
p.disconnect()
plt.plot(time_plot, normal_forces)
plt.xlabel("time")
plt.ylabel("total normal force")
plt.show()
plt.plot(time_plot, normal_forces_lastLink)
plt.xlabel("time")
plt.ylabel("total normal force actuator tip")
plt.show()
pitch = -10.0
roll=0
upAxisIndex = 2
camDistance = 4
pixelWidth = 320
pixelHeight = 200
nearPlane = 0.01
farPlane = 100
fov = 60
main_start = time.time()
while(1):
for yaw in range (0,360,10) :
pybullet.stepSimulation()
start = time.time()
viewMatrix = pybullet.computeViewMatrixFromYawPitchRoll(camTargetPos, camDistance, yaw, pitch, roll, upAxisIndex)
aspect = pixelWidth / pixelHeight;
projectionMatrix = pybullet.computeProjectionMatrixFOV(fov, aspect, nearPlane, farPlane);
img_arr = pybullet.getCameraImage(pixelWidth, pixelHeight, viewMatrix,projectionMatrix, shadow=1,lightDirection=[1,1,1],renderer=pybullet.ER_BULLET_HARDWARE_OPENGL)
stop = time.time()
print ("renderImage %f" % (stop - start))
w=img_arr[0] #width of the image, in pixels
h=img_arr[1] #height of the image, in pixels
rgb=img_arr[2] #color data RGB
dep=img_arr[3] #depth data
#print(rgb)
print ('width = %d height = %d' % (w,h))
def step(self):
p.stepSimulation()
for o in self.binds:
o.update()
f = [aabbMax[0], aabbMax[1], aabbMax[2]]
t = [aabbMax[0], aabbMin[1], aabbMax[2]]
p.addUserDebugLine(f, t, [1, 1, 1])
f = [aabbMax[0], aabbMax[1], aabbMax[2]]
t = [aabbMax[0], aabbMax[1], aabbMin[2]]
p.addUserDebugLine(f, t, [1, 1, 1])
aabb = p.getAABB(r2d2)
aabbMin = aabb[0]
aabbMax = aabb[1]
if (printtext):
print(aabbMin)
print(aabbMax)
if (draw == 1):
drawAABB(aabb)
for i in range(p.getNumJoints(r2d2)):
aabb = p.getAABB(r2d2, i)
aabbMin = aabb[0]
aabbMax = aabb[1]
if (printtext):
print(aabbMin)
print(aabbMax)
if (draw == 1):
drawAABB(aabb)
while (1):
a = 0
p.stepSimulation()
#### Apply z-axis torque to the base ###############################################################
# p.applyExternalTorque(DRONE_IDS[0], linkIndex=-1, torqueObj=[0.,0.,5e-6], flags=p.WORLD_FRAME, physicsClientId=PYB_CLIENT)
# p.applyExternalTorque(DRONE_IDS[0], linkIndex=-1, torqueObj=[0.,0.,5e-6], flags=p.LINK_FRAME, physicsClientId=PYB_CLIENT)
#### To propeller 0 ################################################################################
# p.applyExternalTorque(DRONE_IDS[0], linkIndex=0, torqueObj=[0.,0.,5e-6], flags=p.WORLD_FRAME, physicsClientId=PYB_CLIENT)
# p.applyExternalTorque(DRONE_IDS[0], linkIndex=0, torqueObj=[0.,0.,5e-6], flags=p.LINK_FRAME, physicsClientId=PYB_CLIENT)
#### To the center of mass #########################################################################
# p.applyExternalTorque(DRONE_IDS[0], linkIndex=4, torqueObj=[0.,0.,5e-6], flags=p.WORLD_FRAME, physicsClientId=PYB_CLIENT)
p.applyExternalTorque(DRONE_IDS[0],
linkIndex=4,
torqueObj=[0., 0., 5e-6],
flags=p.LINK_FRAME,
physicsClientId=PYB_CLIENT)
#### Draw base frame ###############################################################################
env._showDroneFrame(0)
#### Step, sync the simulation, printout the state #################################################
p.stepSimulation(physicsClientId=PYB_CLIENT)
sync(i, START, env.TIMESTEP)
if i % env.SIM_FREQ == 0: env.render()
#### Reset #########################################################################################
if i > 1 and i % ((DURATION_SEC /
(NUM_RESETS + 1)) * env.SIM_FREQ) == 0:
env.reset()
p.setGravity(0, 0, 0, physicsClientId=PYB_CLIENT)
#### Close the environment #########################################################################
env.close()
