How to use the pybullet.getNumJoints function in pybullet

To help you get started, we’ve selected a few pybullet examples, based on popular ways it is used in public projects.

Secure your code as it's written. Use Snyk Code to scan source code in minutes - no build needed - and fix issues immediately.

github erwincoumans / pybullet_robots / laikago.py View on Github external
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)):
github StanfordVL / GibsonEnvV2 / gibson2 / core / physics / robot_bases.py View on Github external
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:
github Healthcare-Robotics / assistive-gym / assistive_gym / envs / world_creation.py View on Github external
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)):
github Froser / gamemachine / coding / 3rdparty / bullet3 / examples / pybullet / robotcontrol.py View on Github external
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()
github erwincoumans / pybullet_robots / corl_demo / kuka_demo.py View on Github external
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
github benelot / pybullet-gym / pybulletgym / examples / roboschool-weights / enjoy_TF_HopperPyBulletEnv_v0_2017may.py View on Github external
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
github erwincoumans / pybullet_robots / corl_demo / minitaur.py View on Github external
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]
github benelot / bullet-gym / bullet-gym-primitive / envs / VelocityHelper.py View on Github external
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()