当前位置: 首页>>代码示例>>Python>>正文


Python pybullet.calculateInverseKinematics方法代码示例

本文整理汇总了Python中pybullet.calculateInverseKinematics方法的典型用法代码示例。如果您正苦于以下问题:Python pybullet.calculateInverseKinematics方法的具体用法?Python pybullet.calculateInverseKinematics怎么用?Python pybullet.calculateInverseKinematics使用的例子?那么, 这里精选的方法代码示例或许可以为您提供帮助。您也可以进一步了解该方法所在pybullet的用法示例。


在下文中一共展示了pybullet.calculateInverseKinematics方法的6个代码示例,这些例子默认根据受欢迎程度排序。您可以为喜欢或者感觉有用的代码点赞,您的评价将有助于系统推荐出更棒的Python代码示例。

示例1: accurateCalculateInverseKinematics

# 需要导入模块: import pybullet [as 别名]
# 或者: from pybullet import calculateInverseKinematics [as 别名]
def accurateCalculateInverseKinematics( kukaId, endEffectorId, targetPos, threshold, maxIter):
	closeEnough = False
	iter = 0
	dist2 = 1e30
	while (not closeEnough and iter<maxIter):
		jointPoses = p.calculateInverseKinematics(kukaId,kukaEndEffectorIndex,targetPos)
		for i in range (numJoints):
			p.resetJointState(kukaId,i,jointPoses[i])
		ls = p.getLinkState(kukaId,kukaEndEffectorIndex)	
		newPos = ls[4]
		diff = [targetPos[0]-newPos[0],targetPos[1]-newPos[1],targetPos[2]-newPos[2]]
		dist2 = (diff[0]*diff[0] + diff[1]*diff[1] + diff[2]*diff[2])
		closeEnough = (dist2 < threshold)
		iter=iter+1
	#print ("Num iter: "+str(iter) + "threshold: "+str(dist2))
	return jointPoses 
开发者ID:utra-robosoccer,项目名称:soccer-matlab,代码行数:18,代码来源:inverse_kinematics_husky_kuka.py

示例2: _calculate_ik

# 需要导入模块: import pybullet [as 别名]
# 或者: from pybullet import calculateInverseKinematics [as 别名]
def _calculate_ik(self, targetPos, targetQuat, threshold=1e-5, maxIter=1000, nJoints=6):
        closeEnough = False
        iter_count = 0
        dist2 = None
        
        best_ret, best_dist = None, float('inf')

        while (not closeEnough and iter_count < maxIter):
            jointPoses = list(p.calculateInverseKinematics(self._armID, 5, targetPos, targetQuat, JOINT_MIN, JOINT_MAX))
            for i in range(nJoints):
                jointPoses[i] = max(min(jointPoses[i], JOINT_MAX[i]), JOINT_MIN[i])
                p.resetJointState(self._armID, i, jointPoses[i])
            
            ls = p.getLinkState(self._armID, 5, computeForwardKinematics=1)
            newPos, newQuat = ls[4], ls[5]
            dist2 = sum([(targetPos[i] - newPos[i]) ** 2 for i in range(3)])
            closeEnough = dist2 < threshold
            iter_count += 1
            
            if dist2 < best_dist:
                best_ret, best_dist = (jointPoses, newPos, newQuat), dist2
        
        return best_ret 
开发者ID:SudeepDasari,项目名称:visual_foresight,代码行数:25,代码来源:widowx_controller.py

示例3: inverse_kinematics

# 需要导入模块: import pybullet [as 别名]
# 或者: from pybullet import calculateInverseKinematics [as 别名]
def inverse_kinematics(self, target_position, target_orientation, rest_poses=None):
        """
        Helper function to do inverse kinematics for a given target position and 
        orientation in the PyBullet world frame.

        Args:
            target_position: A tuple, list, or numpy array of size 3 for position.
            target_orientation: A tuple, list, or numpy array of size 4 for
                a orientation quaternion.
            rest_poses: (optional) A list of size @num_joints to favor ik solutions close by.

        Returns:
            A list of size @num_joints corresponding to the joint angle solution.
        """

        if rest_poses is None:
            ik_solution = list(
                p.calculateInverseKinematics(
                    self.ik_robot,
                    6,
                    target_position,
                    targetOrientation=target_orientation,
                    restPoses=[0, -1.18, 0.00, 2.18, 0.00, 0.57, 3.3161],
                    jointDamping=[0.1] * 7,
                )
            )
        else:
            ik_solution = list(
                p.calculateInverseKinematics(
                    self.ik_robot,
                    6,
                    target_position,
                    targetOrientation=target_orientation,
                    lowerLimits=[-3.05, -3.82, -3.05, -3.05, -2.98, -2.98, -4.71],
                    upperLimits=[3.05, 2.28, 3.05, 3.05, 2.98, 2.98, 4.71],
                    jointRanges=[6.1, 6.1, 6.1, 6.1, 5.96, 5.96, 9.4],
                    restPoses=rest_poses,
                    jointDamping=[0.1] * 7,
                )
            )
        return ik_solution 
开发者ID:StanfordVL,项目名称:robosuite,代码行数:43,代码来源:sawyer_ik_controller.py

示例4: inverse_kinematics

# 需要导入模块: import pybullet [as 别名]
# 或者: from pybullet import calculateInverseKinematics [as 别名]
def inverse_kinematics(self, target_position, target_orientation, rest_poses=None):
        """
        Helper function to do inverse kinematics for a given target position and
        orientation in the PyBullet world frame.

        Args:
            target_position: A tuple, list, or numpy array of size 3 for position.
            target_orientation: A tuple, list, or numpy array of size 4 for
                a orientation quaternion.
            rest_poses: (optional) A list of size @num_joints to favor ik solutions close by.

        Returns:
            A list of size @num_joints corresponding to the joint angle solution.
        """

        if rest_poses is None:
            ik_solution = list(
                p.calculateInverseKinematics(
                    self.ik_robot,
                    7,
                    target_position,
                    targetOrientation=target_orientation,
                    restPoses=rest_poses,
                    jointDamping=[0.1] * 8,
                )
            )
        else:
            ik_solution = list(
                p.calculateInverseKinematics(
                    self.ik_robot,
                    7,
                    target_position,
                    targetOrientation=target_orientation,
                    lowerLimits=[-2.8973, -1.7628, -2.8973, -3.0718, -2.8973, -0.0175, -2.8973],
                    upperLimits=[2.8973, 1.7628, 2.8973, -0.0698, 2.8973, 3.7525, 2.8973],
                    jointRanges=[5.8, 3.5, 5.8, 3.1, 5.8, 3.8, 5.8],
                    restPoses=rest_poses,
                    jointDamping=[0.1] * 8,
                )
            )
        return ik_solution 
开发者ID:StanfordVL,项目名称:robosuite,代码行数:43,代码来源:panda_ik_controller.py

示例5: ik

# 需要导入模块: import pybullet [as 别名]
# 或者: from pybullet import calculateInverseKinematics [as 别名]
def ik(self, body, target_joint, target_pos, target_orient, ik_indices=range(29, 29+7), max_iterations=1000, half_range=False):
        key = '%d_%d' % (body, target_joint)
        if key not in self.ik_lower_limits:
            self.ik_lower_limits[key] = []
            self.ik_upper_limits[key] = []
            self.ik_joint_ranges[key] = []
            self.ik_rest_poses[key] = []
            j_names = []
            for j in range(p.getNumJoints(body, physicsClientId=self.id)):
                if p.getJointInfo(body, j, physicsClientId=self.id)[2] != p.JOINT_FIXED:
                    joint_info = p.getJointInfo(body, j, physicsClientId=self.id)
                    lower_limit = joint_info[8]
                    upper_limit = joint_info[9]
                    # print(len(self.ik_lower_limits[key]), joint_info[1], lower_limit, upper_limit)
                    if lower_limit == 0 and upper_limit == -1:
                        # lower_limit = -1e10
                        # upper_limit = 1e10
                        lower_limit = -2*np.pi
                        upper_limit = 2*np.pi
                    self.ik_lower_limits[key].append(lower_limit)
                    self.ik_upper_limits[key].append(upper_limit)
                    if not half_range:
                        self.ik_joint_ranges[key].append(upper_limit - lower_limit)
                    else:
                        self.ik_joint_ranges[key].append((upper_limit - lower_limit)/2.0)
                    # self.ik_rest_poses[key].append((upper_limit + lower_limit)/2.0)
                    j_names.append([len(j_names)] + list(joint_info[:2]))
        self.ik_rest_poses[key] = self.np_random.uniform(self.ik_lower_limits[key], self.ik_upper_limits[key]).tolist()
        if target_orient is not None:
            ik_joint_poses = np.array(p.calculateInverseKinematics(body, target_joint, targetPosition=target_pos, targetOrientation=target_orient, lowerLimits=self.ik_lower_limits[key], upperLimits=self.ik_upper_limits[key], jointRanges=self.ik_joint_ranges[key], restPoses=self.ik_rest_poses[key], maxNumIterations=max_iterations, physicsClientId=self.id))
        else:
            ik_joint_poses = np.array(p.calculateInverseKinematics(body, target_joint, targetPosition=target_pos, lowerLimits=self.ik_lower_limits[key], upperLimits=self.ik_upper_limits[key], jointRanges=self.ik_joint_ranges[key], restPoses=self.ik_rest_poses[key], maxNumIterations=max_iterations, physicsClientId=self.id))
        # print(j_names)
        # print(ik_joint_poses)
        # exit()
        target_joint_positions = ik_joint_poses[ik_indices]
        return target_joint_positions 
开发者ID:Healthcare-Robotics,项目名称:assistive-gym,代码行数:39,代码来源:util.py

示例6: inverse_kinematics

# 需要导入模块: import pybullet [as 别名]
# 或者: from pybullet import calculateInverseKinematics [as 别名]
def inverse_kinematics(
        self,
        target_position_right,
        target_orientation_right,
        target_position_left,
        target_orientation_left,
        rest_poses,
    ):
        """
        Helper function to do inverse kinematics for a given target position and
        orientation in the PyBullet world frame.

        Args:
            target_position_{right, left}: A tuple, list, or numpy array of size 3 for position.
            target_orientation_{right, left}: A tuple, list, or numpy array of size 4 for
                a orientation quaternion.
            rest_poses: A list of size @num_joints to favor ik solutions close by.

        Returns:
            A list of size @num_joints corresponding to the joint angle solution.
        """

        ndof = 48

        ik_solution = list(
            p.calculateInverseKinematics(
                self.ik_robot,
                self.effector_right,
                target_position_right,
                targetOrientation=target_orientation_right,
                restPoses=rest_poses[:7],
                lowerLimits=self.lower,
                upperLimits=self.upper,
                jointRanges=self.ranges,
                jointDamping=[0.7] * ndof,
            )
        )
        ik_solution2 = list(
            p.calculateInverseKinematics(
                self.ik_robot,
                self.effector_left,
                target_position_left,
                targetOrientation=target_orientation_left,
                restPoses=rest_poses[7:],
                lowerLimits=self.lower,
                upperLimits=self.upper,
                jointRanges=self.ranges,
                jointDamping=[0.7] * ndof,
            )
        )
        for i in range(8, 15):
            ik_solution[i] = ik_solution2[i]

        return ik_solution[1:] 
开发者ID:StanfordVL,项目名称:robosuite,代码行数:56,代码来源:baxter_ik_controller.py


注:本文中的pybullet.calculateInverseKinematics方法示例由纯净天空整理自Github/MSDocs等开源代码及文档管理平台,相关代码片段筛选自各路编程大神贡献的开源项目,源码版权归原作者所有,传播和使用请参考对应项目的License;未经允许,请勿转载。