robot.print_info()
# define useful variables for IK
dt = 1. / 240
link_id = robot.get_end_effector_ids(end_effector=0)
joint_ids = robot.joints # actuated joint
# joint_ids = joint_ids[2:]
damping = 0.01 # for damped-least-squares IK
wrt_link_id = -1 # robot.get_link_ids('iiwa_link_1')
# desired position
xd = np.array([0.5, 0., 0.5])
world.load_visual_sphere(xd, radius=0.05, color=(1, 0, 0, 0.5))
# change the robot visual
robot.change_transparency()
robot.draw_link_frames(link_ids=[-1, 0])
robot.draw_bounding_boxes(link_ids=joint_ids[0])
# robot.draw_link_coms([-1,0])
qIdx = robot.get_q_indices(joint_ids)
# OPTION 1: using `calculate_inverse_kinematics`###
if solver_flag == 0:
for _ in count():
# get current position in the task/operational space
x = robot.get_link_world_positions(link_id)
# print("(xd - x) = {}".format(xd - x))
# perform full IK
q = robot.calculate_inverse_kinematics(link_id, position=xd)
class Robot:
def __init__(self, sim, world):
self.robot = KukaIIWA(sim)
self.world = world
# change the robot visual
self.robot.change_transparency()
self.robot.draw_link_frames(link_ids=[-1, 0])
self.start_pos = np.array([-0.75, 0., 0.75])
self.end_pos = np.array([0.75, 0., 0.75])
self.pid = {'kp': 100, 'kd': 0}
self.ee_id = self.robot.get_end_effector_ids(end_effector=0)
def send_robot_to(self, location):
# define useful variables for IK
dt = 1. / 240
link_id = self.robot.get_end_effector_ids(end_effector=0)
joint_ids = self.robot.joints # actuated joint
# joint_ids = joint_ids[2:]
damping = 0.01 # for damped-least-squares IK
qIdx = self.robot.get_q_indices(joint_ids)
# for t in count():
# get current position in the task/operational space
x = self.robot.sim.get_link_world_positions(body_id=self.robot.id,
link_ids=link_id)
dx = self.robot.get_link_world_linear_velocities(link_id)
# Get joint configuration
q = self.robot.sim.get_joint_positions(self.robot.id,
self.robot.joints)
# Get linear jacobian
if self.robot.has_floating_base():
J = self.robot.get_linear_jacobian(link_id, q=q)[:, qIdx + 6]
else:
J = self.robot.get_linear_jacobian(link_id, q=q)[:, qIdx]
# Pseudo-inverse
Jp = self.robot.get_damped_least_squares_inverse(J, damping)
# evaluate damped-least-squares IK
dq = Jp.dot(self.pid['kp'] * (location - x) - self.pid['kd'] * dx)
# set joint velocities
# robot.set_joint_velocities(dq)
# set joint positions
q = q[qIdx] + dq * dt
self.robot.set_joint_positions(q, joint_ids=joint_ids)
return x
def go_home(self):
for t in count():
x = self.send_robot_to(self.start_pos)
# step in simulation
self.world.step()
# Check if robot has reached the required position
error = np.linalg.norm(self.start_pos - x)
if error < 0.01 or t > 500:
break
def go_to_end(self):
self.send_robot_to(self.end_pos)