def reset(self):
objects = p.loadSDF(os.path.join(self.urdfRootPath,"kuka_iiwa/kuka_with_gripper2.sdf"))
self.kukaUid = objects[0]
#for i in range (p.getNumJoints(self.kukaUid)):
# print(p.getJointInfo(self.kukaUid,i))
p.resetBasePositionAndOrientation(self.kukaUid,[-0.100000,0.000000,0.070000],[0.000000,0.000000,0.000000,1.000000])
self.jointPositions=[ 0.006418, 0.413184, -0.011401, -1.589317, 0.005379, 1.137684, -0.006539, 0.000048, -0.299912, 0.000000, -0.000043, 0.299960, 0.000000, -0.000200 ]
self.numJoints = p.getNumJoints(self.kukaUid)
for jointIndex in range (self.numJoints):
p.resetJointState(self.kukaUid,jointIndex,self.jointPositions[jointIndex])
p.setJointMotorControl2(self.kukaUid,jointIndex,p.POSITION_CONTROL,targetPosition=self.jointPositions[jointIndex],force=self.maxForce)
self.trayUid = p.loadURDF(os.path.join(self.urdfRootPath,"tray/tray.urdf"), 0.640000,0.075000,-0.190000,0.000000,0.000000,1.000000,0.000000)
self.endEffectorPos = [0.537,0.0,0.5]
self.endEffectorAngle = 0
self.motorNames = []
self.motorIndices = []
for i in range (self.numJoints):
jointInfo = p.getJointInfo(self.kukaUid,i)
qIndex = jointInfo[3]
if qIndex > -1:
#print("motorname")
#print(jointInfo[1])
self.motorNames.append(str(jointInfo[1]))
self.motorIndices.append(i)
def Step(stepIndex): for objectId in range(objectNum): record = log[stepIndex*objectNum+objectId] Id = record[2] pos = [record[3],record[4],record[5]] orn = [record[6],record[7],record[8],record[9]] p.resetBasePositionAndOrientation(Id,pos,orn) numJoints = p.getNumJoints(Id) for i in range (numJoints): jointInfo = p.getJointInfo(Id,i) qIndex = jointInfo[3] if qIndex > -1: p.resetJointState(Id,i,record[qIndex-7+17])
def accurateCalculateInverseKinematics( kukaId, endEffectorId, targetPos, threshold, maxIter):
closeEnough = False
iter = 0
dist2 = 1e30
while (not closeEnough and iter<maxIter):
jointPoses = p.calculateInverseKinematics(kukaId,kukaEndEffectorIndex,targetPos)
for i in range (numJoints):
p.resetJointState(kukaId,i,jointPoses[i])
ls = p.getLinkState(kukaId,kukaEndEffectorIndex)
newPos = ls[4]
diff = [targetPos[0]-newPos[0],targetPos[1]-newPos[1],targetPos[2]-newPos[2]]
dist2 = (diff[0]*diff[0] + diff[1]*diff[1] + diff[2]*diff[2])
closeEnough = (dist2 < threshold)
iter=iter+1
#print ("Num iter: "+str(iter) + "threshold: "+str(dist2))
return jointPoses
def _reset(self):
# print("-----------reset simulation---------------")
p.resetSimulation()
self.cartpole = p.loadURDF(os.path.join(pybullet_data.getDataPath(),"cartpole.urdf"),[0,0,0])
self.timeStep = 0.01
p.setJointMotorControl2(self.cartpole, 1, p.VELOCITY_CONTROL, force=0)
p.setGravity(0,0, -10)
p.setTimeStep(self.timeStep)
p.setRealTimeSimulation(0)
initialCartPos = self.np_random.uniform(low=-0.5, high=0.5, size=(1,))
initialAngle = self.np_random.uniform(low=-0.5, high=0.5, size=(1,))
p.resetJointState(self.cartpole, 1, initialAngle)
p.resetJointState(self.cartpole, 0, initialCartPos)
self.state = p.getJointState(self.cartpole, 1)[0:2] + p.getJointState(self.cartpole, 0)[0:2]
return np.array(self.state)
def _reset(self):
# print("-----------reset simulation---------------")
p.resetSimulation()
self.cartpole = p.loadURDF(os.path.join(pybullet_data.getDataPath(),"cartpole.urdf"),[0,0,0])
p.changeDynamics(self.cartpole, -1, linearDamping=0, angularDamping=0)
p.changeDynamics(self.cartpole, 0, linearDamping=0, angularDamping=0)
p.changeDynamics(self.cartpole, 1, linearDamping=0, angularDamping=0)
self.timeStep = 0.02
p.setJointMotorControl2(self.cartpole, 1, p.VELOCITY_CONTROL, force=0)
p.setJointMotorControl2(self.cartpole, 0, p.VELOCITY_CONTROL, force=0)
p.setGravity(0,0, -9.8)
p.setTimeStep(self.timeStep)
p.setRealTimeSimulation(0)
randstate = self.np_random.uniform(low=-0.05, high=0.05, size=(4,))
p.resetJointState(self.cartpole, 1, randstate[0], randstate[1])
p.resetJointState(self.cartpole, 0, randstate[2], randstate[3])
#print("randstate=",randstate)
self.state = p.getJointState(self.cartpole, 1)[0:2] + p.getJointState(self.cartpole, 0)[0:2]
#print("self.state=", self.state)
return np.array(self.state)
def initialize(self):
''''''
ur5_urdf_filepath = os.path.join(self.root_dir, 'simulators/urdf/kuka/kuka_with_gripper2.sdf')
self.id = pb.loadSDF(ur5_urdf_filepath)[0]
pb.resetBasePositionAndOrientation(self.id, [-0.2,0,0], [0,0,0,1])
# self.is_holding = False
self.gripper_closed = False
self.holding_obj = None
self.num_joints = pb.getNumJoints(self.id)
[pb.resetJointState(self.id, idx, self.home_positions[idx]) for idx in range(self.num_joints)]
self.openGripper()
self.arm_joint_names = list()
self.arm_joint_indices = list()
for i in range (self.num_joints):
joint_info = pb.getJointInfo(self.id, i)
if i in range(7):
self.arm_joint_names.append(str(joint_info[1]))
self.arm_joint_indices.append(i)
def initialize(self):
''''''
ur5_urdf_filepath = os.path.join(
self.root_dir,
'simulators/urdf/ur5/ur5_robotiq_85_gripper_fake.urdf')
self.id = pb.loadURDF(ur5_urdf_filepath, [0, 0, 0.1], [0, 0, 0, 1])
# self.is_holding = False
self.gripper_closed = False
self.holding_obj = None
self.num_joints = pb.getNumJoints(self.id)
[
pb.resetJointState(self.id, idx, self.home_positions[idx])
for idx in range(self.num_joints)
]
self.arm_joint_names = list()
self.arm_joint_indices = list()
self.gripper_joint_names = list()
self.gripper_joint_indices = list()
for i in range(self.num_joints):
joint_info = pb.getJointInfo(self.id, i)
if i in range(1, 7):
self.arm_joint_names.append(str(joint_info[1]))
self.arm_joint_indices.append(i)
elif i in range(10, 12):
self.gripper_joint_names.append(str(joint_info[1]))
self.gripper_joint_indices.append(i)
elif i in range(14, self.num_joints):
info = pb.getJointInfo(self.id, i)
jointID = info[0]
jointName = info[1].decode("utf-8")
jointType = jointTypeList[info[2]]
jointLowerLimit = info[8]
jointUpperLimit = info[9]
jointMaxForce = info[10]
jointMaxVelocity = info[11]
singleInfo = jointInfo(jointID, jointName, jointType,
jointLowerLimit, jointUpperLimit,
jointMaxForce, jointMaxVelocity)
self.robotiq_joints[singleInfo.name] = singleInfo
def resetPose(self):
# move z prismatic back to 0
p.setJointMotorControl2(self.gripperUid,
self.baseIndex,
p.POSITION_CONTROL,
targetPosition=-0.05,
force=500.0)
p.resetJointState(self.gripperUid, self.wristXIndex, targetValue=0)
p.resetJointState(self.gripperUid, self.wristYIndex, targetValue=0)
p.resetJointState(self.gripperUid, self.wristWIndex, targetValue=0)
# reset fingers to home position
self.setJointControl(qDes=0.04, maxForce=self.gripperMaxForce)
def reset(self):
if not isinstance(self.observation_space, gym.spaces.Dict):
raise error.Error(
'GoalEnv requires an observation space of type gym.spaces.Dict'
)
for key in ['observation', 'achieved_goal', 'desired_goal']:
if key not in self.observation_space.spaces:
raise error.Error(
'GoalEnv requires the "{}" key to be part of the observation dictionary.'
.format(key))
if self.config["randomize_env"]:
self.robot = self.load_robot()
self.step_ctr = 0
joint_init_pos_list = self.norm_to_rads([0] * 12)
[
p.resetJointState(self.robot,
i,
joint_init_pos_list[i],
0,
physicsClientId=self.client_ID)
for i in range(12)
]
p.resetBasePositionAndOrientation(self.robot, [0, 0, .15],
[0, 0, 0, 1],
physicsClientId=self.client_ID)
p.setJointMotorControlArray(bodyUniqueId=self.robot,
jointIndices=range(12),
controlMode=p.POSITION_CONTROL,
targetPositions=[0] * 12,
forces=[1.4] * 12,
physicsClientId=self.client_ID)
for i in range(10):
p.stepSimulation(physicsClientId=self.client_ID)
obs, _, _, _ = self.step(np.zeros(self.act_dim))
return obs
def reset(self):
self.episode_reward = 0
self.step_counter = 0
p.resetSimulation()
p.configureDebugVisualizer(
p.COV_ENABLE_RENDERING,
0) # we will enable rendering after we loaded everything
urdfRootPath = pybullet_data.getDataPath()
p.setGravity(0, 0, -40)
planeUid = p.loadURDF(os.path.join(urdfRootPath, "plane.urdf"),
basePosition=[0, 0, -0.65])
rest_poses = [0, -0.215, 0, -2.57, 0, 2.356, 2.356, 0.08, 0.08]
self.pandaUid = p.loadURDF(os.path.join(urdfRootPath,
"franka_panda/panda.urdf"),
useFixedBase=True)
for i in range(7):
p.resetJointState(self.pandaUid, i, rest_poses[i])
p.resetJointState(self.pandaUid, 9, 0.08)
p.resetJointState(self.pandaUid, 10, 0.08)
tableUid = p.loadURDF(os.path.join(urdfRootPath, "table/table.urdf"),
basePosition=[0.5, 0, -0.65])
trayUid = p.loadURDF(os.path.join(urdfRootPath, "tray/traybox.urdf"),
basePosition=[0.65, 0, 0])
# state_object= [random.uniform(0.5,0.8),random.uniform(-0.2,0.2),0.05]
state_object = [
0.75 + random.uniform(-0.05, 0.05),
0 + random.uniform(-0.05, 0.05),
0.08,
]
self.object_loc = state_object
self.objectUid = p.loadURDF(
os.path.join(urdfRootPath, "random_urdfs/000/000.urdf"),
basePosition=state_object,
)
state_robot = p.getLinkState(self.pandaUid, 11)[0]
state_fingers = (
p.getJointState(self.pandaUid, 9)[0],
p.getJointState(self.pandaUid, 10)[0],
)
self.observation = state_robot + state_fingers + tuple(state_object)
p.configureDebugVisualizer(p.COV_ENABLE_RENDERING, 0)
return np.array(self.observation).astype(np.float32)
def resetRightBackLeg():
#right back leg
p.resetJointState(self.quadruped,
self.jointNameToId['motor_back_rightL_joint'],
self.motorDir[6] * halfpi)
p.resetJointState(self.quadruped,
self.jointNameToId['knee_back_rightL_link'],
self.motorDir[6] * kneeangle)
p.resetJointState(self.quadruped,
self.jointNameToId['motor_back_rightR_joint'],
self.motorDir[7] * halfpi)
p.resetJointState(self.quadruped,
self.jointNameToId['knee_back_rightR_link'],
self.motorDir[7] * kneeangle)
p.createConstraint(self.quadruped,
self.jointNameToId['knee_back_rightR_link'],
self.quadruped,
self.jointNameToId['knee_back_rightL_link'],
p.JOINT_POINT2POINT, [0, 0, 0], [0, 0.005, 0.2],
[0, 0.01, 0.2])
self.setMotorAngleByName('motor_back_rightL_joint',
self.motorDir[6] * halfpi)
self.setMotorAngleByName('motor_back_rightR_joint',
self.motorDir[7] * halfpi)
p.setJointMotorControl2(
bodyIndex=self.quadruped,
jointIndex=self.jointNameToId['knee_back_rightL_link'],
controlMode=p.VELOCITY_CONTROL,
targetVelocity=0,
force=kneeFrictionForce)
p.setJointMotorControl2(
bodyIndex=self.quadruped,
jointIndex=self.jointNameToId['knee_back_rightR_link'],
controlMode=p.VELOCITY_CONTROL,
targetVelocity=0,
force=kneeFrictionForce)
def setup_human_joints(self, human, joints_positions, controllable_joints, use_static_joints=True, human_reactive_force=None, human_reactive_gain=0.05, non_static_joints=[], add_joint_positions=False):
if self.human_impairment != 'tremor':
self.human_tremors = np.zeros(len(controllable_joints))
elif len(controllable_joints) == 4:
self.human_tremors = self.np_random.uniform(np.deg2rad(-20), np.deg2rad(20), size=len(controllable_joints))
else:
self.human_tremors = self.np_random.uniform(np.deg2rad(-10), np.deg2rad(10), size=len(controllable_joints))
# Set starting joint positions
human_joint_states = p.getJointStates(human, jointIndices=list(range(p.getNumJoints(human, physicsClientId=self.id))), physicsClientId=self.id)
human_joint_positions = np.array([x[0] for x in human_joint_states])
for j in range(p.getNumJoints(human, physicsClientId=self.id)):
set_position = None
for j_index, j_angle in joints_positions:
if j == j_index:
if self.vr or self.replay or self.new or self.task == 'bed_bathing':
set_position = j_angle if not add_joint_positions else (human_joint_positions[j] + j_angle)
p.resetJointState(human, jointIndex=j, targetValue=set_position, targetVelocity=0, physicsClientId=self.id)
break
else:
p.resetJointState(human, jointIndex=j, targetValue=j_angle, targetVelocity=0, physicsClientId=self.id)
set_position = j_angle
break
if use_static_joints and j not in controllable_joints and j not in non_static_joints:
# Make all non controllable joints on the person static by setting mass of each link (joint) to 0
p.changeDynamics(human, j, mass=0, physicsClientId=self.id)
# Set velocities to 0
p.resetJointState(human, jointIndex=j, targetValue=human_joint_positions[j] if set_position is None else set_position, targetVelocity=0, physicsClientId=self.id)
# By default, all joints have motors enabled by default that prevent free motion. Disable these motors in human
if not self.vr:
if self.replay or self.task == 'bed_bathing':
for j in range(p.getNumJoints(human, physicsClientId=self.id)):
p.setJointMotorControl2(human, jointIndex=j, controlMode=p.VELOCITY_CONTROL, force=0.1, physicsClientId=self.id)
elif not self.new:
for j in range(p.getNumJoints(human, physicsClientId=self.id)):
p.setJointMotorControl2(human, jointIndex=j, controlMode=p.VELOCITY_CONTROL, force=0, physicsClientId=self.id)
self.enforce_joint_limits(human)
if human_reactive_force is not None:
# NOTE: This runs a Position / Velocity PD controller for each joint motor on the human
human_joint_states = p.getJointStates(human, jointIndices=controllable_joints, physicsClientId=self.id)
target_human_joint_positions = np.array([x[0] for x in human_joint_states])
forces = [human_reactive_force * self.human_strength] * len(target_human_joint_positions)
p.setJointMotorControlArray(human, jointIndices=controllable_joints, controlMode=p.POSITION_CONTROL, targetPositions=target_human_joint_positions, positionGains=np.array([human_reactive_gain]*len(forces)), forces=forces, physicsClientId=self.id)
def reset(self):
self.completed = False
self.step_counter = 0
p.resetSimulation()
p.configureDebugVisualizer(
p.COV_ENABLE_RENDERING,
0) # we will enable rendering after we loaded everything
urdfRootPath = pybullet_data.getDataPath()
p.setGravity(0, 0, -10)
planeUid = p.loadURDF(os.path.join(urdfRootPath, "plane.urdf"),
basePosition=[0, 0, -0.65])
self.pandaUid = p.loadURDF(os.path.join(urdfRootPath,
"franka_panda/panda.urdf"),
useFixedBase=True)
rest_poses = [0, -0.215, 0, -2.57, 0, 2.356, 2.356, 0.08, 0.08]
for i in range(7):
p.resetJointState(self.pandaUid, i, rest_poses[i])
p.resetJointState(self.pandaUid, 9, 0.08)
p.resetJointState(self.pandaUid, 10, 0.08)
tableUid = p.loadURDF(os.path.join(urdfRootPath, "table/table.urdf"),
basePosition=[0.5, 0, -0.65])
trayUid = p.loadURDF(os.path.join(urdfRootPath, "tray/traybox.urdf"),
basePosition=[0.65, 0, 0])
posObj = [
np.random.uniform(0.5, 0.7),
np.random.uniform(-0.15, 0.15), 0.05
]
orientationObj = p.getQuaternionFromEuler(
[0, 0, np.random.uniform(-np.pi / 5, np.pi / 5)])
self.objectUid = p.loadURDF(os.path.join(urdfRootPath,
"random_urdfs/000/000.urdf"),
basePosition=posObj,
baseOrientation=orientationObj)
# state_object = np.array(state_object) + np.random.uniform(0.05, 0.1, 3) * np.random.choice([-1, 1])
# secondObject = p.loadURDF(os.path.join(urdfRootPath, "random_urdfs/002/002.urdf"), basePosition=state_object)
self.observation, _ = self.get_obs()
p.configureDebugVisualizer(p.COV_ENABLE_RENDERING, 1)
return self.observation
def initialize(self):
ur5_urdf_filepath = os.path.join(
self.root_dir, 'simulators/urdf/ur5/ur5_simple_gripper.urdf')
self.id = pb.loadURDF(ur5_urdf_filepath, [0, 0, 0], [0, 0, 0, 1])
self.gripper_closed = False
self.holding_obj = None
self.num_joints = pb.getNumJoints(self.id)
[
pb.resetJointState(self.id, idx, self.home_positions[idx])
for idx in range(self.num_joints)
]
self.arm_joint_names = list()
self.arm_joint_indices = list()
self.gripper_joint_names = list()
self.gripper_joint_indices = list()
for i in range(self.num_joints):
joint_info = pb.getJointInfo(self.id, i)
if i in range(1, 7):
self.arm_joint_names.append(str(joint_info[1]))
self.arm_joint_indices.append(i)
elif i in range(10, 12):
self.gripper_joint_names.append(str(joint_info[1]))
self.gripper_joint_indices.append(i)
def reset_robot_joints(self, robot):
# Reset all robot joints
for rj in range(p.getNumJoints(robot, physicsClientId=self.id)):
p.resetJointState(robot,
jointIndex=rj,
targetValue=0,
targetVelocity=0,
physicsClientId=self.id)
# Position end effectors whith dual arm robots
for i, j in enumerate(self.robot_left_arm_joint_indices):
p.resetJointState(robot,
jointIndex=j,
targetValue=[1.75, 1.25, 1.5, -0.5, 1, 0, 1][i],
targetVelocity=0,
physicsClientId=self.id)
for i, j in enumerate(self.robot_right_arm_joint_indices):
p.resetJointState(robot,
jointIndex=j,
targetValue=[-1.75, 1.25, -1.5, -0.5, -1, 0,
-1][i],
targetVelocity=0,
physicsClientId=self.id)
def reset(self):
self.step_ctr = 0
self.theta_prev = 1
self.target = np.random.rand() * 2 * self.target_var - self.target_var
p.resetBasePositionAndOrientation(self.target_vis, [self.target, 0, -1], [0, 0, 0, 1])
self.mass_1, self.mass_2 = self.mass_min + np.random.rand(2) * self.mass_range
p.resetJointState(self.cartpole, 0, targetValue=0, targetVelocity=0)
p.resetJointState(self.cartpole, 1, targetValue=0, targetVelocity=0)
p.resetJointState(self.cartpole, 2, targetValue=0, targetVelocity=0)
p.changeDynamics(self.cartpole, 1, mass=self.mass_1)
p.changeDynamics(self.cartpole, 2, mass=self.mass_2)
p.changeVisualShape(self.cartpole, 1, rgbaColor=[self.mass_1 / (self.mass_min + self.mass_range),
1 - self.mass_1 / (self.mass_min + self.mass_range), 0, 1])
p.changeVisualShape(self.cartpole, 2, rgbaColor=[self.mass_2 / (self.mass_min + self.mass_range),
1 - self.mass_2 / (self.mass_min + self.mass_range), 0, 1])
p.setJointMotorControl2(self.cartpole, 0, p.VELOCITY_CONTROL, force=0)
p.setJointMotorControl2(self.cartpole, 1, p.VELOCITY_CONTROL, force=0)
p.setJointMotorControl2(self.cartpole, 2, p.VELOCITY_CONTROL, force=0)
obs, _, _, _ = self.step(np.zeros(self.act_dim))
return obs
def initializeMotionTarget(self):
'''
Convert the motion file into a format that matches the observations collected from the agent in order to compare and compute rewards.
'''
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
]
processedTargetMotion = []
targetFrames = self.targetMotion['Frames']
for frameIndex in range(len(targetFrames)-1):
# Set joint positions
for joint in self.targetDummy.revoluteJoints:
p.resetJointState(
self.targetDummyID,
jointIndex=joint,
targetValue=targetFrames[frameIndex][JointFrameMapIndices[joint]]
)
for joint in self.targetDummy.sphericalJoints:
p.resetJointStateMultiDof(
self.targetDummyID,
jointIndex=joint,
targetValue=[targetFrames[frameIndex][i] for i in JointFrameMapIndices[3]]
)
currentFrame = self.targetDummy.collectObservations()
deltaTime = targetFrames[frameIndex][0]
if frameIndex < len(targetFrames)-1:
# Set joint positions
for joint in self.targetDummy.revoluteJoints:
p.resetJointState(
self.targetDummyID,
jointIndex=joint,
targetValue=targetFrames[frameIndex+1][JointFrameMapIndices[joint]]
)
for joint in self.targetDummy.sphericalJoints:
p.resetJointStateMultiDof(
self.targetDummyID,
jointIndex=joint,
targetValue=[targetFrames[frameIndex+1][i] for i in JointFrameMapIndices[3]]
)
nextFrame = self.targetDummy.collectObservations()
processedTargetMotion.append(self.processVelocities(currentFrame, nextFrame, deltaTime))
return processedTargetMotion
def set_occluder_motion(self, link_id, time):
"""Set the rotation of the occluder to a specific rotation"""
p.resetJointState(self.ground_id, link_id,
self.joint_patterns[link_id][time])
def resetPose(self):
kneeFrictionForce = 0
halfpi = 1.57079632679
kneeangle = -2.1834 #halfpi - acos(upper_leg_length / lower_leg_length)
#left front leg
p.resetJointState(self.quadruped,self.jointNameToId['motor_front_leftL_joint'],self.motorDir[0]*halfpi)
p.resetJointState(self.quadruped,self.jointNameToId['knee_front_leftL_link'],self.motorDir[0]*kneeangle)
p.resetJointState(self.quadruped,self.jointNameToId['motor_front_leftR_joint'],self.motorDir[1]*halfpi)
p.resetJointState(self.quadruped,self.jointNameToId['knee_front_leftR_link'],self.motorDir[1]*kneeangle)
p.createConstraint(self.quadruped,self.jointNameToId['knee_front_leftR_link'],self.quadruped,self.jointNameToId['knee_front_leftL_link'],p.JOINT_POINT2POINT,[0,0,0],[0,0.005,0.2],[0,0.01,0.2])
self.setMotorAngleByName('motor_front_leftL_joint', self.motorDir[0]*halfpi)
self.setMotorAngleByName('motor_front_leftR_joint', self.motorDir[1]*halfpi)
p.setJointMotorControl2(bodyIndex=self.quadruped,jointIndex=self.jointNameToId['knee_front_leftL_link'],controlMode=p.VELOCITY_CONTROL,targetVelocity=0,force=kneeFrictionForce)
p.setJointMotorControl2(bodyIndex=self.quadruped,jointIndex=self.jointNameToId['knee_front_leftR_link'],controlMode=p.VELOCITY_CONTROL,targetVelocity=0,force=kneeFrictionForce)
#left back leg
p.resetJointState(self.quadruped,self.jointNameToId['motor_back_leftL_joint'],self.motorDir[2]*halfpi)
p.resetJointState(self.quadruped,self.jointNameToId['knee_back_leftL_link'],self.motorDir[2]*kneeangle)
p.resetJointState(self.quadruped,self.jointNameToId['motor_back_leftR_joint'],self.motorDir[3]*halfpi)
p.resetJointState(self.quadruped,self.jointNameToId['knee_back_leftR_link'],self.motorDir[3]*kneeangle)
p.createConstraint(self.quadruped,self.jointNameToId['knee_back_leftR_link'],self.quadruped,self.jointNameToId['knee_back_leftL_link'],p.JOINT_POINT2POINT,[0,0,0],[0,0.005,0.2],[0,0.01,0.2])
self.setMotorAngleByName('motor_back_leftL_joint',self.motorDir[2]*halfpi)
self.setMotorAngleByName('motor_back_leftR_joint',self.motorDir[3]*halfpi)
p.setJointMotorControl2(bodyIndex=self.quadruped,jointIndex=self.jointNameToId['knee_back_leftL_link'],controlMode=p.VELOCITY_CONTROL,targetVelocity=0,force=kneeFrictionForce)
p.setJointMotorControl2(bodyIndex=self.quadruped,jointIndex=self.jointNameToId['knee_back_leftR_link'],controlMode=p.VELOCITY_CONTROL,targetVelocity=0,force=kneeFrictionForce)
#right front leg
p.resetJointState(self.quadruped,self.jointNameToId['motor_front_rightL_joint'],self.motorDir[4]*halfpi)
p.resetJointState(self.quadruped,self.jointNameToId['knee_front_rightL_link'],self.motorDir[4]*kneeangle)
p.resetJointState(self.quadruped,self.jointNameToId['motor_front_rightR_joint'],self.motorDir[5]*halfpi)
p.resetJointState(self.quadruped,self.jointNameToId['knee_front_rightR_link'],self.motorDir[5]*kneeangle)
p.createConstraint(self.quadruped,self.jointNameToId['knee_front_rightR_link'],self.quadruped,self.jointNameToId['knee_front_rightL_link'],p.JOINT_POINT2POINT,[0,0,0],[0,0.005,0.2],[0,0.01,0.2])
self.setMotorAngleByName('motor_front_rightL_joint',self.motorDir[4]*halfpi)
self.setMotorAngleByName('motor_front_rightR_joint',self.motorDir[5]*halfpi)
p.setJointMotorControl2(bodyIndex=self.quadruped,jointIndex=self.jointNameToId['knee_front_rightL_link'],controlMode=p.VELOCITY_CONTROL,targetVelocity=0,force=kneeFrictionForce)
p.setJointMotorControl2(bodyIndex=self.quadruped,jointIndex=self.jointNameToId['knee_front_rightR_link'],controlMode=p.VELOCITY_CONTROL,targetVelocity=0,force=kneeFrictionForce)
#right back leg
p.resetJointState(self.quadruped,self.jointNameToId['motor_back_rightL_joint'],self.motorDir[6]*halfpi)
p.resetJointState(self.quadruped,self.jointNameToId['knee_back_rightL_link'],self.motorDir[6]*kneeangle)
p.resetJointState(self.quadruped,self.jointNameToId['motor_back_rightR_joint'],self.motorDir[7]*halfpi)
p.resetJointState(self.quadruped,self.jointNameToId['knee_back_rightR_link'],self.motorDir[7]*kneeangle)
p.createConstraint(self.quadruped,self.jointNameToId['knee_back_rightR_link'],self.quadruped,self.jointNameToId['knee_back_rightL_link'],p.JOINT_POINT2POINT,[0,0,0],[0,0.005,0.2],[0,0.01,0.2])
self.setMotorAngleByName('motor_back_rightL_joint',self.motorDir[6]*halfpi)
self.setMotorAngleByName('motor_back_rightR_joint',self.motorDir[7]*halfpi)
p.setJointMotorControl2(bodyIndex=self.quadruped,jointIndex=self.jointNameToId['knee_back_rightL_link'],controlMode=p.VELOCITY_CONTROL,targetVelocity=0,force=kneeFrictionForce)
p.setJointMotorControl2(bodyIndex=self.quadruped,jointIndex=self.jointNameToId['knee_back_rightR_link'],controlMode=p.VELOCITY_CONTROL,targetVelocity=0,force=kneeFrictionForce)
def set_state(self, x, vx):
p.resetJointState(self.bodies[self.bodyIndex], self.jointIndex, x, vx)
def train_rnd(use_correlated_noise=False, param_sigma=.0):
leg_model_nn = LegModelLSTM(n_inputs=6, n_actions=3)
optim = T.optim.Adam(leg_model_nn.parameters(), lr=0.003)
print("Training:")
ou = OrnsteinUhlenbeckActionNoise(mu=np.zeros(3),
sigma=0.5,
theta=0.15,
dt=1e-1,
x0=None)
joints = []
acts = []
for iter in range(n_iters):
max_velocity = 2.0 - np.random.rand() * param_sigma
ou.reset()
correlated_noise = [ou() for i in range(episode_len)]
ep_joints = []
ep_acts = []
# Sample batchsize episodes
[
p.resetJointState(leg,
i,
np.random.randn(),
0,
physicsClientId=client_ID) for i in range(3)
]
for st in range(episode_len):
# Joint
obs = p.getJointStates(leg, range(3), physicsClientId=client_ID
) # pos, vel, reaction(6), prev_torque
joint_angles = []
joint_velocities = []
joint_torques = []
for o in obs:
joint_angles.append(o[0])
joint_velocities.append(o[1])
joint_torques.append(o[3])
joint_angles_normed = scale_joints(joint_angles)
ep_joints.append(joint_angles_normed)
# Action
noise = np.random.rand(3) * 2 - 1
if use_correlated_noise:
noise = correlated_noise[st]
act_normed = noise
action_rads = scale_action(act_normed)
ep_acts.append(act_normed)
# Simulate
for i in range(3):
p.setJointMotorControl2(bodyUniqueId=leg,
jointIndex=i,
controlMode=p.POSITION_CONTROL,
targetPosition=action_rads[i],
force=max_joint_force,
positionGain=0.1,
velocityGain=0.1,
maxVelocity=max_velocity,
physicsClientId=client_ID)
for i in range(sim_steps_per_iter):
p.stepSimulation(physicsClientId=client_ID)
if (animate) and True: time.sleep(0.0038)
joints.append(ep_joints)
acts.append(ep_acts)
if iter % batchsize == 0 and iter > 0:
# Forward pass
joints_T = T.tensor([j[:-1] for j in joints], dtype=T.float32)
joints_next_T = T.tensor([j[1:] for j in joints], dtype=T.float32)
acts_T = T.tensor([j[:-1] for j in acts], dtype=T.float32)
pred, _ = leg_model_nn(T.cat((joints_T, acts_T), dim=2))
joints = []
acts = []
# update
optim.zero_grad()
loss = F.mse_loss(pred, joints_next_T)
loss.backward()
optim.step()
print("Iter: {}/{}, loss: {}".format(iter, n_iters, loss))
sdir = os.path.join(os.path.dirname(os.path.realpath(__file__)),
"legtester.p")
T.save(leg_model_nn.state_dict(), sdir)
print("Training done")
def resetPose(self):
#right front leg
self.disableAllMotors()
p.resetJointState(self.quadruped,self.jointNameToId['motor_front_rightR_joint'],1.57)
p.resetJointState(self.quadruped,self.jointNameToId['knee_front_rightR_link'],-2.2)
p.resetJointState(self.quadruped,self.jointNameToId['motor_front_rightL_joint'],-1.57)
p.resetJointState(self.quadruped,self.jointNameToId['knee_front_rightL_link'],2.2)
p.createConstraint(self.quadruped,self.jointNameToId['knee_front_rightR_link'],self.quadruped,self.jointNameToId['knee_front_rightL_link'],p.JOINT_POINT2POINT,[0,0,0],[0,0.01,0.2],[0,-0.015,0.2])
self.setMotorAngleByName('motor_front_rightR_joint', 1.57)
self.setMotorAngleByName('motor_front_rightL_joint',-1.57)
#left front leg
p.resetJointState(self.quadruped,self.jointNameToId['motor_front_leftR_joint'],1.57)
p.resetJointState(self.quadruped,self.jointNameToId['knee_front_leftR_link'],-2.2)
p.resetJointState(self.quadruped,self.jointNameToId['motor_front_leftL_joint'],-1.57)
p.resetJointState(self.quadruped,self.jointNameToId['knee_front_leftL_link'],2.2)
p.createConstraint(self.quadruped,self.jointNameToId['knee_front_leftR_link'],self.quadruped,self.jointNameToId['knee_front_leftL_link'],p.JOINT_POINT2POINT,[0,0,0],[0,-0.01,0.2],[0,0.015,0.2])
self.setMotorAngleByName('motor_front_leftR_joint', 1.57)
self.setMotorAngleByName('motor_front_leftL_joint',-1.57)
#right back leg
p.resetJointState(self.quadruped,self.jointNameToId['motor_back_rightR_joint'],1.57)
p.resetJointState(self.quadruped,self.jointNameToId['knee_back_rightR_link'],-2.2)
p.resetJointState(self.quadruped,self.jointNameToId['motor_back_rightL_joint'],-1.57)
p.resetJointState(self.quadruped,self.jointNameToId['knee_back_rightL_link'],2.2)
p.createConstraint(self.quadruped,self.jointNameToId['knee_back_rightR_link'],self.quadruped,self.jointNameToId['knee_back_rightL_link'],p.JOINT_POINT2POINT,[0,0,0],[0,0.01,0.2],[0,-0.015,0.2])
self.setMotorAngleByName('motor_back_rightR_joint', 1.57)
self.setMotorAngleByName('motor_back_rightL_joint',-1.57)
#left back leg
p.resetJointState(self.quadruped,self.jointNameToId['motor_back_leftR_joint'],1.57)
p.resetJointState(self.quadruped,self.jointNameToId['knee_back_leftR_link'],-2.2)
p.resetJointState(self.quadruped,self.jointNameToId['motor_back_leftL_joint'],-1.57)
p.resetJointState(self.quadruped,self.jointNameToId['knee_back_leftL_link'],2.2)
p.createConstraint(self.quadruped,self.jointNameToId['knee_back_leftR_link'],self.quadruped,self.jointNameToId['knee_back_leftL_link'],p.JOINT_POINT2POINT,[0,0,0],[0,-0.01,0.2],[0,0.015,0.2])
self.setMotorAngleByName('motor_back_leftR_joint', 1.57)
self.setMotorAngleByName('motor_back_leftL_joint',-1.57)
float(value_string_list[1]), float(value_string_list[2]), inverse = True) # pelvis to right leg applyMMMRotationToURDFJoint(human_adult_ID, 3, float(value_string_list[28]), float(value_string_list[29]), float(value_string_list[30])) # pelvis to left leg applyMMMRotationToURDFJoint(human_adult_ID, 4, float(value_string_list[14]), float(value_string_list[15]), float(value_string_list[16])) # right leg to right shin p.resetJointState(human_adult_ID, 5, -float(value_string_list[31])) # left leg to left shin p.resetJointState(human_adult_ID, 6, -float(value_string_list[17])) # right shin to right foot applyMMMRotationToURDFJoint(human_adult_ID, 7, float(value_string_list[23]), float(value_string_list[24]), float(value_string_list[25])) # left shin to left foot applyMMMRotationToURDFJoint(human_adult_ID, 8, float(value_string_list[9]), float(value_string_list[10]), float(value_string_list[11])) # right_shoulder_to_right_arm
import pybullet as p
import time
import math
p.connect(p.SHARED_MEMORY)
p.loadURDF("plane.urdf")
quadruped = p.loadURDF("quadruped/quadruped.urdf", 0, 0, .3)
#p.getNumJoints(1)
#right front leg
p.resetJointState(quadruped, 0, 1.57)
p.resetJointState(quadruped, 2, -2.2)
p.resetJointState(quadruped, 3, -1.57)
p.resetJointState(quadruped, 5, 2.2)
p.createConstraint(quadruped, 2, quadruped, 5, 3, [0, 0, 0], [0, 0.01, 0.2],
[0, -0.015, 0.2])
p.setJointMotorControl(quadruped, 0, p.POSITION_CONTROL, 1.57, 1)
p.setJointMotorControl(quadruped, 1, p.VELOCITY_CONTROL, 0, 0)
p.setJointMotorControl(quadruped, 2, p.VELOCITY_CONTROL, 0, 0)
p.setJointMotorControl(quadruped, 3, p.POSITION_CONTROL, -1.57, 1)
p.setJointMotorControl(quadruped, 4, p.VELOCITY_CONTROL, 0, 0)
p.setJointMotorControl(quadruped, 5, p.VELOCITY_CONTROL, 0, 0)
#left front leg
p.resetJointState(quadruped, 6, 1.57)
p.resetJointState(quadruped, 8, -2.2)
p.resetJointState(quadruped, 9, -1.57)
p.resetJointState(quadruped, 11, 2.2)
p.createConstraint(quadruped, 8, quadruped, 11, 3, [0, 0, 0], [0, -0.01, 0.2],
[0, 0.015, 0.2])
p.setJointMotorControl2(quadruped, knee_back_leftR_link, p.POSITION_CONTROL,
motordir[3] * (kneeangle + twopi) * float(aa) / steps)
p.setJointMotorControl2(quadruped, knee_front_rightL_link, p.POSITION_CONTROL,
motordir[4] * (kneeangle) * float(aa) / steps)
p.setJointMotorControl2(quadruped, knee_front_rightR_link, p.POSITION_CONTROL,
motordir[5] * (kneeangle + twopi) * float(aa) / steps)
p.setJointMotorControl2(quadruped, knee_back_rightL_link, p.POSITION_CONTROL,
motordir[6] * (kneeangle + twopi) * float(aa) / steps)
p.setJointMotorControl2(quadruped, knee_back_rightR_link, p.POSITION_CONTROL,
motordir[7] * kneeangle * float(aa) / steps)
p.stepSimulation()
#time.sleep(fixedTimeStep)
else:
p.resetJointState(quadruped, motor_front_leftL_joint, motordir[0] * halfpi)
p.resetJointState(quadruped, knee_front_leftL_link, motordir[0] * kneeangle)
p.resetJointState(quadruped, motor_front_leftR_joint, motordir[1] * halfpi)
p.resetJointState(quadruped, knee_front_leftR_link, motordir[1] * kneeangle)
p.resetJointState(quadruped, motor_back_leftL_joint, motordir[2] * halfpi)
p.resetJointState(quadruped, knee_back_leftL_link, motordir[2] * kneeangle)
p.resetJointState(quadruped, motor_back_leftR_joint, motordir[3] * halfpi)
p.resetJointState(quadruped, knee_back_leftR_link, motordir[3] * kneeangle)
p.resetJointState(quadruped, motor_front_rightL_joint, motordir[4] * halfpi)
p.resetJointState(quadruped, knee_front_rightL_link, motordir[4] * kneeangle)
p.resetJointState(quadruped, motor_front_rightR_joint, motordir[5] * halfpi)
p.resetJointState(quadruped, knee_front_rightR_link, motordir[5] * kneeangle)
p.resetJointState(quadruped, motor_back_rightL_joint, motordir[6] * halfpi)
if (numJoints != 7): exit() #lower limits for null space ll = [-.967, -2, -2.96, 0.19, -2.96, -2.09, -3.05] #upper limits for null space ul = [.967, 2, 2.96, 2.29, 2.96, 2.09, 3.05] #joint ranges for null space jr = [5.8, 4, 5.8, 4, 5.8, 4, 6] #restposes for null space rp = [0, 0, 0, 0.5 * math.pi, 0, -math.pi * 0.5 * 0.66, 0] #joint damping coefficents jd = [0.1, 0.1, 0.1, 0.1, 0.1, 0.1, 0.1] for i in range(numJoints): p.resetJointState(kukaId, i, rp[i]) p.setGravity(0, 0, 0) t = 0. prevPose = [0, 0, 0] prevPose1 = [0, 0, 0] hasPrevPose = 0 useNullSpace = 1 useOrientation = 1 #If we set useSimulation=0, it sets the arm pose to be the IK result directly without using dynamic control. #This can be used to test the IK result accuracy. useSimulation = 0 useRealTimeSimulation = 1 ikSolver = 0 p.setRealTimeSimulation(useRealTimeSimulation)
def reset_position(self, position, velocity): p.resetJointState(self.bodies[self.bodyIndex],self.jointIndex,targetValue=position, targetVelocity=velocity) self.disable_motor()
def set_state(self, x, vx): p.resetJointState(self.bodies[self.bodyIndex], self.jointIndex, x, vx)
import math
from datetime import datetime
from datetime import datetime
clid = p.connect(p.SHARED_MEMORY)
if (clid<0):
p.connect(p.GUI)
p.loadURDF("plane.urdf",[0,0,-0.3])
husky = p.loadURDF("husky/husky.urdf",[0.290388,0.329902,-0.310270],[0.002328,-0.000984,0.996491,0.083659])
for i in range (p.getNumJoints(husky)):
print(p.getJointInfo(husky,i))
kukaId = p.loadURDF("kuka_iiwa/model_free_base.urdf", 0.193749,0.345564,0.120208,0.002327,-0.000988,0.996491,0.083659)
ob = kukaId
jointPositions=[ 3.559609, 0.411182, 0.862129, 1.744441, 0.077299, -1.129685, 0.006001 ]
for jointIndex in range (p.getNumJoints(ob)):
p.resetJointState(ob,jointIndex,jointPositions[jointIndex])
#put kuka on top of husky
cid = p.createConstraint(husky,-1,kukaId,-1,p.JOINT_FIXED,[0,0,0],[0,0,0],[0.,0.,-.5],[0,0,0,1])
baseorn = p.getQuaternionFromEuler([3.1415,0,0.3])
baseorn = [0,0,0,1]
#[0, 0, 0.707, 0.707]
#p.resetBasePositionAndOrientation(kukaId,[0,0,0],baseorn)#[0,0,0,1])
kukaEndEffectorIndex = 6
numJoints = p.getNumJoints(kukaId)
if (numJoints!=7):
def create_human(self,
static=True,
limit_scale=1.0,
specular_color=[0.1, 0.1, 0.1],
gender='random',
config=None,
hipbone_to_mouth_height=None,
new=False):
if gender not in ['male', 'female']:
gender = self.np_random.choice(['male', 'female'])
if gender == 'male':
hmhs = hipbone_to_mouth_height / 0.6
else:
hmhs = hipbone_to_mouth_height / 0.54
def create_body(shape=p.GEOM_CAPSULE,
radius=0,
length=0,
position_offset=[0, 0, 0],
orientation=[0, 0, 0, 1]):
visual_shape = p.createVisualShape(
shape,
radius=radius,
length=length,
rgbaColor=[0.8, 0.6, 0.4, 1],
specularColor=specular_color,
visualFramePosition=position_offset,
visualFrameOrientation=orientation,
physicsClientId=self.id)
collision_shape = p.createCollisionShape(
shape,
radius=radius,
height=length,
collisionFramePosition=position_offset,
collisionFrameOrientation=orientation,
physicsClientId=self.id)
return collision_shape, visual_shape
joint_c, joint_v = -1, -1
if gender == 'male':
c = lambda tag: config(tag, 'human_male')
m = c('mass') # mass of 50% male in kg
rs = c('radius_scale')
hs = c('height_scale')
hs *= hmhs
chest_c, chest_v = create_body(
shape=p.GEOM_CAPSULE,
radius=0.127 * rs,
length=0.056,
orientation=p.getQuaternionFromEuler([0, np.pi / 2.0, 0],
physicsClientId=self.id))
right_shoulders_c, right_shoulders_v = create_body(
shape=p.GEOM_CAPSULE,
radius=0.106 * rs,
length=0.253 / 8,
position_offset=[-0.253 / 2.5 + 0.253 / 16, 0, 0],
orientation=p.getQuaternionFromEuler([0, np.pi / 2.0, 0],
physicsClientId=self.id))
left_shoulders_c, left_shoulders_v = create_body(
shape=p.GEOM_CAPSULE,
radius=0.106 * rs,
length=0.253 / 8,
position_offset=[0.253 / 2.5 - 0.253 / 16, 0, 0],
orientation=p.getQuaternionFromEuler([0, np.pi / 2.0, 0],
physicsClientId=self.id))
neck_c, neck_v = create_body(
shape=p.GEOM_CAPSULE,
radius=0.06 * rs,
length=0.124 * hs,
position_offset=[0, 0, (0.2565 - 0.1415 - 0.025) * hs])
upperarm_c, upperarm_v = create_body(
shape=p.GEOM_CAPSULE,
radius=0.043 * rs,
length=0.279 * hs,
position_offset=[0, 0, -0.279 / 2.0 * hs])
forearm_c, forearm_v = create_body(
shape=p.GEOM_CAPSULE,
radius=0.033 * rs,
length=0.257 * hs,
position_offset=[0, 0, -0.257 / 2.0 * hs])
hand_c, hand_v = create_body(shape=p.GEOM_SPHERE,
radius=0.043 * rs,
length=0,
position_offset=[0, 0, -0.043 * rs])
self.hand_radius, self.elbow_radius, self.shoulder_radius = 0.043 * rs, 0.043 * rs, 0.043 * rs
waist_c, waist_v = create_body(
shape=p.GEOM_CAPSULE,
radius=0.1205 * rs,
length=0.049,
orientation=p.getQuaternionFromEuler([0, np.pi / 2.0, 0],
physicsClientId=self.id))
hips_c, hips_v = create_body(shape=p.GEOM_CAPSULE,
radius=0.1335 * rs,
length=0.094,
position_offset=[0, 0, -0.08125 * hs],
orientation=p.getQuaternionFromEuler(
[0, np.pi / 2.0, 0],
physicsClientId=self.id))
thigh_c, thigh_v = create_body(
shape=p.GEOM_CAPSULE,
radius=0.08 * rs,
length=0.424 * hs,
position_offset=[0, 0, -0.424 / 2.0 * hs])
shin_c, shin_v = create_body(
shape=p.GEOM_CAPSULE,
radius=0.05 * rs,
length=0.403 * hs,
position_offset=[0, 0, -0.403 / 2.0 * hs])
foot_c, foot_v = create_body(
shape=p.GEOM_CAPSULE,
radius=0.05 * rs,
length=0.215 * hs,
position_offset=[0, -0.1, -0.025 * rs],
orientation=p.getQuaternionFromEuler([np.pi / 2.0, 0, 0],
physicsClientId=self.id))
elbow_v = p.createVisualShape(shapeType=p.GEOM_SPHERE,
radius=(0.043 + 0.033) / 2 * rs,
length=0,
rgbaColor=[0.8, 0.6, 0.4, 1],
visualFramePosition=[0, 0.01, 0],
physicsClientId=self.id)
if self.cloth:
# Cloth penetrates the spheres at the end of each capsule, so we create physical spheres at the joints
invisible_v = p.createVisualShape(shapeType=p.GEOM_SPHERE,
radius=0.01 * rs,
length=0,
rgbaColor=[0.8, 0.6, 0.4, 0],
physicsClientId=self.id)
shoulder_cloth_c, _ = create_body(shape=p.GEOM_SPHERE,
radius=0.043 * rs,
length=0)
elbow_cloth_c, _ = create_body(shape=p.GEOM_SPHERE,
radius=0.043 * rs,
length=0)
wrist_cloth_c, _ = create_body(shape=p.GEOM_SPHERE,
radius=0.033 * rs,
length=0)
head_scale = [0.89] * 3
head_pos = [0.09, 0.08, -0.07 + 0.01]
head_c = p.createCollisionShape(
shapeType=p.GEOM_MESH,
fileName=os.path.join(
self.directory, 'head_female_male',
'BaseHeadMeshes_v5_male_cropped_reduced_compressed_vhacd.obj'
),
collisionFramePosition=head_pos,
collisionFrameOrientation=p.getQuaternionFromEuler(
[np.pi / 2.0, 0, 0], physicsClientId=self.id),
meshScale=head_scale,
physicsClientId=self.id)
head_v = p.createVisualShape(
shapeType=p.GEOM_MESH,
fileName=os.path.join(
self.directory, 'head_female_male',
'BaseHeadMeshes_v5_male_cropped_reduced_compressed.obj'),
rgbaColor=[0.8, 0.6, 0.4, 1],
specularColor=specular_color,
visualFramePosition=head_pos,
visualFrameOrientation=p.getQuaternionFromEuler(
[np.pi / 2.0, 0, 0], physicsClientId=self.id),
meshScale=head_scale,
physicsClientId=self.id)
joint_p, joint_o = [0, 0, 0], [0, 0, 0, 1]
chest_p = [0, 0, 0.156 * hs]
shoulders_p = [0, 0, 0.1415 / 2 * hs]
neck_p = [0, 0, 0.1515 * hs]
head_p = [0, 0, (0.399 - 0.1415 - 0.1205) * hs]
right_upperarm_p = [-0.106 * rs - 0.073, 0, 0]
left_upperarm_p = [0.106 * rs + 0.073, 0, 0]
forearm_p = [0, 0, -0.279 * hs]
hand_p = [0, 0, -(0.033 * rs + 0.257 * hs)]
waist_p = [0, 0, 0.08125 * hs]
hips_p = [0, 0, 1.00825 * hs]
right_thigh_p = [-0.08 * rs - 0.009, 0, -0.08125 * hs]
left_thigh_p = [0.08 * rs + 0.009, 0, -0.08125 * hs]
shin_p = [0, 0, -0.424 * hs]
foot_p = [0, 0, -0.403 * hs - 0.025]
else:
c = lambda tag: config(tag, 'human_female')
m = c('mass') # mass of 50% female in kg
rs = c('radius_scale')
hs = c('height_scale')
hs *= hmhs
chest_c, chest_v = create_body(
shape=p.GEOM_CAPSULE,
radius=0.127 * rs,
length=0.01,
orientation=p.getQuaternionFromEuler(
[0, np.pi / 2.0, 0], physicsClientId=self.id)) #
right_shoulders_c, right_shoulders_v = create_body(
shape=p.GEOM_CAPSULE,
radius=0.092 * rs,
length=0.225 / 8,
position_offset=[-0.225 / 2.5 + 0.225 / 16, 0, 0],
orientation=p.getQuaternionFromEuler(
[0, np.pi / 2.0, 0], physicsClientId=self.id)) #
left_shoulders_c, left_shoulders_v = create_body(
shape=p.GEOM_CAPSULE,
radius=0.092 * rs,
length=0.225 / 8,
position_offset=[0.225 / 2.5 - 0.225 / 16, 0, 0],
orientation=p.getQuaternionFromEuler(
[0, np.pi / 2.0, 0], physicsClientId=self.id)) #
neck_c, neck_v = create_body(shape=p.GEOM_CAPSULE,
radius=0.05 * rs,
length=0.121 * hs,
position_offset=[
0, 0,
(0.2565 - 0.1415 - 0.025) * hs
]) # not position
upperarm_c, upperarm_v = create_body(
shape=p.GEOM_CAPSULE,
radius=0.0355 * rs,
length=0.264 * hs,
position_offset=[0, 0, -0.264 / 2.0 * hs]) #
forearm_c, forearm_v = create_body(
shape=p.GEOM_CAPSULE,
radius=0.027 * rs,
length=0.234 * hs,
position_offset=[0, 0, -0.234 / 2.0 * hs]) #
hand_c, hand_v = create_body(shape=p.GEOM_SPHERE,
radius=0.0355 * rs,
length=0,
position_offset=[0, 0,
-0.0355 * rs]) #
self.hand_radius, self.elbow_radius, self.shoulder_radius = 0.0355 * rs, 0.0355 * rs, 0.0355 * rs
waist_c, waist_v = create_body(
shape=p.GEOM_CAPSULE,
radius=0.11 * rs,
length=0.009,
orientation=p.getQuaternionFromEuler(
[0, np.pi / 2.0, 0], physicsClientId=self.id)) #
hips_c, hips_v = create_body(
shape=p.GEOM_CAPSULE,
radius=0.127 * rs,
length=0.117,
position_offset=[0, 0, -0.15 / 2 * hs],
orientation=p.getQuaternionFromEuler(
[0, np.pi / 2.0, 0], physicsClientId=self.id)) #
thigh_c, thigh_v = create_body(
shape=p.GEOM_CAPSULE,
radius=0.0775 * rs,
length=0.391 * hs,
position_offset=[0, 0, -0.391 / 2.0 * hs]) #
shin_c, shin_v = create_body(
shape=p.GEOM_CAPSULE,
radius=0.045 * rs,
length=0.367 * hs,
position_offset=[0, 0, -0.367 / 2.0 * hs]) #
foot_c, foot_v = create_body(
shape=p.GEOM_CAPSULE,
radius=0.045 * rs,
length=0.195 * hs,
position_offset=[0, -0.09, -0.0225 * rs],
orientation=p.getQuaternionFromEuler(
[np.pi / 2.0, 0, 0], physicsClientId=self.id)) #
elbow_v = p.createVisualShape(shapeType=p.GEOM_SPHERE,
radius=(0.0355 + 0.027) / 2 * rs,
length=0,
rgbaColor=[0.8, 0.6, 0.4, 1],
visualFramePosition=[0, 0.01, 0],
physicsClientId=self.id)
if self.cloth:
# Cloth penetrates the spheres at the end of each capsule, so we create physical spheres at the joints
invisible_v = p.createVisualShape(shapeType=p.GEOM_SPHERE,
radius=0.01 * rs,
length=0,
rgbaColor=[0.8, 0.6, 0.4, 0],
physicsClientId=self.id)
shoulder_cloth_c, _ = create_body(shape=p.GEOM_SPHERE,
radius=0.0355 * rs,
length=0)
elbow_cloth_c, _ = create_body(shape=p.GEOM_SPHERE,
radius=0.0355 * rs,
length=0)
wrist_cloth_c, _ = create_body(shape=p.GEOM_SPHERE,
radius=0.027 * rs,
length=0)
head_scale = [0.89] * 3
head_pos = [-0.089, -0.09, -0.07]
head_c = p.createCollisionShape(
shapeType=p.GEOM_MESH,
fileName=os.path.join(
self.directory, 'head_female_male',
'BaseHeadMeshes_v5_female_cropped_reduced_compressed_vhacd.obj'
),
collisionFramePosition=head_pos,
collisionFrameOrientation=p.getQuaternionFromEuler(
[np.pi / 2.0, 0, 0], physicsClientId=self.id),
meshScale=head_scale,
physicsClientId=self.id)
head_v = p.createVisualShape(
shapeType=p.GEOM_MESH,
fileName=os.path.join(
self.directory, 'head_female_male',
'BaseHeadMeshes_v5_female_cropped_reduced_compressed.obj'),
rgbaColor=[0.8, 0.6, 0.4, 1],
specularColor=specular_color,
visualFramePosition=head_pos,
visualFrameOrientation=p.getQuaternionFromEuler(
[np.pi / 2.0, 0, 0], physicsClientId=self.id),
meshScale=head_scale,
physicsClientId=self.id)
joint_p, joint_o = [0, 0, 0], [0, 0, 0, 1]
chest_p = [0, 0, 0.15 * hs]
shoulders_p = [0, 0, 0.132 / 2 * hs]
neck_p = [0, 0, 0.132 * hs]
head_p = [0, 0, 0.12 * hs]
right_upperarm_p = [-0.092 * rs - 0.067, 0, 0]
left_upperarm_p = [0.092 * rs + 0.067, 0, 0]
forearm_p = [0, 0, -0.264 * hs]
hand_p = [0, 0, -(0.027 * rs + 0.234 * hs)]
waist_p = [0, 0, 0.15 / 2 * hs]
hips_p = [0, 0, 0.923 * hs]
right_thigh_p = [-0.0775 * rs - 0.0145, 0, -0.15 / 2 * hs]
left_thigh_p = [0.0775 * rs + 0.0145, 0, -0.15 / 2 * hs]
shin_p = [0, 0, -0.391 * hs]
foot_p = [0, 0, -0.367 * hs - 0.045 / 2]
linkMasses = []
linkCollisionShapeIndices = []
linkVisualShapeIndices = []
linkPositions = []
linkOrientations = []
linkInertialFramePositions = []
linkInertialFrameOrientations = []
linkParentIndices = []
linkJointTypes = []
linkJointAxis = []
linkLowerLimits = []
linkUpperLimits = []
# NOTE: Waist and chest
linkMasses.extend(m * np.array([0, 0, 0.13, 0.1]))
linkCollisionShapeIndices.extend([joint_c, joint_c, waist_c, chest_c])
linkVisualShapeIndices.extend([joint_v, joint_v, waist_v, chest_v])
linkPositions.extend([waist_p, joint_p, joint_p, chest_p])
linkOrientations.extend([joint_o] * 4)
linkInertialFramePositions.extend([[0, 0, 0]] * 4)
linkInertialFrameOrientations.extend([[0, 0, 0, 1]] * 4)
linkParentIndices.extend([0, 1, 2, 3])
if new:
linkJointTypes.extend([p.JOINT_REVOLUTE] * 3 + [p.JOINT_FIXED])
linkJointAxis.extend([[1, 0, 0], [0, 1, 0], [0, 0, 1], [0, 0, 0]])
linkLowerLimits.extend(
np.array(
[np.deg2rad(-30),
np.deg2rad(-30),
np.deg2rad(-30), 0]))
linkUpperLimits.extend(
np.array([np.deg2rad(75),
np.deg2rad(30),
np.deg2rad(30), 0]))
else:
linkJointTypes.extend([p.JOINT_FIXED] * 4)
linkJointAxis.extend([[0, 0, 0], [0, 0, 0], [0, 0, 0], [0, 0, 0]])
linkLowerLimits.extend(np.array([0, 0, 0, 0]))
linkUpperLimits.extend(np.array([0, 0, 0, 0]))
# NOTE: Shoulders, neck, and head
linkMasses.extend(m *
np.array([0, 0, 0.05, 0, 0, 0.05, 0.01, 0, 0, 0.07]))
linkCollisionShapeIndices.extend([
joint_c, joint_c, right_shoulders_c, joint_c, joint_c,
left_shoulders_c, neck_c, joint_c, joint_c, head_c
])
linkVisualShapeIndices.extend([
joint_v, joint_v, right_shoulders_v, joint_v, joint_v,
left_shoulders_v, neck_v, joint_v, joint_v, head_v
])
linkPositions.extend([
shoulders_p, shoulders_p, joint_p, shoulders_p, shoulders_p,
joint_p, neck_p, head_p, joint_p, joint_p
])
linkOrientations.extend([joint_o] * 10)
linkInertialFramePositions.extend([[0, 0, 0]] * 10)
linkInertialFrameOrientations.extend([[0, 0, 0, 1]] * 10)
linkParentIndices.extend([4, 5, 6, 4, 8, 9, 4, 11, 12, 13])
linkJointTypes.extend([p.JOINT_FIXED] * 6 + [p.JOINT_REVOLUTE] * 4)
linkJointAxis.extend([[0, 0, 0], [0, 0, 0], [0, 0, 0], [0, 0, 0],
[0, 0, 0], [0, 0, 0], [1, 0, 0], [1, 0, 0],
[0, 1, 0], [0, 0, 1]])
linkLowerLimits.extend(
np.array([
0, 0, 0, 0, 0, 0,
np.deg2rad(-10),
np.deg2rad(-50),
np.deg2rad(-34),
np.deg2rad(-70)
]) * limit_scale)
linkUpperLimits.extend(
np.array([
0, 0, 0, 0, 0, 0,
np.deg2rad(20),
np.deg2rad(50),
np.deg2rad(34),
np.deg2rad(70)
]) * limit_scale)
# NOTE: Right arm
linkMasses.extend(m * np.array([0, 0, 0.033, 0, 0.019, 0, 0.0065]))
if not self.cloth:
linkCollisionShapeIndices.extend([
joint_c, joint_c, upperarm_c, joint_c, forearm_c, joint_c,
hand_c
])
linkVisualShapeIndices.extend([
joint_v, joint_v, upperarm_v, elbow_v, forearm_v, joint_v,
hand_v
])
else:
linkCollisionShapeIndices.extend([
joint_c, shoulder_cloth_c, upperarm_c, elbow_cloth_c,
forearm_c, wrist_cloth_c, hand_c
])
linkVisualShapeIndices.extend([
joint_v, invisible_v, upperarm_v, elbow_v, forearm_v,
invisible_v, hand_v
])
linkPositions.extend([
right_upperarm_p, joint_p, joint_p, forearm_p, joint_p, hand_p,
joint_p
])
linkOrientations.extend([joint_o] * 7)
linkInertialFramePositions.extend([[0, 0, 0]] * 7)
linkInertialFrameOrientations.extend([[0, 0, 0, 1]] * 7)
linkParentIndices.extend([7, 15, 16, 17, 18, 19, 20])
linkJointTypes.extend([p.JOINT_REVOLUTE] * 7)
linkJointAxis.extend([[0, 1, 0], [1, 0, 0], [0, 0, 1], [1, 0, 0],
[0, 0, 1], [1, 0, 0], [0, 1, 0]])
linkLowerLimits.extend(
np.array([
np.deg2rad(5),
np.deg2rad(-188),
np.deg2rad(-90),
np.deg2rad(-128),
np.deg2rad(-90),
np.deg2rad(-81),
np.deg2rad(-27)
]) * limit_scale)
linkUpperLimits.extend(
np.array([
np.deg2rad(198),
np.deg2rad(61),
np.deg2rad(90),
np.deg2rad(0),
np.deg2rad(90),
np.deg2rad(90),
np.deg2rad(47)
]) * limit_scale)
# NOTE: Left arm
linkMasses.extend(m * np.array([0, 0, 0.033, 0, 0.019, 0, 0.0065]))
if not self.cloth:
linkCollisionShapeIndices.extend([
joint_c, joint_c, upperarm_c, joint_c, forearm_c, joint_c,
hand_c
])
linkVisualShapeIndices.extend([
joint_v, joint_v, upperarm_v, elbow_v, forearm_v, joint_v,
hand_v
])
else:
linkCollisionShapeIndices.extend([
joint_c, shoulder_cloth_c, upperarm_c, elbow_cloth_c,
forearm_c, wrist_cloth_c, hand_c
])
linkVisualShapeIndices.extend([
joint_v, invisible_v, upperarm_v, elbow_v, forearm_v,
invisible_v, hand_v
])
linkPositions.extend([
left_upperarm_p, joint_p, joint_p, forearm_p, joint_p, hand_p,
joint_p
])
linkOrientations.extend([joint_o] * 7)
linkInertialFramePositions.extend([[0, 0, 0]] * 7)
linkInertialFrameOrientations.extend([[0, 0, 0, 1]] * 7)
linkParentIndices.extend([10, 22, 23, 24, 25, 26, 27])
linkJointTypes.extend([p.JOINT_REVOLUTE] * 7)
linkJointAxis.extend([[0, 1, 0], [1, 0, 0], [0, 0, 1], [1, 0, 0],
[0, 0, 1], [1, 0, 0], [0, 1, 0]])
linkLowerLimits.extend(
np.array([
np.deg2rad(-198),
np.deg2rad(-188),
np.deg2rad(-90),
np.deg2rad(-128),
np.deg2rad(-90),
np.deg2rad(-81),
np.deg2rad(-47)
]) * limit_scale)
linkUpperLimits.extend(
np.array([
np.deg2rad(-5),
np.deg2rad(61),
np.deg2rad(90),
np.deg2rad(0),
np.deg2rad(90),
np.deg2rad(90),
np.deg2rad(27)
]) * limit_scale)
# NOTE: Right leg
linkMasses.extend(m * np.array([0, 0, 0.105, 0.0475, 0, 0, 0.014]))
linkCollisionShapeIndices.extend(
[joint_c, joint_c, thigh_c, shin_c, joint_c, joint_c, foot_c])
linkVisualShapeIndices.extend(
[joint_v, joint_v, thigh_v, shin_v, joint_v, joint_v, foot_v])
linkPositions.extend([
right_thigh_p, joint_p, joint_p, shin_p, foot_p, joint_p, joint_p
])
linkOrientations.extend([joint_o] * 7)
linkInertialFramePositions.extend([[0, 0, 0]] * 7)
linkInertialFrameOrientations.extend([[0, 0, 0, 1]] * 7)
linkParentIndices.extend([0, 29, 30, 31, 32, 33, 34])
linkJointTypes.extend([p.JOINT_REVOLUTE] * 7)
linkJointAxis.extend([[1, 0, 0], [0, 1, 0], [0, 0, 1], [1, 0, 0],
[1, 0, 0], [0, 1, 0], [0, 0, 1]])
linkLowerLimits.extend(
np.array([
np.deg2rad(-127),
np.deg2rad(-40),
np.deg2rad(-45), 0,
np.deg2rad(-35),
np.deg2rad(-23),
np.deg2rad(-43)
]))
linkUpperLimits.extend(
np.array([
np.deg2rad(30),
np.deg2rad(45),
np.deg2rad(40),
np.deg2rad(130),
np.deg2rad(38),
np.deg2rad(24),
np.deg2rad(35)
]))
# NOTE: Left leg
linkMasses.extend(m * np.array([0, 0, 0.105, 0.0475, 0, 0, 0.014]))
linkCollisionShapeIndices.extend(
[joint_c, joint_c, thigh_c, shin_c, joint_c, joint_c, foot_c])
linkVisualShapeIndices.extend(
[joint_v, joint_v, thigh_v, shin_v, joint_v, joint_v, foot_v])
linkPositions.extend(
[left_thigh_p, joint_p, joint_p, shin_p, foot_p, joint_p, joint_p])
linkOrientations.extend([joint_o] * 7)
linkInertialFramePositions.extend([[0, 0, 0]] * 7)
linkInertialFrameOrientations.extend([[0, 0, 0, 1]] * 7)
linkParentIndices.extend([0, 36, 37, 38, 39, 40, 41])
linkJointTypes.extend([p.JOINT_REVOLUTE] * 7)
linkJointAxis.extend([[1, 0, 0], [0, 1, 0], [0, 0, 1], [1, 0, 0],
[1, 0, 0], [0, 1, 0], [0, 0, 1]])
linkLowerLimits.extend(
np.array([
np.deg2rad(-127),
np.deg2rad(-45),
np.deg2rad(-40), 0,
np.deg2rad(-35),
np.deg2rad(-24),
np.deg2rad(-35)
]))
linkUpperLimits.extend(
np.array([
np.deg2rad(30),
np.deg2rad(40),
np.deg2rad(45),
np.deg2rad(130),
np.deg2rad(38),
np.deg2rad(23),
np.deg2rad(43)
]))
human = p.createMultiBody(
baseMass=0 if static else m * 0.14,
baseCollisionShapeIndex=hips_c,
baseVisualShapeIndex=hips_v,
basePosition=[10, 10, 10],
baseOrientation=[0, 0, 0, 1],
linkMasses=linkMasses,
linkCollisionShapeIndices=linkCollisionShapeIndices,
linkVisualShapeIndices=linkVisualShapeIndices,
linkPositions=linkPositions,
linkOrientations=linkOrientations,
linkInertialFramePositions=linkInertialFramePositions,
linkInertialFrameOrientations=linkInertialFrameOrientations,
linkParentIndices=linkParentIndices,
linkJointTypes=linkJointTypes,
linkJointAxis=linkJointAxis,
linkLowerLimits=linkLowerLimits,
linkUpperLimits=linkUpperLimits,
useMaximalCoordinates=False,
flags=p.URDF_USE_SELF_COLLISION,
physicsClientId=self.id)
# Self collision has been enabled for the person
# For stability: Remove all collisions except between the arms/legs and the other body parts
num_joints = p.getNumJoints(human, physicsClientId=self.id)
for i in range(-1, num_joints):
for j in range(-1, num_joints):
p.setCollisionFilterPair(human,
human,
i,
j,
0,
physicsClientId=self.id)
for i in range(7, 14): # Right arm
for j in list(range(-1, 4)) + list(range(14, num_joints)):
p.setCollisionFilterPair(human,
human,
i,
j,
1,
physicsClientId=self.id)
for i in range(17, 24): # Left arm
for j in list(range(-1, 14)) + list(range(24, num_joints)):
p.setCollisionFilterPair(human,
human,
i,
j,
1,
physicsClientId=self.id)
for i in range(28, 35): # Right leg
for j in [-1] + list(range(4, 28)) + list(range(35, num_joints)):
p.setCollisionFilterPair(human,
human,
i,
j,
1,
physicsClientId=self.id)
for i in range(35, num_joints): # Left leg
for j in [-1] + list(range(4, 35)):
p.setCollisionFilterPair(human,
human,
i,
j,
1,
physicsClientId=self.id)
human_joint_states = p.getJointStates(
human,
jointIndices=list(
range(p.getNumJoints(human, physicsClientId=self.id))),
physicsClientId=self.id)
human_joint_positions = np.array([x[0] for x in human_joint_states])
for j in range(p.getNumJoints(human, physicsClientId=self.id)):
p.resetJointState(human,
jointIndex=j,
targetValue=0,
targetVelocity=0,
physicsClientId=self.id)
return human
def train_adversarial():
leg_model_nn = LegModelLSTM(n_inputs=6, n_actions=3)
act_gen_nn = LegModelLSTM(n_inputs=3, n_actions=3, use_tanh=True)
optim_disc = T.optim.Adam(leg_model_nn.parameters(), lr=0.002)
optim_gen = T.optim.Adam(act_gen_nn.parameters(), lr=0.004)
print("Training:")
joints = []
acts = []
batch_rnd_noises = []
for iter in range(n_iters):
# Sample noise vec from generator
rnd_noise = np.random.rand(episode_len, 3).astype(np.float32) * 2. - 1.
batch_rnd_noises.append(rnd_noise)
with T.no_grad():
act_noise, _ = act_gen_nn(T.tensor(rnd_noise).unsqueeze(0))
ep_joints = []
ep_acts = []
# Sample batchsize episodes
[
p.resetJointState(leg,
i,
np.random.rand() * 2 - 1,
0,
physicsClientId=client_ID) for i in range(3)
]
for st in range(episode_len):
# Joint
obs = p.getJointStates(leg, range(3), physicsClientId=client_ID
) # pos, vel, reaction(6), prev_torque
joint_angles = []
joint_velocities = []
joint_torques = []
for o in obs:
joint_angles.append(o[0])
joint_velocities.append(o[1])
joint_torques.append(o[3])
joint_angles_normed = scale_joints(joint_angles)
ep_joints.append(joint_angles_normed)
# Action
act = act_noise[0, st]
#if np.random.rand() < 0.001: print(act)
action_rads = scale_action(act)
ep_acts.append(act)
# Simulate
for i in range(3):
p.setJointMotorControl2(bodyUniqueId=leg,
jointIndex=i,
controlMode=p.POSITION_CONTROL,
targetPosition=action_rads[i],
force=max_joint_force,
positionGain=0.1,
velocityGain=0.1,
maxVelocity=2.0,
physicsClientId=client_ID)
for i in range(sim_steps_per_iter):
p.stepSimulation(physicsClientId=client_ID)
if (animate) and True: time.sleep(0.0038)
joints.append(ep_joints)
acts.append(ep_acts)
if iter % batchsize == 0 and iter > 0:
# Forward pass
joints_T = T.tensor([j[:-1] for j in joints], dtype=T.float32)
joints_T_gen = joints_T.clone()
joints_next_T = T.tensor([j[1:] for j in joints], dtype=T.float32)
joints_next_T_gen = joints_next_T.clone()
act_noises_T = T.tensor([j[:-1] for j in batch_rnd_noises])
acts_T, _ = act_gen_nn(act_noises_T)
pred, _ = leg_model_nn(T.cat((joints_T, acts_T), dim=2))
if iter % batchsize * 3 == 0:
# Do discriminator first
with T.no_grad():
acts_T, _ = act_gen_nn(act_noises_T)
pred, _ = leg_model_nn(T.cat((joints_T_gen, acts_T), dim=2))
optim_disc.zero_grad()
loss_disc = F.mse_loss(pred, joints_next_T_gen)
loss_disc.backward()
optim_disc.step()
# Update gen
acts_T, _ = act_gen_nn(act_noises_T)
pred, _ = leg_model_nn(T.cat((joints_T, acts_T), dim=2))
optim_gen.zero_grad()
loss_gen = -F.mse_loss(pred, joints_next_T)
loss_gen.backward()
optim_gen.step()
print("Iter: {}/{}, loss: {}".format(iter, n_iters, loss_disc))
joints = []
acts = []
batch_rnd_noises = []
sdir_disc = os.path.join(os.path.dirname(os.path.realpath(__file__)),
"legtester.p")
sdir_gen = os.path.join(os.path.dirname(os.path.realpath(__file__)),
"legtester_gen.p")
T.save(leg_model_nn.state_dict(), sdir_disc)
T.save(leg_model_nn.state_dict(), sdir_gen)
print("Training done")
def reset(self):
random_delta = np.random.random(len(
self.joint_ids)) * self.random_reset
for joint_id, angle, d_angle in zip(self.joint_ids, self.rest_position,
random_delta):
p.resetJointState(self.uid, joint_id, angle + d_angle)
import pybullet as p
import time
cid = p.connect(p.SHARED_MEMORY)
if (cid<0):
p.connect(p.GUI)
p.loadURDF("plane.urdf")
kuka = p.loadURDF("kuka_iiwa/model.urdf")
p.addUserDebugText("tip", [0,0,0.1],textColorRGB=[1,0,0],textSize=1.5,parentObjectUniqueId=kuka, parentLinkIndex=6)
p.addUserDebugLine([0,0,0],[0.1,0,0],[1,0,0],parentObjectUniqueId=kuka, parentLinkIndex=6)
p.addUserDebugLine([0,0,0],[0,0.1,0],[0,1,0],parentObjectUniqueId=kuka, parentLinkIndex=6)
p.addUserDebugLine([0,0,0],[0,0,0.1],[0,0,1],parentObjectUniqueId=kuka, parentLinkIndex=6)
p.setRealTimeSimulation(0)
angle=0
while (True):
time.sleep(0.01)
p.resetJointState(kuka,2,angle)
p.resetJointState(kuka,3,angle)
angle+=0.01
p.setAdditionalSearchPath(pybullet_data.getDataPath())
if (clid < 0):
p.connect(p.GUI)
p.loadURDF("plane.urdf", [0, 0, -0.3])
husky = p.loadURDF("husky/husky.urdf", [0.290388, 0.329902, -0.310270],
[0.002328, -0.000984, 0.996491, 0.083659])
for i in range(p.getNumJoints(husky)):
print(p.getJointInfo(husky, i))
kukaId = p.loadURDF("kuka_iiwa/model_free_base.urdf", 0.193749, 0.345564,
0.120208, 0.002327, -0.000988, 0.996491, 0.083659)
ob = kukaId
jointPositions = [
3.559609, 0.411182, 0.862129, 1.744441, 0.077299, -1.129685, 0.006001
]
for jointIndex in range(p.getNumJoints(ob)):
p.resetJointState(ob, jointIndex, jointPositions[jointIndex])
#put kuka on top of husky
cid = p.createConstraint(husky, -1, kukaId, -1, p.JOINT_FIXED, [0, 0, 0],
[0, 0, 0], [0., 0., -.5], [0, 0, 0, 1])
baseorn = p.getQuaternionFromEuler([3.1415, 0, 0.3])
baseorn = [0, 0, 0, 1]
#[0, 0, 0.707, 0.707]
#p.resetBasePositionAndOrientation(kukaId,[0,0,0],baseorn)#[0,0,0,1])
kukaEndEffectorIndex = 6
numJoints = p.getNumJoints(kukaId)
if (numJoints != 7):
exit()
if (useRealTimeSimulation):
dt = datetime.now()
t = (dt.second / 60.) * 2. * math.pi
else:
t = t + 0.01
time.sleep(0.01)
for i in range(1):
pos = [2. * math.cos(t), 2. * math.cos(t), 0. + 2. * math.sin(t)]
jointPoses = p.calculateInverseKinematics(sawyerId,
sawyerEndEffectorIndex,
pos,
jointDamping=jd,
solver=ikSolver,
maxNumIterations=100)
#reset the joint state (ignoring all dynamics, not recommended to use during simulation)
for i in range(numJoints):
jointInfo = p.getJointInfo(sawyerId, i)
qIndex = jointInfo[3]
if qIndex > -1:
p.resetJointState(sawyerId, i, jointPoses[qIndex - 7])
ls = p.getLinkState(sawyerId, sawyerEndEffectorIndex)
if (hasPrevPose):
p.addUserDebugLine(prevPose, pos, [0, 0, 0.3], 1, trailDuration)
p.addUserDebugLine(prevPose1, ls[4], [1, 0, 0], 1, trailDuration)
prevPose = pos
prevPose1 = ls[4]
hasPrevPose = 1
record.append(values[i])
log.append(record)
return log
#clid = p.connect(p.SHARED_MEMORY)
p.connect(p.GUI)
p.loadURDF("plane.urdf",[0,0,-0.3])
p.loadURDF("kuka_iiwa/model.urdf",[0,0,0])
p.loadURDF("cube.urdf",[2,2,5])
p.loadURDF("cube.urdf",[-2,-2,5])
p.loadURDF("cube.urdf",[2,-2,5])
log = readLogFile("LOG0001.txt")
recordNum = len(log)
itemNum = len(log[0])
print('record num:'),
print(recordNum)
print('item num:'),
print(itemNum)
for record in log:
Id = record[1]
pos = [record[2],record[3],record[4]]
orn = [record[5],record[6],record[7],record[8]]
p.resetBasePositionAndOrientation(Id,pos,orn)
numJoints = record[15]
for i in range (numJoints):
p.resetJointState(Id,i,record[i+16])
sleep(0.0005)
import pybullet as p
import time
import math
p.connect(p.SHARED_MEMORY)
p.loadURDF("plane.urdf")
quadruped = p.loadURDF("quadruped/quadruped.urdf",0,0,.3)
#p.getNumJoints(1)
#right front leg
p.resetJointState(quadruped,0,1.57)
p.resetJointState(quadruped,2,-2.2)
p.resetJointState(quadruped,3,-1.57)
p.resetJointState(quadruped,5,2.2)
p.createConstraint(quadruped,2,quadruped,5,3,[0,0,0],[0,0.01,0.2],[0,-0.015,0.2])
p.setJointMotorControl(quadruped,0,p.POSITION_CONTROL,1.57,1)
p.setJointMotorControl(quadruped,1,p.VELOCITY_CONTROL,0,0)
p.setJointMotorControl(quadruped,2,p.VELOCITY_CONTROL,0,0)
p.setJointMotorControl(quadruped,3,p.POSITION_CONTROL,-1.57,1)
p.setJointMotorControl(quadruped,4,p.VELOCITY_CONTROL,0,0)
p.setJointMotorControl(quadruped,5,p.VELOCITY_CONTROL,0,0)
#left front leg
p.resetJointState(quadruped,6,1.57)
p.resetJointState(quadruped,8,-2.2)
p.resetJointState(quadruped,9,-1.57)
p.resetJointState(quadruped,11,2.2)
p.createConstraint(quadruped,8,quadruped,11,3,[0,0,0],[0,-0.01,0.2],[0,0.015,0.2])
p.setJointMotorControl(quadruped,6,p.POSITION_CONTROL,1.57,1)
def test_rnd(param_sigma=.0):
print("Loading model and testing")
sdir = os.path.join(os.path.dirname(os.path.realpath(__file__)),
"legtester.p")
leg_model_nn = LegModelLSTM(n_inputs=6, n_actions=3)
leg_model_nn.load_state_dict(T.load(sdir))
ou = OrnsteinUhlenbeckActionNoise(mu=np.zeros(3),
sigma=0.5,
theta=0.15,
dt=1e-1,
x0=None)
# Set seed
np.random.seed(1337)
n_test_iters = batchsize * 5
# Test
joints = []
acts = []
losses = []
for iter in range(n_test_iters):
max_velocity = 2.0 - np.random.rand() * param_sigma
ou.reset()
correlated_noise = [ou() for i in range(episode_len)]
ep_joints = []
ep_acts = []
# Sample batchsize episodes
[
p.resetJointState(leg,
i,
np.random.randn(),
0,
physicsClientId=client_ID) for i in range(3)
]
for st in range(episode_len):
# Joint
obs = p.getJointStates(leg, range(3), physicsClientId=client_ID
) # pos, vel, reaction(6), prev_torque
joint_angles = []
joint_velocities = []
joint_torques = []
for o in obs:
joint_angles.append(o[0])
joint_velocities.append(o[1])
joint_torques.append(o[3])
joint_angles_normed = scale_joints(joint_angles)
ep_joints.append(joint_angles_normed)
# Action
# act_normed = np.clip(np.random.randn(3), -1, 1)
act_normed = np.clip(correlated_noise[st], -1, 1)
scaled_action = scale_action(act_normed)
ep_acts.append(act_normed)
# Simulate
for i in range(3):
p.setJointMotorControl2(
bodyUniqueId=leg,
jointIndex=i,
controlMode=p.POSITION_CONTROL,
targetPosition=scaled_action[i],
force=max_joint_force,
positionGain=0.1, # 0.1
velocityGain=0.1, # 0.1
maxVelocity=max_velocity, # 2.0
physicsClientId=client_ID)
for i in range(sim_steps_per_iter):
p.stepSimulation(physicsClientId=client_ID)
if (animate) and True: time.sleep(0.0038)
joints.append(ep_joints)
acts.append(ep_acts)
if iter % batchsize == 0 and iter > 0:
# Forward pass
joints_T = T.tensor([j[:-1] for j in joints], dtype=T.float32)
joints_next_T = T.tensor([j[1:] for j in joints], dtype=T.float32)
acts_T = T.tensor([j[:-1] for j in acts], dtype=T.float32)
pred, _ = leg_model_nn(T.cat((joints_T, acts_T), dim=2))
joints = []
acts = []
# update
loss = F.mse_loss(pred, joints_next_T)
losses.append(loss.data)
print("Iter: {}/{}, loss: {}".format(iter, n_test_iters, loss))
print("Mean loss: {}".format(np.mean(losses)))
print("loaded!")
#p.changeDynamics(sphere ,-1, mass=1000)
door = p.loadURDF("door.urdf",[0,0,-11])
p.changeDynamics(door ,1, linearDamping=0, angularDamping=0, jointDamping=0, mass=1)
print("numJoints = ", p.getNumJoints(door))
p.setGravity(0,0,-10)
position_control = True
angle = math.pi*0.25
p.resetJointState(door,1,angle)
angleread = p.getJointState(door,1)
print("angleread = ",angleread)
prevTime = time.time()
angle = math.pi*0.5
count=0
while (1):
count+=1
if (count==12):
p.stopStateLogging(logId)
p.configureDebugVisualizer(p.COV_ENABLE_RENDERING,1)
curTime = time.time()
def loadRobot(self, translation, quaternion, physicsClientId=0):
"""
Overloads @loadRobot from the @RobotVirtual class, loads the robot into
the simulated instance. This method also updates the max velocity of
the robot's fingers, adds self collision filters to the model and
defines the cameras of the model in the camera_dict.
Parameters:
translation - List containing 3 elements, the translation [x, y, z]
of the robot in the WORLD frame
quaternion - List containing 4 elements, the quaternion
[x, y, z, q] of the robot in the WORLD frame
physicsClientId - The id of the simulated instance in which the
robot is supposed to be loaded
"""
pybullet.setAdditionalSearchPath(os.path.dirname(
os.path.realpath(__file__)),
physicsClientId=physicsClientId)
# Add 0.50 meters on the z component, avoing to spawn NAO in the ground
translation = [translation[0], translation[1], translation[2] + 1.05]
RobotVirtual.loadRobot(self,
translation,
quaternion,
physicsClientId=physicsClientId)
balance_constraint = pybullet.createConstraint(
parentBodyUniqueId=self.robot_model,
parentLinkIndex=-1,
childBodyUniqueId=-1,
childLinkIndex=-1,
jointType=pybullet.JOINT_FIXED,
jointAxis=[0, 0, 0],
parentFramePosition=[0, 0, 0],
parentFrameOrientation=[0, 0, 0, 1],
childFramePosition=translation,
childFrameOrientation=quaternion,
physicsClientId=self.physics_client)
self.goToPosture("Stand", 1.0)
pybullet.setCollisionFilterPair(self.robot_model,
self.robot_model,
self.link_dict["REye"].getIndex(),
self.link_dict["LEye"].getIndex(),
0,
physicsClientId=self.physics_client)
for link in ["torso", "HeadRoll_link"]:
pybullet.setCollisionFilterPair(
self.robot_model,
self.robot_model,
self.link_dict["NeckPitch_link"].getIndex(),
self.link_dict[link].getIndex(),
0,
physicsClientId=self.physics_client)
for side in ["R", "L"]:
pybullet.setCollisionFilterPair(
self.robot_model,
self.robot_model,
self.link_dict[side + "Eye"].getIndex(),
self.link_dict["HeadRoll_link"].getIndex(),
0,
physicsClientId=self.physics_client)
pybullet.setCollisionFilterPair(
self.robot_model,
self.robot_model,
self.link_dict[side + "Thigh"].getIndex(),
self.link_dict["body"].getIndex(),
0,
physicsClientId=self.physics_client)
pybullet.setCollisionFilterPair(
self.robot_model,
self.robot_model,
self.link_dict[side.lower() + "_ankle"].getIndex(),
self.link_dict[side + "Tibia"].getIndex(),
0,
physicsClientId=self.physics_client)
pybullet.setCollisionFilterPair(
self.robot_model,
self.robot_model,
self.link_dict[side + "ShoulderYaw_link"].getIndex(),
self.link_dict["torso"].getIndex(),
0,
physicsClientId=self.physics_client)
pybullet.setCollisionFilterPair(
self.robot_model,
self.robot_model,
self.link_dict[side + "WristRoll_link"].getIndex(),
self.link_dict[side.lower() + "_wrist"].getIndex(),
0,
physicsClientId=self.physics_client)
for link in ["ShoulderYaw_link", "WristYaw_link"]:
pybullet.setCollisionFilterPair(
self.robot_model,
self.robot_model,
self.link_dict[side + link].getIndex(),
self.link_dict[side + "Elbow"].getIndex(),
0,
physicsClientId=self.physics_client)
for name, link in self.link_dict.items():
if side + "Finger" in name or\
side + "Thumb" in name:
pybullet.setCollisionFilterPair(
self.robot_model,
self.robot_model,
self.link_dict[side.lower() + "_wrist"].getIndex(),
link.getIndex(),
0,
physicsClientId=self.physics_client)
for joint in self.joint_dict.values():
pybullet.resetJointState(self.robot_model,
joint.getIndex(),
0.0,
physicsClientId=self.physics_client)
pybullet.removeConstraint(balance_constraint,
physicsClientId=self.physics_client)
for joint_name in list(self.joint_dict):
if 'RFinger' in joint_name or 'RThumb' in joint_name:
self.joint_dict[joint_name].setMaxVelocity(
self.joint_dict["RHand"].getMaxVelocity())
elif 'LFinger' in joint_name or 'LThumb' in joint_name:
self.joint_dict[joint_name].setMaxVelocity(
self.joint_dict["LHand"].getMaxVelocity())
camera_right = CameraRgb(
self.robot_model,
RomeoVirtual.ID_CAMERA_RIGHT,
self.link_dict["CameraRightEye_optical_frame"],
hfov=60.9,
vfov=47.6,
physicsClientId=self.physics_client)
camera_left = CameraRgb(self.robot_model,
RomeoVirtual.ID_CAMERA_LEFT,
self.link_dict["CameraLeftEye_optical_frame"],
hfov=60.9,
vfov=47.6,
physicsClientId=self.physics_client)
camera_depth = CameraDepth(self.robot_model,
RomeoVirtual.ID_CAMERA_DEPTH,
self.link_dict["CameraDepth_optical_frame"],
hfov=58.0,
vfov=45.0,
physicsClientId=self.physics_client)
self.camera_dict = {
RomeoVirtual.ID_CAMERA_RIGHT: camera_right,
RomeoVirtual.ID_CAMERA_LEFT: camera_left,
RomeoVirtual.ID_CAMERA_DEPTH: camera_depth
}
# The frequency of the IMU is set to 100Hz
self.imu = Imu(self.robot_model,
self.link_dict["torso"],
100.0,
physicsClientId=self.physics_client)
def applyAction(self, motorCommands):
#print ("self.numJoints")
#print (self.numJoints)
if (self.useInverseKinematics):
dx = motorCommands[0]
dy = motorCommands[1]
dz = motorCommands[2]
da = motorCommands[3]
fingerAngle = motorCommands[4]
state = p.getLinkState(self.kukaUid,self.kukaEndEffectorIndex)
actualEndEffectorPos = state[0]
#print("pos[2] (getLinkState(kukaEndEffectorIndex)")
#print(actualEndEffectorPos[2])
self.endEffectorPos[0] = self.endEffectorPos[0]+dx
if (self.endEffectorPos[0]>0.65):
self.endEffectorPos[0]=0.65
if (self.endEffectorPos[0]<0.50):
self.endEffectorPos[0]=0.50
self.endEffectorPos[1] = self.endEffectorPos[1]+dy
if (self.endEffectorPos[1]<-0.17):
self.endEffectorPos[1]=-0.17
if (self.endEffectorPos[1]>0.22):
self.endEffectorPos[1]=0.22
#print ("self.endEffectorPos[2]")
#print (self.endEffectorPos[2])
#print("actualEndEffectorPos[2]")
#print(actualEndEffectorPos[2])
#if (dz<0 or actualEndEffectorPos[2]<0.5):
self.endEffectorPos[2] = self.endEffectorPos[2]+dz
self.endEffectorAngle = self.endEffectorAngle + da
pos = self.endEffectorPos
orn = p.getQuaternionFromEuler([0,-math.pi,0]) # -math.pi,yaw])
if (self.useNullSpace==1):
if (self.useOrientation==1):
jointPoses = p.calculateInverseKinematics(self.kukaUid,self.kukaEndEffectorIndex,pos,orn,self.ll,self.ul,self.jr,self.rp)
else:
jointPoses = p.calculateInverseKinematics(self.kukaUid,self.kukaEndEffectorIndex,pos,lowerLimits=self.ll, upperLimits=self.ul, jointRanges=self.jr, restPoses=self.rp)
else:
if (self.useOrientation==1):
jointPoses = p.calculateInverseKinematics(self.kukaUid,self.kukaEndEffectorIndex,pos,orn,jointDamping=self.jd)
else:
jointPoses = p.calculateInverseKinematics(self.kukaUid,self.kukaEndEffectorIndex,pos)
#print("jointPoses")
#print(jointPoses)
#print("self.kukaEndEffectorIndex")
#print(self.kukaEndEffectorIndex)
if (self.useSimulation):
for i in range (self.kukaEndEffectorIndex+1):
#print(i)
p.setJointMotorControl2(bodyUniqueId=self.kukaUid,jointIndex=i,controlMode=p.POSITION_CONTROL,targetPosition=jointPoses[i],targetVelocity=0,force=self.maxForce,maxVelocity=self.maxVelocity, positionGain=0.3,velocityGain=1)
else:
#reset the joint state (ignoring all dynamics, not recommended to use during simulation)
for i in range (self.numJoints):
p.resetJointState(self.kukaUid,i,jointPoses[i])
#fingers
p.setJointMotorControl2(self.kukaUid,7,p.POSITION_CONTROL,targetPosition=self.endEffectorAngle,force=self.maxForce)
p.setJointMotorControl2(self.kukaUid,8,p.POSITION_CONTROL,targetPosition=-fingerAngle,force=self.fingerAForce)
p.setJointMotorControl2(self.kukaUid,11,p.POSITION_CONTROL,targetPosition=fingerAngle,force=self.fingerBForce)
p.setJointMotorControl2(self.kukaUid,10,p.POSITION_CONTROL,targetPosition=0,force=self.fingerTipForce)
p.setJointMotorControl2(self.kukaUid,13,p.POSITION_CONTROL,targetPosition=0,force=self.fingerTipForce)
else:
for action in range (len(motorCommands)):
motor = self.motorIndices[action]
p.setJointMotorControl2(self.kukaUid,motor,p.POSITION_CONTROL,targetPosition=motorCommands[action],force=self.maxForce)
def reset_position(self, position, velocity):
p.resetJointState(self.bodies[self.bodyIndex],
self.jointIndex,
targetValue=position,
targetVelocity=velocity)
self.disable_motor()
objects = [
p.loadURDF("plane.urdf", 0.000000, 0.000000, -.300000, 0.000000, 0.000000, 0.000000, 1.000000)
]
objects = [
p.loadURDF("quadruped/minitaur.urdf", [-0.000046, -0.000068, 0.200774],
[-0.000701, 0.000387, -0.000252, 1.000000],
useFixedBase=False)
]
ob = objects[0]
jointPositions = [
0.000000, 1.531256, 0.000000, -2.240112, 1.527979, 0.000000, -2.240646, 1.533105, 0.000000,
-2.238254, 1.530335, 0.000000, -2.238298, 0.000000, -1.528038, 0.000000, 2.242656, -1.525193,
0.000000, 2.244008, -1.530011, 0.000000, 2.240683, -1.528687, 0.000000, 2.240517
]
for ji in range(p.getNumJoints(ob)):
p.resetJointState(ob, ji, jointPositions[ji])
p.setJointMotorControl2(bodyIndex=ob, jointIndex=ji, controlMode=p.VELOCITY_CONTROL, force=0)
cid0 = p.createConstraint(1, 3, 1, 6, p.JOINT_POINT2POINT, [0.000000, 0.000000, 0.000000],
[0.000000, 0.005000, 0.200000], [0.000000, 0.010000, 0.200000],
[0.000000, 0.000000, 0.000000, 1.000000],
[0.000000, 0.000000, 0.000000, 1.000000])
p.changeConstraint(cid0, maxForce=500.000000)
cid1 = p.createConstraint(1, 16, 1, 19, p.JOINT_POINT2POINT, [0.000000, 0.000000, 0.000000],
[0.000000, 0.005000, 0.200000], [0.000000, 0.010000, 0.200000],
[0.000000, 0.000000, 0.000000, 1.000000],
[0.000000, 0.000000, 0.000000, 1.000000])
p.changeConstraint(cid1, maxForce=500.000000)
cid2 = p.createConstraint(1, 9, 1, 12, p.JOINT_POINT2POINT, [0.000000, 0.000000, 0.000000],
[0.000000, 0.005000, 0.200000], [0.000000, 0.010000, 0.200000],
[0.000000, 0.000000, 0.000000, 1.000000],
import pybullet as p
import pybullet_data
import utils
useMaximalCoordinates = False
p.connect(p.GUI)
p.setAdditionalSearchPath(pybullet_data.getDataPath())
pole = p.loadURDF("cartpole.urdf", [0, 0, 0], useMaximalCoordinates=useMaximalCoordinates)
for i in range(2):
p.setJointMotorControl2(pole, i, p.POSITION_CONTROL, targetPosition=0, force=500)
for i in range(2):
p.resetJointState(pole, i, 0.3, targetVelocity=0.3)
timeStep = 0.0000001
p.setTimeStep(timeStep)
while p.isConnected():
position, velocity, _, _ = p.getJointState(pole, 0)
print(velocity)
p.stepSimulation()
rightLegAngles= ik.rl_com_from_foot(com_0 , orientation)
leftLegAngles = ik.ll_com_from_foot(com_0 , orientation)
for i, index in enumerate(rightLegIndices):
if (not np.isnan(rightLegAngles[i])) and (rightLegAngles[i] > p.getJointInfo(teoId, index)[8] and rightLegAngles[i] < p.getJointInfo(teoId, index)[9]):
targetPos[index]=rightLegAngles[i]
for i, index in enumerate(leftLegIndices):
if (not np.isnan(leftLegAngles[i])) and ( leftLegAngles[i] > p.getJointInfo(teoId, index)[8] and leftLegAngles[i] < p.getJointInfo(teoId, index)[9]):
targetPos[index]=leftLegAngles[i]
for jointIndex in range(n_joints):
p.resetJointState(teoId,
jointIndex,
targetPos[jointIndex])
mode = p.POSITION_CONTROL
p.setJointMotorControlArray(teoId,
jointIndices,
controlMode=mode,
forces=maxForces,
targetPositions = targetPos
)
offset_torque = maxForces[name2id[b'l_hip_roll']]
max_offset = radians(1.2581265067842706)
min_offset = radians(0.7139893352971366)
print("max_offset:", max_offset)
cid = p.connect(p.SHARED_MEMORY)
if (cid<0):
p.connect(p.GUI)
p.resetSimulation()
#disable rendering during loading makes it much faster
p.configureDebugVisualizer(p.COV_ENABLE_RENDERING, 0)
objects = [p.loadURDF("plane.urdf", 0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,1.000000)]
objects = [p.loadURDF("samurai.urdf", 0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,1.000000)]
objects = [p.loadURDF("pr2_gripper.urdf", 0.500000,0.300006,0.700000,-0.000000,-0.000000,-0.000031,1.000000)]
pr2_gripper = objects[0]
print ("pr2_gripper=")
print (pr2_gripper)
jointPositions=[ 0.550569, 0.000000, 0.549657, 0.000000 ]
for jointIndex in range (p.getNumJoints(pr2_gripper)):
p.resetJointState(pr2_gripper,jointIndex,jointPositions[jointIndex])
pr2_cid = p.createConstraint(pr2_gripper,-1,-1,-1,p.JOINT_FIXED,[0,0,0],[0.2,0,0],[0.500000,0.300006,0.700000])
print ("pr2_cid")
print (pr2_cid)
objects = [p.loadURDF("kuka_iiwa/model_vr_limits.urdf", 1.400000,-0.200000,0.600000,0.000000,0.000000,0.000000,1.000000)]
kuka = objects[0]
jointPositions=[ -0.000000, -0.000000, 0.000000, 1.570793, 0.000000, -1.036725, 0.000001 ]
for jointIndex in range (p.getNumJoints(kuka)):
p.resetJointState(kuka,jointIndex,jointPositions[jointIndex])
p.setJointMotorControl2(kuka,jointIndex,p.POSITION_CONTROL,jointPositions[jointIndex],0)
objects = [p.loadURDF("lego/lego.urdf", 1.000000,-0.200000,0.700000,0.000000,0.000000,0.000000,1.000000)]
objects = [p.loadURDF("lego/lego.urdf", 1.000000,-0.200000,0.800000,0.000000,0.000000,0.000000,1.000000)]
objects = [p.loadURDF("lego/lego.urdf", 1.000000,-0.200000,0.900000,0.000000,0.000000,0.000000,1.000000)]
import pybullet as p
import time
p.connect(p.GUI)
p.loadURDF("plane.urdf", [0, 0, -0.25])
minitaur = p.loadURDF("quadruped/minitaur_single_motor.urdf", useFixedBase=True)
print(p.getNumJoints(minitaur))
p.resetDebugVisualizerCamera(cameraDistance=1,
cameraYaw=23.2,
cameraPitch=-6.6,
cameraTargetPosition=[-0.064, .621, -0.2])
motorJointId = 1
p.setJointMotorControl2(minitaur, motorJointId, p.VELOCITY_CONTROL, targetVelocity=100000, force=0)
p.resetJointState(minitaur, motorJointId, targetValue=0, targetVelocity=1)
angularDampingSlider = p.addUserDebugParameter("angularDamping", 0, 1, 0)
jointFrictionForceSlider = p.addUserDebugParameter("jointFrictionForce", 0, 0.1, 0)
textId = p.addUserDebugText("jointVelocity=0", [0, 0, -0.2])
p.setRealTimeSimulation(1)
while (1):
frictionForce = p.readUserDebugParameter(jointFrictionForceSlider)
angularDamping = p.readUserDebugParameter(angularDampingSlider)
p.setJointMotorControl2(minitaur,
motorJointId,
p.VELOCITY_CONTROL,
targetVelocity=0,
force=frictionForce)
p.changeDynamics(minitaur, motorJointId, linearDamping=0, angularDamping=angularDamping)
time.sleep(0.01)
applyMMMRotationToURDFJoint(human_adult_ID,
1,
JProcessed[t, 3],
JProcessed[t, 4],
JProcessed[t, 5],
inverse=True)
# pelvis to right leg
applyMMMRotationToURDFJoint(human_adult_ID, 2, JProcessed[t, 33],
JProcessed[t, 34], JProcessed[t, 35])
# pelvis to left leg
applyMMMRotationToURDFJoint(human_adult_ID, 3, JProcessed[t, 17],
JProcessed[t, 18], JProcessed[t, 19])
# right leg to right shin
p.resetJointState(human_adult_ID, 4, -JProcessed[t, 36])
# left leg to left shin
p.resetJointState(human_adult_ID, 5, -JProcessed[t, 20])
# right shin to right foot
applyMMMRotationToURDFJoint(human_adult_ID, 6, JProcessed[t, 28],
JProcessed[t, 29], JProcessed[t, 30])
# left shin to left foot
applyMMMRotationToURDFJoint(human_adult_ID, 7, JProcessed[t, 12],
JProcessed[t, 13], JProcessed[t, 14])
# chest_to_right_arm
applyMMMRotationToURDFJoint(human_adult_ID, 8, JProcessed[t, 37],
JProcessed[t, 38], JProcessed[t, 39])
# chest_to_left_arm
import pybullet as p
import time
p.connect(p.DIRECT)
p.setGravity(0,0,-10)
p.setPhysicsEngineParameter(numSolverIterations=5)
p.setPhysicsEngineParameter(fixedTimeStep=1./240.)
p.setPhysicsEngineParameter(numSubSteps=1)
objects = p.loadMJCF("mjcf/humanoid_symmetric.xml")
ob = objects[0]
p.resetBasePositionAndOrientation(ob,[0.000000,0.000000,0.000000],[0.000000,0.000000,0.000000,1.000000])
ob = objects[1]
p.resetBasePositionAndOrientation(ob,[0.789351,0.962124,0.113124],[0.710965,0.218117,0.519402,-0.420923])
jointPositions=[ -0.200226, 0.123925, 0.000000, -0.224016, 0.000000, -0.022247, 0.099119, -0.041829, 0.000000, -0.344372, 0.000000, 0.000000, 0.090687, -0.578698, 0.044461, 0.000000, -0.185004, 0.000000, 0.000000, 0.039517, -0.131217, 0.000000, 0.083382, 0.000000, -0.165303, -0.140802, 0.000000, -0.007374, 0.000000 ]
for jointIndex in range (p.getNumJoints(ob)):
p.resetJointState(ob,jointIndex,jointPositions[jointIndex])
#first let the humanoid fall
#p.setRealTimeSimulation(1)
#time.sleep(5)
p.setRealTimeSimulation(0)
#p.saveWorld("lyiing.py")
#now do a benchmark
print("Starting benchmark")
logId = p.startStateLogging(p.STATE_LOGGING_PROFILE_TIMINGS,"pybullet_humanoid_timings.json")
for i in range(1000):
p.stepSimulation()
p.stopStateLogging(logId)
def reset_sim(com_0 = np.array([0,0,l9+l10+l11+l12-0.001]), orientation=np.array([0,0,0]), g=-9.79983):
p.resetSimulation()
p.setTimeStep(timestep)
p.setGravity(0,0,g)
planeId = p.loadURDF("plane.urdf")
teoStartOrientation = p.getQuaternionFromEuler(-orientation)
teoStartPosition = [0,0,com_0[2]]
urdf_path = os.path.join(urdf_root,"TEO_wobble.urdf")
teoId = p.loadURDF(urdf_path,teoStartPosition ,teoStartOrientation)
#print("TEO pos =",p.getBasePositionAndOrientation(teoId)[0])
n_joints = p.getNumJoints(teoId)
name2id={}
for i in range(n_joints):
name2id[p.getJointInfo(teoId,i)[1]]=i
n_joints = p.getNumJoints(teoId)
jointIndices = [0] * n_joints
targetPos = [0.0] * n_joints
maxVelocities = [radians(1)] * n_joints
maxForces = [0.0] * n_joints
for i in range(n_joints):
jointIndices[i] = i
maxForces[i] = p.getJointInfo(teoId,i)[10]
targetPos[i] = 0
rightLegAngles= ik.rl_com_from_foot(com_0 , orientation)
leftLegAngles = ik.ll_com_from_foot(com_0 , orientation)
'''
print("com =",com_0)
print("com_f =",com_0+np.array([0,l16,-l12]))
print()
print("Hst0_r =",[0,-l16,l9+l10+l11+l12])
print("rightLegP(com_0) =",rightLegP(com_0))
print("rightLegAngles =",[degrees(q) for q in rightLegAngles])
print()
print("Hst0_l =",[0,l16,l9+l10+l11+l12])
print("leftLegP(com_0) =",leftLegP(com_0))
print("leftLegAngles =",[degrees(q) for q in leftLegAngles])
'''
for i, index in enumerate(rightLegIndices):
if (not np.isnan(rightLegAngles[i])) and (rightLegAngles[i] > p.getJointInfo(teoId, index)[8] and rightLegAngles[i] < p.getJointInfo(teoId, index)[9]):
targetPos[index]=rightLegAngles[i]
for i, index in enumerate(leftLegIndices):
if (not np.isnan(leftLegAngles[i])) and ( leftLegAngles[i] > p.getJointInfo(teoId, index)[8] and leftLegAngles[i] < p.getJointInfo(teoId, index)[9]):
targetPos[index]=leftLegAngles[i]
for jointIndex in range(n_joints):
p.resetJointState(teoId,
jointIndex,
targetPos[jointIndex])
mode = p.POSITION_CONTROL
p.setJointMotorControlArray(teoId,
jointIndices,
controlMode=mode,
forces=maxForces,
targetPositions = targetPos
)
return teoId
return log
#clid = p.connect(p.SHARED_MEMORY)
p.connect(p.GUI)
p.loadURDF("plane.urdf",[0,0,-0.3])
p.loadURDF("kuka_iiwa/model.urdf",[0,0,0])
p.loadURDF("cube.urdf",[2,2,5])
p.loadURDF("cube.urdf",[-2,-2,5])
p.loadURDF("cube.urdf",[2,-2,5])
log = readLogFile("LOG0001.txt")
recordNum = len(log)
itemNum = len(log[0])
print('record num:'),
print(recordNum)
print('item num:'),
print(itemNum)
for record in log:
Id = record[2]
pos = [record[3],record[4],record[5]]
orn = [record[6],record[7],record[8],record[9]]
p.resetBasePositionAndOrientation(Id,pos,orn)
numJoints = p.getNumJoints(Id)
for i in range (numJoints):
jointInfo = p.getJointInfo(Id,i)
qIndex = jointInfo[3]
if qIndex > -1:
p.resetJointState(Id,i,record[qIndex-7+17])
sleep(0.0005)
