sphere.position = np.array([
0.5, r * np.cos(w * t + np.pi / 2),
(1. - r) + r * np.sin(w * t + np.pi / 2)
])
# get current end-effector position and velocity in the task/operational space
x = robot.get_link_world_positions(link_id)
dx = robot.get_link_world_linear_velocities(link_id)
# Get joint positions
q = robot.get_joint_positions()
# Get linear jacobian
if robot.has_floating_base():
J = robot.get_linear_jacobian(link_id, q=q)[:, qIdx + 6]
else:
J = robot.get_linear_jacobian(link_id, q=q)[:, qIdx]
# Pseudo-inverse: \hat{J} = J^T (JJ^T + k^2 I)^{-1}
Jp = robot.get_damped_least_squares_inverse(J, damping)
# evaluate damped-least-squares IK
dq = Jp.dot(kp * (sphere.position - x) - kd * dx)
# set joint positions
q = q[qIdx] + dq * dt
robot.set_joint_positions(q, joint_ids=joint_ids)
# step in simulation
world.step(sleep_dt=dt)
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)