def tripod_gait(self, x_stride, y_stride, yaw_stride, inc, foot_height): x_inc = inc * x_stride y_inc = inc * y_stride yaw_inc = inc * yaw_stride for leg in self.leg_list: new_point = [] T_osc = np.identity(4) if (leg == "right_front" or leg == "right_back" or leg == "left_middle"): T_osc[:3, :3] = ang.eulerAnglesToRotationMatrix( [0, 0, -yaw_inc]) p_f = [ -x_inc, -y_inc, 0 if self.step_cntr < self.num_steps / 2.0 else foot_height ] else: T_osc[:3, :3] = ang.eulerAnglesToRotationMatrix( [0, 0, yaw_inc]) p_f = [ x_inc, y_inc, 0 if self.step_cntr > self.num_steps / 2.0 else foot_height ] T_osc[:3, 3] = p_f new_point = np.dot(T_osc, np.r_[self.foot_points[leg], 1]) success = self.update_joint_command(new_point[:3], leg) if not success: return False return True
def set_ee_cartesian_trajectory(self, x=0, y=0, z=0, roll=0, pitch=0, yaw=0, moving_time=None, wp_moving_time=0.2, wp_accel_time=0.1, wp_period=0.05): if self.group_info.num_joints < 6 and (y != 0 or yaw != 0): rospy.loginfo("Please leave the 'y' and 'yaw' fields at '0' when working with arms that have less than 6dof.") return False rpy = ang.rotationMatrixToEulerAngles(self.T_sb[:3,:3]) T_sy = np.identity(4) T_sy[:3,:3] = ang.eulerAnglesToRotationMatrix([0.0, 0.0, rpy[2]]) T_yb = np.dot(mr.TransInv(T_sy), self.T_sb) rpy[2] = 0.0 if (moving_time == None): moving_time = self.moving_time accel_time = self.accel_time N = int(moving_time / wp_period) inc = 1.0 / float(N) joint_traj = JointTrajectory() joint_positions = list(self.joint_commands) for i in range(N+1): joint_traj_point = JointTrajectoryPoint() joint_traj_point.positions = joint_positions joint_traj_point.time_from_start = rospy.Duration.from_sec(i * wp_period) joint_traj.points.append(joint_traj_point) if (i == N): break T_yb[:3,3] += [inc * x, inc * y, inc * z] rpy[0] += inc * roll rpy[1] += inc * pitch rpy[2] += inc * yaw T_yb[:3,:3] = ang.eulerAnglesToRotationMatrix(rpy) T_sd = np.dot(T_sy, T_yb) theta_list, success = self.set_ee_pose_matrix(T_sd, joint_positions, False, blocking=False) if success: joint_positions = theta_list else: rospy.loginfo("%.1f%% of trajectory successfully planned. Trajectory will not be executed." % (i/float(N) * 100)) break if success: self.set_trajectory_time(wp_moving_time, wp_accel_time) joint_traj.joint_names = self.group_info.joint_names current_positions = [] with self.core.js_mutex: for name in joint_traj.joint_names: current_positions.append(self.core.joint_states.position[self.core.js_index_map[name]]) joint_traj.points[0].positions = current_positions joint_traj.header.stamp = rospy.Time.now() self.core.pub_traj.publish(JointTrajectoryCommand("group", self.group_name, joint_traj)) rospy.sleep(moving_time + wp_moving_time) self.T_sb = T_sd self.joint_commands = joint_positions self.set_trajectory_time(moving_time, accel_time) return success
def wave_gait(self, x_stride, y_stride, yaw_stride, inc, foot_height): for leg in self.wave_legs: z_inc = 0 new_point = [] T_osc = np.identity(4) if leg != self.wave_legs[0]: self.wave_incs[leg] -= abs(inc - self.inc_prev) else: self.wave_incs[leg] += abs(inc - self.inc_prev) * 5.0 z_inc = foot_height x_inc = self.wave_incs[leg] * x_stride y_inc = self.wave_incs[leg] * y_stride yaw_inc = self.wave_incs[leg] * yaw_stride p_f = [x_inc, y_inc, z_inc] T_osc[:3, 3] = p_f T_osc[:3, :3] = ang.eulerAnglesToRotationMatrix([0, 0, yaw_inc]) new_point = np.dot(T_osc, np.r_[self.foot_points[leg], 1]) success = self.update_joint_command(new_point[:3], leg) if not success: self.wave_legs = [ "right_front", "left_front", "right_middle", "left_middle", "right_back", "left_back" ] self.wave_incs = {l: 0 for l in self.wave_legs} self.period_cntr = 0 return False self.period_cntr += 1.0 if (self.period_cntr == self.num_steps / 2.0): old_leg = self.wave_legs.pop(0) self.wave_legs.append(old_leg) self.period_cntr = 0 return True
def ripple_gait(self, x_stride, y_stride, yaw_stride, inc, foot_height): for pair in self.ripple_leg_pairs: z_inc = 0 new_point = [] T_osc = np.identity(4) if pair != self.ripple_leg_pairs[0]: self.ripple_incs[pair] -= abs(inc - self.inc_prev) else: self.ripple_incs[pair] += abs(inc - self.inc_prev) * 2.0 z_inc = foot_height x_inc = self.ripple_incs[pair] * x_stride y_inc = self.ripple_incs[pair] * y_stride yaw_inc = self.ripple_incs[pair] * yaw_stride p_f = [x_inc, y_inc, z_inc] T_osc[:3, 3] = p_f T_osc[:3, :3] = ang.eulerAnglesToRotationMatrix([0, 0, yaw_inc]) for leg in self.ripple_legs[pair]: new_point = np.dot(T_osc, np.r_[self.foot_points[leg], 1]) success = self.update_joint_command(new_point[:3], leg) if not success: self.ripple_leg_pairs = ["first", "second", "third"] self.ripple_incs = {p: 0 for p in self.ripple_leg_pairs} self.period_cntr = 0 return False self.period_cntr += 1.0 if (self.period_cntr == self.num_steps / 2.0): old_pair = self.ripple_leg_pairs.pop(0) self.ripple_leg_pairs.append(old_pair) self.period_cntr = 0 return True
def move_in_place(self, x=None, y=None, z=None, roll=None, pitch=None, yaw=None, moving_time=1.0, accel_time=0.3, blocking=True): self.set_trajectory_time("all", moving_time, accel_time) T_fb = np.array(self.T_fb) if x is not None: self.T_fb[0, 3] = x if y is not None: self.T_fb[1, 3] = y if z is not None: self.T_fb[2, 3] = z rpy = ang.rotationMatrixToEulerAngles(self.T_fb[:3, :3]) if roll is not None: rpy[0] = roll if pitch is not None: rpy[1] = pitch if yaw is not None: rpy[2] = yaw self.T_fb[:3, :3] = ang.eulerAnglesToRotationMatrix(rpy) for leg, point in self.foot_points.items(): success = self.update_joint_command(point, leg) if not success: self.T_fb = T_fb return False self.core.pub_group.publish(self.hexapod_command) self.update_tfb_transform(moving_time) if blocking: rospy.sleep(moving_time) return True
def set_ee_pose_components(self, x=0, y=0, z=0, roll=0, pitch=0, yaw=None, custom_guess=None, execute=True, moving_time=None, accel_time=None, blocking=True): if (self.group_info.num_joints < 6 or (self.group_info.num_joints >= 6 and yaw is None)): yaw = math.atan2(y,x) T_sd = np.identity(4) T_sd[:3,:3] = ang.eulerAnglesToRotationMatrix([roll, pitch, yaw]) T_sd[:3, 3] = [x, y, z] return self.set_ee_pose_matrix(T_sd, custom_guess, execute, moving_time, accel_time, blocking)
def move_in_world(self, x_stride=0, y_stride=0, yaw_stride=0, max_foot_height=0.04, num_steps=20.0, gait_type="tripod", mp=0.150, ap=0.075, num_cycles=1, cycle_freq=None): self.set_trajectory_time("all", mp, ap) self.num_steps = num_steps num_steps_in_cycle = self.num_steps * self.gait_factors[gait_type] / 2.0 if cycle_freq is None: cycle_freq = num_steps rate = rospy.Rate(cycle_freq) for cycle in range(num_cycles): self.step_cntr = 1 self.inc_prev = 0 while (self.step_cntr <= num_steps_in_cycle and not rospy.is_shutdown()): inc = 1 / self.gait_factors[gait_type] * 0.5 * ( 1 + math.sin(2 * np.pi * (self.step_cntr / self.num_steps) - np.pi / 2)) foot_height = max_foot_height * 0.5 * ( 1 + math.sin(4 * np.pi * (self.step_cntr / self.num_steps) - np.pi / 2)) success = False if gait_type == "tripod": success = self.tripod_gait(x_stride, y_stride, yaw_stride, inc, foot_height) elif gait_type == "ripple": success = self.ripple_gait(x_stride, y_stride, yaw_stride, inc, foot_height) elif gait_type == "wave": success = self.wave_gait(x_stride, y_stride, yaw_stride, inc, foot_height) if not success: self.reset_hexapod() return False aug_inc = abs(inc - self.inc_prev) temp_point = [aug_inc * x_stride, aug_inc * y_stride, 0] world_point = np.dot(self.T_sf[:3, :3], temp_point) self.T_sf[:3, 3] += world_point rpy = ang.rotationMatrixToEulerAngles(self.T_sf[:3, :3]) rpy[2] += aug_inc * yaw_stride self.T_sf[:3, :3] = ang.eulerAnglesToRotationMatrix(rpy) self.core.pub_group.publish(self.hexapod_command) self.update_tsf_transform(mp) self.inc_prev = inc self.step_cntr += 1.0 rate.sleep() return True
def set_ee_cartesian_trajectory(self, x=0, y=0, z=0, roll=0, pitch=0, yaw=0, moving_time=2.0, wp_period=0.02): rpy = ang.rotationMatrixToEulerAngles(self.T_sb[:3, :3]) T_sy = np.identity(4) T_sy[:2, :2] = ang.yawToRotationMatrix(rpy[2]) T_yb = np.dot(mr.TransInv(T_sy), self.T_sb) rpy = ang.rotationMatrixToEulerAngles(T_yb[:3, :3]) N = int(moving_time / wp_period) inc = 1.0 / float(N) joint_traj = [] joint_positions = list(self.joint_commands) for i in range(N + 1): joint_traj.append(joint_positions) if (i == N): break T_yb[:3, 3] += [inc * x, inc * y, inc * z] rpy[0] += inc * roll rpy[1] += inc * pitch rpy[2] += inc * yaw T_yb[:3, :3] = ang.eulerAnglesToRotationMatrix(rpy) T_sd = np.dot(T_sy, T_yb) theta_list, success = self.set_ee_pose_matrix( T_sd, joint_positions, False) if success: joint_positions = theta_list else: rospy.loginfo( "%.1f%% of trajectory successfully planned. Trajectory will not be executed." % (i / float(N) * 100)) break if success: mode = self.core.mode if (mode != 1): self.core.robot_smart_mode_reset(1) r = rospy.Rate(1 / wp_period) for cmd in joint_traj: self.core.robot_move_servoj(cmd) r.sleep() self.T_sb = T_sd self.joint_commands = joint_positions return success
def main(): T_sd = np.identity(4) T_sd[0, 3] = 0.3 T_sd[1, 3] = -0.1 T_sd[2, 3] = 0.2 rpy = [3.14, -1.2, 0.7] T_sd[:3, :3] = ang.eulerAnglesToRotationMatrix(rpy) bot = InterbotixManipulatorUX("uxarm6", mode=0, wait_for_finish=True, gripper_type=None) bot.arm.go_to_holdup_pose() bot.arm.set_ee_pose_matrix(T_sd) bot.arm.go_to_home_pose()
def get_urdf_info(self): full_rd_name = '/' + self.core.robot_name + '/robot_description' while rospy.has_param(full_rd_name) != True: pass robot_description = URDF.from_parameter_server(key=full_rd_name) for leg in self.leg_list: joint_object = next((joint for joint in robot_description.joints if joint.name == (leg + "_coxa")), None) T_bc = np.identity(4) T_bc[:3, 3] = joint_object.origin.xyz T_bc[:3, :3] = ang.eulerAnglesToRotationMatrix( joint_object.origin.rpy) self.T_bc[leg] = T_bc femur_joint = next((joint for joint in robot_description.joints if joint.name == "left_front_femur")) self.coxa_length = femur_joint.origin.xyz[0] tibia_joint = next((joint for joint in robot_description.joints if joint.name == "left_front_tibia")) femur_x = tibia_joint.origin.xyz[0] femur_z = tibia_joint.origin.xyz[2] self.femur_offset_angle = abs(math.atan2(femur_z, femur_x)) self.femur_length = math.sqrt(femur_x**2 + femur_z**2) foot_joint = next((joint for joint in robot_description.joints if joint.name == "left_front_foot")) tibia_x = foot_joint.origin.xyz[0] tibia_z = foot_joint.origin.xyz[2] self.tibia_offset_angle = abs(math.atan2( tibia_z, tibia_x)) - self.femur_offset_angle self.tibia_length = math.sqrt(tibia_x**2 + tibia_z**2) bottom_joint = next((joint for joint in robot_description.joints if joint.name == "base_bottom")) self.bottom_height = abs(bottom_joint.origin.xyz[2]) self.home_height = self.bottom_height + 0.05
def move_in_world_rough(self, x_stride=0, y_stride=0, yaw_stride=0, max_foot_height=0.02, leg_up_time=0.5, num_swing_steps=10.0, mp=0.150, ap=0.075, leg_down_inc=0.001, threshold=70, reset_foot_points=False, reset_height=0.12, num_cycles=1, cycle_freq=20.0): if reset_foot_points: self.foot_points = copy.deepcopy(self.home_foot_points) self.move_in_place(z=reset_height) # setup 'tripod' gait variables first_set = ["left_front", "left_back", "right_middle"] second_set = ["right_front", "right_back", "left_middle"] sets = [first_set, second_set] rate = rospy.Rate(cycle_freq) for x in range(num_cycles): for set in sets: # Move all legs in a set up to 'max_foot_height' for leg in set: new_point = np.r_[self.foot_points[leg][:2], max_foot_height] success = self.update_joint_command(new_point, leg) if not success: return False self.foot_points[leg][2] = max_foot_height self.set_trajectory_time("all", leg_up_time, leg_up_time / 2.0) self.core.pub_group.publish(self.hexapod_command) time_start = rospy.get_time() # Move all legs in the XY plane according to the 'stride' parameters self.set_trajectory_time("all", mp, ap) time_diff = leg_up_time - (rospy.get_time() - time_start) if time_diff > 0: rospy.sleep(time_diff) inc_prev = 0 for step in range(1, int(num_swing_steps) + 1): inc = 0.25 * ( 1 + math.sin(np.pi * (step / num_swing_steps) - np.pi / 2)) x_inc = inc * x_stride y_inc = inc * y_stride yaw_inc = inc * yaw_stride for leg, point in self.foot_points.items(): T_osc = np.identity(4) if leg not in set: T_osc[:3, :3] = ang.eulerAnglesToRotationMatrix( [0, 0, -yaw_inc]) p_f = [-x_inc, -y_inc, 0] else: T_osc[:3, :3] = ang.eulerAnglesToRotationMatrix( [0, 0, yaw_inc]) p_f = [x_inc, y_inc, 0] T_osc[:3, 3] = p_f new_point = np.dot(T_osc, np.r_[self.foot_points[leg], 1]) success = self.update_joint_command(new_point[:3], leg) if not success: return False if step == num_swing_steps: self.foot_points[leg] = list(new_point[:3]) aug_inc = abs(inc - inc_prev) temp_point = [aug_inc * x_stride, aug_inc * y_stride, 0] world_point = np.dot(self.T_sf[:3, :3], temp_point) self.T_sf[:3, 3] += world_point rpy = ang.rotationMatrixToEulerAngles(self.T_sf[:3, :3]) rpy[2] += aug_inc * yaw_stride self.T_sf[:3, :3] = ang.eulerAnglesToRotationMatrix(rpy) self.core.pub_group.publish(self.hexapod_command) self.update_tsf_transform(mp) inc_prev = inc rate.sleep() # Move all legs in a set down until 'ground touch' is achieved all_feet_grounded = False current_foot_height = max_foot_height time_start = rospy.get_time() while not all_feet_grounded: all_feet_grounded = True current_foot_height -= leg_down_inc for leg in set: if (rospy.get_time() <= time_start + 0.6): new_point = np.r_[self.foot_points[leg][:2], current_foot_height] success = self.update_joint_command(new_point, leg) if not success: continue self.foot_points[leg][2] = current_foot_height all_feet_grounded = False elif (rospy.get_time() > time_start + 0.6): joint_effort = self.core.joint_states.effort[ self.core.js_index_map[leg + "_femur"]] if (joint_effort < threshold): new_point = np.r_[self.foot_points[leg][:2], current_foot_height] success = self.update_joint_command( new_point, leg) if not success: continue self.foot_points[leg][2] = current_foot_height all_feet_grounded = False self.core.pub_group.publish(self.hexapod_command) rate.sleep() return True