def update(self): self.prev_fbk = list(self.fbk) self.group.get_next_feedback(reuse_fbk=self.fbk) self.time_now = perf_counter() if not self.at_goal: self.t_traj = self.time_now - self.traj_start_time if self.time_now > (self.traj_start_time + self.duration): self.at_goal = True if self.gripper != None: self.gripper.update() # Create command if self.goal == "grav comp": # Calculate required torques to negate gravity at current position nan_array = np.empty(self.group.size) nan_array[:] = np.nan self.cmd.position = nan_array self.cmd.velocity = nan_array self.cmd.effort = get_grav_comp_efforts(self.model, self.fbk.position, -self.gravity_vec) elif not self.at_goal: # Move to target position self.pos_cmd, self.vel_cmd, self.accel_cmd = self.trajectory.get_state(self.t_traj) self.cmd.position = self.pos_cmd self.cmd.velocity = self.vel_cmd self.cmd.effort = get_grav_comp_efforts(self.model, self.fbk.position, -self.gravity_vec) else: # Stay at current position self.cmd.position = self.fbk.position_command return
def execute_trajectory(group, model, trajectory, feedback): # Set up command object, timing variables, and other necessary variables num_joints = group.size command = hebi.GroupCommand(num_joints) duration = trajectory.duration start = time() t = time() - start while t < duration: # Get feedback and update the timer group.get_next_feedback(reuse_fbk=feedback) t = time() - start # Get new commands from the trajectory pos_cmd, vel_cmd, acc_cmd = trajectory.get_state(t) # Calculate commanded efforts to assist with tracking the trajectory. # Gravity Compensation uses knowledge of the arm's kinematics and mass to # compensate for the weight of the arm. Dynamic Compensation uses the # kinematics and mass to compensate for the commanded accelerations of the arm. eff_cmd = math_utils.get_grav_comp_efforts(model, feedback.position, [0, 0, 1]) # NOTE: dynamic compensation effort computation has not yet been added to the APIs # Fill in the command and send commands to the arm command.position = pos_cmd command.velocity = vel_cmd command.effort = eff_cmd group.send_command(command)
def update_command(self, group_command, pose, soft_start): """ Write into the command object based on the current state of the arm :param group_command: :param pose: :param soft_start: """ commanded_positions = self._joint_angles commanded_velocities = self._joint_velocities positions = self._fbk_position velocities = self._fbk_velocity # ---------------- # Calculate effort np.subtract(self._grip_pos, self.current_tip_fk[0:3, 3], out=self._xyz_error) np.copyto(self._pos_error[0:3], self._xyz_error) np.subtract(commanded_velocities, velocities, out=self._vel_error[0:4]) np.dot(self._current_j_actual_f, np.asmatrix(self._vel_error[0:4]), out=self._vel_error) np.multiply(Arm.spring_gains, self._pos_error, out=self._pos_error) np.multiply(Arm.damper_gains, self._vel_error, out=self._vel_error) np.add(self._pos_error, self._vel_error, out=self._impedance_err) np.dot(self.current_j_actual.T, np.asmatrix(self._impedance_err), out=self._impedance_torque) np.copyto( self._grav_comp_torque.ravel(), math_utils.get_grav_comp_efforts(self._robot, positions, -pose[2, 0:3])) np.multiply(soft_start, self._impedance_torque, out=self._joint_efforts) np.add(self._joint_efforts, self._grav_comp_torque, out=self._joint_efforts) effort = self._joint_efforts # Send commands idx = 0 for i in self.group_indices: cmd = group_command[i] cmd.position = commanded_positions[idx] cmd.velocity = commanded_velocities[idx] cmd.effort = effort[idx] idx = idx + 1
def get_grav_comp_efforts(self, positions, gravity): """ :param positions: :type positions: np.array :param gravity: :type gravity: np.array :return: :rtype: np.array """ kin = self._kin positions = positions[self._group_indices] return math_utils.get_grav_comp_efforts(kin, positions, gravity)
while not m.get_button_state(1): # Update MobileIO state if not m.update(): print("Failed to get feedback from MobileIO") continue # Gather sensor data from the arm group.get_next_feedback(reuse_fbk=fbk) # Update gravity vector the base module of the arm params.update_gravity(fbk) gravity_vec = params.gravity_vec # Calculate required torques to negate gravity at current position cmd.effort = get_grav_comp_efforts(kin, fbk.position, -gravity_vec) + effort_offset # Send to robot group.send_command(cmd) if enable_logging: hebi_log = group.stop_log() # Plot tracking / error from the joints in the arm. time = [] position = [] velocity = [] effort = [] # iterate through log for entry in hebi_log.feedback_iterate: time.append(entry.transmit_time)
start = time() t = time() - start while (t < duration) and not abort_flag: # Get feedback and update the timer group.get_next_feedback(reuse_fbk=fbk) t = time() - start # Get new commands from the trajectory pos_cmd, vel_cmd, acc_cmd = trajectory.get_state(t) # Calculate commanded efforts to assist with tracking the trajectory. # Gravity Compensation uses knowledge of the arm's kinematics and mass to # compensate for the weight of the arm. Dynamic Compensation uses the # kinematics and mass to compensate for the commanded accelerations of the arm. eff_cmd = math_utils.get_grav_comp_efforts(kin, fbk.position, [0, 0, 1]) # NOTE: dynamic compensation effort computation has not yet been added to the APIs # Fill in the command and send commands to the arm cmd.position = pos_cmd cmd.velocity = vel_cmd cmd.effort = eff_cmd group.send_command(cmd) # Update run mode run_mode = "standby" print("Standing by") elif run_mode == "standby": # Hold pos and wait for input # Check if mode toggle requested
end_effector_rot_mat = arm_tip_fk[0:3, 0:3].copy() controller_on = False while not has_esc_been_pressed(): # Gather sensor data from the arm fbk = group.get_next_feedback(reuse_fbk=fbk) fbk_position = fbk.position # Update gravity vector the base module of the arm params.update_gravity(fbk) gravity_vec[:] = -params.gravity_vec # Calculate required torques to negate gravity at current position grav_effort = get_grav_comp_efforts(kin, fbk_position, gravity_vec) + effort_offset if controller_on: # Get Updated Forward Kinematics and Jacobians kin.get_end_effector(fbk_position, output=arm_tip_fk) kin.get_jacobian_end_effector(fbk_position, output=J_arm_tip) # Calculate Impedence Control Wrenches and Appropraite Joint Torque spring_wrench[:] = 0 damper_wrench[:] = 0 # Linear error is easy xyz_error[0:3] = (end_effector_XYZ - arm_tip_fk[0:3, 3]).reshape(3, 1) # Rotational error involves calculating axis-angle from the # resulting error in S03 and providing a torque around that axis.
t0 = fbk_time.min() t = 0 while t < arm_traj.duration: group.get_next_feedback(reuse_fbk=fbk) t = min((fbk_time - t0).min(), arm_traj.duration) pos, vel, accel = arm_traj.get_state(t) cmd.position = pos cmd.velocity = vel if enable_effort_comp: fbk_position = fbk.position dynamics_comp = get_dynamic_comp_efforts(fbk_position, pos, vel, accel, kin) grav_comp = get_grav_comp_efforts(kin, fbk_position, gravity_vec) cmd.effort = dynamics_comp + grav_comp + effort_offset group.send_command(cmd) # Grab initial pose fbk_mobile = mobile_io_controller.current_feedback q = fbk_mobile.ar_orientation[0] R_init = quat2rot(q) xyz_init = fbk_mobile.ar_position[0] xyz_phone_new = xyz_init end_velocities = np.zeros((1, arm_dof_count)) end_accels = np.zeros((1, arm_dof_count))
if run_mode == "points": # Set led to green for point move mode m.set_led_color("green") # Move to point if t < duration: t = time() - start # Get new commands from the trajectory pos_cmd, vel_cmd, acc_cmd = trajectory.get_state(t) # Calculate commanded efforts to assist with tracking the trajectory. # Gravity Compensation uses knowledge of the arm's kinematics and mass to # compensate for the weight of the arm. Dynamic Compensation uses the # kinematics and mass to compensate for the commanded accelerations of the arm. eff_cmd = math_utils.get_grav_comp_efforts(kin, fbk.position, [0, 0, 1]) # NOTE: dynamic compensation effort computation has not yet been added to the APIs # Fill in the command and send commands to the arm cmd.position = pos_cmd cmd.velocity = vel_cmd cmd.effort = eff_cmd group.send_command(cmd) else: cmd.position = pos_cmd group.send_command(cmd) elif run_mode == "grav comp": # Set led to blue for grav comp mode m.set_led_color("blue") # Grav comp mode