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
