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. "