Python pybullet.JOINT_PRISMATIC Examples

The following are 15 code examples of pybullet.JOINT_PRISMATIC(). You can vote up the ones you like or vote down the ones you don't like, and go to the original project or source file by following the links above each example. You may also want to check out all available functions/classes of the module pybullet , or try the search function .
Example #1
Source File: bullet_physics_engine.py    From NTP-vat-release with MIT License 6 votes vote down vote up 
def create_cstr(descr):
        if descr['joint_type'] == 'revolute':
            joint_type = p.JOINT_REVOLUTE
        elif descr['joint_type'] == 'prismatic':
            joint_type = p.JOINT_PRISMATIC
        elif descr['joint_type'] == 'fixed':
            joint_type = p.JOINT_FIXED
        elif descr['joint_type'] == 'point2point':
            joint_type = p.JOINT_POINT2POINT
        else:
            raise ValueError('Unrecognized joint type {}'.format(joint_type))

        if (descr['parent_frame_quat'] is None) or \
                (descr['child_frame_quat'] is None):
            cstr = p.createConstraint(
                    parentBodyUniqueId=descr['parent_body'],
                    parentLinkIndex=descr['parent_link'],
                    childBodyUniqueId=descr['child_body'],
                    childLinkIndex=descr['child_link'],
                    jointType=joint_type,
                    jointAxis=descr['joint_axis'],
                    parentFramePosition=descr['parent_frame_pos'],
                    childFramePosition=descr['child_frame_pos'])
        else:
            cstr = p.createConstraint(
                    parentBodyUniqueId=descr['parent_body'],
                    parentLinkIndex=descr['parent_link'],
                    childBodyUniqueId=descr['child_body'],
                    childLinkIndex=descr['child_link'],
                    jointType=joint_type,
                    jointAxis=descr['joint_axis'],
                    parentFramePosition=descr['parent_frame_pos'],
                    childFramePosition=descr['child_frame_pos'],
                    parentFrameOrientation=descr['parent_frame_quat'],
                    childFrameOrientation=descr['child_frame_quat'])

        return cstr
Example #2
Source File: urdfEditor.py    From soccer-matlab with BSD 2-Clause "Simplified" License 5 votes vote down vote up 
def initialize(self):
		self.urdfLinks=[]
		self.urdfJoints=[]
		self.robotName = ""
		self.linkNameToIndex={}
		self.jointTypeToName={p.JOINT_FIXED:"JOINT_FIXED" ,\
													p.JOINT_REVOLUTE:"JOINT_REVOLUTE",\
													p.JOINT_PRISMATIC:"JOINT_PRISMATIC" }
Example #3
Source File: urdfEditor.py    From soccer-matlab with BSD 2-Clause "Simplified" License 5 votes vote down vote up 
def writeJoint(self, file, urdfJoint, precision=5):
		jointTypeStr = "invalid"
		if urdfJoint.joint_type == p.JOINT_REVOLUTE:
			if urdfJoint.joint_upper_limit < urdfJoint.joint_lower_limit:
				jointTypeStr = "continuous"
			else:
				jointTypeStr = "revolute"
		if urdfJoint.joint_type == p.JOINT_FIXED:
			jointTypeStr  = "fixed"
		if urdfJoint.joint_type == p.JOINT_PRISMATIC:
			jointTypeStr  = "prismatic"
		str = '\t<joint name=\"{}\" type=\"{}\">\n'.format(urdfJoint.joint_name, jointTypeStr)
		file.write(str)
		str = '\t\t<parent link=\"{}\"/>\n'.format(urdfJoint.parent_name)
		file.write(str)
		str = '\t\t<child link=\"{}\"/>\n'.format(urdfJoint.child_name)
		file.write(str)

		if urdfJoint.joint_type == p.JOINT_PRISMATIC:
			#todo: handle limits
			lowerLimit=-0.5
			upperLimit=0.5
			str='<limit effort="1000.0" lower="{:.{prec}f}" upper="{:.{prec}f}" velocity="0.5"/>'.format(lowerLimit,upperLimit,prec=precision)
			file.write(str)

		file.write("\t\t<dynamics damping=\"1.0\" friction=\"0.0001\"/>\n")
		str = '\t\t<origin xyz=\"{:.{prec}f} {:.{prec}f} {:.{prec}f}\"/>\n'.format(urdfJoint.joint_origin_xyz[0],\
			urdfJoint.joint_origin_xyz[1],urdfJoint.joint_origin_xyz[2], prec=precision)
		str = '\t\t<origin rpy=\"{:.{prec}f} {:.{prec}f} {:.{prec}f}\" xyz=\"{:.{prec}f} {:.{prec}f} {:.{prec}f}\"/>\n'.format(urdfJoint.joint_origin_rpy[0],\
			urdfJoint.joint_origin_rpy[1],urdfJoint.joint_origin_rpy[2],\
			urdfJoint.joint_origin_xyz[0],\
			urdfJoint.joint_origin_xyz[1],urdfJoint.joint_origin_xyz[2], prec=precision)

		file.write(str)
		str = '\t\t<axis xyz=\"{:.{prec}f} {:.{prec}f} {:.{prec}f}\"/>\n'.format(urdfJoint.joint_axis_xyz[0],\
			urdfJoint.joint_axis_xyz[1],urdfJoint.joint_axis_xyz[2], prec=precision)
		file.write(str)
		file.write("\t</joint>\n")
Example #4
Source File: robot_bases.py    From GtS with MIT License 5 votes vote down vote up 
def __init__(self, joint_name, bodies, bodyIndex, jointIndex, scale, model_type):
        self.bodies = bodies
        self.bodyIndex = bodyIndex
        self.jointIndex = jointIndex
        self.joint_name = joint_name
        _,_,self.jointType,_,_,_,_,_,self.lowerLimit, self.upperLimit,_,_,_, _,_,_,_ = p.getJointInfo(self.bodies[self.bodyIndex], self.jointIndex)
        self.power_coeff = 0
        if model_type=="MJCF":
            self.scale = scale
        else:
            self.scale = 1
        if self.jointType == p.JOINT_PRISMATIC:
            self.upperLimit *= self.scale
            self.lowerLimit *= self.scale
Example #5
Source File: robot_bases.py    From GtS with MIT License 5 votes vote down vote up 
def get_state(self):
        """Get state of joint
           Position is defined in real world scale """
        x, vx,_,_ = p.getJointState(self.bodies[self.bodyIndex],self.jointIndex)
        if self.jointType == p.JOINT_PRISMATIC:
            x  *= self.scale
            vx *= self.scale
        return x, vx
Example #6
Source File: robot_bases.py    From GtS with MIT License 5 votes vote down vote up 
def set_state(self, x, vx):
        """Set state of joint
           x is defined in real world scale """
        if self.jointType == p.JOINT_PRISMATIC:
            x  /= self.scale
            vx /= self.scale
        p.resetJointState(self.bodies[self.bodyIndex], self.jointIndex, x, vx)
Example #7
Source File: robot_bases.py    From GtS with MIT License 5 votes vote down vote up 
def set_position(self, position):
        """Set position of joint
           Position is defined in real world scale """
        if self.jointType == p.JOINT_PRISMATIC:
            position = np.array(position) / self.scale
        p.setJointMotorControl2(self.bodies[self.bodyIndex],self.jointIndex,p.POSITION_CONTROL, targetPosition=position)
Example #8
Source File: robot_bases.py    From GtS with MIT License 5 votes vote down vote up 
def set_velocity(self, velocity):
        """Set velocity of joint
           Velocity is defined in real world scale """
        if self.jointType == p.JOINT_PRISMATIC:
            velocity = np.array(velocity) / self.scale
        p.setJointMotorControl2(self.bodies[self.bodyIndex],self.jointIndex,p.VELOCITY_CONTROL, targetVelocity=velocity) # , positionGain=0.1, velocityGain=0.1)
Example #9
Source File: robot_bases.py    From midlevel-reps with MIT License 5 votes vote down vote up 
def __init__(self, joint_name, bodies, bodyIndex, jointIndex, scale, model_type):
        self.bodies = bodies
        self.bodyIndex = bodyIndex
        self.jointIndex = jointIndex
        self.joint_name = joint_name
        _,_,self.jointType,_,_,_,_,_,self.lowerLimit, self.upperLimit,_,_,_, _,_,_,_ = p.getJointInfo(self.bodies[self.bodyIndex], self.jointIndex)
        self.power_coeff = 0
        if model_type=="MJCF":
            self.scale = scale
        else:
            self.scale = 1
        if self.jointType == p.JOINT_PRISMATIC:
            self.upperLimit *= self.scale
            self.lowerLimit *= self.scale
Example #10
Source File: robot_bases.py    From midlevel-reps with MIT License 5 votes vote down vote up 
def get_state(self):
        """Get state of joint
           Position is defined in real world scale """
        x, vx,_,_ = p.getJointState(self.bodies[self.bodyIndex],self.jointIndex)
        if self.jointType == p.JOINT_PRISMATIC:
            x  *= self.scale
            vx *= self.scale
        return x, vx
Example #11
Source File: robot_bases.py    From midlevel-reps with MIT License 5 votes vote down vote up 
def set_state(self, x, vx):
        """Set state of joint
           x is defined in real world scale """
        if self.jointType == p.JOINT_PRISMATIC:
            x  /= self.scale
            vx /= self.scale
        p.resetJointState(self.bodies[self.bodyIndex], self.jointIndex, x, vx)
Example #12
Source File: robot_bases.py    From midlevel-reps with MIT License 5 votes vote down vote up 
def set_position(self, position):
        """Set position of joint
           Position is defined in real world scale """
        if self.jointType == p.JOINT_PRISMATIC:
            position = np.array(position) / self.scale
        p.setJointMotorControl2(self.bodies[self.bodyIndex],self.jointIndex,p.POSITION_CONTROL, targetPosition=position)
Example #13
Source File: robot_bases.py    From midlevel-reps with MIT License 5 votes vote down vote up 
def set_velocity(self, velocity):
        """Set velocity of joint
           Velocity is defined in real world scale """
        if self.jointType == p.JOINT_PRISMATIC:
            velocity = np.array(velocity) / self.scale
        p.setJointMotorControl2(self.bodies[self.bodyIndex],self.jointIndex,p.VELOCITY_CONTROL, targetVelocity=velocity) # , positionGain=0.1, velocityGain=0.1)
Example #14
Source File: panda_env.py    From pybullet-robot-envs with GNU Lesser General Public License v2.1 5 votes vote down vote up 
def reset(self):
        # Load robot model
        flags = p.URDF_ENABLE_CACHED_GRAPHICS_SHAPES | p.URDF_USE_INERTIA_FROM_FILE
        self.robot_id = p.loadURDF(os.path.join(franka_panda.get_data_path(), "panda_model.urdf"),
                                   basePosition=self._base_position, useFixedBase=True, flags=flags,
                                   physicsClientId=self._physics_client_id)

        assert self.robot_id is not None, "Failed to load the panda model"

        # reset joints to home position
        num_joints = p.getNumJoints(self.robot_id, physicsClientId=self._physics_client_id)
        idx = 0
        for i in range(num_joints):
            joint_info = p.getJointInfo(self.robot_id, i, physicsClientId=self._physics_client_id)
            joint_name = joint_info[1].decode("UTF-8")
            joint_type = joint_info[2]

            if joint_type is p.JOINT_REVOLUTE or joint_type is p.JOINT_PRISMATIC:
                assert joint_name in self.initial_positions.keys()

                self._joint_name_to_ids[joint_name] = i

                p.resetJointState(self.robot_id, i, self.initial_positions[joint_name], physicsClientId=self._physics_client_id)
                p.setJointMotorControl2(self.robot_id, i, p.POSITION_CONTROL,
                                        targetPosition=self.initial_positions[joint_name],
                                        positionGain=0.2, velocityGain=1.0,
                                        physicsClientId=self._physics_client_id)

                idx += 1

        self.ll, self.ul, self.jr, self.rs = self.get_joint_ranges()

        if self._use_IK:

            self._home_hand_pose = [0.2, 0.0, 0.8,
                                    min(m.pi, max(-m.pi, m.pi)),
                                    min(m.pi, max(-m.pi, 0)),
                                    min(m.pi, max(-m.pi, 0))]

            self.apply_action(self._home_hand_pose)
            p.stepSimulation(physicsClientId=self._physics_client_id)
Example #15
Source File: robot_virtual.py    From qibullet with Apache License 2.0 4 votes vote down vote up 
def loadRobot(self, translation, quaternion, physicsClientId=0):
        """
        Loads the robot into a simulation, loads the joints and the links
        descriptions. The joints are set to 0 rad.

        Parameters:
            translation - List containing 3 elements, the translation [x, y, z]
            of the robot in the WORLD frame
            quaternion - List containing 4 elements, the quaternion
            [x, y, z, q] of the robot in the WORLD frame
            physicsClientId - The id of the simulated instance in which the
            robot is supposed to be loaded

        Returns:
            boolean - True if the method ran correctly, False otherwise
        """
        try:
            self.physics_client = physicsClientId
            self.robot_model = pybullet.loadURDF(
                self.description_file,
                translation,
                quaternion,
                useFixedBase=False,
                globalScaling=1.0,
                physicsClientId=self.physics_client,
                flags=pybullet.URDF_USE_SELF_COLLISION |
                pybullet.URDF_USE_MATERIAL_COLORS_FROM_MTL)

        except pybullet.error as e:
            raise pybullet.error("Cannot load robot model: " + str(e))

        for i in range(pybullet.getNumJoints(
                self.robot_model,
                physicsClientId=self.physics_client)):
            if IS_VERSION_PYTHON_3:
                # PYTHON 3 version needs a conversion bytes to str
                joint_info = pybullet.getJointInfo(
                    self.robot_model,
                    i,
                    physicsClientId=self.physics_client)
                self.link_dict[joint_info[12].decode('utf-8')] =\
                    Link(joint_info)

                if joint_info[2] == pybullet.JOINT_PRISMATIC or\
                        joint_info[2] == pybullet.JOINT_REVOLUTE:
                    self.joint_dict[joint_info[1].decode('utf-8')] =\
                        Joint(joint_info)
            else:
                # PYTHON 2 Version
                joint_info = pybullet.getJointInfo(
                    self.robot_model,
                    i,
                    physicsClientId=self.physics_client)

                self.link_dict[joint_info[12]] = Link(joint_info)

                if joint_info[2] == pybullet.JOINT_PRISMATIC or\
                        joint_info[2] == pybullet.JOINT_REVOLUTE:
                    self.joint_dict[joint_info[1]] = Joint(joint_info)