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 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 update_tfb_transform(self, moving_time): self.t_fb.transform.translation.x = self.T_fb[0, 3] self.t_fb.transform.translation.y = self.T_fb[1, 3] self.t_fb.transform.translation.z = self.T_fb[2, 3] rpy = ang.rotationMatrixToEulerAngles(self.T_fb[:3, :3]) q = quaternion_from_euler(rpy[0], rpy[1], rpy[2]) self.t_fb.transform.rotation = Quaternion(q[0], q[1], q[2], q[3]) self.t_fb.header.stamp = rospy.Time.now() + rospy.Duration(moving_time)
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 update_tsf_transform(self, moving_time): self.t_sf.transform.translation.x = self.T_sf[0, 3] self.t_sf.transform.translation.y = self.T_sf[1, 3] rpy = ang.rotationMatrixToEulerAngles(self.T_sf[:3, :3]) q = quaternion_from_euler(rpy[0], rpy[1], rpy[2]) self.t_sf.transform.rotation = Quaternion(q[0], q[1], q[2], q[3]) self.t_sf.header.stamp = rospy.Time.now() + rospy.Duration(moving_time) self.pose.pose.position = Point(self.T_sf[0, 3], self.T_sf[1, 3], 0) self.pose.pose.orientation = Quaternion(q[0], q[1], q[2], q[3]) self.pose.header.stamp = rospy.Time.now() + rospy.Duration(moving_time)
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 get_odometry(self): rpy = ang.rotationMatrixToEulerAngles(self.T_sf[:3, :3]) return [self.T_sf[0, 3], self.T_sf[1, 3], rpy[2]]
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
def find_ref_to_arm_base_transform(self, ref_frame=None, arm_base_frame=None, num_samples=5, position_only=False): if ref_frame == None: ref_frame = self.ref_frame if arm_base_frame == None: arm_base_frame = self.arm_base_frame # take the average pose (w.r.t. the camera frame) of the AprilTag over 'num_samples' samples point = Point() rpy = [0, 0, 0] for x in range(num_samples): ps = self.apriltag.find_pose() if ps == Pose(): return False point.x += ps.position.x / float(num_samples) point.y += ps.position.y / float(num_samples) point.z += ps.position.z / float(num_samples) quat_sample = ps.orientation quat_list = [ quat_sample.x, quat_sample.y, quat_sample.z, quat_sample.w ] rpy_sample = euler_from_quaternion(quat_list) rpy[0] += rpy_sample[0] / float(num_samples) rpy[1] += rpy_sample[1] / float(num_samples) rpy[2] += rpy_sample[2] / float(num_samples) T_CamTag = ang.poseToTransformationMatrix( [point.x, point.y, point.z, rpy[0], rpy[1], rpy[2]]) tfBuffer = tf2_ros.Buffer() listener = tf2_ros.TransformListener(tfBuffer) # If position_only, set the orientation of the found AR tag to be equivalent to the orientation of the arm's AR tag as dictated by the URDF if (position_only): T_CamActualTag = self.get_transform(tfBuffer, self.apriltag.image_frame_id, self.arm_tag_frame) T_CamTag[:3, :3] = T_CamActualTag[:3, :3] # Now, get a snapshot of the pose of arm's base_link frame w.r.t. the AR tag link (as defined in the URDF - not the one found by the algorithm) # We can't publish the AR tag pose found using the AprilTag algorithm to the /tf tree since ROS forbids a link to have multiple parents T_TagBase = self.get_transform(tfBuffer, self.arm_tag_frame, arm_base_frame) # Now, lets find the transform of the arm's base_link frame w.r.t. the reference frame T_CamBase = np.dot(T_CamTag, T_TagBase) if ref_frame == self.apriltag.image_frame_id: T_RefBase = T_CamBase else: T_RefCam = self.get_transform(tfBuffer, ref_frame, self.apriltag.image_frame_id) T_RefBase = np.dot(T_RefCam, T_CamBase) # Now, we can publish the transform from the reference link to the arm's base_link legally as the arm's base_link has no parent # (or even if it does, we can safely overwrite it since the 'tf' tree will remain intact) self.rpy = ang.rotationMatrixToEulerAngles(T_RefBase[:3, :3]) quat = quaternion_from_euler(self.rpy[0], self.rpy[1], self.rpy[2]) self.trans = TransformStamped() self.trans.transform.translation.x = T_RefBase[0, 3] self.trans.transform.translation.y = T_RefBase[1, 3] self.trans.transform.translation.z = T_RefBase[2, 3] self.trans.transform.rotation = Quaternion(quat[0], quat[1], quat[2], quat[3]) self.trans.header.frame_id = ref_frame self.trans.child_frame_id = arm_base_frame self.trans.header.stamp = rospy.Time.now() self.apriltag.pub_transforms.publish(self.trans) return True