def getMotorJointStates(robot): joint_states = p.getJointStates(robot, range(p.getNumJoints(robot))) joint_infos = [p.getJointInfo(robot, i) for i in range(p.getNumJoints(robot))] joint_states = [j for j, i in zip(joint_states, joint_infos) if i[3] > -1] joint_positions = [state[0] for state in joint_states] joint_velocities = [state[1] for state in joint_states] joint_torques = [state[3] for state in joint_states] return joint_positions, joint_velocities, joint_torques
def addToScene(self, bodies):
if self.parts is not None:
parts = self.parts
else:
parts = {}
if self.jdict is not None:
joints = self.jdict
else:
joints = {}
if self.ordered_joints is not None:
ordered_joints = self.ordered_joints
else:
ordered_joints = []
dump = 0
for i in range(len(bodies)):
if p.getNumJoints(bodies[i]) == 0:
part_name, robot_name = p.getBodyInfo(bodies[i], 0)
robot_name = robot_name.decode("utf8")
part_name = part_name.decode("utf8")
parts[part_name] = BodyPart(part_name, bodies, i, -1)
for j in range(p.getNumJoints(bodies[i])):
p.setJointMotorControl2(bodies[i],j,p.POSITION_CONTROL,positionGain=0.1,velocityGain=0.1,force=0)
_,joint_name,_,_,_,_,_,_,_,_,_,_,part_name = p.getJointInfo(bodies[i], j)
joint_name = joint_name.decode("utf8")
part_name = part_name.decode("utf8")
if dump: print("ROBOT PART '%s'" % part_name)
if dump: print("ROBOT JOINT '%s'" % joint_name) # limits = %+0.2f..%+0.2f effort=%0.3f speed=%0.3f" % ((joint_name,) + j.limits()) )
parts[part_name] = BodyPart(part_name, bodies, i, j)
if part_name == self.robot_name:
self.robot_body = parts[part_name]
if i == 0 and j == 0 and self.robot_body is None: # if nothing else works, we take this as robot_body
parts[self.robot_name] = BodyPart(self.robot_name, bodies, 0, -1)
self.robot_body = parts[self.robot_name]
if joint_name[:6] == "ignore":
Joint(joint_name, bodies, i, j).disable_motor()
continue
if joint_name[:8] != "jointfix":
joints[joint_name] = Joint(joint_name, bodies, i, j)
ordered_joints.append(joints[joint_name])
joints[joint_name].power_coef = 100.0
return parts, joints, ordered_joints, self.robot_body
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()
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 setJointPosition(self, robot, position, kp=1.0, kv=0.3):
import pybullet as p
num_joints = p.getNumJoints(robot)
zero_vec = [0.0] * num_joints
if len(position) == num_joints:
p.setJointMotorControlArray(robot, range(num_joints), p.POSITION_CONTROL,
targetPositions=position, targetVelocities=zero_vec,
positionGains=[kp] * num_joints, velocityGains=[kv] * num_joints)
def initializeFromBulletBody(self, bodyUid, physicsClientId):
self.initialize()
#always create a base link
baseLink = UrdfLink()
baseLinkIndex = -1
self.convertLinkFromMultiBody(bodyUid, baseLinkIndex, baseLink, physicsClientId)
baseLink.link_name = p.getBodyInfo(bodyUid, physicsClientId=physicsClientId)[0].decode("utf-8")
self.linkNameToIndex[baseLink.link_name]=len(self.urdfLinks)
self.urdfLinks.append(baseLink)
#print(visualShapes)
#optionally create child links and joints
for j in range(p.getNumJoints(bodyUid,physicsClientId=physicsClientId)):
jointInfo = p.getJointInfo(bodyUid,j,physicsClientId=physicsClientId)
urdfLink = UrdfLink()
self.convertLinkFromMultiBody(bodyUid, j, urdfLink,physicsClientId)
urdfLink.link_name = jointInfo[12].decode("utf-8")
self.linkNameToIndex[urdfLink.link_name]=len(self.urdfLinks)
self.urdfLinks.append(urdfLink)
urdfJoint = UrdfJoint()
urdfJoint.link = urdfLink
urdfJoint.joint_name = jointInfo[1].decode("utf-8")
urdfJoint.joint_type = jointInfo[2]
urdfJoint.joint_axis_xyz = jointInfo[13]
orgParentIndex = jointInfo[16]
if (orgParentIndex<0):
urdfJoint.parent_name = baseLink.link_name
else:
parentJointInfo = p.getJointInfo(bodyUid,orgParentIndex,physicsClientId=physicsClientId)
urdfJoint.parent_name = parentJointInfo[12].decode("utf-8")
urdfJoint.child_name = urdfLink.link_name
#todo, compensate for inertia/link frame offset
dynChild = p.getDynamicsInfo(bodyUid,j,physicsClientId=physicsClientId)
childInertiaPos = dynChild[3]
childInertiaOrn = dynChild[4]
parentCom2JointPos=jointInfo[14]
parentCom2JointOrn=jointInfo[15]
tmpPos,tmpOrn = p.multiplyTransforms(childInertiaPos,childInertiaOrn,parentCom2JointPos,parentCom2JointOrn)
tmpPosInv,tmpOrnInv = p.invertTransform(tmpPos,tmpOrn)
dynParent = p.getDynamicsInfo(bodyUid,orgParentIndex,physicsClientId=physicsClientId)
parentInertiaPos = dynParent[3]
parentInertiaOrn = dynParent[4]
pos,orn = p.multiplyTransforms(parentInertiaPos,parentInertiaOrn, tmpPosInv, tmpOrnInv)
pos,orn_unused=p.multiplyTransforms(parentInertiaPos,parentInertiaOrn, parentCom2JointPos,[0,0,0,1])
urdfJoint.joint_origin_xyz = pos
urdfJoint.joint_origin_rpy = p.getEulerFromQuaternion(orn)
self.urdfJoints.append(urdfJoint)
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 setJointPosition(robot, position, kp=1.0, kv=0.3):
num_joints = p.getNumJoints(robot)
zero_vec = [0.0] * num_joints
if len(position) == num_joints:
p.setJointMotorControlArray(robot, range(num_joints), p.POSITION_CONTROL,
targetPositions=position, targetVelocities=zero_vec,
positionGains=[kp] * num_joints, velocityGains=[kv] * num_joints)
else:
print("Not setting torque. "
"Expected torque vector of "
"length {}, got {}".format(num_joints, len(torque)))
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 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 episode_restart(self): Scene.episode_restart(self) # contains cpp_world.clean_everything() if (self.stadiumLoaded==0): self.stadiumLoaded=1 # stadium_pose = cpp_household.Pose() # if self.zero_at_running_strip_start_line: # stadium_pose.set_xyz(27, 21, 0) # see RUN_STARTLINE, RUN_RAD constants filename = os.path.join(pybullet_data.getDataPath(),"stadium_no_collision.sdf") self.ground_plane_mjcf = p.loadSDF(filename) for i in self.ground_plane_mjcf: p.changeDynamics(i,-1,lateralFriction=0.8, restitution=0.5) for j in range(p.getNumJoints(i)): p.changeDynamics(i,j,lateralFriction=0)
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 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 reset(self):
self.initial_z = None
objs = p.loadMJCF(os.path.join(self.urdfRootPath,"mjcf/humanoid_symmetric_no_ground.xml"),flags = p.URDF_USE_SELF_COLLISION_EXCLUDE_ALL_PARENTS)
self.human = objs[0]
self.jdict = {}
self.ordered_joints = []
self.ordered_joint_indices = []
for j in range( p.getNumJoints(self.human) ):
info = p.getJointInfo(self.human, j)
link_name = info[12].decode("ascii")
if link_name=="left_foot": self.left_foot = j
if link_name=="right_foot": self.right_foot = j
self.ordered_joint_indices.append(j)
if info[2] != p.JOINT_REVOLUTE: continue
jname = info[1].decode("ascii")
self.jdict[jname] = j
lower, upper = (info[8], info[9])
self.ordered_joints.append( (j, lower, upper) )
p.setJointMotorControl2(self.human, j, controlMode=p.VELOCITY_CONTROL, force=0)
self.motor_names = ["abdomen_z", "abdomen_y", "abdomen_x"]
self.motor_power = [100, 100, 100]
self.motor_names += ["right_hip_x", "right_hip_z", "right_hip_y", "right_knee"]
self.motor_power += [100, 100, 300, 200]
self.motor_names += ["left_hip_x", "left_hip_z", "left_hip_y", "left_knee"]
self.motor_power += [100, 100, 300, 200]
self.motor_names += ["right_shoulder1", "right_shoulder2", "right_elbow"]
self.motor_power += [75, 75, 75]
self.motor_names += ["left_shoulder1", "left_shoulder2", "left_elbow"]
self.motor_power += [75, 75, 75]
self.motors = [self.jdict[n] for n in self.motor_names]
print("self.motors")
print(self.motors)
print("num motors")
print(len(self.motors))
def getJointStates(robot): joint_states = p.getJointStates(robot, range(p.getNumJoints(robot))) joint_positions = [state[0] for state in joint_states] joint_velocities = [state[1] for state in joint_states] joint_torques = [state[3] for state in joint_states] return joint_positions, joint_velocities, joint_torques
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)
#for _ in range (10):
p.stepSimulation()
if i % 10 == 0:
pos, names = check_joints(goalJoints)
for pi, px in enumerate(pos):
stats[pi].append(px)
print("matplotlib")
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')
def setup_simulation(self, gui=False, easy_bookcases=False, clientId=None):
'''
Initializes the simulation by setting up the environment and spawning
all objects used later.
Params:
gui (bool): Specifies if a GUI should be spawned.
'''
# Setup simulation parameters
if clientId is None:
mode = pb.GUI if gui else pb.DIRECT
self.clientId = pb.connect(mode)
else:
self.clientId = clientId
pb.setGravity(0.0, 0.0, 0.0, self.clientId)
pb.setPhysicsEngineParameter(fixedTimeStep=self.p["world"]["tau"],
physicsClientId=self.clientId)
pb.setAdditionalSearchPath(pybullet_data.getDataPath())
pb.configureDebugVisualizer(pb.COV_ENABLE_GUI, 0)
pb.configureDebugVisualizer(pb.COV_ENABLE_SEGMENTATION_MARK_PREVIEW, 0)
pb.configureDebugVisualizer(pb.COV_ENABLE_DEPTH_BUFFER_PREVIEW, 0)
pb.configureDebugVisualizer(pb.COV_ENABLE_RGB_BUFFER_PREVIEW, 0)
pb.setPhysicsEngineParameter(enableFileCaching=0)
# Setup humans
self.humans = [
Human(self.clientId, self.tau)
for _ in range(self.p['world']['n_humans'])
]
# Spawn robot
self.robotId = self.spawn_robot()
# Spawn setpoint
self.spId = self.spawn_setpoint()
# Spawn all objects in the environment
self.additionalIds = self.spawn_additional_objects()
# Enable collision of base and all objects
# for id in self.additionalIds:
# pb.setCollisionFilterPair(self.robotId, id, -1, -1, True,
# self.clientId)
# Spawn bookcases
self.spawn_kallax()
# Figure out joint mapping: self.joint_mapping maps as in
# desired_mapping list.
self.joint_mapping = np.zeros(7, dtype=int)
self.link_mapping = np.zeros(self.n_links, dtype=int)
self.joint_limits = np.zeros((7, 2), dtype=float)
self.eeLinkId = None
self.baseLinkId = None
self.lidarLinkId1 = None
self.lidarLinkId2 = None
joint_names = ["panda_joint{}".format(x) for x in range(1, 8)]
link_names = self.p["joints"]["link_names"]
for j in range(
pb.getNumJoints(self.robotId, physicsClientId=self.clientId)):
info = pb.getJointInfo(self.robotId,
j,
physicsClientId=self.clientId)
j_name, l_name = info[1].decode("utf-8"), info[12].decode("utf-8")
idx = info[0]
if j_name in joint_names:
map_idx = joint_names.index(j_name)
self.joint_mapping[map_idx] = idx
self.joint_limits[map_idx, :] = info[8:10]
if l_name in link_names:
self.link_mapping[link_names.index(l_name)] = idx
if l_name == self.p["joints"]["ee_link_name"]:
self.eeLinkId = idx
if l_name == self.p["joints"]["base_link_name"]:
self.baseLinkId = idx
if l_name == self.p["sensors"]["lidar"]["link_id1"]:
self.lidarLinkId1 = idx
if l_name == self.p["sensors"]["lidar"]["link_id2"]:
self.lidarLinkId2 = idx
for j in range(
pb.getNumJoints(self.spId, physicsClientId=self.clientId)):
info = pb.getJointInfo(self.spId, j, physicsClientId=self.clientId)
link_name = info[12].decode("utf-8")
idx = info[0]
if link_name == "grasp_loc":
self.spGraspLinkId = idx
self.actuator_selection = np.zeros(7, bool)
for i, name in enumerate(joint_names):
if name in self.p["joints"]["joint_names"]:
self.actuator_selection[i] = 1
# Prepare lidar
n_scans = self.p["sensors"]["lidar"]["n_scans"]
mag_ang = self.p["sensors"]["lidar"]["ang_mag"]
scan_range = self.p["sensors"]["lidar"]["range"]
angs = ((np.array(range(n_scans)) - (n_scans - 1) / 2.0) * 2.0 /
n_scans * mag_ang)
r_uv = np.vstack((np.cos(angs), np.sin(angs), np.zeros(angs.shape[0])))
r_from = r_uv * 0.1
r_to = r_uv * scan_range
self.rays = (r_from, r_to)
for human in self.humans:
self.configure_ext_collisions(human.leg_l, self.robotId,
self.collision_links)
self.configure_ext_collisions(human.leg_r, self.robotId,
self.collision_links)
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)]
def move_dir(bodyID, direction_matrix):
ee_position = p.getLinkState(bodyID, p.getNumJoints(bodyID) - 1)[4]
new_ee_position = [ee_position[0] + direction_matrix[0], ee_position[1] + direction_matrix[1], ee_position[2] + direction_matrix[2]]
move_to_pos(bodyID, new_ee_position)
p.stepSimulation()
linkMasses=link_Masses,
linkCollisionShapeIndices=linkCollisionShapeIndices,
linkVisualShapeIndices=linkVisualShapeIndices,
linkPositions=linkPositions,
linkOrientations=linkOrientations,
linkInertialFramePositions=linkInertialFramePositions,
linkInertialFrameOrientations=linkInertialFrameOrientations,
linkParentIndices=indices,
linkJointTypes=jointTypes,
linkJointAxis=axis)
p.changeDynamics(boxId,
-1,
spinningFriction=0.001,
rollingFriction=0.001,
linearDamping=0.0)
print(p.getNumJoints(boxId))
for joint in range(p.getNumJoints(boxId)):
targetVelocity = 1
if (i % 2):
targetVelocity = -1
p.setJointMotorControl2(boxId,
joint,
p.VELOCITY_CONTROL,
targetVelocity=targetVelocity,
force=10)
segmentStart = segmentStart - 1.1
p.configureDebugVisualizer(p.COV_ENABLE_RENDERING, 1)
while (1):
camData = p.getDebugVisualizerCamera()
viewMat = camData[2]
import time
p.connect(p.GUI)
p.setAdditionalSearchPath(pd.getDataPath())
plane = p.loadURDF("plane.urdf")
p.setGravity(0,0,-9.8)
p.setTimeStep(1./500)
#p.setDefaultContactERP(0)
#urdfFlags = p.URDF_USE_SELF_COLLISION+p.URDF_USE_SELF_COLLISION_EXCLUDE_ALL_PARENTS
urdfFlags = p.URDF_USE_SELF_COLLISION
quadruped = p.loadURDF("laikago/laikago.urdf",[0,0,.5],[0,0.5,0.5,0], flags = urdfFlags,useFixedBase=False)
#enable collision between lower legs
for j in range (p.getNumJoints(quadruped)):
print(p.getJointInfo(quadruped,j))
#2,5,8 and 11 are the lower legs
lower_legs = [2,5,8,11]
for l0 in lower_legs:
for l1 in lower_legs:
if (l1>l0):
enableCollision = 1
print("collision for pair",l0,l1, p.getJointInfo(quadruped,l0)[12],p.getJointInfo(quadruped,l1)[12], "enabled=",enableCollision)
p.setCollisionFilterPair(quadruped, quadruped, 2,5,enableCollision)
jointIds=[]
paramIds=[]
jointOffsets=[]
jointDirections=[-1,1,1,1,1,1,-1,1,1,1,1,1]
import time
# class Dog:
p.connect(p.GUI)
plane = p.loadURDF("plane.urdf")
p.setGravity(0, 0, -9.8)
p.setTimeStep(1. / 500)
#p.setDefaultContactERP(0)
#urdfFlags = p.URDF_USE_SELF_COLLISION+p.URDF_USE_SELF_COLLISION_EXCLUDE_ALL_PARENTS
urdfFlags = p.URDF_USE_SELF_COLLISION
quadruped = p.loadURDF("aliengo/urdf/aliengo.urdf", [0, 0, 0.48], [0, 0, 0, 1],
flags=urdfFlags,
useFixedBase=False)
#enable collision between lower legs
for j in range(p.getNumJoints(quadruped)):
print(p.getJointInfo(quadruped, j))
lower_legs = [2, 5, 8, 11]
for l0 in lower_legs:
for l1 in lower_legs:
if (l1 > l0):
enableCollision = 1
print("collision for pair", l0, l1,
p.getJointInfo(quadruped, l0)[12],
p.getJointInfo(quadruped, l1)[12], "enabled=",
enableCollision)
p.setCollisionFilterPair(quadruped, quadruped, 2, 5,
enableCollision)
jointIds = []
# This next part looks for XACRO files with the given name and then force-recompiles it to URDF
# ("force" because if there is already a URDF file with the right name, it usually doesn't recompile - this is just for development)
xml_path = get_scene("ergojr-sword")
robot_file = URDF(xml_path, force_recompile=True).get_path()
# Actually load the URDF file into simulation, make the base of the robot unmoving
robot = p.loadURDF(robot_file, startPos, startOrientation, useFixedBase=1)
# Query all joints and print their infos. A robot URDF file consists of joints (moving parts) and links (visible parts).
# If you have a robot arm, it's usually base (link) -> joint1 -> link1 -> joint2 -> link2 -> joint3 -> link3 -> etc.
# But if you have a car for example, it can be
# base (link) -> joint -> chassis (link)
# -> joint_wheel_left -> wheel_left (link)
# -> joint_wheel_right -> wheel_right (link)
# (i.e. it's an acyclical graph; a link can have many joints attached)
for i in range(p.getNumJoints(robot)):
print(p.getJointInfo(robot, i))
# Indices of all the motors we can actually move. This information comes from the print statemtnt above.
motors = [3, 4, 6, 8, 10, 12]
# Container for debug inputs
debugParams = []
# In the user interface, create a slider for each motor to control them separately.
for i in range(len(motors)):
motor = p.addUserDebugParameter("motor{}".format(i + 1), -1, 1, 0)
debugParams.append(motor)
start = time.time()
import pybullet as p
p.connect(p.GUI)
r2d2 = p.loadURDF("r2d2.urdf", [0, 0, 1])
for l in range(p.getNumJoints(r2d2)):
print(p.getJointInfo(r2d2, l))
p.loadURDF("r2d2.urdf", [2, 0, 1])
p.loadURDF("r2d2.urdf", [4, 0, 1])
p.getCameraImage(320, 200, flags=p.ER_SEGMENTATION_MASK_OBJECT_AND_LINKINDEX)
segLinkIndex = 1
verbose = 0
while (1):
keys = p.getKeyboardEvents()
#for k in keys:
# print("key=",k,"state=", keys[k])
if ord('1') in keys:
state = keys[ord('1')]
if (state & p.KEY_WAS_RELEASED):
verbose = 1 - verbose
if ord('s') in keys:
state = keys[ord('s')]
if (state & p.KEY_WAS_RELEASED):
segLinkIndex = 1 - segLinkIndex
#print("segLinkIndex=",segLinkIndex)
flags = 0
if (segLinkIndex):
flags = p.ER_SEGMENTATION_MASK_OBJECT_AND_LINKINDEX
img = p.getCameraImage(320, 200, flags=flags)
robotId = p.loadURDF(config["robot"]["model"], robot_origin_pos, robot_origin_orientation)
robotEndeffectorIndex = config["robot"].get("endeffector_index")
robotEndeffectorName = config["robot"].get("endeffector_name")
p.configureDebugVisualizer(p.COV_ENABLE_RENDERING,1)
p.configureDebugVisualizer(p.COV_ENABLE_GUI,0)
p.resetDebugVisualizerCamera(cameraDistance=2.0,
cameraYaw=180.0,
cameraPitch=0.0,
cameraTargetPosition=[0,0,1.6])
# p.resetBasePositionAndOrientation(sawyerId,[0,0,0],[0,0,0,1])
#bad, get it from name! sawyerEndEffectorIndex = 18
# numJoints = p.getNumJoints(sawyerId)
numJoints = p.getNumJoints(robotId)
if robotEndeffectorIndex is None and robotEndeffectorName is not None:
for i in range(numJoints):
info = p.getJointInfo(robotId, i)
idx, name = info[0], info[1].decode("utf-8")
if name == robotEndeffectorName:
robotEndeffectorIndex = idx
#joint damping coefficents
# jd=[0.001,0.001,0.001,0.001,0.001,0.001,0.001,0.001,0.001,0.001,0.001,0.001,0.001,0.001,0.001,0.001,0.001]
p.setGravity(0,0,0)
t=0.
prevPose=[0,0,0]
prev_pose=[0,0,0]
prevPose2=[0,0,0]
def __init__(self, animate=False, max_steps=1000):
self.animate = animate
self.max_steps = max_steps
# Initialize simulation
if (animate):
self.client_ID = p.connect(p.GUI)
else:
self.client_ID = p.connect(p.DIRECT)
assert self.client_ID != -1, "Physics client failed to connect"
# Set simulation world params
p.setGravity(0, 0, -9.8)
p.setTimeStep(1. / 120.)
# Input and output dimensions defined in the environment
self.obs_dim = 12
self.act_dim = 2
# sum rewards
self.lastrew = 0
# parametrs for the track
self.ctr = 0
self.iteration = 100
self.first = True
self.X = []
self.Y = []
self.index = 0
self.amount = 0
self.reset_index = 0
env_width = 100
env_height = 100
# generating track
heightfieldData = [0] * (env_height * env_width)
# if we would like to generate new track every time
'''
datasetx, datasety, x1,y1,cx,cy = getdataset()
datasetx = np.int64(datasetx)
datasety = np.int64(datasety)
x1 = np.int64(x1)
y1 = np.int64(y1)'''
# for training we used pregenerated tracks
if self.animate:
self.tracking = 5
else:
self.tracking = 1
exx = os.path.join(os.path.dirname(os.path.realpath(__file__)),
'outx0.npy')
exy = os.path.join(os.path.dirname(os.path.realpath(__file__)),
'outy0.npy')
inx = os.path.join(os.path.dirname(os.path.realpath(__file__)),
'inx0.npy')
iny = os.path.join(os.path.dirname(os.path.realpath(__file__)),
'iny0.npy')
checkx = os.path.join(os.path.dirname(os.path.realpath(__file__)),
'checkpointsx0.npy')
checky = os.path.join(os.path.dirname(os.path.realpath(__file__)),
'checkpointsy0.npy')
datasetx = np.load(exx)
datasety = np.load(exy)
x1 = np.load(inx)
y1 = np.load(iny)
cx = np.load(checkx)
cy = np.load(checky)
first = True
for i in range(len(datasetx)):
x = (datasetx[i])
y = (datasety[i])
xnew = x1[i]
ynew = y1[i]
if not first and xold == x and yold == y:
continue
else:
heightfieldData.pop(env_height * x + y)
heightfieldData.insert(env_height * x + y, 1)
heightfieldData.pop(env_height * xnew + ynew)
heightfieldData.insert(env_height * xnew + ynew, 1)
xold = x
yold = y
first = False
self.X = cy
self.Y = cx
self.amount = len(cx)
terrainShape = p.createCollisionShape(
shapeType=p.GEOM_HEIGHTFIELD,
meshScale=[1, 1, 1],
heightfieldTextureScaling=(env_width - 1) / 2,
heightfieldData=heightfieldData,
numHeightfieldRows=env_height,
numHeightfieldColumns=env_width,
physicsClientId=self.client_ID)
mass = 0
terrain = p.createMultiBody(mass, terrainShape)
p.resetBasePositionAndOrientation(terrain, [49.5, 49.5, 0],
[0, 0, 0, 1])
# generating car in the right angle
if (self.X[self.reset_index + 5] - self.X[self.reset_index]) < 0 and (
(self.Y[self.reset_index + 5] - self.Y[self.reset_index]) < 2 and
(self.Y[self.reset_index + 5] - self.Y[self.reset_index]) > -2):
angle = 100
elif (self.X[self.reset_index + 5] - self.X[self.reset_index]) > 0 and (
(self.Y[self.reset_index + 5] - self.Y[self.reset_index]) < 2 and
(self.Y[self.reset_index + 5] - self.Y[self.reset_index]) > -2):
angle = 0
elif (self.Y[self.reset_index + 5] - self.Y[self.reset_index]) < 0 and (
(self.X[self.reset_index + 5] - self.X[self.reset_index]) < 2 and
(self.X[self.reset_index + 5] - self.X[self.reset_index]) > -2):
angle = -1
elif (self.Y[self.reset_index + 5] - self.Y[self.reset_index]) > 0 and (
(self.X[self.reset_index + 5] - self.X[self.reset_index]) < 2 and
(self.X[self.reset_index + 5] - self.X[self.reset_index]) > -2):
angle = 1
elif (self.X[self.reset_index + 5] -
self.X[self.reset_index]) > 0 and (self.Y[self.reset_index + 5] -
self.Y[self.reset_index]) > 0:
angle = 0.5
elif (self.X[self.reset_index + 5] -
self.X[self.reset_index]) > 0 and (self.Y[self.reset_index + 5] -
self.Y[self.reset_index]) < 0:
angle = -0.5
elif (self.X[self.reset_index + 5] -
self.X[self.reset_index]) < 0 and (self.Y[self.reset_index + 5] -
self.Y[self.reset_index]) < 0:
angle = -2
elif (self.X[self.reset_index + 5] -
self.X[self.reset_index]) < 0 and (self.Y[self.reset_index + 5] -
self.Y[self.reset_index]) > 0:
angle = 2
else:
angle = 1
self.car = p.loadURDF("f10_racecar/racecar_differential.urdf",
[self.X[0], self.Y[0], .3], [0, 0, angle, 1])
p.resetDebugVisualizerCamera(
cameraDistance=10,
cameraYaw=0,
cameraPitch=270,
cameraTargetPosition=[self.X[0], self.Y[0], 0])
# Input and output dimensions defined in the environment
for wheel in range(p.getNumJoints(self.car)):
p.setJointMotorControl2(self.car,
wheel,
p.VELOCITY_CONTROL,
targetVelocity=0,
force=0)
p.changeDynamics(self.car, wheel, mass=1, lateralFriction=1.0)
self.wheels = [8, 15]
# spojuje klouby přes ramena dohromady a tím vytváří celek auta
self.c = p.createConstraint(self.car,
9,
self.car,
11,
jointType=p.JOINT_GEAR,
jointAxis=[0, 1, 0],
parentFramePosition=[0, 0, 0],
childFramePosition=[0, 0, 0])
p.changeConstraint(self.c, gearRatio=1, maxForce=10000)
self.c = p.createConstraint(self.car,
10,
self.car,
13,
jointType=p.JOINT_GEAR,
jointAxis=[0, 1, 0],
parentFramePosition=[0, 0, 0],
childFramePosition=[0, 0, 0])
p.changeConstraint(self.c, gearRatio=-1, maxForce=10000)
self.c = p.createConstraint(self.car,
9,
self.car,
13,
jointType=p.JOINT_GEAR,
jointAxis=[0, 1, 0],
parentFramePosition=[0, 0, 0],
childFramePosition=[0, 0, 0])
p.changeConstraint(self.c, gearRatio=-1, maxForce=10000)
self.c = p.createConstraint(self.car,
16,
self.car,
18,
jointType=p.JOINT_GEAR,
jointAxis=[0, 1, 0],
parentFramePosition=[0, 0, 0],
childFramePosition=[0, 0, 0])
p.changeConstraint(self.c, gearRatio=1, maxForce=10000)
self.c = p.createConstraint(self.car,
16,
self.car,
19,
jointType=p.JOINT_GEAR,
jointAxis=[0, 1, 0],
parentFramePosition=[0, 0, 0],
childFramePosition=[0, 0, 0])
p.changeConstraint(self.c, gearRatio=-1, maxForce=10000)
self.c = p.createConstraint(self.car,
17,
self.car,
19,
jointType=p.JOINT_GEAR,
jointAxis=[0, 1, 0],
parentFramePosition=[0, 0, 0],
childFramePosition=[0, 0, 0])
p.changeConstraint(self.c, gearRatio=-1, maxForce=10000)
self.c = p.createConstraint(self.car,
1,
self.car,
18,
jointType=p.JOINT_GEAR,
jointAxis=[0, 1, 0],
parentFramePosition=[0, 0, 0],
childFramePosition=[0, 0, 0])
p.changeConstraint(self.c,
gearRatio=-1,
gearAuxLink=15,
maxForce=10000)
self.c = p.createConstraint(self.car,
3,
self.car,
19,
jointType=p.JOINT_GEAR,
jointAxis=[0, 1, 0],
parentFramePosition=[0, 0, 0],
childFramePosition=[0, 0, 0])
p.changeConstraint(self.c,
gearRatio=-1,
gearAuxLink=15,
maxForce=10000)
self.steering = [0, 2]
# Limits of our joints. When using the * (multiply) operation on a list, it repeats the list that many times
self.joints_rads_low = -1.57
self.joints_rads_high = 4.71
self.joints_rads_diff = self.joints_rads_high - self.joints_rads_low
# self.Velocity = 1
self.stepCtr = 0
self.hokuyo_joint = 4
# Lidar rays initialisation
self.replaceLines = True
self.numRays = 10
self.rayFrom = []
self.rayTo = []
self.rayIds = []
self.rayHitColor = [1, 0, 0]
self.rayMissColor = [0, 1, 0]
self.rayLen = 7
self.rayStartLen = 0.25
for i in range(self.numRays):
self.rayFrom.append([
self.rayStartLen *
math.sin(-0.5 * 0.25 * 2. * math.pi +
0.75 * 2. * math.pi * float(i) / self.numRays),
self.rayStartLen *
math.cos(-0.5 * 0.25 * 2. * math.pi +
0.75 * 2. * math.pi * float(i) / self.numRays), 0
])
self.rayTo.append([
self.rayLen *
math.sin(-0.5 * 0.25 * 2. * math.pi +
0.75 * 2. * math.pi * float(i) / self.numRays),
self.rayLen *
math.cos(-0.5 * 0.25 * 2. * math.pi +
0.75 * 2. * math.pi * float(i) / self.numRays), 0
])
if (self.replaceLines):
self.rayIds.append(
p.addUserDebugLine(self.rayFrom[i],
self.rayTo[i],
self.rayMissColor,
parentObjectUniqueId=self.car,
parentLinkIndex=self.hokuyo_joint))
else:
self.rayIds.append(-1)
physicsClient = p.connect(p.GUI)#or p.DIRECT for non-graphical version
p.setAdditionalSearchPath(pybullet_data.getDataPath()) #optionally
p.setGravity(0,0,-9.8)
cubeStartPos = [0,0,0.2]
FixedBase = False #if fixed no plane is imported
if (FixedBase == False):
p.loadURDF("plane.urdf")
boxId = p.loadURDF("4leggedRobot.urdf",cubeStartPos, useFixedBase=FixedBase)
jointIds = []
paramIds = []
time.sleep(0.5)
for j in range(p.getNumJoints(boxId)):
# p.changeDynamics(boxId, j, linearDamping=0, angularDamping=0)
info = p.getJointInfo(boxId, j)
print(info)
jointName = info[1]
jointType = info[2]
jointIds.append(j)
footFR_index = 3
footFL_index = 7
footBR_index = 11
footBL_index = 15
pybulletDebug = pybulletDebug()
robotKinematics = robotKinematics()
trot = trotGait()
def __init__(
self,
xml_path='/home/shjliu/workspace/EPR/bullet3-new/test-bullet/roomdoor',
num_solver_iterations=10,
frame_skip=4,
time_step=1 / 240.,
debug=0,
render=0,
actuator_lower_limit=(),
actuator_upper_limit=(),
robot_init_pos=(0, 0, 2),
door_init_pos=(3, 0, 0),
goal_pos=(3, 0, 0),
door_controller_k=10**5,
door_controller_d1=10**4,
door_controller_d2=10**2,
door_controller_th_handle=-30 * np.pi / 180,
door_controller_th_door=5 * np.pi / 180,
handle_controller_k=10**0.3,
handle_controller_d=10**-1):
# Set goal
self.goal_pos = goal_pos
self.robot_init_pos = robot_init_pos
self.door_init_pos = door_init_pos
# Camera parameters
self._cam_dist = 5
self._cam_yaw = 60
self._cam_pitch = -66
self._render_width = 320
self._render_height = 240
# Open physics client, load files and set parameters
self.physicsClient_ID = -1
self.xml_path = xml_path
self.num_solver_iterations = num_solver_iterations
self.frame_skip = frame_skip
self.time_step = time_step
self._set_physics_client(render)
# Create relevant dictionaries
body_name_2_joints = {}
for body_id in range(pybullet.getNumBodies()):
# Obtain name of specific body and decode it
(bin_body_name, _) = pybullet.getBodyInfo(body_id)
body_name = bin_body_name.decode("utf8")
if body_name == "base_link":
body_name = "door"
# Get the amount of joints, their names and put them into a dictionary
num_joints = pybullet.getNumJoints(body_id)
joint_name_2_joint_id = {}
if num_joints:
for joint_id in range(num_joints):
(_, bin_joint_name, joint_type, q_index, u_index, flags,
joint_damping, joint_friction, joint_lower_limit,
joint_upper_limit, joint_max_force, joint_max_velocity,
link_name, joint_axis, parent_frame_pos, parent_frame_orn,
parent_index) = pybullet.getJointInfo(body_id, joint_id)
joint_name = bin_joint_name.decode("utf8")
if joint_type == 4:
num_joints -= 1
else:
joint_name_2_joint_id.update({
joint_name: {
"joint_id": joint_id,
"joint_type": joint_type,
"joint_damping": joint_damping,
"joint_lower_limit": joint_lower_limit,
"joint_upper_limit": joint_upper_limit
}
})
body_name_2_joints.update({
body_name: {
"body_id": body_id,
"unique_body_id": pybullet.getBodyUniqueId(body_id),
"body_name": body_name,
"num_joints": num_joints,
"joint_dict": joint_name_2_joint_id
}
})
self.body_name_2_joints = body_name_2_joints
# Important link ID's
self.door_link_IDs = {
'handle_inside': 7,
'handle_outside': 9,
'door_COM': 5
}
self.robot_link_IDs = {
'torso': 0,
'right_hand': 28,
'left_hand': 34,
}
# Indicate names of controllable joints: ASSUMES ROBOT ROOT BODY IS NAMED 'robot' AND DOOR ROOT BODY 'door'
self.robot_joints = [
joint_name
for joint_name in body_name_2_joints['robot']['joint_dict']
]
self.robot_joints_index = [
body_name_2_joints['robot']['joint_dict'][joint_name]['joint_id']
for joint_name in body_name_2_joints['robot']['joint_dict']
]
self.door_joints = [
joint_name
for joint_name in body_name_2_joints['door']['joint_dict']
]
self.door_joints_index = [
body_name_2_joints['door']['joint_dict'][joint_name]['joint_id']
for joint_name in body_name_2_joints['door']['joint_dict']
]
# joint_low = [body_name_2_joints['robot']['joint_dict'][joint_name]['joint_lower_limit'] for joint_name in body_name_2_joints['robot']['joint_dict']]
# joint_high = [body_name_2_joints['robot']['joint_dict'][joint_name]['joint_upper_limit'] for joint_name in body_name_2_joints['robot']['joint_dict']]
# Initialize door controllers
self.door_controller_k = door_controller_k
self.door_controller_d1 = door_controller_d1
self.door_controller_d2 = door_controller_d2
self.door_controller_th_handle = door_controller_th_handle
self.door_controller_th_door = door_controller_th_door
self.handle_controller_k = handle_controller_k
self.handle_controller_d = handle_controller_d
# Initialize cost function weight placeholders. Will be set in hidden method
self.w_alive, self.w_hand_on_handle, self.w_at_target, self.w_move_door, self.w_move_handle, self.w_time, self.w_control = (
0, 0, 0, 0, 0, 0, 0)
self.set_reward_weights()
# Initialize action and observation space
if not actuator_lower_limit:
actuator_lower_limit = -np.pi * np.ones_like(
self.robot_joints_index)
actuator_upper_limit = np.pi * np.ones_like(
self.robot_joints_index)
else:
assert (len(self.robot_joints_index) ==
len(actuator_lower_limit) & len(self.robot_joints_index) ==
len(actuator_upper_limit))
self.action_space = gym.spaces.Box(low=actuator_lower_limit,
high=actuator_upper_limit,
dtype=np.float32)
obs, _, _, _ = self.step(self.action_space.sample(), debug=0)
low = np.full(obs.shape, -float('inf'))
high = np.full(obs.shape, float('inf'))
self.observation_space = gym.spaces.Box(low=low,
high=high,
dtype=obs.dtype)
self.obs_dict = {}
self.reward_dict = {}
# Debug
if debug:
self._debug_link_ids = []
self._debug_lines_ids = []
self._debug_links_text_init()
self._debug_links_text_remove()
self._debug_links_lines_init()
self._debug_links_lines_remove()
linkVisualShapeIndices=linkVisualShapeIndices,
linkPositions=linkPositions,
linkOrientations=linkOrientations,
linkInertialFramePositions=linkInertialFramePositions,
linkInertialFrameOrientations=linkInertialFrameOrientations,
linkParentIndices=indices,
linkJointTypes=jointTypes,
linkJointAxis=axis,
useMaximalCoordinates=useMaximalCoordinates)
p.setGravity(0, 0, -10)
p.setRealTimeSimulation(0)
anistropicFriction = [1, 0.01, 0.01]
p.changeDynamics(sphereUid, -1, lateralFriction=2, anisotropicFriction=anistropicFriction)
p.getNumJoints(sphereUid)
for i in range(p.getNumJoints(sphereUid)):
p.getJointInfo(sphereUid, i)
p.changeDynamics(sphereUid, i, lateralFriction=2, anisotropicFriction=anistropicFriction)
dt = 1. / 240.
SNAKE_NORMAL_PERIOD = 0.1 #1.5
m_wavePeriod = SNAKE_NORMAL_PERIOD
m_waveLength = 4
m_wavePeriod = 1.5
m_waveAmplitude = 0.4
m_waveFront = 0.0
#our steering value
m_steering = 0.0
m_segmentLength = sphereRadius * 2.0
import pybullet as p
import time
import pybullet_data
urdf_path = "robot.urdf"
physicsClient = p.connect(p.GUI)
p.setAdditionalSearchPath(pybullet_data.getDataPath())
p.setGravity(0, 0, -10)
planeId = p.loadURDF("plane.urdf")
robotId = p.loadURDF(urdf_path, useFixedBase=True)
print("JOINT")
for _id in range(p.getNumJoints(robotId)):
print(f'{_id} {p.getJointInfo(robotId, _id)[12]}')
jointIndices = [1, 2]
home_pos = [0.78, 0.78]
p.setJointMotorControlArray(robotId,
jointIndices=jointIndices,
controlMode=p.POSITION_CONTROL,
targetPositions=home_pos)
while True:
p.stepSimulation()
p.disconnect()
import pybullet as p
import time
import math
from datetime import datetime
clid = p.connect(p.SHARED_MEMORY)
if (clid < 0):
p.connect(p.GUI)
p.loadURDF("plane.urdf", [0, 0, -1.3])
p.configureDebugVisualizer(p.COV_ENABLE_RENDERING, 0)
sawyerId = p.loadURDF("pole.urdf", [0, 0, 0])
p.configureDebugVisualizer(p.COV_ENABLE_RENDERING, 1)
p.resetBasePositionAndOrientation(sawyerId, [0, 0, 0], [0, 0, 0, 1])
sawyerEndEffectorIndex = 3
numJoints = p.getNumJoints(sawyerId)
#joint damping coefficents
jd = [0.1, 0.1, 0.1, 0.1]
p.setGravity(0, 0, 0)
t = 0.
prevPose = [0, 0, 0]
prevPose1 = [0, 0, 0]
hasPrevPose = 0
ikSolver = 0
useRealTimeSimulation = 0
p.setRealTimeSimulation(useRealTimeSimulation)
#trailDuration is duration (in seconds) after debug lines will be removed automatically
#use 0 for no-removal
trailDuration = 1
def setup_simulation(self):
""" Setup the simulation. """
########## PYBULLET SETUP ##########
if self.record_movie and self.gui == p.GUI:
p.connect(self.gui,
options=('--background_color_red={}'
' --background_color_green={}'
' --background_color_blue={}'
' --mp4={}'
' --mp4fps={}').format(
self.vis_options_background_color_red,
self.vis_options_background_color_green,
self.vis_options_background_color_blue,
self.movie_name,
int(self.movie_speed / self.time_step)))
p.configureDebugVisualizer(p.COV_ENABLE_SINGLE_STEP_RENDERING, 1)
elif self.gui == p.GUI:
p.connect(self.gui,
options=(' --background_color_red={}'
' --background_color_green={}'
' --background_color_blue={}').format(
self.vis_options_background_color_red,
self.vis_options_background_color_green,
self.vis_options_background_color_blue))
else:
p.connect(self.gui)
p.resetSimulation()
p.setAdditionalSearchPath(pybullet_data.getDataPath())
# Everything should fall down
p.setGravity(*[g * self.units.gravity for g in self.gravity])
p.setPhysicsEngineParameter(fixedTimeStep=self.time_step,
numSolverIterations=self.solver_iterations,
numSubSteps=self.num_substep,
solverResidualThreshold=1e-10,
erp=0.0,
contactERP=self.contactERP,
frictionERP=0.0,
globalCFM=self.globalCFM,
reportSolverAnalytics=1)
# Turn off rendering while loading the models
self.rendering(0)
if self.ground == 'floor':
# Add floor
self.plane = p.loadURDF('plane.urdf', [0, 0, -0.],
globalScaling=0.01 * self.units.meters)
# When plane is used the link id is -1
self.link_plane = -1
p.changeDynamics(self.plane,
-1,
lateralFriction=self.ground_friction_coef)
self.sim_data.add_table('base_linear_velocity')
self.sim_data.add_table('base_angular_velocity')
self.sim_data.add_table('base_orientation')
elif self.ground == 'ball':
# Add floor and ball
self.floor = p.loadURDF('plane.urdf', [0, 0, -0.],
globalScaling=0.01 * self.units.meters)
if self.ball_info:
self.ball_radius, ball_pos = self.load_ball_info()
else:
ball_pos = None
self.plane = self.add_ball(self.ball_radius, self.ball_density,
self.ball_mass,
self.ground_friction_coef, ball_pos)
# When ball is used the plane id is 2 as the ball has 3 links
self.link_plane = 2
self.sim_data.add_table('ball_rotations')
self.sim_data.add_table('ball_velocity')
# Add the animal model
if '.sdf' in self.model and self.behavior is not None:
self.animal, self.link_id, self.joint_id = load_sdf(
self.model, force_concave=self.enable_concave_mesh)
elif '.sdf' in self.model and self.behavior is None:
self.animal = p.loadSDF(self.model)[0]
# Generate joint_name to id dict
self.link_id[p.getBodyInfo(self.animal)[0].decode('UTF-8')] = -1
for n in range(p.getNumJoints(self.animal)):
info = p.getJointInfo(self.animal, n)
_id = info[0]
joint_name = info[1].decode('UTF-8')
link_name = info[12].decode('UTF-8')
_type = info[2]
self.joint_id[joint_name] = _id
self.joint_type[joint_name] = _type
self.link_id[link_name] = _id
elif '.urdf' in self.model:
self.animal = p.loadURDF(self.model)
p.resetBasePositionAndOrientation(
self.animal, self.model_offset,
p.getQuaternionFromEuler([0., 0., 0.]))
self.num_joints = p.getNumJoints(self.animal)
# Body colors
color_wings = [91 / 100, 96 / 100, 97 / 100, 0.7]
color_eyes = [67 / 100, 21 / 100, 12 / 100, 1]
self.color_body = [140 / 255, 100 / 255, 30 / 255, 1]
self.color_legs = [170 / 255, 130 / 255, 50 / 255, 1]
self.color_collision = [0, 1, 0, 1]
nospecular = [0.5, 0.5, 0.5]
# Color the animal
p.changeVisualShape(self.animal,
-1,
rgbaColor=self.color_body,
specularColor=nospecular)
for link_name, _id in self.joint_id.items():
if 'Wing' in link_name and 'Fake' not in link_name:
p.changeVisualShape(self.animal, _id, rgbaColor=color_wings)
elif 'Eye' in link_name and 'Fake' not in link_name:
p.changeVisualShape(self.animal, _id, rgbaColor=color_eyes)
# and 'Fake' not in link_name:
elif ('Tarsus' in link_name or 'Tibia' in link_name
or 'Femur' in link_name or 'Coxa' in link_name):
p.changeVisualShape(self.animal,
_id,
rgbaColor=self.color_legs,
specularColor=nospecular)
elif 'Fake' not in link_name:
p.changeVisualShape(self.animal,
_id,
rgbaColor=self.color_body,
specularColor=nospecular)
# print('Link name {} id {}'.format(link_name, _id))
# Configure contacts
# Disable/Enable all self-collisions
for link0 in self.link_id.keys():
for link1 in self.link_id.keys():
p.setCollisionFilterPair(
bodyUniqueIdA=self.animal,
bodyUniqueIdB=self.animal,
linkIndexA=self.link_id[link0],
linkIndexB=self.link_id[link1],
enableCollision=0,
)
# Disable/Enable tarsi-ground collisions
for link in self.link_id.keys():
if 'Tarsus' in link:
p.setCollisionFilterPair(bodyUniqueIdA=self.animal,
bodyUniqueIdB=self.plane,
linkIndexA=self.link_id[link],
linkIndexB=self.link_plane,
enableCollision=1)
# Disable/Enable selected self-collisions
for (link0, link1) in self.self_collisions:
p.setCollisionFilterPair(
bodyUniqueIdA=self.animal,
bodyUniqueIdB=self.animal,
linkIndexA=self.link_id[link0],
linkIndexB=self.link_id[link1],
enableCollision=1,
)
# ADD container columns
# ADD ground reaction forces and friction forces
_ground_sensor_ids = []
for contact in self.ground_contacts:
_ground_sensor_ids.append((self.animal, self.plane,
self.link_id[contact], self.link_plane))
self.sim_data.contact_flag.add_parameter(f"{contact}_flag")
for axis in ('x', 'y', 'z'):
self.sim_data.contact_position.add_parameter(contact + '_' +
axis)
self.sim_data.contact_normal_force.add_parameter(contact +
'_' + axis)
self.sim_data.contact_lateral_force.add_parameter(contact +
'_' + axis)
# ADD self collision forces
_collision_sensor_ids = []
for link0, link1 in self.self_collisions:
_collision_sensor_ids.append(
(self.animal, self.animal, self.link_id[link0],
self.link_id[link1]))
contacts = '-'.join((link0, link1))
for axis in ['x', 'y', 'z']:
self.sim_data.contact_position.add_parameter(contacts + '_' +
axis)
self.sim_data.contact_normal_force.add_parameter(contacts +
'_' + axis)
self.sim_data.contact_lateral_force.add_parameter(contacts +
'_' + axis)
# Generate sensors
self.joint_sensors = JointSensors(self.animal,
self.sim_data,
meters=self.units.meters,
velocity=self.units.velocity,
torques=self.units.torques)
self.contact_sensors = ContactSensors(
self.sim_data,
tuple([*_ground_sensor_ids, *_collision_sensor_ids]),
meters=self.units.meters,
newtons=self.units.newtons)
self.com_sensor = COMSensor(self.animal,
self.sim_data,
meters=self.units.meters,
kilograms=self.units.kilograms)
# ADD base position parameters
for axis in ['x', 'y', 'z']:
self.sim_data.base_position.add_parameter(f'{axis}')
# self.sim_data.thorax_force.add_parameter(f'{axis}')
if self.ground == 'ball':
self.sim_data.ball_rotations.add_parameter(f'{axis}')
self.sim_data.ball_velocity.add_parameter(f'{axis}')
if self.ground == 'floor':
self.sim_data.base_linear_velocity.add_parameter(f'{axis}')
self.sim_data.base_angular_velocity.add_parameter(f'{axis}')
self.sim_data.base_orientation.add_parameter(f'{axis}')
# ADD joint parameters
for name, _ in self.joint_id.items():
self.sim_data.joint_positions.add_parameter(name)
self.sim_data.joint_velocities.add_parameter(name)
self.sim_data.joint_torques.add_parameter(name)
# ADD Center of mass parameters
for axis in ('x', 'y', 'z'):
self.sim_data.center_of_mass.add_parameter(f"{axis}")
# ADD muscles
if self.use_muscles:
self.initialize_muscles()
# ADD controller
if self.controller_config:
self.controller = NeuralSystem(
config_path=self.controller_config,
container=self.container,
)
# Disable default bullet controllers
p.setJointMotorControlArray(self.animal,
np.arange(self.num_joints),
p.VELOCITY_CONTROL,
targetVelocities=np.zeros(
(self.num_joints, 1)),
forces=np.zeros((self.num_joints, 1)))
p.setJointMotorControlArray(self.animal,
np.arange(self.num_joints),
p.POSITION_CONTROL,
forces=np.zeros((self.num_joints, 1)))
p.setJointMotorControlArray(self.animal,
np.arange(self.num_joints),
p.TORQUE_CONTROL,
forces=np.zeros((self.num_joints, 1)))
# Disable link linear and angular damping
for njoint in range(self.num_joints):
p.changeDynamics(self.animal, njoint, linearDamping=0.0)
p.changeDynamics(self.animal, njoint, angularDamping=0.0)
p.changeDynamics(self.animal, njoint, jointDamping=0.0)
self.total_mass = 0.0
for j in np.arange(-1, p.getNumJoints(self.animal)):
self.total_mass += p.getDynamicsInfo(self.animal,
j)[0] / self.units.kilograms
self.bodyweight = -1 * self.total_mass * self.gravity[2]
print('Total mass = {}'.format(self.total_mass))
if self.gui == p.GUI:
self.rendering(1)
p.connect(p.SHARED_MEMORY)
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],
def ball_rotations(self):
""" Return the ball angular position. """
return tuple(state[0] for state in p.getJointStates(
self.plane, np.arange(0, p.getNumJoints(self.plane))))
boxId = p.createMultiBody(0,
colSphereId,
-1, [segmentStart, 0, -0.1],
baseOrientation,
linkMasses=link_Masses,
linkCollisionShapeIndices=linkCollisionShapeIndices,
linkVisualShapeIndices=linkVisualShapeIndices,
linkPositions=linkPositions,
linkOrientations=linkOrientations,
linkInertialFramePositions=linkInertialFramePositions,
linkInertialFrameOrientations=linkInertialFrameOrientations,
linkParentIndices=indices,
linkJointTypes=jointTypes,
linkJointAxis=axis)
p.changeDynamics(boxId, -1, spinningFriction=0.001, rollingFriction=0.001, linearDamping=0.0)
print(p.getNumJoints(boxId))
for joint in range(p.getNumJoints(boxId)):
targetVelocity = 10
if (i % 2):
targetVelocity = -10
p.setJointMotorControl2(boxId,
joint,
p.VELOCITY_CONTROL,
targetVelocity=targetVelocity,
force=100)
segmentStart = segmentStart - 1.1
p.configureDebugVisualizer(p.COV_ENABLE_RENDERING, 1)
while (1):
camData = p.getDebugVisualizerCamera()
viewMat = camData[2]
def get_num_joints(body):
return p.getNumJoints(body, physicsClientId=CLIENT)
def __init__(self,
camera_attached=False,
# useIK=True,
actionRepeat=1,
renders=False,
maxSteps=100,
# numControlledJoints=3, # XYZ, we use IK here!
simulatedGripper=False,
randObjPos=False,
task=0, # here target number
learning_param=0):
self.renders = renders
self.actionRepeat = actionRepeat
# setup pybullet sim:
if self.renders:
pybullet.connect(pybullet.GUI)
else:
pybullet.connect(pybullet.DIRECT)
pybullet.setTimeStep(1./240.)
pybullet.setGravity(0,0,-10)
pybullet.setRealTimeSimulation(False)
# pybullet.configureDebugVisualizer(pybullet.COV_ENABLE_WIREFRAME,1)
pybullet.resetDebugVisualizerCamera( cameraDistance=1.5, cameraYaw=60, cameraPitch=-30, cameraTargetPosition=[0,0,0])
# setup robot arm:
self.end_effector_index = 7
self.table = pybullet.loadURDF(TABLE_URDF_PATH, [0.5, 0, -0.6300], [0, 0, 0, 1])
flags = pybullet.URDF_USE_SELF_COLLISION
self.ur5 = pybullet.loadURDF(ROBOT_URDF_PATH, [0, 0, 0], [0, 0, 0, 1], flags=flags)
self.num_joints = pybullet.getNumJoints(self.ur5)
self.control_joints = ["shoulder_pan_joint", "shoulder_lift_joint", "elbow_joint", "wrist_1_joint", "wrist_2_joint", "wrist_3_joint"]
self.joint_type_list = ["REVOLUTE", "PRISMATIC", "SPHERICAL", "PLANAR", "FIXED"]
self.joint_info = namedtuple("jointInfo", ["id", "name", "type", "lowerLimit", "upperLimit", "maxForce", "maxVelocity", "controllable"])
self.joints = AttrDict()
for i in range(self.num_joints):
info = pybullet.getJointInfo(self.ur5, i)
jointID = info[0]
jointName = info[1].decode("utf-8")
jointType = self.joint_type_list[info[2]]
jointLowerLimit = info[8]
jointUpperLimit = info[9]
jointMaxForce = info[10]
jointMaxVelocity = info[11]
controllable = True if jointName in self.control_joints else False
info = self.joint_info(jointID, jointName, jointType, jointLowerLimit, jointUpperLimit, jointMaxForce, jointMaxVelocity, controllable)
if info.type == "REVOLUTE":
pybullet.setJointMotorControl2(self.ur5, info.id, pybullet.VELOCITY_CONTROL, targetVelocity=0, force=0)
self.joints[info.name] = info
# object:
self.initial_obj_pos = [0.8, 0.1, 0.0] # initial object pos
self.obj = pybullet.loadURDF(CUBE_URDF_PATH, self.initial_obj_pos)
self.name = 'ur5GymEnv'
self.simulatedGripper = simulatedGripper
self.action_dim = 4
self.stepCounter = 0
self.maxSteps = maxSteps
self.terminated = False
self.randObjPos = randObjPos
self.observation = np.array(0)
self.task = task
self.learning_param = learning_param
self._action_bound = 1.0 # delta limits
action_high = np.array([self._action_bound] * self.action_dim)
self.action_space = spaces.Box(-action_high, action_high, dtype='float32')
self.reset()
high = np.array([10]*self.observation.shape[0])
self.observation_space = spaces.Box(-high, high, dtype='float32')
time_step = 0.001
gravity_constant = -9.81
p.resetSimulation()
p.setTimeStep(time_step)
p.setGravity(0.0, 0.0, gravity_constant)
p.loadURDF("plane.urdf", [0, 0, -0.3])
kukaId = p.loadURDF("TwoJointRobot_w_fixedJoints.urdf", useFixedBase=True)
#kukaId = p.loadURDF("TwoJointRobot_w_fixedJoints.urdf",[0,0,0])
#kukaId = p.loadURDF("kuka_iiwa/model.urdf",[0,0,0])
#kukaId = p.loadURDF("kuka_lwr/kuka.urdf",[0,0,0])
#kukaId = p.loadURDF("humanoid/nao.urdf",[0,0,0])
p.resetBasePositionAndOrientation(kukaId, [0, 0, 0], [0, 0, 0, 1])
numJoints = p.getNumJoints(kukaId)
kukaEndEffectorIndex = numJoints - 1
# Set a joint target for the position control and step the sim.
setJointPosition(kukaId, [0.1] * numJoints)
p.stepSimulation()
# Get the joint and link state directly from Bullet.
pos, vel, torq = getJointStates(kukaId)
mpos, mvel, mtorq = getMotorJointStates(kukaId)
result = p.getLinkState(kukaId,
kukaEndEffectorIndex,
computeLinkVelocity=1,
computeForwardKinematics=1)
link_trn, link_rot, com_trn, com_rot, frame_pos, frame_rot, link_vt, link_vr = result
from pdControllerExplicit import PDControllerExplicit
from pdControllerStable import PDControllerStable
import time
useMaximalCoordinates = False
p.connect(p.GUI)
pole = p.loadURDF("cartpole.urdf", [0, 0, 0], useMaximalCoordinates=useMaximalCoordinates)
pole2 = p.loadURDF("cartpole.urdf", [0, 1, 0], useMaximalCoordinates=useMaximalCoordinates)
pole3 = p.loadURDF("cartpole.urdf", [0, 2, 0], useMaximalCoordinates=useMaximalCoordinates)
pole4 = p.loadURDF("cartpole.urdf", [0, 3, 0], useMaximalCoordinates=useMaximalCoordinates)
exPD = PDControllerExplicitMultiDof(p)
sPD = PDControllerStable(p)
for i in range(p.getNumJoints(pole2)):
#disable default constraint-based motors
p.setJointMotorControl2(pole, i, p.POSITION_CONTROL, targetPosition=0, force=0)
p.setJointMotorControl2(pole2, i, p.POSITION_CONTROL, targetPosition=0, force=0)
p.setJointMotorControl2(pole3, i, p.POSITION_CONTROL, targetPosition=0, force=0)
p.setJointMotorControl2(pole4, i, p.POSITION_CONTROL, targetPosition=0, force=0)
#print("joint",i,"=",p.getJointInfo(pole2,i))
timeStepId = p.addUserDebugParameter("timeStep", 0.001, 0.1, 0.01)
desiredPosCartId = p.addUserDebugParameter("desiredPosCart", -10, 10, 2)
desiredVelCartId = p.addUserDebugParameter("desiredVelCart", -10, 10, 0)
kpCartId = p.addUserDebugParameter("kpCart", 0, 500, 1300)
kdCartId = p.addUserDebugParameter("kdCart", 0, 300, 150)
maxForceCartId = p.addUserDebugParameter("maxForceCart", 0, 5000, 1000)
def getJointStates(robot): joint_states = p.getJointStates(robot, range(p.getNumJoints(robot))) joint_positions = [state[0] for state in joint_states] joint_velocities = [state[1] for state in joint_states] joint_torques = [state[3] for state in joint_states] return joint_positions, joint_velocities, joint_torques
time_step = 0.001
gravity_constant = -9.81
p.resetSimulation()
p.setTimeStep(time_step)
p.setGravity(0.0, 0.0, gravity_constant)
p.loadURDF("plane.urdf",[0,0,-0.3])
kukaId = p.loadURDF("TwoJointRobot_w_fixedJoints.urdf", useFixedBase=True)
#kukaId = p.loadURDF("TwoJointRobot_w_fixedJoints.urdf",[0,0,0])
#kukaId = p.loadURDF("kuka_iiwa/model.urdf",[0,0,0])
#kukaId = p.loadURDF("kuka_lwr/kuka.urdf",[0,0,0])
#kukaId = p.loadURDF("humanoid/nao.urdf",[0,0,0])
p.resetBasePositionAndOrientation(kukaId,[0,0,0],[0,0,0,1])
numJoints = p.getNumJoints(kukaId)
kukaEndEffectorIndex = numJoints - 1
# Set a joint target for the position control and step the sim.
setJointPosition(kukaId, [0.1] * numJoints)
p.stepSimulation()
# Get the joint and link state directly from Bullet.
pos, vel, torq = getJointStates(kukaId)
mpos, mvel, mtorq = getMotorJointStates(kukaId)
result = p.getLinkState(kukaId, kukaEndEffectorIndex, 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.
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)]
renderer=p.ER_TINY_RENDERER)
depth_buffer_tiny = np.reshape(images[3], [width, height])
depth_tiny = far * near / (far - (far - near) * depth_buffer_tiny)
rgb_tiny = np.reshape(images[2], (height, width, 4)) * 1. / 255.
seg_tiny = np.reshape(images[4], [width, height]) * 1. / 255.
bearStartPos1 = [-3.3, 0, 0]
bearStartOrientation1 = p.getQuaternionFromEuler([0, 0, 0])
bearId1 = p.loadURDF("plane.urdf", bearStartPos1, bearStartOrientation1)
bearStartPos2 = [0, 0, 0]
bearStartOrientation2 = p.getQuaternionFromEuler([0, 0, 0])
bearId2 = p.loadURDF("teddy_large.urdf", bearStartPos2, bearStartOrientation2)
textureId = p.loadTexture("checker_grid.jpg")
for b in range(p.getNumBodies()):
p.changeVisualShape(b, linkIndex=-1, textureUniqueId=textureId)
for j in range(p.getNumJoints(b)):
p.changeVisualShape(b, linkIndex=j, textureUniqueId=textureId)
viewMat = [
0.642787516117096, -0.4393851161003113, 0.6275069713592529, 0.0, 0.766044557094574,
0.36868777871131897, -0.5265407562255859, 0.0, -0.0, 0.8191521167755127, 0.5735764503479004,
0.0, 2.384185791015625e-07, 2.384185791015625e-07, -5.000000476837158, 1.0
]
projMat = [
0.7499999403953552, 0.0, 0.0, 0.0, 0.0, 1.0, 0.0, 0.0, 0.0, 0.0, -1.0000200271606445, -1.0,
0.0, 0.0, -0.02000020071864128, 0.0
]
images = p.getCameraImage(width,
height,
viewMatrix=viewMat,
projectionMatrix=projMat,
import pybullet as p
import time
p.connect(p.GUI)
p.resetSimulation()
p.setGravity(0, 0, -10)
useRealTimeSim = 1
p.setRealTimeSimulation(useRealTimeSim) # either this
track = p.loadSDF("f10_racecar/meshes/barca_track.sdf", globalScaling=1)
car = p.loadURDF("f10_racecar/racecar_differential.urdf", [0, 0, 1])
for wheel in range(p.getNumJoints(car)):
p.setJointMotorControl2(car,
wheel,
p.VELOCITY_CONTROL,
targetVelocity=0,
force=0)
p.getJointInfo(car, wheel)
wheels = [8, 15]
print("----------------")
#p.setJointMotorControl2(car,10,p.VELOCITY_CONTROL,targetVelocity=1,force=10)
c = p.createConstraint(car,
9,
car,
11,
jointType=p.JOINT_GEAR,
jointAxis=[0, 1, 0],
##########################################
org2 = p.connect(p.DIRECT)
org = p.connect(p.SHARED_MEMORY)
if (org<0):
org = p.connect(p.DIRECT)
gui = p.connect(p.GUI)
p.resetSimulation(physicsClientId=org)
#door.urdf, hinge.urdf, duck_vhacd.urdf, r2d2.urdf, quadruped/quadruped.urdf
mb = p.loadURDF("r2d2.urdf", flags=p.URDF_USE_IMPLICIT_CYLINDER, physicsClientId=org)
for i in range(p.getNumJoints(mb,physicsClientId=org)):
p.setJointMotorControl2(mb,i,p.VELOCITY_CONTROL,force=0,physicsClientId=org)
#print("numJoints:",p.getNumJoints(mb,physicsClientId=org))
#print("base name:",p.getBodyInfo(mb,physicsClientId=org))
#for i in range(p.getNumJoints(mb,physicsClientId=org)):
# print("jointInfo(",i,"):",p.getJointInfo(mb,i,physicsClientId=org))
# print("linkState(",i,"):",p.getLinkState(mb,i,physicsClientId=org))
parser = urdfEditor.UrdfEditor()
parser.initializeFromBulletBody(mb,physicsClientId=org)
parser.saveUrdf("test.urdf")
if (1):
physicsClient = p.connect(p.GUI)
p.setAdditionalSearchPath(pybullet_data.getDataPath())
p.setGravity(0, 0, -9.8)
planeID = p.loadURDF("plane.urdf")
cubePos = [-0.5, 0, 0]
straightUp = p.getQuaternionFromEuler([0, 0, 0])
kukaPos = [0, 0, 0]
kuka2Pos = [0, 1, 0]
nailGunPos = [0, 0.5, 0.2]
roof1ID = p.loadURDF("urdfs/roof1.urdf", [-0.6, 0, 0.15], straightUp)
roof2ID = p.loadURDF("urdfs/roof2.urdf", [0.6, 1, 0.15], straightUp)
kukaID = p.loadURDF("kuka_iiwa/model.urdf", kukaPos, straightUp)
kuka2ID = p.loadURDF("kuka_iiwa/model.urdf", kuka2Pos, straightUp)
nailGunID = p.loadURDF('urdfs/nailgun.urdf', nailGunPos, straightUp)
jointPositions = [-0, 0, 0, 1.57, 0, -1.04, 0]
for jointIndex in range(p.getNumJoints(kukaID)):
p.resetJointState(kukaID, jointIndex, jointPositions[jointIndex])
p.setJointMotorControl2(kukaID,
jointIndex,
p.POSITION_CONTROL,
targetPosition=jointPositions[jointIndex])
jointPositions = [0, 0, 0, -1.57, 0, 1.04, 0]
for jointIndex in range(p.getNumJoints(kukaID)):
p.resetJointState(kuka2ID, jointIndex, jointPositions[jointIndex])
p.setJointMotorControl2(kuka2ID,
jointIndex,
p.POSITION_CONTROL,
targetPosition=jointPositions[jointIndex])
gripperID = p.loadSDF("gripper/wsg50_one_motor_gripper_new_free_base.sdf")[0]
gripper2ID = p.loadSDF("gripper/wsg50_one_motor_gripper_new_free_base.sdf")[0]
jointPositions = [
cid = p.connect(p.SHARED_MEMORY)
if (cid<0):
p.connect(p.GUI)
p.resetSimulation()
p.setGravity(0,0,-10)
useRealTimeSim = 1
#for video recording (works best on Mac and Linux, not well on Windows)
#p.startStateLogging(p.STATE_LOGGING_VIDEO_MP4, "racecar.mp4")
p.setRealTimeSimulation(useRealTimeSim) # either this
p.loadURDF("plane.urdf")
#p.loadSDF("stadium.sdf")
car = p.loadURDF("racecar/racecar_differential.urdf") #, [0,0,2],useFixedBase=True)
for i in range (p.getNumJoints(car)):
print (p.getJointInfo(car,i))
for wheel in range(p.getNumJoints(car)):
p.setJointMotorControl2(car,wheel,p.VELOCITY_CONTROL,targetVelocity=0,force=0)
p.getJointInfo(car,wheel)
wheels = [8,15]
print("----------------")
#p.setJointMotorControl2(car,10,p.VELOCITY_CONTROL,targetVelocity=1,force=10)
c = p.createConstraint(car,9,car,11,jointType=p.JOINT_GEAR,jointAxis =[0,1,0],parentFramePosition=[0,0,0],childFramePosition=[0,0,0])
p.changeConstraint(c,gearRatio=1, maxForce=10000)
c = p.createConstraint(car,10,car,13,jointType=p.JOINT_GEAR,jointAxis =[0,1,0],parentFramePosition=[0,0,0],childFramePosition=[0,0,0])
p.changeConstraint(c,gearRatio=-1, maxForce=10000)
p.setAdditionalSearchPath(pybullet_data.getDataPath())
p.setGravity(0, 0, -10)
print(c)
if (c < 0):
p.connect(p.GUI)
#load the MuJoCo MJCF hand
objects = p.loadMJCF("MPL/mpl2.xml")
hand = objects[0]
ho = p.getQuaternionFromEuler([0, 3.14, 0])
hand_cid = p.createConstraint(hand, -1, -1, -1, p.JOINT_FIXED, [0, 0, 0],
[0.1, 0, 0], [0.500000, 0.300006, 0.700000], ho)
print("hand_cid")
print(hand_cid)
for i in range(p.getNumJoints(hand)):
p.setJointMotorControl2(hand, i, p.POSITION_CONTROL, 0, 0)
#clamp in range 400-600
minV = 400
maxV = 600
POSITION = 1
ORIENTATION = 2
BUTTONS = 6
p.setRealTimeSimulation(1)
def convertSensor(x):
v = minV
# for i in range (100):
# if (i<99):
# sphere = p.loadURDF("domino/domino.urdf",[i*0.04,1+j*.25,0.03], useMaximalCoordinates=useMaximalCoordinates)
# else:
# orn = p.getQuaternionFromEuler([0,-3.14*0.24,0])
# sphere = p.loadURDF("domino/domino.urdf",[(i-1)*0.04,1+j*.25,0.03], orn, useMaximalCoordinates=useMaximalCoordinates)
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):
if __name__ == '__main__':
TIME_STEP = 0.001
physicsClient = p.connect(p.GUI)
p.setGravity(0, 0, -9.8)
p.setTimeStep(TIME_STEP)
planeId = p.loadURDF("../URDF/plane.urdf", [0, 0, 0])
RobotId = p.loadURDF("../URDF/gankenkun.urdf", [0, 0, 0])
kine = kinematics(RobotId)
index = {
p.getBodyInfo(RobotId)[0].decode('UTF-8'): -1,
}
for id in range(p.getNumJoints(RobotId)):
index[p.getJointInfo(RobotId, id)[12].decode('UTF-8')] = id
left_foot_pos0, left_foot_ori0 = p.getLinkState(
RobotId, index['left_foot_link'])[:2]
right_foot_pos0, right_foot_ori0 = p.getLinkState(
RobotId, index['right_foot_link'])[:2]
index_dof = {
p.getBodyInfo(RobotId)[0].decode('UTF-8'): -1,
}
for id in range(p.getNumJoints(RobotId)):
index_dof[p.getJointInfo(
RobotId,
id)[12].decode('UTF-8')] = p.getJointInfo(RobotId, id)[3] - 7
p.setPhysicsEngineParameter(numSolverIterations=100)
p.loadURDF("plane_transparent.urdf", useMaximalCoordinates=True)
#disable rendering during creation.
#p.configureDebugVisualizer(p.COV_ENABLE_RENDERING,1)
p.configureDebugVisualizer(p.COV_ENABLE_PLANAR_REFLECTION,1)
jointNamesToIndex={}
p.configureDebugVisualizer(p.COV_ENABLE_RENDERING,0)
vision = p.loadURDF("vision60.urdf",[0,0,0.4],useFixedBase=False)
p.configureDebugVisualizer(p.COV_ENABLE_RENDERING,1)
for j in range(p.getNumJoints(vision)):
jointInfo = p.getJointInfo(vision,j)
jointInfoName = jointInfo[1].decode("utf-8")
print("joint ",j," = ",jointInfoName, "type=",jointTypeNames[jointInfo[2]])
jointNamesToIndex[jointInfoName ]=j
#print("jointNamesToIndex[..]=",jointNamesToIndex[jointInfoName])
p.setJointMotorControl2(vision,j,p.VELOCITY_CONTROL,targetVelocity=0, force=jointFriction)
chassis_right_center = jointNamesToIndex['chassis_right_center']
motor_front_rightR_joint = jointNamesToIndex['motor_front_rightR_joint']
motor_front_rightS_joint = jointNamesToIndex['motor_front_rightS_joint']
hip_front_rightR_joint = jointNamesToIndex['hip_front_rightR_joint']
knee_front_rightR_joint = jointNamesToIndex['knee_front_rightR_joint']
motor_front_rightL_joint = jointNamesToIndex['motor_front_rightL_joint']
motor_back_rightR_joint = jointNamesToIndex['motor_back_rightR_joint']
def addToScene(self, bodies):
if self.parts is not None:
parts = self.parts
else:
parts = {}
if self.jdict is not None:
joints = self.jdict
else:
joints = {}
if self.ordered_joints is not None:
ordered_joints = self.ordered_joints
else:
ordered_joints = []
dump = 0
#from IPython import embed; embed()
for i in range(len(bodies)):
if p.getNumJoints(bodies[i]) == 0:
part_name, robot_name = p.getBodyInfo(bodies[i], 0)
robot_name = robot_name.decode("utf8")
part_name = part_name.decode("utf8")
parts[part_name] = BodyPart(part_name, bodies, i, -1, self.scale, model_type=self.model_type)
#if part_name == self.robot_name:
# self.robot_body = parts[part_name]
for j in range(p.getNumJoints(bodies[i])):
p.setJointMotorControl2(bodies[i],j,p.POSITION_CONTROL,positionGain=0.1,velocityGain=0.1,force=0)
## TODO (hzyjerry): the following is diabled due to pybullet update
#_,joint_name,joint_type, _,_,_, _,_,_,_, _,_, part_name = p.getJointInfo(bodies[i], j)
_,joint_name,joint_type, _,_,_, _,_,_,_, _,_, part_name, _,_,_,_ = p.getJointInfo(bodies[i], j)
joint_name = joint_name.decode("utf8")
part_name = part_name.decode("utf8")
if dump: print("ROBOT PART '%s'" % part_name)
if dump: print("ROBOT JOINT '%s'" % joint_name) # limits = %+0.2f..%+0.2f effort=%0.3f speed=%0.3f" % ((joint_name,) + j.limits()) )
parts[part_name] = BodyPart(part_name, bodies, i, j, self.scale, model_type=self.model_type)
if part_name == self.robot_name:
self.robot_body = parts[part_name]
if i == 0 and j == 0 and self.robot_body is None: # if nothing else works, we take this as robot_body
parts[self.robot_name] = BodyPart(self.robot_name, bodies, 0, -1, self.scale, model_type=self.model_type)
self.robot_body = parts[self.robot_name]
#print(joint_name)
if joint_name[:6] == "ignore":
Joint(joint_name, bodies, i, j, self.scale).disable_motor()
continue
if joint_name[:8] != "jointfix" and joint_type != p.JOINT_FIXED:
joints[joint_name] = Joint(joint_name, bodies, i, j, self.scale, model_type=self.model_type)
ordered_joints.append(joints[joint_name])
joints[joint_name].power_coef = 100.0
debugmode = 0
if debugmode:
for j in ordered_joints:
print(j, j.power_coef)
return parts, joints, ordered_joints, self.robot_body
objects = [p.loadURDF("jenga/jenga.urdf", 0.900000,-0.700000,0.750000,0.000000,0.707107,0.000000,0.707107)]
objects = [p.loadURDF("jenga/jenga.urdf", 0.800000,-0.700000,0.750000,0.000000,0.707107,0.000000,0.707107)]
objects = [p.loadURDF("table/table.urdf", 1.000000,-0.200000,0.000000,0.000000,0.000000,0.707107,0.707107)]
objects = [p.loadURDF("cube_small.urdf", 0.950000,-0.100000,0.700000,0.000000,0.000000,0.707107,0.707107)]
objects = [p.loadURDF("sphere_small.urdf", 0.850000,-0.400000,0.700000,0.000000,0.000000,0.707107,0.707107)]
#load the MuJoCo MJCF hand
objects = p.loadMJCF("MPL/mpl2.xml")
hand=objects[0]
ho = p.getQuaternionFromEuler([3.14,-3.14/2,0])
hand_cid = p.createConstraint(hand,-1,-1,-1,p.JOINT_FIXED,[0,0,0],[-0.05,0,0.02],[0.500000,0.300006,0.700000],ho)
print ("hand_cid")
print (hand_cid)
for i in range (p.getNumJoints(hand)):
p.setJointMotorControl2(hand,i,p.POSITION_CONTROL,0,0)
#clamp in range 400-600
#minV = 400
#maxV = 600
minV = 250
maxV = 450
POSITION=1
ORIENTATION=2
BUTTONS=6
p.setRealTimeSimulation(1)
#Load-in a plane
planeId = p.loadURDF("plane.urdf")
#Spawn a robot in a given place
cubeStartPos = [0, 0, 0]
cubeStartOrientation = p.getQuaternionFromEuler([0, 0, 0])
TheArm = p.loadURDF("franka_panda/panda.urdf",
cubeStartPos,
cubeStartOrientation,
useFixedBase=1)
#Information about the setup
print('')
print('Number of joints')
print(p.getNumJoints(TheArm))
print('')
#Waypoints
waypoints = [[0, 0, 0, 0, 0, 0, 0], [0.5, 0.5, 0.5, 0.5, 0.5, 0.5, 0.5],
[-0.5, -0.5, -0.5, -0.5, -0.5, -0.5, -0.5],
[0.5, 0.5, 0.5, 0.5, 0.5, 0.5, 0.5],
[-0.5, -0.5, -0.5, -0.5, -0.5, -0.5, -0.5],
[0.5, 0.5, 0.5, 0.5, 0.5, 0.5, 0.5],
[-0.5, -0.5, -0.5, -0.5, -0.5, -0.5, -0.5]]
timestamps = waypointToTimestamp(maxSpeed, timeStep, waypoints)
print()
print(timestamps)
print()
import pybullet as p
usePort = True
if (usePort):
id = p.connect(p.GRPC,"localhost:12345")
else:
id = p.connect(p.GRPC,"localhost")
print("id=",id)
if (id<0):
print("Cannot connect to GRPC server")
exit(0)
print ("Connected to GRPC")
r2d2 = p.loadURDF("r2d2.urdf")
print("numJoints = ", p.getNumJoints(r2d2))
def print_multibody_links(body_id):
for link_id in range(p.getNumJoints(body_id)):
print(p.getJointInfo(body_id, link_id))
## Loading
physicsClient = p.connect(p.GUI) #or p.DIRECT for non-graphical version
p.setAdditionalSearchPath(pybullet_data.getDataPath()) #optionally
p.setGravity(0, 0, -10)
planeId = p.loadURDF("plane.urdf")
RobotStartPosition = [0, 0, 0.75]
RobotStartOrientation = p.getQuaternionFromEuler([0, 0, 0])
RoboBoi = p.loadURDF("Robot.urdf", RobotStartPosition, RobotStartOrientation)
# Create Parent-Child Map
parent_child_map = numpy.array([[0, 1], [1, 2], [0, 3], [3, 4], [4, 5], \
[3, 6], [6, 7], [7, 8], [0, 9], [9, 10], [10, 11]])
## Print all the links
for i in range(p.getNumJoints(RoboBoi)):
link = p.getJointInfo(RoboBoi, i)
print(link)
print("Here")
## Let's try to move the robot legs so it can stand
## Try moving back legs
p.setJointMotorControl2(RoboBoi, 9, p.POSITION_CONTROL, -0.4)
p.setJointMotorControl2(RoboBoi, 6, p.POSITION_CONTROL, -0.4)
INITSTATE = p.saveState()
originalPos, originalOrientation = p.getBasePositionAndOrientation(RoboBoi)
a = numpy.zeros(p.getNumJoints(RoboBoi))
b = numpy.zeros(p.getNumJoints(RoboBoi))
c = numpy.zeros(p.getNumJoints(RoboBoi))
omega = numpy.zeros(p.getNumJoints(RoboBoi))
import pybullet as p
import time
import math
import numpy as np
import random
PI = math.pi
DIST = 4
t = 0
p.connect(p.GUI)
p.loadURDF("pushing/plane.urdf", [0, 0, 0], globalScaling=100.0, useFixedBase=True)
cube_id = p.loadURDF("pushing/cube.urdf", [2, 2, 1], useFixedBase=False)
grip_id = p.loadURDF("pushing/pr2_gripper.urdf", [0, 0, 4], globalScaling=4.0, useFixedBase=False)
constraint_id = p.createConstraint(grip_id, -1, -1, -1, p.JOINT_FIXED, [0, 0, 0], [0, 0, 0], [0, 0, 1])
num_joints = p.getNumJoints(grip_id)
def eachIter():
"""
Defines an iteration through time
"""
p.setGravity(0, 0, -10)
p.stepSimulation()
# time.sleep(.005)
def apply_push(dist, iters, orn, open_gripper):
"""
Applies a push to the cube with the corresponding parameters
:param dist: distance the gripper will travel after contact with the block
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()
def start():
p.connect(p.GUI)
p.setAdditionalSearchPath(pd.getDataPath())
plane = p.loadURDF("plane.urdf")
p.setGravity(0, 0, -9.8)
p.setTimeStep(1. / 500)
# p.setDefaultContactERP(0)
# urdfFlags = p.URDF_USE_SELF_COLLISION+p.URDF_USE_SELF_COLLISION_EXCLUDE_ALL_PARENTS
urdfFlags = p.URDF_USE_SELF_COLLISION
quadruped = p.loadURDF("laikago/laikago_toes.urdf", [0, 0, .5],
[0, 0.5, 0.5, 0],
flags=urdfFlags,
useFixedBase=False)
# enable collision between lower legs
for j in range(p.getNumJoints(quadruped)):
print(p.getJointInfo(quadruped, j))
# 2,5,8 and 11 are the lower legs
lower_legs = [2, 5, 8, 11]
for l0 in lower_legs:
for l1 in lower_legs:
if (l1 > l0):
enableCollision = 1
print("collision for pair", l0, l1,
p.getJointInfo(quadruped, l0)[12],
p.getJointInfo(quadruped, l1)[12], "enabled=",
enableCollision)
p.setCollisionFilterPair(quadruped, quadruped, 2, 5,
enableCollision)
jointIds = []
paramIds = []
jointOffsets = []
jointDirections = [-1, 1, 1, 1, 1, 1, -1, 1, 1, 1, 1, 1]
jointAngles = [0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0]
for i in range(4):
jointOffsets.append(0)
jointOffsets.append(-0.7)
jointOffsets.append(0.7)
maxForceId = p.addUserDebugParameter("maxForce", 0, 100, 20)
for j in range(p.getNumJoints(quadruped)):
p.changeDynamics(quadruped, j, linearDamping=0, angularDamping=0)
info = p.getJointInfo(quadruped, j)
# print(info)
jointName = info[1]
jointType = info[2]
if (jointType == p.JOINT_PRISMATIC or jointType == p.JOINT_REVOLUTE):
jointIds.append(j)
p.getCameraImage(480, 320)
p.setRealTimeSimulation(0)
joints = []
with open(pd.getDataPath() + "/laikago/data1.txt", "r") as filestream:
for line in filestream:
print("line=", line)
maxForce = p.readUserDebugParameter(maxForceId)
currentline = line.split(",")
# print (currentline)
# print("-----")
frame = currentline[0]
t = currentline[1]
# print("frame[",frame,"]")
joints = currentline[2:14]
# print("joints=",joints)
for j in range(12):
targetPos = float(joints[j])
p.setJointMotorControl2(quadruped,
jointIds[j],
p.POSITION_CONTROL,
jointDirections[j] * targetPos +
jointOffsets[j],
force=maxForce)
p.stepSimulation()
for lower_leg in lower_legs:
# print("points for ", quadruped, " link: ", lower_leg)
pts = p.getContactPoints(quadruped, -1, lower_leg)
# print("num points=",len(pts))
# for pt in pts:
# print(pt[9])
time.sleep(1. / 500.)
index = 0
for j in range(p.getNumJoints(quadruped)):
p.changeDynamics(quadruped, j, linearDamping=0, angularDamping=0)
info = p.getJointInfo(quadruped, j)
js = p.getJointState(quadruped, j)
# print(info)
jointName = info[1]
jointType = info[2]
if (jointType == p.JOINT_PRISMATIC or jointType == p.JOINT_REVOLUTE):
paramIds.append(
p.addUserDebugParameter(jointName.decode("utf-8"), -4, 4,
(js[0] - jointOffsets[index]) /
jointDirections[index]))
index = index + 1
p.setRealTimeSimulation(1)
while (1):
for i in range(len(paramIds)):
c = paramIds[i]
targetPos = p.readUserDebugParameter(c)
maxForce = p.readUserDebugParameter(maxForceId)
p.setJointMotorControl2(quadruped,
jointIds[i],
p.POSITION_CONTROL,
jointDirections[i] * targetPos +
jointOffsets[i],
force=maxForce)
