def handle_input(self, input_message):
if input_message == "Stand":
trajectory = trj.Trajectory(q_f=config.stand_config,
k_v=np.full(8, 20),
k_a=np.full(8, 80))
if trajectory.is_valid():
self.trajectory_queue.put(trajectory)
elif input_message == "Home":
trajectory = trj.Trajectory(q_f=config.home_config,
k_v=config.down_speed,
k_a=config.down_acceleration)
if trajectory.is_valid():
self.trajectory_queue.put(trajectory)
elif input_message == "Current":
with self.shared_position.get_lock():
print(self.shared_position[:])
elif input_message == "Walk" or input_message == "Idle" or input_message == "Trot":
self.change_mode(input_message)
elif input_message == "Exit":
while not self.trajectory_queue.empty():
self.trajectory_queue.get()
self.return_to_home(self._mode)
self.message_queue.put(input_message)
else:
self.message_queue.put(input_message)
def return_to_home(self, mode):
if mode == "Walk":
home_trajectory = trj.Trajectory(config.walk_mode_home)
elif mode == "Idle":
home_trajectory = trj.Trajectory(config.idle_mode_home)
elif mode == "Trot":
#At this moment, trot mode and idle mode have the same home config
home_trajectory = trj.Trajectory(config.idle_mode_home)
self.trajectory_queue.put(home_trajectory)
def trajectory_queue_init(self):
'''
For å få lavere cycle time ved første trajectory
'''
trajectory = trj.Trajectory([0, 0, 0, 0, 0, 0, 0, 0])
self.trajectory_queue.put(trajectory)
self.trajectory_queue.get()
trajectory.set_initial_conditions(0, 0)
def plot_SetVelocity():
# Avec saturation
t0 = 0
p0 = 0
v0 = 0
a0 = 0
v = 15
f = trajectory.Trajectory(p0=p0, v0=v0, a0=a0, t0=t0 - 1).extend(
trajectory.SetVelocity(max_jerk,
accel_max,
v,
p0=p0,
v0=v0,
a0=a0,
t0=t0))
plot_pvaj(
f,
"Accélération jusqu'à %02.0f m/s avec saturation de l'accélération" %
v)
# Sans saturation
v = 2
f = trajectory.Trajectory(p0=p0, v0=v0, a0=a0, t0=t0 - 1).extend(
trajectory.SetVelocity(max_jerk,
accel_max,
v,
p0=p0,
v0=v0,
a0=a0,
t0=t0))
plot_pvaj(
f,
"Accélération jusqu'à %01.0f m/s sans saturation de l'accélération" %
v)
def plan_moveit(self,
position,
orientation,
start_joint_position=None,
euler_flag=False,
time_scale=1.0):
"""
Plans the motions using the IK implemented in MoveIt!. Note that MoveIt! is not used to execute the plan on the
robot, because the controllers are not very useful.
:param position: list of cartesian coordinates (XYZ)
:type position: list or np.array
:param orientation: list of orientation elements (default: quaternion)
:type orientation: list or np.array
:param start_joint_position: starting joint position for the planner
:type: list or np.array
:param euler_flag: a flag to indicate whether the orientation is in euler or quaternions
:type euler_flag: bool
:param time_scale: scales time for the interpolation of raw waypoints
:type time_scale: float
:return the trajectory with the raw waypoints
:rtype: trajectory.Trajectory
"""
if start_joint_position is not None:
self.set_start_pose_moveit(start_joint_position)
pose = self.create_pose(position, orientation, euler_flag=euler_flag)
self.move_group.set_pose_target(pose)
plan = self.move_group.plan()
self.move_group.clear_pose_targets()
# create the raw waypoints
raw_waypoints = np.array(
[p.positions for p in plan.joint_trajectory.points])
t_raw_waypoints = np.array([
self.to_sec(p.time_from_start)
for p in plan.joint_trajectory.points
])
self.traj = trajectory.Trajectory(self.n_joints,
raw_waypoints=raw_waypoints,
t_raw_waypoints=t_raw_waypoints,
time_scale=time_scale)
return self.traj
def append_trajectory(self, traj_1, traj_2):
"""
Appends traj_2 to the end of traj_1. Warning: it is up to the user to check for continuity of the trajectories.
:param traj_1: trajectory 1
:type traj_1: trajectory.Trajectory
:param traj_2: trajectory 2
:type traj_1: trajectory.Trajectory
:return: combined trajectory
:rtype: trajectory.Trajectory
"""
step_size = min(traj_1.step_size, traj_2.step_size)
waypoints = np.append(traj_1.waypoints, traj_2.waypoints, axis=0)
t_waypoints = np.append(traj_1.t_waypoints,
traj_2.t_waypoints + traj_1.total_t +
step_size,
axis=0)
start_pos = traj_1.start_pos
self.traj = trajectory.Trajectory(self.n_joints,
waypoints=waypoints,
t_waypoints=t_waypoints,
start_pos=start_pos,
step_size=step_size)
return self.traj
# m_v_wm_arr_filt = m_v_wm_arr_filt[:N-NPREV,:]
# b_v_ob_arr = b_v_ob_arr[:N-NPREV,:]
# b_v_oc_arr = b_v_oc_arr[:N-NPREV,:]
res_folder = 'res_for_rpg_' + datetime.datetime.now().strftime(
"%y_%m_%d__%H_%M_%S") + '/'
os.makedirs(res_folder)
np.savetxt(res_folder + 'stamped_traj_estimate.txt', pose_est, delimiter=' ')
np.savetxt(res_folder + 'stamped_groundtruth.txt', pose_gtr, delimiter=' ')
"""
Note: Trajectory reads the traj in stamped_traj_estimate.txt and aligns it with the one in stamped_groundtruth.txt.
The resulting arrays are respectively sored in traj.X_es_aligned and traj.X_gt.
traj.X_gt is not modified -> exactly the same as what is in stamped_groundtruth.txt.
"""
traj = rpg_traj.Trajectory(
res_folder, align_type='se3',
align_num_frames=1) # settings ensured by eval_cfg.yaml
# 'a' like aligned
a_p_ab_arr = traj.p_es_aligned
a_q_b_arr = traj.q_es_aligned
# compute the relative transform applied to the estimation trajectory and propagate it to center of mass quantities
a_T_b0 = pin.SE3(
pin.Quaternion(a_q_b_arr[0, :].reshape((4, 1))).toRotationMatrix(),
a_p_ab_arr[0, :])
o_T_b0 = pin.SE3(
pin.Quaternion(o_q_b_arr[0, :].reshape((4, 1))).toRotationMatrix(),
o_p_ob_arr[0, :])
a_T_o = a_T_b0 * o_T_b0.inverse(
) # compute alignment transformation based on the first frame
def run(self):
# Priority settings for "InputTask" process
print(
f"Input task started with pid: {os.getpid()} and priority: {os.sched_getparam(os.getpid())}"
)
pid = os.getpid()
sched = os.SCHED_FIFO
param = os.sched_param(50)
os.sched_setscheduler(pid, sched, param)
print(
f"Input task with pid: {os.getpid()} changed to priority: {os.sched_getparam(os.getpid())}\n"
)
sys.stdin = os.fdopen(self.standard_input)
print()
print(f"Robot is currently in {self._mode} mode")
while (self._running):
try:
input_params, input_message = parser.parse_input(">")
except EOFError:
self.terminate()
continue
if input_message == "Angles":
if self._mode == "Idle":
trajectory = trj.Trajectory(input_params)
if trajectory.is_valid():
self.trajectory_queue.put(trajectory)
else:
print(
"Robot is not in idle mode. Send it to idle by typing >Idle"
)
elif input_message == "Coordinates":
if self._mode == "Walk":
with self.shared_position.get_lock():
current_position = self.shared_position[:]
joint_params = kinematics.simple_walk_controller(
input_params, current_position)
if None in joint_params:
continue
#print(f"Going to : {joint_params} [RAD]")
#print(f"Joint angles : {np.degrees(joint_params)} [deg]")
trajectory = trj.Trajectory(joint_params)
if trajectory.is_valid():
self.trajectory_queue.put(trajectory)
else:
print("Robot is not in walking mode. Type >Walk")
else:
self.handle_input(input_message)
sys.stdin.close()
def plot(self):
# check if the simulation was executed before
if not self._simulated:
# abort method
return
# instantiate a general object that includes the single diagrams
vis = diagram_visualisation.DiagramVisualisation()
# set properties
vis.title = self._title
vis.legend_visibility = self._legend
vis.title_visibility = self._title_visibility
vis.subplots = self._subplots
vis.number_rows = self._num_rows
vis.number_cols = self._num_cols
vis.auto_subplot_allocation = self._auto_subplot
vis.line_width = self._line_width
try:
# iteration through all simulations
for index_sim, item_sim in self._simulations.items():
# recreate the object - otherwise you would have a referencing problem
# instantiate an object that combines single trajectories
d_obj = diagram.Diagram()
# set properties
d_obj.title = item_sim.title
if not self._subplots:
d_obj.xlabel = self._xlabel
d_obj.ylabel = self._ylabel
else:
d_obj.xlabel = item_sim.xlabel
d_obj.ylabel = item_sim.ylabel
d_obj.legend_position = int(item_sim.legend_position)
d_obj.subplot_position = int(item_sim.subplot_position)
d_obj.title_visibility = item_sim.title_visibility
d_obj.legend_visibility = item_sim.legend_visibility
# determine x data
x = []
for i in range(len(self._model.output[index_sim - 1].times)):
x.append(self._model.output[index_sim - 1].times[i])
# iteration through all trajectories
for key, item in self._places.items():
# recreate the object - otherwise you would have a referencing problem
# instantiate a trajectory object
t_obj = trajectory.Trajectory()
# set properties
t_obj.color = item.color
t_obj.legend_text = item.legend_text
if not self._subplots and len(self._simulations) > 1:
t_obj.legend_text = t_obj.legend_text + " - Simulation " + str(index_sim)
t_obj.auto_color_allocation = item.auto_color_allocation
# set trajectory data
t_obj.x_data = copy.deepcopy(x)
t_obj.y_data = []
index = -1
for i in range(len(self._model.data.petri_net_data.places)):
if self._model.data.petri_net_data.places[i] == key:
index = i
break
if index != -1:
for i in range(len(self._model.output[index_sim - 1].markings)):
t_obj.y_data.append(self._model.output[index_sim - 1].markings[i][index])
# add trajectory object to the diagram object
d_obj.add(t_obj, t_obj.legend_text)
if len(self._simulations) == 1 and self._subplots:
d_obj_t = diagram.Diagram()
# set properties
d_obj_t.title = item.legend_text
if not self._subplots:
d_obj_t.xlabel = self._xlabel
d_obj_t.ylabel = self._ylabel
else:
d_obj_t.xlabel = item_sim.xlabel
d_obj_t.ylabel = item_sim.ylabel
d_obj_t.legend_position = int(item_sim.legend_position)
#d_obj_t.subplot_position = int(item_sim.subplot_position)
d_obj_t.title_visibility = item_sim.title_visibility
d_obj_t.legend_visibility = item_sim.legend_visibility
# add trajectory object to the diagram object
d_obj_t.add(t_obj, t_obj.legend_text)
vis.add(d_obj_t, d_obj_t.title)
if len(self._simulations) > 1 or not self._subplots:
# add diagram object to the visualisation object
vis.add(d_obj, d_obj.title)
# visualise diagrams
vis.plot()
except IndexError:
print "Simulation needs to be executed after changing simulation parameter. "
