def _ee_pos_to_qpos_raw(self, ee_pos, ee_quat=None, fing_dist=0.0,
left_ee_pos=None, left_ee_quat=None,
left_fing_dist=0.0):
qpos = np.array(pybullet.calculateInverseKinematics(
self.info.robot_id, self.info.ee_link_id, ee_pos.tolist(),
None if ee_quat is None else ee_quat.tolist(),
maxNumIterations=1000, residualThreshold=0.0001))
for jid in self.info.finger_jids_lst:
qpos[jid] = np.clip( # finger info (not set by IK)
fing_dist/2.0, self.info.joint_minpos[jid],
self.info.joint_maxpos[jid])
# Take care of left arm, if needed.
if len(self.info.left_arm_jids_lst)>0:
if left_ee_pos is not None:
left_qpos = np.array(pybullet.calculateInverseKinematics(
self.info.robot_id, self.info.left_ee_link_id,
left_ee_pos.tolist(),
None if left_ee_quat is None else left_ee_quat.tolist(),
maxNumIterations=1000, residualThreshold=0.0001))
else:
left_qpos = self.rest_qpos
qpos[self.info.left_arm_jids_lst] = \
left_qpos[self.info.left_arm_jids_lst]
for jid in self.info.left_finger_jids_lst:
qpos[jid] = np.clip( # finger info (not set by IK)
left_fing_dist/2.0, self.info.joint_minpos[jid],
self.info.joint_maxpos[jid])
# IK will find solutions outside of joint limits, so clip.
qpos = np.clip(qpos, self.info.joint_minpos, self.info.joint_maxpos)
return qpos
def inverse_kinematics(self, position, orientation=None):
"""
:param position: target position
:param orientation: target orientation in quaternion format (w, x, y , z)
:return: joint positions that take the end effector to the desired target position and/or orientation, and success status (solution_found) of IK operation.
"""
solution = None
if orientation is None:
solution = pb.calculateInverseKinematics(self._id,
self._ee_link_idx,
targetPosition=position,
physicsClientId=self._uid)
else:
orientation = [
orientation[1], orientation[2], orientation[3], orientation[0]
]
solution = pb.calculateInverseKinematics(
self._id,
self._ee_link_idx,
targetPosition=position,
targetOrientation=orientation,
physicsClientId=self._uid)
return np.array(solution), solution is None
def accurateIK(bodyId, end_effector_id, targetPosition, targetOrientation, lowerLimits, upperLimits, jointRanges,
restPoses,
useNullSpace=False, maxIter=10, threshold=1e-4):
"""
Parameters
----------
bodyId : int
end_effector_id : int
targetPosition : [float, float, float]
targetOrientation: Quaternion
lowerLimits : [float]
upperLimits : [float]
jointRanges : [float]
restPoses : [float]
useNullSpace : bool
maxIter : int
threshold : float
Returns
-------
jointPoses : [float] * numDofs
"""
if useNullSpace:
jointPoses = p.calculateInverseKinematics(bodyId, end_effector_id, targetPosition,
targetOrientation=targetOrientation,
lowerLimits=lowerLimits, upperLimits=upperLimits,
jointRanges=jointRanges,
restPoses=restPoses)
else:
jointPoses = p.calculateInverseKinematics(bodyId, end_effector_id, targetPosition,
targetOrientation=targetOrientation)
return jointPoses
def front_grasp2(botId, rightCoords, leftCoords, data, steps=10, tableId1=None, tableId2=None, both=None, primitive_name=None, front_grasp=False):
revoluteJoints = getRevoluteJoints(botId)
x_prev = p.getLinkState(botId, 28)[0][0]
iter = (rightCoords[0]-x_prev)/steps
i = 0
for i in range(steps):
x_prev += iter
gripperPosition1 = p.calculateInverseKinematics(botId, 28, [x_prev, leftCoords[1], leftCoords[2]], maxNumIterations=1000)
gripperPosition2 = p.calculateInverseKinematics(botId, 51, [x_prev, rightCoords[1], rightCoords[2]], maxNumIterations=1000)
get_primitive_data(botId, data, primitive_name, sequence=1, tableId1=tableId1, tableId2=tableId2, front_grasp=front_grasp, both=both)
p.setJointMotorControlArray(botId,
jointIndices=revoluteJoints[0:10],
controlMode=p.POSITION_CONTROL,
targetPositions=gripperPosition1[0:10])
p.setJointMotorControlArray(botId,
jointIndices=revoluteJoints[10:],
controlMode=p.POSITION_CONTROL,
targetPositions=gripperPosition2[10:])
for i in range(10):
p.stepSimulation()
get_primitive_data(botId, data, primitive_name, sequence=2, tableId1=tableId1, tableId2=tableId2, front_grasp=front_grasp, both=both)
return gripperPosition1, gripperPosition2
def _execute(self, desig):
solution = desig.reference()
if solution['cmd'] == 'access':
kitchen = solution['part-of']
robot = BulletWorld.robot
gripper = solution['gripper']
drawer_handle = solution['drawer-handle']
drawer_joint = solution['drawer-joint']
dis = solution['distance']
inv = p.calculateInverseKinematics(
robot.id, robot.get_link_id(gripper),
kitchen.get_link_position(drawer_handle))
_apply_ik(robot, inv)
time.sleep(0.2)
han_pose = kitchen.get_link_position(drawer_handle)
new_p = [han_pose[0] - dis, han_pose[1], han_pose[2]]
inv = p.calculateInverseKinematics(robot.id,
robot.get_link_id(gripper),
new_p)
_apply_ik(robot, inv)
p.resetJointState(kitchen.id, kitchen.get_joint_id(drawer_joint),
0.3)
spoon = BulletWorld.current_bullet_world.get_objects_by_name(
"spoon")[0]
spoon.set_position([1.15, 0.7, 0.8])
time.sleep(0.5)
def apply_action_pos(self, pos_commands):
dx, dy, dz = pos_commands
effector_id = self.effector_id
link_pos = p.getLinkState(self.jaka_id, effector_id)
self.effector_pos = link_pos[0]
target_pos = (self.effector_pos[0] + dx, self.effector_pos[1] + dy,
self.effector_pos[2] + dz)
rest_poses = self.initial_joints_state
joint_ranges = [4] * len(self.joints_key)
target_velocity = [0] * len(self.joints_key)
if self.use_null_space:
joint_poses = p.calculateInverseKinematics(
self.jaka_id,
effector_id,
target_pos,
lowerLimits=self.joint_lower_limits,
upperLimits=self.joint_upper_limits,
jointRanges=joint_ranges,
restPoses=rest_poses)
else: # use regular KI solution
joint_poses = p.calculateInverseKinematics(self.jaka_id,
self.effector_id,
target_pos)
p.setJointMotorControlArray(self.jaka_id,
self.joints_key,
controlMode=p.POSITION_CONTROL,
targetPositions=joint_poses,
targetVelocities=target_velocity,
forces=self.jointMaxForce)
p.stepSimulation()
def inverse_kinematics_helper(robot, link, target_pose, null_space=None):
(target_point, target_quat) = target_pose
assert target_point is not None
if null_space is not None:
assert target_quat is not None
lower, upper, ranges, rest = null_space
kinematic_conf = p.calculateInverseKinematics(robot,
link,
target_point,
lowerLimits=lower,
upperLimits=upper,
jointRanges=ranges,
restPoses=rest,
physicsClientId=CLIENT)
elif target_quat is None:
#ikSolver = p.IK_DLS or p.IK_SDLS
kinematic_conf = p.calculateInverseKinematics(
robot,
link,
target_point,
#lowerLimits=ll, upperLimits=ul, jointRanges=jr, restPoses=rp, jointDamping=jd,
# solver=ikSolver, maxNumIterations=-1, residualThreshold=-1,
physicsClientId=CLIENT)
else:
kinematic_conf = p.calculateInverseKinematics(robot,
link,
target_point,
target_quat,
physicsClientId=CLIENT)
if (kinematic_conf is None) or any(map(math.isnan, kinematic_conf)):
return None
return kinematic_conf
def inverse_kinematics(robot, link, pose, max_iterations=200, tolerance=1e-3):
(target_point, target_quat) = pose
movable_joints = get_movable_joints(robot)
for iterations in range(max_iterations):
# TODO: stop is no progress
# TODO: stop if collision or invalid joint limits
if target_quat is None:
kinematic_conf = p.calculateInverseKinematics(
robot, link, target_point, physicsClientId=CLIENT)
else:
kinematic_conf = p.calculateInverseKinematics(
robot, link, target_point, target_quat, physicsClientId=CLIENT)
if (kinematic_conf is None) or any(map(math.isnan, kinematic_conf)):
return None
# print('kinematic_conf', kinematic_conf)
set_joint_positions(robot, movable_joints, kinematic_conf)
link_point, link_quat = get_link_pose(robot, link)
# print link_point,link_quat
if np.allclose(link_point, target_point, atol=tolerance, rtol=0):
# print 'found'
break
else:
return None
if violates_limits(robot, movable_joints, kinematic_conf):
# print 'violate'
return None
return kinematic_conf
def experimental_inverse_kinematics(robot,
link,
pose,
null_space=False,
max_iterations=200,
tolerance=1e-3):
(point, quat) = pose
# https://github.com/bulletphysics/bullet3/blob/389d7aaa798e5564028ce75091a3eac6a5f76ea8/examples/SharedMemory/PhysicsClientC_API.cpp
# https://github.com/bulletphysics/bullet3/blob/c1ba04a5809f7831fa2dee684d6747951a5da602/examples/pybullet/examples/inverse_kinematics_husky_kuka.py
joints = get_joints(
robot) # Need to have all joints (although only movable returned)
movable_joints = get_movable_joints(robot)
current_conf = get_joint_positions(robot, joints)
# TODO: sample other values for the arm joints as the reference conf
min_limits = [get_joint_limits(robot, joint)[0] for joint in joints]
max_limits = [get_joint_limits(robot, joint)[1] for joint in joints]
#min_limits = current_conf
#max_limits = current_conf
#max_velocities = [get_max_velocity(robot, joint) for joint in joints] # Range of Jacobian
max_velocities = [10000] * len(joints)
# TODO: cannot have zero velocities
# TODO: larger definitely better for velocities
#damping = tuple(0.1*np.ones(len(joints)))
#t0 = time.time()
#kinematic_conf = get_joint_positions(robot, movable_joints)
for iterations in range(max_iterations): # 0.000863273143768 / iteration
# TODO: return none if no progress
if null_space:
kinematic_conf = p.calculateInverseKinematics(
robot,
link,
point,
quat,
lowerLimits=min_limits,
upperLimits=max_limits,
jointRanges=max_velocities,
restPoses=current_conf,
#jointDamping=damping,
)
else:
kinematic_conf = p.calculateInverseKinematics(
robot, link, point, quat)
if (kinematic_conf is None) or any(map(math.isnan, kinematic_conf)):
return None
set_joint_positions(robot, movable_joints, kinematic_conf)
link_point, link_quat = get_link_pose(robot, link)
if np.allclose(link_point, point, atol=tolerance) and np.allclose(
link_quat, quat, atol=tolerance):
#print iterations
break
else:
return None
if violates_limits(robot, movable_joints, kinematic_conf):
return None
#total_time = (time.time() - t0)
#print total_time
#print (time.time() - t0)/max_iterations
return kinematic_conf
def get_ik(self, position, orientation=None):
if orientation is None:
jnts = pb.calculateInverseKinematics(self.robot, self.ee_id,
position)[:7]
else:
jnts = pb.calculateInverseKinematics(self.robot, self.ee_id,
position, orientation)[:7]
return jnts
def move_to(self, pos: Vector3, orn: Optional[Vector3] = None):
"""Перемещает схват робота в pos с ориентацией orn
Возвращает true, если перемещение удалось
Отменяет перемещение и возвращает false, если перемещение не
удалось"""
# сохранение первоначального положения
start_joints = self.state
accuracy = self.kin_eps / 2
iters = round(1 / accuracy)
# значение для orn по умолчанию неизвестно
if orn is None:
tmp: List[float] = pb.calculateInverseKinematics( #type: ignore
self.id,
self.eff_id,
pos,
maxNumIterations=iters,
lowerLimits=self._lower,
upperLimits=self._upper,
jointRanges=self.ranges,
restPoses=self.pose,
jointDamping=self._damping,
residualThreshold=accuracy,
physicsClientId=self.s)
else:
tmp: List[float] = pb.calculateInverseKinematics( #type: ignore
self.id,
self.eff_id,
pos,
orn,
maxNumIterations=iters,
lowerLimits=self._lower,
upperLimits=self._upper,
jointRanges=self.ranges,
restPoses=self.pose,
jointDamping=self._damping,
residualThreshold=accuracy,
physicsClientId=self.s)
IK: State = tuple(tmp)
# log()['IK'].log(IK)
# задание полученного положения
try:
self.state = IK
except RobotStateError:
log()['PYBULLET'].log('unable to set state: vals out of bounds')
self.state = start_joints
return False
# проверка полученного решения
new_pos = self.get_effector()
if not (isclose(new_pos[0], pos[0], abs_tol=accuracy)
and isclose(new_pos[1], pos[1], abs_tol=accuracy)
and isclose(new_pos[2], pos[2], abs_tol=accuracy)):
self.state = start_joints
return False
return True
def ik(self,
target_joint,
target_pos,
target_orient,
ik_indices,
max_iterations=1000,
half_range=False,
use_current_as_rest=False,
randomize_limits=False):
if target_orient is not None and len(target_orient) < 4:
target_orient = self.get_quaternion(target_orient)
ik_lower_limits = self.ik_lower_limits if not randomize_limits else self.np_random.uniform(
0, self.ik_lower_limits)
ik_upper_limits = self.ik_upper_limits if not randomize_limits else self.np_random.uniform(
0, self.ik_upper_limits)
ik_joint_ranges = ik_upper_limits - ik_lower_limits
if half_range:
ik_joint_ranges /= 2.0
if use_current_as_rest:
ik_rest_poses = np.array(self.get_motor_joint_states()[1])
else:
ik_rest_poses = self.np_random.uniform(ik_lower_limits,
ik_upper_limits)
# print('JPO:', target_joint, target_pos, target_orient)
# print('Lower:', self.ik_lower_limits)
# print('Upper:', self.ik_upper_limits)
# print('Range:', ik_joint_ranges)
# print('Rest:', ik_rest_poses)
if target_orient is not None:
ik_joint_poses = np.array(
p.calculateInverseKinematics(
self.body,
target_joint,
targetPosition=target_pos,
targetOrientation=target_orient,
lowerLimits=ik_lower_limits.tolist(),
upperLimits=ik_upper_limits.tolist(),
jointRanges=ik_joint_ranges.tolist(),
restPoses=ik_rest_poses.tolist(),
maxNumIterations=max_iterations,
physicsClientId=self.id))
else:
ik_joint_poses = np.array(
p.calculateInverseKinematics(
self.body,
target_joint,
targetPosition=target_pos,
lowerLimits=ik_lower_limits.tolist(),
upperLimits=ik_upper_limits.tolist(),
jointRanges=ik_joint_ranges.tolist(),
restPoses=ik_rest_poses.tolist(),
maxNumIterations=max_iterations,
physicsClientId=self.id))
return ik_joint_poses[ik_indices]
def applyAction(self, motorCommands):
#print ("self.numJoints")
#print (self.numJoints)
if (self.useInverseKinematics):
dx = motorCommands[0]
dy = motorCommands[1]
dz = motorCommands[2]
da = motorCommands[3]
fingerAngle = motorCommands[4]
state = p.getLinkState(
self.tm700Uid, self.tmEndEffectorIndex
) # returns 1. center of mass cartesian coordinates, 2. rotation around center of mass in quaternion
actualEndEffectorPos = state[0]
#print("pos[2] (getLinkState(tmEndEffectorIndex)")
#print(actualEndEffectorPos[2])
self.endEffectorPos[0] = dx
self.endEffectorPos[1] = dy
self.endEffectorPos[2] = dz
#
self.endEffectorAngle = self.endEffectorAngle + da
pos = [dx, dy, dz]
orn = p.getQuaternionFromEuler([0, -math.pi, 0]) # -math.pi,yaw])
if (self.useOrientation == 1): # FALSE
jointPoses = p.calculateInverseKinematics(
self.tm700Uid,
self.tmEndEffectorIndex,
pos,
orn,
jointDamping=self.jd)
else:
jointPoses = p.calculateInverseKinematics(
self.tm700Uid,
self.tmEndEffectorIndex,
pos,
residualThreshold=0.01)
print('POSES OF JOINTS', jointPoses)
if (self.useSimulation):
for i in range(self.tmEndEffectorIndex + 1):
p.setJointMotorControl2(bodyUniqueId=self.tm700Uid,
jointIndex=i,
controlMode=p.POSITION_CONTROL,
targetPosition=jointPoses[i],
targetVelocity=0,
force=self.maxForce,
maxVelocity=self.maxVelocity,
positionGain=0.3,
velocityGain=1)
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:]
def evaluate(self, path_to_models, EPISODES=10):
path1 = os.path.join(path_to_models, "ddpg_her_policy.pt")
path2 = os.path.join(path_to_models, "ddpg_her_Q.pt")
_, policy, _, _ = self.make_models(self.actor_layer_sizes, self.critic_layer_sizes)
policy.load_state_dict(torch.load(path1))
test_reward_list = []
for e in range(EPISODES):
print(f"STARTING EPISODE {e + 1} / {EPISODES}\n")
done = False
episode_reward = 0
state = self.env.reset()
state = torch.tensor(state, device=device, dtype=dtype)
blockPos, blockOrn = p.getBasePositionAndOrientation(
self.env.blockUid
)
finalposangles = p.calculateInverseKinematics(
self.env._kuka.kukaUid, 6, blockPos
)
finalposangles = torch.tensor(
finalposangles, device=device, dtype=dtype
)
state = torch.cat((state, finalposangles))
t = 0
while not done:
t += 1
print(f"\tTime step {t + 1}, Episode {e + 1} / {EPISODES}")
self.env.render()
action = policy(state)
next_state, reward, done, _ = self.env.step(action.detach())
episode_reward += reward
next_state = torch.tensor(next_state, device=device, dtype=dtype)
gripperState = p.getLinkState(self.env._kuka.kukaUid, self.env._kuka.kukaGripperIndex)
gripperPos = gripperState[0]
state_extension = p.calculateInverseKinematics(self.env._kuka.kukaUid, 6, gripperPos)
state_extension = torch.tensor(state_extension, device=device, dtype=dtype)
next_state = torch.cat((next_state, state_extension))
state = next_state
if done:
print("\n")
break
test_reward_list.append(episode_reward)
self.env.close()
self.plot(rewards_list=test_reward_list, filename="kuka_her_test.png")
def save_towr_angles(base_pos, base_quat, ee1, ee2, ee3, ee4):
base_pos = list(base_pos)
base_quat = [base_quat[1], base_quat[2], base_quat[3], base_quat[0]]
angles_12 = []
for i in range(20):
p.resetBasePositionAndOrientation(geo_anymal, base_pos, base_quat)
leg_LF = p.calculateInverseKinematics(geo_anymal, 5, ee1)
leg_RF = p.calculateInverseKinematics(geo_anymal, 10, ee2)
leg_LH = p.calculateInverseKinematics(geo_anymal, 15, ee3)
leg_RH = p.calculateInverseKinematics(geo_anymal, 20, ee4)
p.stepSimulation()
angles_12 = leg_LF[0:3] + leg_RF[3:6] + leg_LH[6:9] + leg_RH[9:12]
return (angles_12)
def inverse_kinematics(body, eef_link, position, orientation=None):
if orientation is None:
jv = p.calculateInverseKinematics(bodyUniqueId=body,
endEffectorLinkIndex=eef_link,
targetPosition=position,
residualThreshold=1e-3)
else:
jv = p.calculateInverseKinematics(bodyUniqueId=body,
endEffectorLinkIndex=eef_link,
targetPosition=position,
targetOrientation=orientation,
residualThreshold=1e-3)
return jv
def _ee_pos_to_qpos_raw(self,
ee_pos,
ee_quat=None,
fing_dist=0.0,
left_ee_pos=None,
left_ee_quat=None,
left_fing_dist=0.0,
debug=False):
ee_ori = None if ee_quat is None else ee_quat.tolist()
qpos = pybullet.calculateInverseKinematics(
self.info.robot_id,
self.info.ee_link_id,
targetPosition=ee_pos.tolist(),
targetOrientation=ee_ori,
lowerLimits=self.info.joint_minpos.tolist(),
upperLimits=self.info.joint_maxpos.tolist(),
jointRanges=(self.info.joint_maxpos -
self.info.joint_minpos).tolist(),
restPoses=self.rest_qpos.tolist(),
#solver=pybullet.IK_SDLS,
maxNumIterations=1000,
residualThreshold=0.0001)
qpos = np.array(qpos)
if debug: print('_ee_pos_to_qpos_raw() qpos from IK', qpos)
for jid in self.info.finger_jids_lst:
qpos[jid] = np.clip( # finger info (not set by IK)
fing_dist / 2.0, self.info.joint_minpos[jid],
self.info.joint_maxpos[jid])
# Take care of left arm, if needed.
if len(self.info.left_arm_jids_lst) > 0:
if left_ee_pos is not None:
left_qpos = np.array(
pybullet.calculateInverseKinematics(
self.info.robot_id,
self.info.left_ee_link_id,
left_ee_pos.tolist(),
None
if left_ee_quat is None else left_ee_quat.tolist(),
maxNumIterations=1000,
residualThreshold=0.0001))
else:
left_qpos = self.get_qpos()
qpos[self.info.left_arm_jids_lst] = \
left_qpos[self.info.left_arm_jids_lst]
for jid in self.info.left_finger_jids_lst:
qpos[jid] = np.clip( # finger info (not set by IK)
left_fing_dist / 2.0, self.info.joint_minpos[jid],
self.info.joint_maxpos[jid])
# IK will find solutions outside of joint limits, so clip.
qpos = np.clip(qpos, self.info.joint_minpos, self.info.joint_maxpos)
return qpos
def accurateIK(bodyId, endEffectorId, targetPosition, lowerLimits, upperLimits, jointRanges, restPoses,
useNullSpace=False, maxIter=10, threshold=1e-4):
"""
Parameters
----------
bodyId : int
endEffectorId : int
targetPosition : [float, float, float]
lowerLimits : [float]
upperLimits : [float]
jointRanges : [float]
restPoses : [float]
useNullSpace : bool
maxIter : int
threshold : float
Returns
-------
jointPoses : [float] * numDofs
"""
closeEnough = False
iter = 0
dist2 = 1e30
numJoints = p.getNumJoints(baxterId)
while (not closeEnough and iter<maxIter):
if useNullSpace:
jointPoses = p.calculateInverseKinematics(bodyId, endEffectorId, targetPosition,
lowerLimits=lowerLimits, upperLimits=upperLimits, jointRanges=jointRanges,
restPoses=restPoses)
else:
jointPoses = p.calculateInverseKinematics(bodyId, endEffectorId, targetPosition)
for i in range(numJoints):
jointInfo = p.getJointInfo(bodyId, i)
qIndex = jointInfo[3]
if qIndex > -1:
p.resetJointState(bodyId,i,jointPoses[qIndex-7]) # Why 7?
ls = p.getLinkState(bodyId,endEffectorId)
newPos = ls[4]
diff = [targetPosition[0]-newPos[0],targetPosition[1]-newPos[1],targetPosition[2]-newPos[2]]
dist2 = np.sqrt((diff[0]*diff[0] + diff[1]*diff[1] + diff[2]*diff[2]))
print("dist2=",dist2)
closeEnough = (dist2 < threshold)
iter=iter+1
print("iter=",iter)
return jointPoses
def lift(botId, z, data, steps=10, tableId1=None, tableId2=None, both=None, primitive_name=None, front_grasp=False):
rightGripperPosition, rightGripperOrientation = p.getLinkState(botId, 28)[0:2]
rightGripperPosition = list(rightGripperPosition)
leftGripperPosition, leftGripperOrientation = p.getLinkState(botId, 51)[0:2]
leftGripperPosition = list(leftGripperPosition)
revoluteJoints = getRevoluteJoints(botId)
count = 0
for i in range(steps):
rightGripperPosition[2] += 0.04
leftGripperPosition[2] += 0.04
gripperPosition1 = p.calculateInverseKinematics(botId, 28, rightGripperPosition, rightGripperOrientation, maxNumIterations=1000)
gripperPosition2 = p.calculateInverseKinematics(botId, 51, leftGripperPosition, leftGripperOrientation, maxNumIterations=1000)
get_primitive_data(botId, data, primitive_name, sequence=1, tableId1=tableId1, tableId2=tableId2, front_grasp=front_grasp, both=both)
p.setJointMotorControlArray(botId,
jointIndices=revoluteJoints[0:10],
controlMode=p.POSITION_CONTROL,
targetPositions=gripperPosition1[0:10],
forces=[1000]*10)
p.setJointMotorControlArray(botId,
jointIndices=revoluteJoints[10:],
controlMode=p.POSITION_CONTROL,
targetPositions=gripperPosition2[10:],
forces=[1000]*9)
p.setJointMotorControlArray(botId,
jointIndices=[29, 31, 52, 54],
controlMode=p.POSITION_CONTROL,
targetPositions=[-0.75]*4,
forces = [10000]*4)
for i in range(10):
p.stepSimulation()
p.setJointMotorControlArray(botId,
jointIndices=[19, 42],
controlMode=p.POSITION_CONTROL,
targetPositions=[-0.45]*2,
forces=[100]*2)
get_primitive_data(botId, data, primitive_name, sequence=2, tableId1=tableId1, tableId2=tableId2, front_grasp=front_grasp, both=both)
return gripperPosition1, gripperPosition2
def calculate_inverse_kinematics(self, body, eef_link, target_pose, obstacles=None, max_iterations=200,
tolerance=1e-3):
"""
Calculate the inverse kinematics for an arm
:param body:int
body's ID
:param eef_link: int
link's name
:param target_pose: list
List of target position and quaternion
:param max_iterations: int
Maximum iterations for IK calcul
:param tolerance: float
Tolerance between the arm positions and the target positions
:return: list
arm positions
"""
(target_point, target_quat) = target_pose
current_config = [state.jointPosition for state in self.get_joint_state(body, range(self.get_num_joints(body)))]
for iterations in range(max_iterations):
if target_quat is None:
kinematic_conf = p.calculateInverseKinematics(body, eef_link, target_point,
physicsClientId=self.client_id)
else:
kinematic_conf = p.calculateInverseKinematics(body, eef_link, target_point, target_quat,
physicsClientId=self.client_id)
if kinematic_conf:
kinematic_conf = kinematic_conf[:len(
self.movable_a_joints)] # [float(q) for q in kinematic_conf[:len(self.movable_a_joints)]]
else:
continue
self.set_joint_positions(body, self.movable_a_joints, kinematic_conf)
link_point, link_quat = self.get_link_pose(body, eef_link)
if np.allclose(link_point, target_point, atol=tolerance, rtol=0):
if self.violates_limits(body, kinematic_conf):
continue
if self.pairs_link_in_collision(body):
continue
if self.bodies_in_collision(body, obstacles):
continue
break
else:
return None
self.set_joint_positions(body, range(self.get_num_joints(body)), current_config)
return kinematic_conf[:6] if kinematic_conf else None
def move_sideways(botId, rightCoords, leftCoords, data, steps=10, tableId1=None, tableId2=None, both=None, primitive_name=None, front_grasp=False):
revoluteJoints = getRevoluteJoints(botId)
y_prev_left = p.getLinkState(botId, 28)[0][1]
iter_left = (leftCoords[1]-y_prev_left)/steps
y_prev_right = p.getLinkState(botId, 51)[0][1]
iter_right = (rightCoords[1]-y_prev_right)/steps
i = 0
for i in range(steps):
y_prev_left += iter_left
y_prev_right += iter_right
gripperPosition1 = p.calculateInverseKinematics(botId, 28, [leftCoords[0], y_prev_left, leftCoords[2]], maxNumIterations=1000)
gripperPosition2 = p.calculateInverseKinematics(botId, 51, [rightCoords[0], y_prev_right, rightCoords[2]], maxNumIterations=1000)
get_primitive_data(botId, data, primitive_name, sequence=1, tableId1=tableId1, tableId2=tableId2, front_grasp=front_grasp, both=both)
p.setJointMotorControlArray(botId,
jointIndices=revoluteJoints[0:10],
controlMode=p.POSITION_CONTROL,
targetPositions=gripperPosition1[0:10])
p.setJointMotorControlArray(botId,
jointIndices=revoluteJoints[10:],
controlMode=p.POSITION_CONTROL,
targetPositions=gripperPosition2[10:])
p.setJointMotorControlArray(botId,
jointIndices=[29, 31, 52, 54],
controlMode=p.POSITION_CONTROL,
targetPositions=[-0.75]*4,
forces = [10000]*4)
for i in range(10):
p.stepSimulation()
get_primitive_data(botId, data, primitive_name, sequence=2, tableId1=tableId1, tableId2=tableId2, front_grasp=front_grasp, both=both)
if tableId1 and not tableId2:
return gripperPosition1, gripperPosition2, p.getBasePositionAndOrientation(tableId1)[0], p.getBasePositionAndOrientation(tableId1)[1], p.getBaseVelocity(tableId1)[0], p.getBaseVelocity(tableId1)[1]
if tableId2 and tableId1:
return gripperPosition1, gripperPosition2, p.getBasePositionAndOrientation(tableId2)[0], p.getBasePositionAndOrientation(tableId2)[1], p.getBaseVelocity(tableId2)[0], p.getBaseVelocity(tableId2)[1]
return gripperPosition1, gripperPosition2
def moveRobotToTouchBall(kukaID, gripperID, ballID, TP):
mode = p.POSITION_CONTROL
resultPosition = p.getBasePositionAndOrientation(ballID)[
0] #p.calculateInverseKinematics(kukaID,6,
#resultOrientation = p.getQuaternionFromEuler([0,-3.14159,0])
resultOrientation = p.getQuaternionFromEuler([0, -3.14159, 0])
resultPosition = [4.0, -300.0, -300.0]
jointPositions = p.calculateInverseKinematics(kukaID, 6, resultPosition)
jointPositions = [0, -.8, 0, 8, 0, -1.8, 0]
print('Ball position:')
print(resultPosition)
print('Joint Positions: ')
print(jointPositions)
for i in range(0, 6):
p.setJointMotorControl2(kukaID,
i,
controlMode=mode,
targetPosition=TP[i],
force=200)
for i in range(0, SIM_SECOND):
moveSim()
print('moveRobotToTouchBall() yet implemented')
def moveKukaEndtoPos(self, newxy, orn=None):
if orn is None:
orn = (-0.707, -0.707, 0, 0) # so gripper is always pointing down
kuka_min_height = 0.0 #limit min height
newxy[2] = max(kuka_min_height, newxy[2])
for kuka_sec in range(500):
jointPoses = p.calculateInverseKinematics(
self.kukaId,
self.kukaEndEffectorIndex,
newxy,
orn,
lowerLimits=ll,
upperLimits=ul,
jointRanges=jr,
restPoses=rp)
for i in range(self.numJoints):
p.setJointMotorControl2(bodyIndex=self.kukaId,jointIndex=i,controlMode=p.POSITION_CONTROL,\
targetPosition=jointPoses[i],targetVelocity=0,force=self.maxForce,positionGain=0.03,velocityGain=1)
self.gripper_stabler()
p.stepSimulation()
if self.render:
if kuka_sec < 50:
sleep(0.001)
else:
sleep(0.0001)
def step(self, action):
#Clean this my putting the robot specific functions in a different function
self._get_obs()
# for ii in range(np.size(action)):
self._observation[:3] += action
#Clipping action to not touch the ground
if self._observation[2] < 0.2:
self._observation[2] = 0.2
targetPos = p.calculateInverseKinematics(self.robotId,
self.flangeIndex,
self._observation[:3],
self._observation[3:6])
p.setJointMotorControlArray(self.robotId,
range(self.numJoints - 2),
p.POSITION_CONTROL,
targetPositions=targetPos)
if self._is_render:
for _ in range(self.sub_steps):
self._pybullet_client.stepSimulation()
# time.sleep(self.time_to_sleep)
else:
for _ in range(self.sub_steps):
self._pybullet_client.stepSimulation()
obs = self._get_obs()
done = False
info = {
'is_success': self._is_success(obs['achieved_goal'], self.goalPos),
}
reward = self.compute_reward(obs['achieved_goal'], self.goalPos, info)
# info = {
# 'is_success': self._is_success(obs['achieved_goal'], self.goalPos),
# }
# reward = self.compute_reward(obs['achieved_goal'], self.goalPos, info)
return obs, reward, done, info
def ik(self, x):
"""Performs Inverse Kinematics to obtain the joint positions
from the end-effector position (Config space from Task space)
Parameters
----------
x : list or list-like of 3 floats
Task space state (End-effector position) in 3D space
Returns
-------
numpy.array
Configuration space state (Joint positions)
Array of size = number of joints on robot
"""
q = p.calculateInverseKinematics(
bodyUniqueId = self.robot,
endEffectorLinkIndex = self.robotEndEffectorIndex,
targetPosition = list(x),
lowerLimits = self.ll,
upperLimits = self.ul,
jointRanges = self.jr,
restPoses = self.rp
)
return np.array(q)
def cartesian_forward(self, action, dt):
control = action["control"]
orientation = p.getQuaternionFromEuler([0.,-math.pi,math.pi/2.])
control = np.maximum(control, -1)
control = np.minimum(control, 1)
self.vel = np.vstack(control)
dx = control[0] * dt
dy = control[1] * dt
dz = control[2] * dt
fingers = 0
currentPose = p.getLinkState(self.model_uid, 11)
currentPosition = currentPose[0]
newPosition = [currentPosition[0] + dx,
currentPosition[1] + dy,
currentPosition[2] + dz]
jointPoses = p.calculateInverseKinematics(self.model_uid,11,newPosition, orientation)
p.setJointMotorControlArray(self.model_uid, list(range(7))+[9,10], p.POSITION_CONTROL, list(jointPoses[:7])+2*[fingers])
self.joint_state = p.getLinkState(self.model_uid, 11)[0]
self.finger_state = (p.getJointState(self.model_uid,9)[0], p.getJointState(self.model_uid, 10)[0])
self.broadcast = action["broadcast"] if "broadcast" in action.keys() else {}
def blocking(object, robot, gripper_name, world=None):
"""
This reasoning query checks if any objects are blocking an other object when an robot tries to pick it. This works
similar to the reachable predicate. First the inverse kinematics between the robot and the object will be calculated
and applied. Then it will be checked if the robot is in contact with any object except the given one.
:param object: The object for which blocking objects should be found
:param robot: The robot who reaches for the object
:param gripper_name: The name of the end effector of the robot
:param world: The BulletWorld if more than one BulletWorld is active
:return: A list of objects the robot is in collision with when reaching for the specified object
"""
world, world_id = _world_and_id(world)
state = p.saveState(physicsClientId=world_id)
inv = p.calculateInverseKinematics(robot.id,
robot.get_link_id(gripper_name),
object.get_pose(),
maxNumIterations=100,
physicsClientId=world_id)
for i in range(0, p.getNumJoints(robot.id, world_id)):
qIndex = p.getJointInfo(robot.id, i, world_id)[3]
if qIndex > -1:
p.resetJointState(robot.id,
i,
inv[qIndex - 7],
physicsClientId=world_id)
block = []
for obj in world.objects:
if obj == object:
continue
if contact(robot, obj, world):
block.append(obj)
p.restoreState(state, physicsClientId=world_id)
return block
def compute_inverse_kinematics(self,
link_uid,
link_pose,
upper_limits=None,
lower_limits=None,
ranges=None,
damping=None,
neutral_positions=None):
"""Compute the inverse kinematics.
Args:
link_uid: The unique ID of the link.
link_pose: The target pose of the link.
upper_limits: The upper limits of joints.
lower_limits: The lower limits of joints.
ranges: The ranges of joints.
dampings: The list of joint damping parameters.
neutral_positions: The neutral joint positions.
returns:
target_positions: The list of target joint positions.
"""
body_uid, link_ind = link_uid
if not isinstance(link_pose, Pose):
link_pose = Pose(link_pose)
position = link_pose.position
quaternion = link_pose.quaternion
kwargs = dict()
kwargs['bodyUniqueId'] = body_uid
kwargs['endEffectorLinkIndex'] = link_ind
kwargs['targetPosition'] = list(position)
if quaternion is not None:
kwargs['targetOrientation'] = list(quaternion)
# TODO(kuanfang): Not sure if those are necessary for computing IK.
if lower_limits is not None:
kwargs['lowerLimits'] = lower_limits
if upper_limits is not None:
kwargs['upperLimits'] = upper_limits
if ranges is not None:
kwargs['jointRanges'] = ranges
if damping is not None:
kwargs['jointDamping'] = damping
if neutral_positions is not None:
kwargs['restPoses'] = neutral_positions
kwargs['physicsClientId'] = self.uid
target_positions = pybullet.calculateInverseKinematics(**kwargs)
return target_positions
def accurateCalculateInverseKinematics(robotid, endEffectorId, targetPos,
threshold, maxIter):
sample_fn = get_sample_fn(robotid, arm_joints)
set_joint_positions(robotid, arm_joints, sample_fn())
it = 0
while it < maxIter:
jointPoses = p.calculateInverseKinematics(robotid,
endEffectorId,
targetPos,
lowerLimits=min_limits,
upperLimits=max_limits,
jointRanges=joint_range,
restPoses=rest_position,
jointDamping=jd)
set_joint_positions(robotid, arm_joints, jointPoses[2:10])
ls = p.getLinkState(robotid, endEffectorId)
newPos = ls[4]
dist = np.linalg.norm(np.array(targetPos) - np.array(newPos))
if dist < threshold:
break
it += 1
print("Num iter: " + str(it) + ", threshold: " + str(dist))
return jointPoses
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
numJoints = p.getNumJoints(kukaId)
#Joint damping coefficents. Using large values for the joints that we don't want to move.
jd = [100.0, 100.0, 100.0, 100.0, 100.0, 100.0, 0.5]
#jd=[0.5,0.5,0.5,0.5,0.5,0.5,0.5]
p.setGravity(0, 0, 0)
while 1:
p.stepSimulation()
for i in range(1):
pos = [0, 0, 1.26]
orn = p.getQuaternionFromEuler([0, 0, 3.14])
jointPoses = p.calculateInverseKinematics(kukaId,
kukaEndEffectorIndex,
pos,
orn,
jointDamping=jd)
for i in range(numJoints):
p.setJointMotorControl2(bodyIndex=kukaId,
jointIndex=i,
controlMode=p.POSITION_CONTROL,
targetPosition=jointPoses[i],
targetVelocity=0,
force=500,
positionGain=0.03,
velocityGain=1)
sleep(0.05)
#t = (dt.second/60.)*2.*math.pi else: t=t+0.001 if (useSimulation and useRealTimeSimulation==0): p.stepSimulation() for i in range (1): #pos = [-0.4,0.2*math.cos(t),0.+0.2*math.sin(t)] pos = [0.2*math.cos(t),0,0.+0.2*math.sin(t)+0.7] #end effector points down, not up (in case useOrientation==1) orn = p.getQuaternionFromEuler([0,-math.pi,0]) if (useNullSpace==1): if (useOrientation==1): jointPoses = p.calculateInverseKinematics(kukaId,kukaEndEffectorIndex,pos,orn,ll,ul,jr,rp) else: jointPoses = p.calculateInverseKinematics(kukaId,kukaEndEffectorIndex,pos,lowerLimits=ll, upperLimits=ul, jointRanges=jr, restPoses=rp) else: if (useOrientation==1): jointPoses = p.calculateInverseKinematics(kukaId,kukaEndEffectorIndex,pos,orn,jointDamping=jd) else: threshold =0.001 maxIter = 100 jointPoses = accurateCalculateInverseKinematics(kukaId,kukaEndEffectorIndex,pos, threshold, maxIter) if (useSimulation): for i in range (numJoints): p.setJointMotorControl2(bodyIndex=kukaId,jointIndex=i,controlMode=p.POSITION_CONTROL,targetPosition=jointPoses[i],targetVelocity=0,force=500,positionGain=1,velocityGain=0.1) else: #reset the joint state (ignoring all dynamics, not recommended to use during simulation)
dt = datetime.now()
t = (dt.second / 60.) * 2. * math.pi
else:
t = t + 0.001
if (useSimulation and useRealTimeSimulation == 0):
p.stepSimulation()
for i in range(1):
pos = [-0.4, 0.2 * math.cos(t), 0. + 0.2 * math.sin(t)]
#end effector points down, not up (in case useOrientation==1)
orn = p.getQuaternionFromEuler([0, -math.pi, 0])
if (useNullSpace == 1):
if (useOrientation == 1):
jointPoses = p.calculateInverseKinematics(kukaId, kukaEndEffectorIndex, pos, orn, ll, ul,
jr, rp)
else:
jointPoses = p.calculateInverseKinematics(kukaId,
kukaEndEffectorIndex,
pos,
lowerLimits=ll,
upperLimits=ul,
jointRanges=jr,
restPoses=rp)
else:
if (useOrientation == 1):
jointPoses = p.calculateInverseKinematics(kukaId,
kukaEndEffectorIndex,
pos,
orn,
jointDamping=jd,
#use 0 for no-removal
trailDuration = 1
while 1:
if (useRealTimeSimulation):
dt = datetime.now()
t = (dt.second / 60.) * 2. * math.pi
else:
t = t + 0.01
time.sleep(0.01)
for i in range(1):
pos = [2. * math.cos(t), 2. * math.cos(t), 0. + 2. * math.sin(t)]
jointPoses = p.calculateInverseKinematics(sawyerId,
sawyerEndEffectorIndex,
pos,
jointDamping=jd,
solver=ikSolver,
maxNumIterations=100)
#reset the joint state (ignoring all dynamics, not recommended to use during simulation)
for i in range(numJoints):
jointInfo = p.getJointInfo(sawyerId, i)
qIndex = jointInfo[3]
if qIndex > -1:
p.resetJointState(sawyerId, i, jointPoses[qIndex - 7])
ls = p.getLinkState(sawyerId, sawyerEndEffectorIndex)
if (hasPrevPose):
p.addUserDebugLine(prevPose, pos, [0, 0, 0.3], 1, trailDuration)
p.addUserDebugLine(prevPose1, ls[4], [1, 0, 0], 1, trailDuration)
prevPose = pos
force=10)
i = 6
p.setJointMotorControl2(kuka_gripper,
i,
p.POSITION_CONTROL,
targetPosition=e[ANALOG] * 0.05,
force=10)
if sq_len < THRESHOLD * THRESHOLD:
eef_pos = e[POSITION]
eef_orn = p.getQuaternionFromEuler([0, -math.pi, 0])
joint_pos = p.calculateInverseKinematics(kuka,
6,
eef_pos,
eef_orn,
lowerLimits=LOWER_LIMITS,
upperLimits=UPPER_LIMITS,
jointRanges=JOINT_RANGE,
restPoses=REST_POSE,
jointDamping=JOINT_DAMP)
for i in range(len(joint_pos)):
p.setJointMotorControl2(kuka,
i,
p.POSITION_CONTROL,
targetPosition=joint_pos[i],
targetVelocity=0,
positionGain=0.15,
velocityGain=1.0,
force=MAX_FORCE)
def applyAction(self, motorCommands):
#print ("self.numJoints")
#print (self.numJoints)
if (self.useInverseKinematics):
dx = motorCommands[0]
dy = motorCommands[1]
dz = motorCommands[2]
da = motorCommands[3]
fingerAngle = motorCommands[4]
state = p.getLinkState(self.kukaUid,self.kukaEndEffectorIndex)
actualEndEffectorPos = state[0]
#print("pos[2] (getLinkState(kukaEndEffectorIndex)")
#print(actualEndEffectorPos[2])
self.endEffectorPos[0] = self.endEffectorPos[0]+dx
if (self.endEffectorPos[0]>0.65):
self.endEffectorPos[0]=0.65
if (self.endEffectorPos[0]<0.50):
self.endEffectorPos[0]=0.50
self.endEffectorPos[1] = self.endEffectorPos[1]+dy
if (self.endEffectorPos[1]<-0.17):
self.endEffectorPos[1]=-0.17
if (self.endEffectorPos[1]>0.22):
self.endEffectorPos[1]=0.22
#print ("self.endEffectorPos[2]")
#print (self.endEffectorPos[2])
#print("actualEndEffectorPos[2]")
#print(actualEndEffectorPos[2])
#if (dz<0 or actualEndEffectorPos[2]<0.5):
self.endEffectorPos[2] = self.endEffectorPos[2]+dz
self.endEffectorAngle = self.endEffectorAngle + da
pos = self.endEffectorPos
orn = p.getQuaternionFromEuler([0,-math.pi,0]) # -math.pi,yaw])
if (self.useNullSpace==1):
if (self.useOrientation==1):
jointPoses = p.calculateInverseKinematics(self.kukaUid,self.kukaEndEffectorIndex,pos,orn,self.ll,self.ul,self.jr,self.rp)
else:
jointPoses = p.calculateInverseKinematics(self.kukaUid,self.kukaEndEffectorIndex,pos,lowerLimits=self.ll, upperLimits=self.ul, jointRanges=self.jr, restPoses=self.rp)
else:
if (self.useOrientation==1):
jointPoses = p.calculateInverseKinematics(self.kukaUid,self.kukaEndEffectorIndex,pos,orn,jointDamping=self.jd)
else:
jointPoses = p.calculateInverseKinematics(self.kukaUid,self.kukaEndEffectorIndex,pos)
#print("jointPoses")
#print(jointPoses)
#print("self.kukaEndEffectorIndex")
#print(self.kukaEndEffectorIndex)
if (self.useSimulation):
for i in range (self.kukaEndEffectorIndex+1):
#print(i)
p.setJointMotorControl2(bodyUniqueId=self.kukaUid,jointIndex=i,controlMode=p.POSITION_CONTROL,targetPosition=jointPoses[i],targetVelocity=0,force=self.maxForce,maxVelocity=self.maxVelocity, positionGain=0.3,velocityGain=1)
else:
#reset the joint state (ignoring all dynamics, not recommended to use during simulation)
for i in range (self.numJoints):
p.resetJointState(self.kukaUid,i,jointPoses[i])
#fingers
p.setJointMotorControl2(self.kukaUid,7,p.POSITION_CONTROL,targetPosition=self.endEffectorAngle,force=self.maxForce)
p.setJointMotorControl2(self.kukaUid,8,p.POSITION_CONTROL,targetPosition=-fingerAngle,force=self.fingerAForce)
p.setJointMotorControl2(self.kukaUid,11,p.POSITION_CONTROL,targetPosition=fingerAngle,force=self.fingerBForce)
p.setJointMotorControl2(self.kukaUid,10,p.POSITION_CONTROL,targetPosition=0,force=self.fingerTipForce)
p.setJointMotorControl2(self.kukaUid,13,p.POSITION_CONTROL,targetPosition=0,force=self.fingerTipForce)
else:
for action in range (len(motorCommands)):
motor = self.motorIndices[action]
p.setJointMotorControl2(self.kukaUid,motor,p.POSITION_CONTROL,targetPosition=motorCommands[action],force=self.maxForce)
