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
