def inverse_kinematics(self, desired_tip_positions, rest_pose):
"""
:param desired_tip_positions: (list) desired tip positions in world frame.
:param rest_pose: (list) initial inverse kinemetics solution to start from.
:return:
"""
desired = np.array(desired_tip_positions)
desired[2] += WorldConstants.FLOOR_HEIGHT
desired[5] += WorldConstants.FLOOR_HEIGHT
desired[8] += WorldConstants.FLOOR_HEIGHT
if self._pybullet_client_w_o_goal_id is not None:
client = self._pybullet_client_w_o_goal_id
else:
client = self._pybullet_client_full_id
joint_pos = np.zeros([9])
finger_tip_ids = self._finger_tip_ids
final_joint_pose = pybullet.calculateInverseKinematics2(
WorldConstants.ROBOT_ID,
[finger_tip_ids[0], finger_tip_ids[1], finger_tip_ids[2]],
[desired[0:3], desired[3:6], desired[6:]],
solver=pybullet.IK_DLS,
currentPositions=rest_pose,
physicsClientId=client)
joint_pos[:3] = final_joint_pose[:3]
final_joint_pose = pybullet.calculateInverseKinematics2(
WorldConstants.ROBOT_ID,
[finger_tip_ids[1], finger_tip_ids[0], finger_tip_ids[2]],
[desired[3:6], desired[0:3], desired[6:]],
solver=pybullet.IK_DLS,
currentPositions=rest_pose,
physicsClientId=client)
joint_pos[3:6] = final_joint_pose[3:6]
final_joint_pose = pybullet.calculateInverseKinematics2(
WorldConstants.ROBOT_ID,
[finger_tip_ids[2], finger_tip_ids[0], finger_tip_ids[1]],
[desired[6:], desired[0:3], desired[3:6]],
solver=pybullet.IK_DLS,
currentPositions=rest_pose,
physicsClientId=client)
joint_pos[6:] = final_joint_pose[6:]
if np.isnan(joint_pos).any():
joint_pos = rest_pose
return joint_pos
def retarget_pose(robot, default_pose, ref_joint_pos):
joint_lim_low, joint_lim_high = get_joint_limits(robot)
root_pos, root_rot = retarget_root_pose(ref_joint_pos)
root_pos += config.SIM_ROOT_OFFSET
pybullet.resetBasePositionAndOrientation(robot, root_pos, root_rot)
inv_init_rot = transformations.quaternion_inverse(config.INIT_ROT)
heading_rot = motion_util.calc_heading_rot(
transformations.quaternion_multiply(root_rot, inv_init_rot))
tar_toe_pos = []
for i in range(len(REF_TOE_JOINT_IDS)):
ref_toe_id = REF_TOE_JOINT_IDS[i]
ref_hip_id = REF_HIP_JOINT_IDS[i]
sim_hip_id = config.SIM_HIP_JOINT_IDS[i]
toe_offset_local = config.SIM_TOE_OFFSET_LOCAL[i]
ref_toe_pos = ref_joint_pos[ref_toe_id]
ref_hip_pos = ref_joint_pos[ref_hip_id]
hip_link_state = pybullet.getLinkState(robot,
sim_hip_id,
computeForwardKinematics=True)
sim_hip_pos = np.array(hip_link_state[4])
toe_offset_world = pose3d.QuaternionRotatePoint(
toe_offset_local, heading_rot)
ref_hip_toe_delta = ref_toe_pos - ref_hip_pos
sim_tar_toe_pos = sim_hip_pos + ref_hip_toe_delta
sim_tar_toe_pos[2] = ref_toe_pos[2]
sim_tar_toe_pos += toe_offset_world
tar_toe_pos.append(sim_tar_toe_pos)
joint_pose = pybullet.calculateInverseKinematics2(
robot,
config.SIM_TOE_JOINT_IDS,
tar_toe_pos,
jointDamping=config.JOINT_DAMPING,
lowerLimits=joint_lim_low,
upperLimits=joint_lim_high,
restPoses=default_pose)
joint_pose = np.array(joint_pose)
pose = np.concatenate([root_pos, root_rot, joint_pose])
return pose
def inverse_kinematics(self,
link_indices,
target_positions,
num_iters=1000):
angles = pb.calculateInverseKinematics2(
self.robot_id,
link_indices,
target_positions,
# residualThreshold=1e-4, # default 1e-4 not enough for ergo jr
maxNumIterations=num_iters, # default 20 usually not enough
)
a = 0
result = self.get_position()
for r in range(self.num_joints):
if not self.joint_fixed[r]:
result[r] = angles[a]
a += 1
return result
t = 0
step = 0
log = p.startStateLogging(
p.STATE_LOGGING_VIDEO_MP4,
"wspace.mp4",
)
last_angles = [0] * 12
while True:
t += 0.01
step += 1
p.resetBasePositionAndOrientation(Laikago, [0, 0, 0.65], [0, 0, 0, 1])
x = 0.5 * math.cos(t)
y = 0
z = 0.1 + 0.5 * math.sin(t)
targets = [[x, y, z], [x, y, z], [x, y, z], [x, y, z]]
angles = p.calculateInverseKinematics2(Laikago, [3, 7, 11, 15], targets)
print(angles)
p.setJointMotorControlArray(
Laikago, [0, 1, 2, 4, 5, 6, 8, 9, 10, 12, 13, 14],
controlMode=p.POSITION_CONTROL,
targetPositions=angles) #,currentPosition=last_angles)
p.stepSimulation()
last_angles = angles
if (step % 10 == 0):
vis_foot_traj()
time.sleep(0.01)
#p.setJointMotorControl2(bodyIndex=plane, jointIndex=0, controlMode=p.POSITION_CONTROL,targetPosition=0.5,
##currentPosition=state[0] ,
#force=1) #move link to target position
#uncomment this #lockJoint
#p.createConstraint(parentBodyUniqueId=plane, parentLinkIndex=-1, childBodyUniqueId=plane, childLinkIndex=0, jointType=p.JOINT_FIXED, jointAxis=[0,0,0], parentFramePosition=[2,2,2], childFramePosition=[2,2,2] )
#########LOCK joint with force
#p.setJointMotorControl2(bodyIndex=plane, jointIndex=0, controlMode=p.VELOCITY_CONTROL, targetVelocity=0, force=10000)#lock joint with large force
#p.setJointMotorControl2(bodyIndex=plane, jointIndex=0, controlMode=p.VELOCITY_CONTROL, targetVelocity=1, force=0) #free all joint constraints with force 0
angles = p.calculateInverseKinematics2(
bodyUniqueId=plane,
endEffectorLinkIndices=[3],
targetPositions=[[0, 0.1, 0.1]],
solver=p.IK_DLS
) #the x coordinate is zero because it is a planar robot at the moment #the x coordinate bounds are (-6,6), the y coordinate bounds are (-4,8)
p.setJointMotorControlArray(bodyUniqueId=plane,
jointIndices=[0, 1, 2, 3],
controlMode=p.POSITION_CONTROL,
targetPositions=angles,
forces=[1000, 1000, 1000, 1000])
print(angles)
while (1):
#print(angles)
((l1,
l2, l3, l4, l5, l6), ) = p.getLinkStates(bodyUniqueId=plane,