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 __init__(self, spheres=[[0.09, -0.44, 0.715, 1]], cuboids=None):
# Define size of the map
self.res = 1.
# Initialize XBox controller
self.controller = XboxControllerInterface(use_thread=True,
verbose=False)
# Create simulator
self.sim = Bullet()
# create world
self.world = BasicWorld(self.sim)
# create robot
self.robot = KukaIIWA(self.sim)
# define start/end pt
self.start_pt = [-0.75, 0., 0.75]
self.end_pt = [0.75, 0., 0.75]
# Initialize robot location
self.send_robot_to(np.array(self.start_pt), dt=2)
# Get via points from user
via_pts = self.get_via_pts(
) # [[-0.19, -0.65, 0.77], [0.51, -0.44, 0.58]]
self.motion_targets = [self.start_pt] + via_pts + [self.end_pt]
import os
import pickle
from itertools import count
from pyrobolearn.simulators import Bullet
from pyrobolearn.worlds import BasicWorld
from pyrobolearn.robots import KukaIIWA, Body
# Create simulator
sim = Bullet()
# create world
world = BasicWorld(sim)
# create robot
robot = KukaIIWA(sim)
robot.print_info()
# define useful variables for FK
link_id = robot.get_end_effector_ids(end_effector=0)
joint_ids = robot.joints # actuated joint
# load data
with open(os.path.dirname(os.path.abspath(__file__)) + '/data.txt', 'rb') as f:
positions = pickle.load(f)
# set initial joint position
robot.reset_joint_states(q=positions[0])
# draw a sphere at the position of the end-effector
sphere = world.load_visual_sphere(
import numpy as np
from itertools import count
from pyrobolearn.simulators import Bullet
from pyrobolearn.worlds import BasicWorld
from pyrobolearn.robots import KukaIIWA, Body
# Create simulator
sim = Bullet()
# create world
world = BasicWorld(sim)
# create robot
robot = KukaIIWA(sim)
robot.print_info()
world.load_robot(robot)
# define useful variables for IK
dt = 1. / 240
link_id = robot.get_end_effector_ids(end_effector=0)
joint_ids = robot.joints # actuated joint
damping = 0.01 # for damped-least-squares IK
wrt_link_id = -1 # robot.get_link_ids('iiwa_link_1')
qIdx = robot.get_q_indices(joint_ids)
# define gains
kp = 50 # 5 if velocity control, 50 if position control
kd = 0 # 2*np.sqrt(kp)
"""
import numpy as np
from itertools import count
from pyrobolearn.simulators import Bullet
from pyrobolearn.worlds import BasicWorld
from pyrobolearn.robots import KukaIIWA
# Create simulator
sim = Bullet()
# create world
world = BasicWorld(sim)
# create robot
robot = KukaIIWA(sim)
# print information about the robot
robot.print_info()
# H = robot.get_mass_matrix()
# print("Inertia matrix: H(q) = {}".format(H))
# print(robot.get_link_world_positions(flatten=False))
K = 5000 * np.identity(3)
# D = 2 * np.sqrt(K)
# D = np.zeros((3,3))
D = 100 * np.identity(3)
x_des = np.array([0.3, 0.0, 0.8])
x_des = np.array([0.52557296, 0.09732758, 0.80817658])
link_id = robot.get_link_ids('iiwa_link_ee')
choices=['nao', 'kuka_iiwa', 'cogimon', 'centauro'],
default='nao')
args = parser.parse_args()
# get the robot to use for the example
robot_name = args.robot
dt = 0.01
# Create simulator and world
sim = Bullet()
world = BasicWorld(sim)
# Define robot, velocity manipulability for CoM, and proportional gain
if robot_name == 'kuka_iiwa':
# load robot
robot = KukaIIWA(sim)
# desired Velocity Manipulability for CoM
desired_velocity_manip = np.array([[0.02193921, -0.01192746, -0.0155832],
[-0.01192746, 0.04524443, 0.01892251],
[-0.0155832, 0.01892251, 0.02259072]])
# proportional gain
Km = 5 * np.eye(6)
elif robot_name == 'nao':
# load robot
robot = Nao(sim, fixed_base=True)
# # desired Velocity Manipulability for CoM
# desired_velocity_manip = np.array([[2.53907684e-03, 2.65373209e-04, 1.83354058e-04],
default=1)
args = parser.parse_args()
# select IK solver, by setting the flag:
# 0 = pybullet + calculate_inverse_kinematics()
# 1 = pybullet + damped-least-squares IK using Jacobian (provided by pybullet)
solver_flag = args.solver # 1 gives a pretty good result
# Create simulator
sim = Bullet()
# create world
world = BasicWorld(sim)
# create robot
robot = KukaIIWA(sim)
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
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)
class Environment:
def __init__(self, spheres=[[0.09, -0.44, 0.715, 1]], cuboids=None):
# Define size of the map
self.res = 1.
# Initialize XBox controller
self.controller = XboxControllerInterface(use_thread=True,
verbose=False)
# Create simulator
self.sim = Bullet()
# create world
self.world = BasicWorld(self.sim)
# create robot
self.robot = KukaIIWA(self.sim)
# define start/end pt
self.start_pt = [-0.75, 0., 0.75]
self.end_pt = [0.75, 0., 0.75]
# Initialize robot location
self.send_robot_to(np.array(self.start_pt), dt=2)
# Get via points from user
via_pts = self.get_via_pts(
) # [[-0.19, -0.65, 0.77], [0.51, -0.44, 0.58]]
self.motion_targets = [self.start_pt] + via_pts + [self.end_pt]
def send_robot_to(self, location, dt):
start = time.perf_counter()
for _ in count():
joint_ids = self.robot.joints
link_id = self.robot.get_end_effector_ids(end_effector=0)
qIdx = self.robot.get_q_indices(joint_ids)
x = self.robot.sim.get_link_world_positions(body_id=self.robot.id,
link_ids=link_id)
q = self.robot.calculate_inverse_kinematics(link_id,
position=location)
self.robot.set_joint_positions(q[qIdx], joint_ids)
self.world.step()
if time.perf_counter() - start >= dt:
break
def get_via_pts(self):
print(
"Move green dot to via point. Press 'A' to save. Press 'X' to finish."
)
X_desired = [0., 0., 1.3]
speed_scale_factor = 0.01
via_pts = []
sphere = None
for _ in count():
last_trigger = self.controller.last_updated_button
movement = self.controller[last_trigger]
if last_trigger == 'LJ':
X_desired[0] = X_desired[0] + round(movement[0],
1) * speed_scale_factor
X_desired[1] = X_desired[1] + round(movement[1],
1) * speed_scale_factor
elif last_trigger == 'RJ':
X_desired[2] = X_desired[2] + round(movement[1],
1) * speed_scale_factor
elif last_trigger == 'A' and movement == 1:
print("Via point added")
via_pts.append(X_desired)
time.sleep(0.5)
elif last_trigger == 'X' and movement == 1:
self.world.sim.remove_body(sphere)
print("Via point generation complete.")
break
if sphere:
self.world.sim.remove_body(sphere)
sphere = self.world.load_visual_sphere(X_desired,
radius=0.01,
color=(0, 1, 0, 1))
return via_pts
plt.ylabel("vertical position")
plt.title("The z axis position during the task")
plt.show()
if __name__ == '__main__':
# Create simulator
sim = Bullet()
# create world
world = BasicWorld(sim)
# flag : 0 # PI control
flag = 0
# create robot
robot = KukaIIWA(sim)
robot.print_info()
world.load_robot(robot)
world.load_table(position=np.array([1, 0., 0.]),
orientation=np.array([0.0, 0.0, 0.0, 1.0]))
# define useful variables for IK
dt = 1. / 240
link_id = robot.get_end_effector_ids(end_effector=0)
joint_ids = robot.joints # actuated joint
damping = 0.01 # for damped-least-squares IK
wrt_link_id = -1 # robot.get_link_ids('iiwa_link_1')
qIdx = robot.get_q_indices(joint_ids)
# define gains
kp = 500 # 5 if velocity control, 50 if position control
kd = 5 # 2*np.sqrt(kp)
try:
import rbdl
except ImportError as e:
raise ImportError(
repr(e) +
'\nTry to install `rbdl` manually from `https://bitbucket.org/rbdl/rbdl`'
)
# Create simulator
sim = Bullet()
# create world
world = BasicWorld(sim)
# create robot
robot = KukaIIWA(sim)
# define useful variables for IK
dt = 1. / 240
link_id = robot.get_end_effector_ids(end_effector=0)
joint_ids = robot.joints # actuated joint
base_name = robot.base_name
end_effector_name = robot.get_link_names(link_id)
urdf = os.path.dirname(
os.path.abspath(__file__)
) + '/../../../pyrobolearn/robots/urdfs/kuka/kuka_iiwa/iiwa14.urdf'
damping = 0.01 # for damped-least-squares IK
wrt_link_id = -1 # robot.get_link_ids('iiwa_link_1')
chain_name = robot.get_link_names(joint_ids)
# desired position
def move_robot(pred_path):
for pt in pred_path:
dt = kmp.output_dt
send_robot_to(np.array(pt[:3]), 0.05)
if __name__ == "__main__":
# Initialize XBox controller
controller = XboxControllerInterface(use_thread=True, verbose=False)
# Create simulator
sim = Bullet()
# create world
world = BasicWorld(sim)
# create robot
robot = KukaIIWA(sim)
send_robot_to(np.array([-0.75, 0., 0.75]), dt=2)
# Load trained kmp
filehandler = open("./trained_kmps/kmp.obj", 'rb')
kmp = pkl.load(filehandler)
# Get via points from user
start_pt = [[-0.75, 0., 0.75]]
end_pt = [[0.75, 0., 0.75]]
via_pts = get_via_pts() # [[-0.19, -0.65, 0.77], [0.51, -0.44, 0.58]]
via_pts = start_pt + via_pts + end_pt
print("Via Points: ", via_pts)
via_pt_var = 1e-6 * np.eye(kmp.ref_traj[0].sigma.shape[0])
# Query KMP
