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