Python pybullet.calculateInverseKinematics() Examples
The following are 6
code examples of pybullet.calculateInverseKinematics().
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: inverse_kinematics_husky_kuka.py From soccer-matlab with BSD 2-Clause "Simplified" License | 6 votes |
|
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
Example #2
| Source File: panda_ik_controller.py From robosuite with MIT License | 6 votes |
|
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
Example #3
| Source File: widowx_controller.py From visual_foresight with MIT License | 6 votes |
|
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
Example #4
| Source File: sawyer_ik_controller.py From robosuite with MIT License | 5 votes |
|
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
Example #5
| Source File: util.py From assistive-gym with MIT License | 5 votes |
|
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
Example #6
| Source File: baxter_ik_controller.py From robosuite with MIT License | 4 votes |
|
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:]
