Python pybullet.createConstraint方法代碼示例

# 需要導入模塊: import pybullet [as 別名]
# 或者: from pybullet import createConstraint [as 別名]
def place(self, pos, rot, joints):
        pb.resetBasePositionAndOrientation(self.handle, pos, rot)
        pb.createConstraint(
            self.handle, -1, -1, -1, pb.JOINT_FIXED, pos, [0, 0, 0], rot)
        for i, q in enumerate(joints):
            pb.resetJointState(self.handle, i, q)

        # gripper
        pb.resetJointState(self.handle, self.left_knuckle, 0)
        pb.resetJointState(self.handle, self.right_knuckle, 0)

        pb.resetJointState(self.handle, self.left_finger, 0)
        pb.resetJointState(self.handle, self.right_finger, 0)

        pb.resetJointState(self.handle, self.left_fingertip, 0)
        pb.resetJointState(self.handle, self.right_fingertip, 0)

        self.arm(joints,)
        self.gripper(0) 

# 需要導入模塊: import pybullet [as 別名]
# 或者: from pybullet import createConstraint [as 別名]
def robot_specific_reset(self, reload_urdf=True):
        """Reset the minitaur to its initial states.

        Args:
          reload_urdf: Whether to reload the urdf file. If not, Reset() just place
            the minitaur back to its starting position.
        """
        if self.minitaur is None:
            self.minitaur = self.robot_ids[0]

        if self.joint_name_to_id is None:
            self._BuildJointNameToIdDict()
            self._BuildMotorIdList()
            self._RecordMassInfoFromURDF()
            self.ResetPose(add_constraint=True)
            self._overheat_counter = np.zeros(self.num_motors)
            self._motor_enabled_list = [True] * self.num_motors
            if self.on_rack:
                p.createConstraint(self.minitaur, -1, -1, -1, p.JOINT_FIXED,[0, 0, 0], [0, 0, 0], [0, 0, 1])
        self.ResetPose(add_constraint=True) 

# 需要導入模塊: import pybullet [as 別名]
# 或者: from pybullet import createConstraint [as 別名]
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 

# 需要導入模塊: import pybullet [as 別名]
# 或者: from pybullet import createConstraint [as 別名]
def init_tool(self, robot, mesh_scale=[1]*3, pos_offset=[0]*3, orient_offset=[0, 0, 0, 1], left=True, maximal=False, alpha=1.0):
        if left:
            gripper_pos, gripper_orient = p.getLinkState(robot, 76 if self.robot_type=='pr2' else 18 if self.robot_type=='sawyer' else 47 if self.robot_type=='baxter' else 8, computeForwardKinematics=True, physicsClientId=self.id)[:2]
        else:
            gripper_pos, gripper_orient = p.getLinkState(robot, 54 if self.robot_type=='pr2' else 18 if self.robot_type=='sawyer' else 25 if self.robot_type=='baxter' else 8, computeForwardKinematics=True, physicsClientId=self.id)[:2]
        transform_pos, transform_orient = p.multiplyTransforms(positionA=gripper_pos, orientationA=gripper_orient, positionB=pos_offset, orientationB=orient_offset, physicsClientId=self.id)
        if self.task == 'scratch_itch':
            tool = p.loadURDF(os.path.join(self.directory, 'scratcher', 'tool_scratch.urdf'), basePosition=transform_pos, baseOrientation=transform_orient, physicsClientId=self.id)
        elif self.task == 'bed_bathing':
            tool = p.loadURDF(os.path.join(self.directory, 'bed_bathing', 'wiper.urdf'), basePosition=transform_pos, baseOrientation=transform_orient, physicsClientId=self.id)
        elif self.task in ['drinking', 'scooping', 'feeding', 'arm_manipulation']:
            if self.task == 'drinking':
                visual_filename = os.path.join(self.directory, 'dinnerware', 'plastic_coffee_cup.obj')
                collision_filename = os.path.join(self.directory, 'dinnerware', 'plastic_coffee_cup_vhacd.obj')
            elif self.task in ['scooping', 'feeding']:
                visual_filename = os.path.join(self.directory, 'dinnerware', 'spoon_reduced_compressed.obj')
                collision_filename = os.path.join(self.directory, 'dinnerware', 'spoon_vhacd.obj')
            elif self.task == 'arm_manipulation':
                visual_filename = os.path.join(self.directory, 'arm_manipulation', 'arm_manipulation_scooper.obj')
                collision_filename = os.path.join(self.directory, 'arm_manipulation', 'arm_manipulation_scooper_vhacd.obj')
            tool_visual = p.createVisualShape(shapeType=p.GEOM_MESH, fileName=visual_filename, meshScale=mesh_scale, rgbaColor=[1, 1, 1, alpha], physicsClientId=self.id)
            tool_collision = p.createCollisionShape(shapeType=p.GEOM_MESH, fileName=collision_filename, meshScale=mesh_scale, physicsClientId=self.id)
            tool = p.createMultiBody(baseMass=0.01, baseCollisionShapeIndex=tool_collision, baseVisualShapeIndex=tool_visual, basePosition=transform_pos, baseOrientation=transform_orient, useMaximalCoordinates=maximal, physicsClientId=self.id)
        if left:
            # Disable collisions between the tool and robot
            for j in (range(71, 86) if self.robot_type == 'pr2' else [18, 20, 21, 22, 23] if self.robot_type == 'sawyer' else [47, 49, 50, 51, 52] if self.robot_type == 'baxter' else [7, 8, 9, 10, 11, 12, 13, 14]):
                for tj in list(range(p.getNumJoints(tool, physicsClientId=self.id))) + [-1]:
                    p.setCollisionFilterPair(robot, tool, j, tj, False, physicsClientId=self.id)
            # Create constraint that keeps the tool in the gripper
            constraint = p.createConstraint(robot, 76 if self.robot_type=='pr2' else 18 if self.robot_type=='sawyer' else 47 if self.robot_type=='baxter' else 8, tool, -1, p.JOINT_FIXED, [0, 0, 0], parentFramePosition=pos_offset, childFramePosition=[0, 0, 0], parentFrameOrientation=orient_offset, physicsClientId=self.id)
        else:
            # Disable collisions between the tool and robot
            for j in (range(49, 64) if self.robot_type == 'pr2' else [18, 20, 21, 22, 23] if self.robot_type == 'sawyer' else [25, 27, 28, 29, 30] if self.robot_type == 'baxter' else [7, 8, 9, 10, 11, 12, 13, 14]):
                for tj in list(range(p.getNumJoints(tool, physicsClientId=self.id))) + [-1]:
                    p.setCollisionFilterPair(robot, tool, j, tj, False, physicsClientId=self.id)
            # Create constraint that keeps the tool in the gripper
            constraint = p.createConstraint(robot, 54 if self.robot_type=='pr2' else 18 if self.robot_type=='sawyer' else 25 if self.robot_type=='baxter' else 8, tool, -1, p.JOINT_FIXED, [0, 0, 0], parentFramePosition=pos_offset, childFramePosition=[0, 0, 0], parentFrameOrientation=orient_offset, physicsClientId=self.id)
        p.changeConstraint(constraint, maxForce=500, physicsClientId=self.id)
        return tool 

# 需要導入模塊: import pybullet [as 別名]
# 或者: from pybullet import createConstraint [as 別名]
def place(self, pos, rot, joints):
        pb.resetBasePositionAndOrientation(self.handle, pos, rot)
        pb.createConstraint(
            self.handle, -1, -1, -1, pb.JOINT_FIXED, pos, [0, 0, 0], rot)
        for i, q in enumerate(joints):
            pb.resetJointState(self.handle, i, q)

        # gripper state
        for joint in self.gripper_indices:
            pb.resetJointState(self.handle, joint, 0.)

        # send commands
        self.arm(joints,)
        self.gripper(self.gripperOpenCommand()) 

# 需要導入模塊: import pybullet [as 別名]
# 或者: from pybullet import createConstraint [as 別名]
def robot_specific_reset(self):
        WalkerBase.robot_specific_reset(self)
        
        humanoidId = -1
        numBodies = p.getNumBodies()
        for i in range (numBodies):
            bodyInfo = p.getBodyInfo(i)
            if bodyInfo[1].decode("ascii") == 'humanoid':
                humanoidId = i
        ## Spherical radiance/glass shield to protect the robot's camera
        if self.glass_id is None:
            glass_id = p.loadMJCF(os.path.join(self.physics_model_dir, "glass.xml"))[0]
            #print("setting up glass", glass_id, humanoidId)
            p.changeVisualShape(glass_id, -1, rgbaColor=[0, 0, 0, 0])
            cid = p.createConstraint(humanoidId, -1, glass_id,-1,p.JOINT_FIXED,[0,0,0],[0,0,1.4],[0,0,1])

        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] 

# 需要導入模塊: import pybullet [as 別名]
# 或者: from pybullet import createConstraint [as 別名]
def resetPose(self):
    kneeFrictionForce = 0
    halfpi = 1.57079632679
    kneeangle = -2.1834 #halfpi - acos(upper_leg_length / lower_leg_length)

    #left front leg
    p.resetJointState(self.quadruped,self.jointNameToId['motor_front_leftL_joint'],self.motorDir[0]*halfpi)
    p.resetJointState(self.quadruped,self.jointNameToId['knee_front_leftL_link'],self.motorDir[0]*kneeangle)
    p.resetJointState(self.quadruped,self.jointNameToId['motor_front_leftR_joint'],self.motorDir[1]*halfpi)
    p.resetJointState(self.quadruped,self.jointNameToId['knee_front_leftR_link'],self.motorDir[1]*kneeangle)
    p.createConstraint(self.quadruped,self.jointNameToId['knee_front_leftR_link'],self.quadruped,self.jointNameToId['knee_front_leftL_link'],p.JOINT_POINT2POINT,[0,0,0],[0,0.005,0.2],[0,0.01,0.2])
    self.setMotorAngleByName('motor_front_leftL_joint', self.motorDir[0]*halfpi)
    self.setMotorAngleByName('motor_front_leftR_joint', self.motorDir[1]*halfpi)
    p.setJointMotorControl2(bodyIndex=self.quadruped,jointIndex=self.jointNameToId['knee_front_leftL_link'],controlMode=p.VELOCITY_CONTROL,targetVelocity=0,force=kneeFrictionForce)
    p.setJointMotorControl2(bodyIndex=self.quadruped,jointIndex=self.jointNameToId['knee_front_leftR_link'],controlMode=p.VELOCITY_CONTROL,targetVelocity=0,force=kneeFrictionForce)

    #left back leg
    p.resetJointState(self.quadruped,self.jointNameToId['motor_back_leftL_joint'],self.motorDir[2]*halfpi)
    p.resetJointState(self.quadruped,self.jointNameToId['knee_back_leftL_link'],self.motorDir[2]*kneeangle)
    p.resetJointState(self.quadruped,self.jointNameToId['motor_back_leftR_joint'],self.motorDir[3]*halfpi)
    p.resetJointState(self.quadruped,self.jointNameToId['knee_back_leftR_link'],self.motorDir[3]*kneeangle)
    p.createConstraint(self.quadruped,self.jointNameToId['knee_back_leftR_link'],self.quadruped,self.jointNameToId['knee_back_leftL_link'],p.JOINT_POINT2POINT,[0,0,0],[0,0.005,0.2],[0,0.01,0.2])
    self.setMotorAngleByName('motor_back_leftL_joint',self.motorDir[2]*halfpi)
    self.setMotorAngleByName('motor_back_leftR_joint',self.motorDir[3]*halfpi)
    p.setJointMotorControl2(bodyIndex=self.quadruped,jointIndex=self.jointNameToId['knee_back_leftL_link'],controlMode=p.VELOCITY_CONTROL,targetVelocity=0,force=kneeFrictionForce)
    p.setJointMotorControl2(bodyIndex=self.quadruped,jointIndex=self.jointNameToId['knee_back_leftR_link'],controlMode=p.VELOCITY_CONTROL,targetVelocity=0,force=kneeFrictionForce)
 
    #right front leg
    p.resetJointState(self.quadruped,self.jointNameToId['motor_front_rightL_joint'],self.motorDir[4]*halfpi)
    p.resetJointState(self.quadruped,self.jointNameToId['knee_front_rightL_link'],self.motorDir[4]*kneeangle)
    p.resetJointState(self.quadruped,self.jointNameToId['motor_front_rightR_joint'],self.motorDir[5]*halfpi)
    p.resetJointState(self.quadruped,self.jointNameToId['knee_front_rightR_link'],self.motorDir[5]*kneeangle)
    p.createConstraint(self.quadruped,self.jointNameToId['knee_front_rightR_link'],self.quadruped,self.jointNameToId['knee_front_rightL_link'],p.JOINT_POINT2POINT,[0,0,0],[0,0.005,0.2],[0,0.01,0.2])
    self.setMotorAngleByName('motor_front_rightL_joint',self.motorDir[4]*halfpi)
    self.setMotorAngleByName('motor_front_rightR_joint',self.motorDir[5]*halfpi)
    p.setJointMotorControl2(bodyIndex=self.quadruped,jointIndex=self.jointNameToId['knee_front_rightL_link'],controlMode=p.VELOCITY_CONTROL,targetVelocity=0,force=kneeFrictionForce)
    p.setJointMotorControl2(bodyIndex=self.quadruped,jointIndex=self.jointNameToId['knee_front_rightR_link'],controlMode=p.VELOCITY_CONTROL,targetVelocity=0,force=kneeFrictionForce)
 

    #right back leg
    p.resetJointState(self.quadruped,self.jointNameToId['motor_back_rightL_joint'],self.motorDir[6]*halfpi)
    p.resetJointState(self.quadruped,self.jointNameToId['knee_back_rightL_link'],self.motorDir[6]*kneeangle)
    p.resetJointState(self.quadruped,self.jointNameToId['motor_back_rightR_joint'],self.motorDir[7]*halfpi)
    p.resetJointState(self.quadruped,self.jointNameToId['knee_back_rightR_link'],self.motorDir[7]*kneeangle)
    p.createConstraint(self.quadruped,self.jointNameToId['knee_back_rightR_link'],self.quadruped,self.jointNameToId['knee_back_rightL_link'],p.JOINT_POINT2POINT,[0,0,0],[0,0.005,0.2],[0,0.01,0.2])
    self.setMotorAngleByName('motor_back_rightL_joint',self.motorDir[6]*halfpi)
    self.setMotorAngleByName('motor_back_rightR_joint',self.motorDir[7]*halfpi)
    p.setJointMotorControl2(bodyIndex=self.quadruped,jointIndex=self.jointNameToId['knee_back_rightL_link'],controlMode=p.VELOCITY_CONTROL,targetVelocity=0,force=kneeFrictionForce)
    p.setJointMotorControl2(bodyIndex=self.quadruped,jointIndex=self.jointNameToId['knee_back_rightR_link'],controlMode=p.VELOCITY_CONTROL,targetVelocity=0,force=kneeFrictionForce)