Secure your code as it's written. Use Snyk Code to scan source code in minutes - no build needed - and fix issues immediately.
joints=currentline[2:14]
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)):
joints = {}
if self.ordered_joints is not None:
ordered_joints = self.ordered_joints
else:
ordered_joints = []
part_name, robot_name = p.getBodyInfo(bodies[0])
part_name = part_name.decode("utf8")
parts[part_name] = BodyPart(part_name, bodies, 0, -1, self.scale, model_type=self.model_type)
# By default, use base_link as self.robot_body
if self.robot_name == part_name:
self.robot_body = parts[part_name]
for j in range(p.getNumJoints(bodies[0])):
p.setJointMotorControl2(bodies[0], j, p.POSITION_CONTROL, positionGain=0.1, velocityGain=0.1, force=0)
_, joint_name, joint_type, _, _, _, _, _, _, _, _, _, part_name, _, _, _, _ = p.getJointInfo(bodies[0], j)
joint_name = joint_name.decode("utf8")
part_name = part_name.decode("utf8")
parts[part_name] = BodyPart(part_name, bodies, 0, j, self.scale, model_type=self.model_type)
# If self.robot_name is not base_link, but a body part, use it as self.robot_body
if self.robot_name == part_name:
self.robot_body = parts[part_name]
if joint_name[:6] == "ignore":
Joint(joint_name, bodies, 0, j, self.scale, model_type=self.model_type).disable_motor()
continue
if joint_name[:8] != "jointfix" and joint_type != p.JOINT_FIXED:
def setup_human_joints(self, human, joints_positions, controllable_joints, use_static_joints=True, human_reactive_force=None, human_reactive_gain=0.05):
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:
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:
# 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
for j in range(p.getNumJoints(human, physicsClientId=self.id)):
import pybullet as p
p.connect(p.GUI) #or p.SHARED_MEMORY or p.DIRECT
p.loadURDF("plane.urdf")
p.setGravity(0,0,-10)
huskypos = [0,0,0.1]
husky = p.loadURDF("husky/husky.urdf",huskypos[0],huskypos[1],huskypos[2])
numJoints = p.getNumJoints(husky)
for joint in range (numJoints) :
print (p.getJointInfo(husky,joint))
targetVel = 10 #rad/s
maxForce = 100 #Newton
for joint in range (2,6) :
p.setJointMotorControl(husky,joint,p.VELOCITY_CONTROL,targetVel,maxForce)
for step in range (300):
p.stepSimulation()
targetVel=-10
for joint in range (2,6) :
p.setJointMotorControl(husky,joint,p.VELOCITY_CONTROL,targetVel,maxForce)
for step in range (400):
p.stepSimulation()
def __init__(self):
kukaId = p.loadSDF("kuka_iiwa/kuka_with_gripper2.sdf")
self._kukaId = kukaId[0]
p.resetBasePositionAndOrientation(self._kukaId,[4.8,3.5,-1.2],[0,0,0,1])
self._kukaEndEffectorIndex = 6
self._numJoints = p.getNumJoints(self._kukaId)
self._jointDamping = [0.1]*self._numJoints
print('numJoints')
print(self._numJoints)
self._t = 0
self._hasPrevPose = 0
self._prevPose=[0,0,0]
self._prevPose1=[0,0,0]
self._gripperMaxAngle = 0.5
self._fingerJointIndices = (8, 10, 11, 13)
self._maxTorqueValues = (1.0, 0.5, 1.0, 0.5)
self._fingerJointsOpen = (-self._gripperMaxAngle, 0, self._gripperMaxAngle, 0)
self._fingerJointsClose = (0, 0,
0, 0)
self._controlMode = p.POSITION_CONTROL
def main():
print("create env")
env = gym.make("HopperPyBulletEnv-v0")
env.render(mode="human")
pi = SmallReactivePolicy(env.observation_space, env.action_space)
env.reset()
for i in range (p.getNumBodies()):
print(p.getBodyInfo(i))
if p.getBodyInfo(i)[1].decode() == "hopper":
torsoId = i
print("found torso")
print(p.getNumJoints(torsoId))
for j in range (p.getNumJoints(torsoId)):
print(p.getJointInfo(torsoId,j)) # LinkState(torsoId,j))
while 1:
frame = 0
score = 0
restart_delay = 0
#disable rendering during reset, makes loading much faster
obs = env.reset()
print("frame")
while 1:
time.sleep(0.02)
a = pi.act(obs)
obs, r, done, _ = env.step(a)
score += r
frame += 1
distance = 5
def _BuildJointNameToIdDict(self):
num_joints = pybullet.getNumJoints(self.quadruped)
self._joint_name_to_id = {}
for i in range(num_joints):
joint_info = pybullet.getJointInfo(self.quadruped, i)
self._joint_name_to_id[joint_info[1].decode("UTF-8")] = joint_info[0]
def __init__(self,body_id):
self.body_id = body_id
self.num_links = p.getNumJoints(self.body_id)
self.pose = np.zeros((self.num_links+1,7))
self.velocity = np.zeros((self.num_links+1,6))
self.update()