def __init__(self, body, world=None, window_size=1, axis=None, ticks=1):
"""
Initialize the body state.
Args:
body (Body, int): body or unique body id.
world (None, World): world instance if the body id was given.
window_size (int): window size of the state. This is the total number of states we should remember. That
is, if the user wants to remember the current state :math:`s_t` and the previous state :math:`s_{t-1}`,
the window size is 2. By default, the :attr:`window_size` is one which means we only remember the
current state. The window size has to be bigger than 1. If it is below, it will be set automatically
to 1. The :attr:`window_size` attribute is only valid when the state is not a combination of states,
but is given some :attr:`data`.
axis (int, None): axis to concatenate or stack the states in the current window. If you have a state with
shape (n,), then if the axis is None (by default), it will just concatenate it such that resulting
state has a shape (n*w,) where w is the window size. If the axis is an integer, then it will just stack
the states in the specified axis. With the example, for axis=0, the resulting state has a shape of
(w,n), and for axis=-1 or 1, it will have a shape of (n,w). The :attr:`axis` attribute is only when the
state is not a combination of states, but is given some :attr:`data`.
ticks (int): number of ticks to sleep before getting the next state data.
"""
# check given body
if not isinstance(body, (Body, int, long)):
raise TypeError(
"Expecting an instance of Body, or an identifier from the simulator/world, instead got: "
"{}".format(type(body)))
if isinstance(body, (int, long)):
if not isinstance(world, World):
# try to look for the world in global variables
if 'world' in globals() and isinstance(globals()['world'],
World): # O(1)
world = globals()['world']
else:
raise ValueError(
"When giving the body identifier, the world need to be given as well."
)
body = Body(world.simulator, body)
self.body = body
# initialize parent class
super(BodyState, self).__init__(window_size=window_size,
axis=axis,
ticks=ticks)
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(
position=robot.get_link_world_positions(link_id),
radius=0.05,
color=(1, 0, 0, 0.5))
sphere = Body(sim, body_id=sphere)
# perform simulation
for t in count():
# if no more joint positions, get out of the loop
if t >= len(positions):
break
# set joint positions
robot.set_joint_positions(positions[t], joint_ids=joint_ids)
# make the sphere follow the end effector
sphere.position = robot.get_link_world_positions(link_id)
# step in simulation
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)
# create sphere to follow
sphere = world.load_visual_sphere(position=np.array([0.5, 0., 1.]),
radius=0.05,
color=(1, 0, 0, 0.5))
sphere = Body(sim, body_id=sphere)
# set initial joint positions (based on the position of the sphere at [0.5, 0, 1])
robot.reset_joint_states(q=[
8.84305270e-05, 7.11378917e-02, -1.68059886e-04, -9.71690439e-01,
1.68308810e-05, 3.71467111e-01, 5.62890805e-05
])
# define amplitude and angular velocity when moving the sphere
w = 0.01
r = 0.2
for t in count():
# move sphere
sphere.position = np.array([
0.5, r * np.cos(w * t + np.pi / 2),
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)
# create sphere to follow
sphere = world.load_visual_sphere(position=np.array([0.5, 0., 0.5]),
radius=0.05,
color=(1, 0, 0, 0.5))
sphere = Body(sim, body_id=sphere)
# set initial joint p
# ositions (based on the position of the sphere at [0.5, 0, 1])
robot.reset_joint_states(q=[
8.84305270e-05, 7.11378917e-02, -1.68059886e-04, -9.71690439e-01,
1.68308810e-05, 3.71467111e-01, 5.62890805e-05
])
# define amplitude and angular velocity when moving the sphere
w = 0.01
r = 0.1
# I set the reference orientation to a constant
sphere.orientation = np.array([1, 0, 0, 0])
if self._plot_Tx:
data['Tx'] = self._sensor.sense()[3]
if self._plot_Ty:
data['Ty'] = self._sensor.sense()[4]
if self._plot_Tz:
data['Tz'] = self._sensor.sense()[5]
return data
if __name__ == '__main__':
# Try to move the robot in the simulator
# WARNING: DON'T FORGET TO CLOSE FIRST THE FIGURE THEN THE SIMULATOR OTHERWISE YOU WILL HAVE THE PLOTTING PROCESS
# STILL RUNNING
from itertools import count
sim = prl.simulators.Bullet()
world = prl.worlds.BasicWorld(sim)
robot = world.load_robot('kuka_iiwa')
box = world.load_visual_box(position=[0.7, 0., 0.2], orientation=get_quaternion_from_rpy([0, 1.57, 0]),
dimensions=(0.2, 0.2, 0.2))
box = Body(sim, body_id=box)
sensor = prl.robots.sensors.JointForceTorqueSensor(sim, body_id=robot.id, joint_ids=5)
plot = EeFtRealTimePlot(robot, sensor=sensor, forcex=True, forcey=True, forcez=True,
torquex=True, torquey=True, torquez=True, ticks=24)
for t in count():
plot.update()
world.step(sim.dt)