Python pybullet.getContactPoints方法代码示例

# 需要导入模块: import pybullet [as 别名]
# 或者: from pybullet import getContactPoints [as 别名]
def contactPoints(self):
        """Gets all contact points and forces
        
        Returns:
            list -- list of entries (link_name, position in m, force in N)
        """
        result = []
        contacts = p.getContactPoints(bodyA=self.floor, bodyB=self.robot)
        for contact in contacts:
            link_index = contact[4]
            if link_index >= 0:
                link_name = (p.getJointInfo(self.robot, link_index)[12]).decode()
            else:
                link_name = 'base'
            result.append((link_name, contact[6], contact[9]))

        return result 

# 需要导入模块: import pybullet [as 别名]
# 或者: from pybullet import getContactPoints [as 别名]
def ik_random_restarts(self, body, target_joint, target_pos, target_orient, world_creation, robot_arm_joint_indices, robot_lower_limits, robot_upper_limits, ik_indices=range(29, 29+7), max_iterations=1000, max_ik_random_restarts=50, random_restart_threshold=0.01, half_range=False, step_sim=False, check_env_collisions=False):
        orient_orig = target_orient
        best_ik_joints = None
        best_ik_distance = 0
        for r in range(max_ik_random_restarts):
            target_joint_positions = self.ik(body, target_joint, target_pos, target_orient, ik_indices=ik_indices, max_iterations=max_iterations, half_range=half_range)
            world_creation.setup_robot_joints(body, robot_arm_joint_indices, robot_lower_limits, robot_upper_limits, randomize_joint_positions=False, default_positions=np.array(target_joint_positions), tool=None)
            if step_sim:
                for _ in range(5):
                    p.stepSimulation(physicsClientId=self.id)
                if len(p.getContactPoints(bodyA=body, bodyB=body, physicsClientId=self.id)) > 0 and orient_orig is not None:
                    # The robot's arm is in contact with itself. Continually randomize end effector orientation until a solution is found
                    target_orient = p.getQuaternionFromEuler(p.getEulerFromQuaternion(orient_orig, physicsClientId=self.id) + np.deg2rad(self.np_random.uniform(-45, 45, size=3)), physicsClientId=self.id)
            if check_env_collisions:
                for _ in range(25):
                    p.stepSimulation(physicsClientId=self.id)
            gripper_pos, gripper_orient = p.getLinkState(body, target_joint, computeForwardKinematics=True, physicsClientId=self.id)[:2]
            if np.linalg.norm(target_pos - np.array(gripper_pos)) < random_restart_threshold and (target_orient is None or np.linalg.norm(target_orient - np.array(gripper_orient)) < random_restart_threshold or np.isclose(np.linalg.norm(target_orient - np.array(gripper_orient)), 2, atol=random_restart_threshold)):
                return True, np.array(target_joint_positions)
            if best_ik_joints is None or np.linalg.norm(target_pos - np.array(gripper_pos)) < best_ik_distance:
                best_ik_joints = target_joint_positions
                best_ik_distance = np.linalg.norm(target_pos - np.array(gripper_pos))
        world_creation.setup_robot_joints(body, robot_arm_joint_indices, robot_lower_limits, robot_upper_limits, randomize_joint_positions=False, default_positions=np.array(best_ik_joints), tool=None)
        return False, np.array(best_ik_joints) 

# 需要导入模块: import pybullet [as 别名]
# 或者: from pybullet import getContactPoints [as 别名]
def setup_robot_joints(self, robot, robot_joint_indices, lower_limits, upper_limits, randomize_joint_positions=False, default_positions=[1, 1, 0, -1.75, 0, -1.1, -0.5], tool=None):
        if randomize_joint_positions:
            # Randomize arm joint positions
            # Keep trying random joint positions until the end effector is not colliding with anything
            retry = True
            while retry:
                for j, lower_limit, upper_limit in zip(robot_joint_indices, lower_limits, upper_limits):
                    if lower_limit == -1e10:
                        lower_limit = -np.pi
                        upper_limit = np.pi
                    joint_range = upper_limit - lower_limit
                    p.resetJointState(robot, jointIndex=j, targetValue=self.np_random.uniform(lower_limit + joint_range/6.0, upper_limit - joint_range/6.0), targetVelocity=0, physicsClientId=self.id)
                p.stepSimulation(physicsClientId=self.id)
                retry = len(p.getContactPoints(bodyA=robot, physicsClientId=self.id)) > 0
                if tool is not None:
                    retry = retry or (len(p.getContactPoints(bodyA=tool, physicsClientId=self.id)) > 0)
        else:
            default_positions[default_positions < lower_limits] = lower_limits[default_positions < lower_limits]
            default_positions[default_positions > upper_limits] = upper_limits[default_positions > upper_limits]
            for i, j in enumerate(robot_joint_indices):
                p.resetJointState(robot, jointIndex=j, targetValue=default_positions[i], targetVelocity=0, physicsClientId=self.id) 

# 需要导入模块: import pybullet [as 别名]
# 或者: from pybullet import getContactPoints [as 别名]
def get_total_force(self):
        total_force_on_human = 0
        tool_force = 0
        tool_force_at_target = 0
        target_contact_pos = None
        for c in p.getContactPoints(bodyA=self.tool, physicsClientId=self.id):
            tool_force += c[9]
        for c in p.getContactPoints(bodyA=self.tool, bodyB=self.human, physicsClientId=self.id):
            total_force_on_human += c[9]
            linkA = c[3]
            contact_position = c[6]
            if linkA in [0, 1]:
                # Enforce that contact is close to the target location
                if np.linalg.norm(contact_position - self.target_pos) < 0.025:
                    tool_force_at_target += c[9]
                    target_contact_pos = contact_position
        for c in p.getContactPoints(bodyA=self.robot, bodyB=self.human, physicsClientId=self.id):
            total_force_on_human += c[9]
        return total_force_on_human, tool_force, tool_force_at_target, target_contact_pos 

# 需要导入模块: import pybullet [as 别名]
# 或者: from pybullet import getContactPoints [as 别名]
def get_total_force(self):
        tool_left_force = 0
        tool_right_force = 0
        total_force_on_human = 0
        tool_left_force_on_human = 0
        tool_right_force_on_human = 0
        for c in p.getContactPoints(bodyA=self.robot, physicsClientId=self.id):
            linkA = c[3]
            if linkA == (55 if self.robot_type=='pr2' else 24 if self.robot_type=='sawyer' else 31 if self.robot_type=='baxter' else 9 if self.robot_type=='jaco' else 7):
                tool_right_force += c[9]
            elif linkA == (78 if self.robot_type=='pr2' else 24 if self.robot_type=='sawyer' else 54 if self.robot_type=='baxter' else 9 if self.robot_type=='jaco' else 7):
                tool_left_force += c[9]
        for c in p.getContactPoints(bodyA=self.robot, bodyB=self.human, physicsClientId=self.id):
            total_force_on_human += c[9]
            linkA = c[3]
            linkB = c[4]
            if linkA == (55 if self.robot_type=='pr2' else 24 if self.robot_type=='sawyer' else 31 if self.robot_type=='baxter' else 9 if self.robot_type=='jaco' else 7):
                tool_right_force_on_human += c[9]
            elif linkA == (78 if self.robot_type=='pr2' else 24 if self.robot_type=='sawyer' else 54 if self.robot_type=='baxter' else 9 if self.robot_type=='jaco' else 7):
                tool_left_force_on_human += c[9]
        return tool_left_force, tool_right_force, total_force_on_human, tool_left_force_on_human, tool_right_force_on_human 

# 需要导入模块: import pybullet [as 别名]
# 或者: from pybullet import getContactPoints [as 别名]
def autoCollisions(self):
        """Returns the total amount of N in autocollisions (not with ground)
        
        Returns:
            float -- Newtons of collisions not with ground
        """
        total = 0
        for k in range(1, p.getNumJoints(self.robot)):
            contacts = p.getContactPoints(bodyA=k)
            for contact in contacts:
                if contact[2] != self.floor:
                    total += contact[9]
        return total 

# 需要导入模块: import pybullet [as 别名]
# 或者: from pybullet import getContactPoints [as 别名]
def get_total_force(self):
        robot_force_on_human = 0
        cup_force_on_human = 0
        for c in p.getContactPoints(bodyA=self.robot, bodyB=self.human, physicsClientId=self.id):
            robot_force_on_human += c[9]
        for c in p.getContactPoints(bodyA=self.cup, bodyB=self.human, physicsClientId=self.id):
            cup_force_on_human += c[9]
        return robot_force_on_human, cup_force_on_human 

# 需要导入模块: import pybullet [as 别名]
# 或者: from pybullet import getContactPoints [as 别名]
def get_total_force(self):
        robot_force_on_human = 0
        spoon_force_on_human = 0
        for c in p.getContactPoints(bodyA=self.robot, bodyB=self.human, physicsClientId=self.id):
            robot_force_on_human += c[9]
        for c in p.getContactPoints(bodyA=self.spoon, bodyB=self.human, physicsClientId=self.id):
            spoon_force_on_human += c[9]
        return robot_force_on_human, spoon_force_on_human 

# 需要导入模块: import pybullet [as 别名]
# 或者: from pybullet import getContactPoints [as 别名]
def get_food_rewards(self):
        # Check all food particles to see if they have left the spoon or entered the person's mouth
        # Give the robot a reward or penalty depending on food particle status
        food_reward = 0
        food_hit_human_reward = 0
        food_mouth_velocities = []
        foods_to_remove = []
        for f in self.foods:
            food_pos, food_orient = p.getBasePositionAndOrientation(f, physicsClientId=self.id)
            distance_to_mouth = np.linalg.norm(self.target_pos - food_pos)
            if distance_to_mouth < 0.02:
                # Delete particle and give robot a reward
                food_reward += 20
                self.task_success += 1
                food_velocity = np.linalg.norm(p.getBaseVelocity(f, physicsClientId=self.id)[0])
                food_mouth_velocities.append(food_velocity)
                foods_to_remove.append(f)
                p.resetBasePositionAndOrientation(f, self.np_random.uniform(1000, 2000, size=3), [0, 0, 0, 1], physicsClientId=self.id)
                continue
            elif food_pos[-1] < 0.5 or len(p.getContactPoints(bodyA=f, bodyB=self.table, physicsClientId=self.id)) > 0 or len(p.getContactPoints(bodyA=f, bodyB=self.bowl, physicsClientId=self.id)) > 0:
                # Delete particle and give robot a penalty for spilling food
                food_reward -= 5
                foods_to_remove.append(f)
                continue
            if len(p.getContactPoints(bodyA=f, bodyB=self.human, physicsClientId=self.id)) > 0 and f not in self.foods_hit_person:
                # Record that this food particle just hit the person, so that we can penalize the robot
                self.foods_hit_person.append(f)
                food_hit_human_reward -= 1
        self.foods = [f for f in self.foods if f not in foods_to_remove]
        return food_reward, food_mouth_velocities, food_hit_human_reward 

# 需要导入模块: import pybullet [as 别名]
# 或者: from pybullet import getContactPoints [as 别名]
def _reward(self):
        gripper_pos = self.getArmPos()
        distance = np.linalg.norm(self.button_pos - gripper_pos, 2)
        # print(distance)

        contact_points = p.getContactPoints(self.button_uid, self._kuka.kuka_uid, BUTTON_LINK_IDX)
        reward = int(len(contact_points) > 0)
        self.n_contacts += reward

        contact_with_table = len(p.getContactPoints(self.table_uid, self._kuka.kuka_uid)) > 0

        if distance > self._max_distance or contact_with_table:
            reward = -1
            self.n_steps_outside += 1
        else:
            self.n_steps_outside = 0

        if contact_with_table or self.n_contacts >= N_CONTACTS_BEFORE_TERMINATION \
                or self.n_steps_outside >= N_STEPS_OUTSIDE_SAFETY_SPHERE:
            self.terminated = True

        if self._shape_reward:
            if self._is_discrete:
                return -distance
            else:
                # Button pushed
                if self.terminated and reward > 0:
                    return 50
                # out of bounds
                elif self.terminated and reward < 0:
                    return -250
                # anything else
                else:
                    return -distance

        return reward 

# 需要导入模块: import pybullet [as 别名]
# 或者: from pybullet import getContactPoints [as 别名]
def _check(self, world, state, actor, prev_state=None):
        # Get the pybullet handle for this actor
        handle = actor.robot.handle

        # Check for contact points
        pts = pb.getContactPoints(bodyA=handle, bodyB=self.not_allowed)

        # If return was empty then we can proceed
        return len(pts) == 0 

# 需要导入模块: import pybullet [as 别名]
# 或者: from pybullet import getContactPoints [as 别名]
def contact_list(self):
        return p.getContactPoints(self.bodies[self.bodyIndex], -1, self.bodyPartIndex, -1) 

# 需要导入模块: import pybullet [as 别名]
# 或者: from pybullet import getContactPoints [as 别名]
def contact_list(self):
		return p.getContactPoints(self.bodies[self.bodyIndex], -1, self.bodyPartIndex, -1) 

# 需要导入模块: import pybullet [as 别名]
# 或者: from pybullet import getContactPoints [as 别名]
def check_collision(self, obj_id):
        # check if there is any collision with an object

        contact_pts = p.getContactPoints(obj_id, self.robot_id, physicsClientId=self._physics_client_id)

        # check if the contact is on the fingertip(s)
        n_fingertips_contact, _ = self.check_contact_fingertips(obj_id)

        return (len(contact_pts) - n_fingertips_contact) > 0 

# 需要导入模块: import pybullet [as 别名]
# 或者: from pybullet import getContactPoints [as 别名]
def check_contact(self, body_id, obj_id=None):
        if obj_id is None:
            obj_id = self.obj_id

        pts = p.getContactPoints(obj_id, body_id, physicsClientId=self._physics_client_id)

        return len(pts) > 0