本文整理汇总了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 示例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 示例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 示例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 示例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 示例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:]