def execute_trajectories(self):
trajectory_goals, tmp = self._get_trajectory_and_end_effector_goals_from_waypoints(
)
# execute initial position-to-start trajectory
if len(trajectory_goals) == 0:
print("Error: No trajectory goals to execute!")
else:
# execute initial position to start trajectory
init_goal = TrajectoryGoal()
waypoint_1 = WaypointMsg()
waypoint_1.names = self.hebi_mapping
waypoint_1.positions = self._get_joint_angles()
waypoint_1.velocities = [0] * len(self.hebi_mapping)
waypoint_1.accelerations = [0] * len(self.hebi_mapping)
waypoint_2 = trajectory_goals[0].waypoints[0]
init_goal.waypoints = [waypoint_1, waypoint_2]
init_goal.times = [0, 3]
self._executing_trajectory = True
self.trajectory_action_client.send_goal(init_goal)
self.trajectory_action_client.wait_for_result()
# execute trajectory goals
for goal in trajectory_goals:
self.trajectory_action_client.send_goal(goal)
self.trajectory_action_client.wait_for_result()
self.trajectory_action_client.get_result()
self._executing_trajectory = False
def execute(self, ud):
self.enter(ud)
init_wp = WaypointMsg()
lift_wp = WaypointMsg()
end_wp = WaypointMsg()
eff_prev_target_pos = self.kdl_fk.forward(
ud.prev_joint_pos)[:3, 3].reshape(1, 3).tolist()[0]
jt_init_pos = self.hebi_wrap.get_joint_positions()
eff_init_pos = self.kdl_fk.forward(jt_init_pos)[:3, 3].reshape(
1, 3).tolist()[0]
eff_target_pos = [
ud.target_end_link_point.x, ud.target_end_link_point.y,
ud.target_end_link_point.z
]
# init_wp
init_wp.names = self.hebi_wrap.hebi_mapping
init_wp.positions = jt_init_pos
init_wp.velocities = [0.0] * self.hebi_wrap.hebi_count
init_wp.accelerations = [0.0] * self.hebi_wrap.hebi_count
# lift_wp
lift_wp.names = self.hebi_wrap.hebi_mapping
eff_lift_pos = [(a + b + 2.0 * c) / 2.0 for a, b, c in zip(
eff_init_pos, eff_target_pos, self.step_height_vector)]
success, lift_wp.positions = self._get_pos_ik(
self.trac_ik,
jt_init_pos,
eff_lift_pos,
seed_xyz=eff_prev_target_pos)
lift_wp.velocities = [NAN] * self.hebi_wrap.hebi_count
lift_wp.accelerations = [NAN] * self.hebi_wrap.hebi_count
# end_wp
end_wp.names = self.hebi_wrap.hebi_mapping
success, end_wp.positions = self._get_pos_ik(self.trac_ik,
lift_wp.positions,
eff_target_pos,
seed_xyz=eff_lift_pos)
end_wp.velocities = [0.0] * self.hebi_wrap.hebi_count
end_wp.accelerations = [0.0] * self.hebi_wrap.hebi_count
goal = TrajectoryGoal()
goal.waypoints = [init_wp, lift_wp, end_wp]
goal.times = [0.0, ud.execution_time / 2.0, ud.execution_time]
# send goal to trajectory action server
self._hold_leg_position = False
self.hebi_wrap.trajectory_action_client.send_goal(goal)
self.hebi_wrap.trajectory_action_client.wait_for_result()
self._hold_leg_position = True
ud.prev_joint_pos = end_wp.positions
self.exit(ud)
return 'success'
def get_traj_goal_to_target_jt_pos(target_jt_pos, time):
goal = TrajectoryGoal()
i_wp = WaypointMsg()
i_wp.names = hebi_mapping_flat
i_wp.positions = get_jt_angles()
i_wp.velocities = [0.0]*len(hebi_mapping_flat)
i_wp.accelerations = [0.0]*len(hebi_mapping_flat)
f_wp = WaypointMsg()
f_wp.positions = target_jt_pos
f_wp.velocities = [0.0]*len(hebi_mapping_flat)
f_wp.accelerations = [0.0]*len(hebi_mapping_flat)
goal.waypoints = [i_wp, f_wp]
goal.times = [0, time]
return goal
def generateGoal(self):
self.goal = TrajectoryGoal()
self.goal.times = self.interp_times
self.goal.waypoints = []
for i in range(0, self.num_interp_waypoints):
waypoint = WaypointMsg()
waypoint.names = self.names
waypoint.velocities = [NaN, NaN, NaN, NaN]
waypoint.accelerations = [NaN, NaN, NaN, NaN]
workspace_waypoint = []
# construct workspace waypoint
for j in range(0, self.num_workspace_dof):
workspace_waypoint.append( \
self.workspace_waypoints[j][i])
# get elbow up
elbow = self.elbow_up[0]
for j in range(0, self.num_waypoints):
if (self.interp_times[i] <= self.times[j]):
elbow = self.elbow_up[j]
# run ik to get configspace waypoint
configuration_waypoint = kin.ik(workspace_waypoint, elbow)
waypoint.positions = configuration_waypoint
self.goal.waypoints.append(waypoint)
self.goal.waypoints[0].velocities = [0, 0, 0, 0]
self.goal.waypoints[0].accelerations = [0, 0, 0, 0]
self.goal.waypoints[-1].velocities = [0, 0, 0, 0]
self.goal.waypoints[-1].accelerations = [0, 0, 0, 0]
def _get_trajectory_goal_from_json_data(trajectory_json):
"""Converts Python object into hebiros TrajectoryGoal()
Args:
trajectory_json (obj): Python object from customized json.load(...) with an custom object_hook function
Returns:
obj: TrajectoryGoal() with current joint state appended to front
"""
# Get data from json data structure
waypoints = []
for i in range(len(trajectory_json["waypoints"])):
waypoints.append(trajectory_json["waypoints"][str(i)])
times = trajectory_json["times"]
# Create ROS msg
trajectory_goal = TrajectoryGoal()
for waypoint in waypoints:
waypoint_msg = WaypointMsg()
waypoint_msg.names = waypoint["names"]
waypoint_msg.positions = waypoint["positions"]
waypoint_msg.velocities = waypoint["velocities"]
waypoint_msg.accelerations = waypoint["accelerations"]
trajectory_goal.waypoints.append(waypoint_msg)
trajectory_goal.times = times
return trajectory_goal
def stand_up(self):
rospy.loginfo("Hexapod standing up...")
current_leg_positions = self._get_joint_angles()
goal = TrajectoryGoal()
start_wp = WaypointMsg()
start_wp.names = self.hebi_mapping_flat
start_wp.positions = self._flatten(current_leg_positions)
start_wp.velocities = [0.0] * ACTUATORS_TOTAL
start_wp.accelerations = [0.0] * ACTUATORS_TOTAL
goal.waypoints.append(start_wp)
goal.times.append(0.0)
end_wp = WaypointMsg()
end_wp.names = self.hebi_mapping_flat
end_wp.positions = self._flatten(self.leg_jt_home_pos)
end_wp.velocities = [0.0] * ACTUATORS_TOTAL
end_wp.accelerations = [0.0] * ACTUATORS_TOTAL
goal.waypoints.append(end_wp)
goal.times.append(4.0)
self.trajectory_action_client.send_goal(
goal) # TODO: Add the various callbacks
self.trajectory_action_client.wait_for_result()
self.hold_pos([1, 2, 3, 4, 5, 6])
def _get_trajectory_and_end_effector_goals_from_waypoints(self):
trajectory_goals = []
end_effector_goals = []
prev_breakpoint = False
prev_time = 0.0
for waypoint in self.waypoints:
if waypoint is not None:
if len(trajectory_goals) == 0 or prev_breakpoint:
trajectory_goals.append(TrajectoryGoal())
end_effector_goals.append([])
# if applicable, 1st append last waypoint from previous trajectory
if len(trajectory_goals) > 1 and prev_breakpoint:
waypoint_msg = WaypointMsg()
waypoint_msg.names = list(
trajectory_goals[-2].waypoints[-1].names)
waypoint_msg.velocities = [0] * len(waypoint_msg.names)
waypoint_msg.accelerations = [0] * len(waypoint_msg.names)
trajectory_goals[-1].waypoints.append(waypoint_msg)
trajectory_goals[-1].times.append(0.0)
end_effector_goals[-1].append(
copy.deepcopy(end_effector_goals[-2]))
# append a new waypoint
trajectory_goals[-1].waypoints.append(
copy.deepcopy(waypoint.joint_state))
prev_time += waypoint.time
trajectory_goals[-1].times.append(prev_time)
end_effector_goals[-1].append(
copy.deepcopy(waypoint.end_effector_pose))
prev_breakpoint = False
else: # breakpoint
prev_time = 0.0
prev_breakpoint = True
return trajectory_goals, end_effector_goals
def __init__(self, hebi_group_name, hebi_mapping, hebi_gains, urdf_str, base_link, end_link):
rospy.loginfo("Creating {} instance".format(self.__class__.__name__))
self.openmeta_testbench_manifest_path = rospy.get_param('~openmeta/testbench_manifest_path', None)
if self.openmeta_testbench_manifest_path is not None:
self._testbench_manifest = load_json_file(self.openmeta_testbench_manifest_path)
# TestBench Parameters
self._params = {}
for tb_param in self._testbench_manifest['Parameters']:
self._params[tb_param['Name']] = tb_param['Value'] # WARNING: If you use these values - make sure to check the type
# TestBench Metrics
self._metrics = {}
for tb_metric in self._testbench_manifest['Metrics']: # FIXME: Hmm, this is starting to look a lot like OpenMDAO...
self._metrics[tb_metric['Name']] = tb_metric['Value']
if hebi_gains is None:
hebi_gains = {
'p': [float(self._params['p_gain'])]*3,
'i': [float(self._params['i_gain'])]*3,
'd': [float(self._params['d_gain'])]*3
}
hebi_families = []
hebi_names = []
for actuator in hebi_mapping:
family, name = actuator.split('/')
hebi_families.append(family)
hebi_names.append(name)
rospy.loginfo(" hebi_group_name: %s", hebi_group_name)
rospy.loginfo(" hebi_families: %s", hebi_families)
rospy.loginfo(" hebi_names: %s", hebi_names)
self.hebi_wrap = HebirosWrapper(hebi_group_name, hebi_families, hebi_names)
# Set PID Gains
cmd_msg = CommandMsg()
cmd_msg.name = hebi_mapping
cmd_msg.settings.name = hebi_mapping
cmd_msg.settings.control_strategy = [4, 4, 4]
cmd_msg.settings.position_gains.name = hebi_mapping
cmd_msg.settings.position_gains.kp = hebi_gains['p']
cmd_msg.settings.position_gains.ki = hebi_gains['i']
cmd_msg.settings.position_gains.kd = hebi_gains['d']
cmd_msg.settings.position_gains.i_clamp = [0.25, 0.25, 0.25] # TODO: Tune me.
self.hebi_wrap.send_command_with_acknowledgement(cmd_msg)
if base_link is None or end_link is None:
robot = Robot.from_xml_string(urdf_str)
if not base_link:
base_link = robot.get_root()
# WARNING: There may be multiple leaf nodes
if not end_link:
end_link = [x for x in robot.link_map.keys() if x not in robot.child_map.keys()][0]
# pykdl
self.kdl_fk = KDLKinematics(URDF.from_xml_string(urdf_str), base_link, end_link)
self._active_joints = self.kdl_fk.get_joint_names()
# joint data
self.position_fbk = [0.0]*self.hebi_wrap.hebi_count
self.velocity_fbk = [0.0]*self.hebi_wrap.hebi_count
self.effort_fbk = [0.0]*self.hebi_wrap.hebi_count
# joint state publisher
while not rospy.is_shutdown() and len(self.hebi_wrap.get_joint_positions()) < len(self.hebi_wrap.hebi_mapping):
rospy.sleep(0.1)
self._joint_state_pub = rospy.Publisher('joint_states', JointState, queue_size=1)
self.hebi_wrap.add_feedback_callback(self._joint_state_cb)
# Set up Waypoint/Trajectory objects
self.start_wp = WaypointMsg()
self.start_wp.names = self.hebi_wrap.hebi_mapping
self.end_wp = copy.deepcopy(self.start_wp)
self.goal = TrajectoryGoal()
self.goal.waypoints = [self.start_wp, self.end_wp]
self._hold_positions = [0.0]*3
self._hold = True
self.jointstate = JointState()
self.jointstate.name = self.hebi_wrap.hebi_mapping
rospy.sleep(1.0)
def loop(self):
"""Main Hexapod loop (distant - somewhat less accomplished - relative of HEBI algorithm)
- Get chassis translation
- Get leg end-effector translations (relative to leg base link)
- side_alpha:odd, side_beta:even or side_alpha:even, side_beta:odd
- side_alpha legs lift, plant to +transformation
- side_alpha legs push to new home positions; side_beta legs push to -transformation
- swap side_alpha and side_beta
"""
rospy.loginfo("Hexapod entering main loop...")
rospy.loginfo(
" Waiting for initial velocity command on /hexapod/cmd_vel/ ...")
while self.last_vel_cmd is None:
self._loop_rate.sleep()
# start main loop
while not rospy.is_shutdown():
chassis_pos_delta = None
if self.last_vel_cmd is not None:
dt = 1 # FIXME: Temporary for debugging
lin_disp_lmt = self.linear_displacement_limit
ang_disp_lmt = self.angular_displacement_limit
chassis_pos_delta = Twist()
chassis_pos_delta.linear.x = clamp(
self.last_vel_cmd.linear.x * dt, -lin_disp_lmt,
lin_disp_lmt)
chassis_pos_delta.linear.y = clamp(
self.last_vel_cmd.linear.y * dt, -lin_disp_lmt,
lin_disp_lmt)
chassis_pos_delta.linear.z = clamp(
self.last_vel_cmd.linear.z * dt, -lin_disp_lmt,
lin_disp_lmt)
chassis_pos_delta.angular.x = clamp(
self.last_vel_cmd.angular.x * dt, -ang_disp_lmt,
ang_disp_lmt)
chassis_pos_delta.angular.y = clamp(
self.last_vel_cmd.angular.y * dt, -ang_disp_lmt,
ang_disp_lmt)
chassis_pos_delta.angular.z = clamp(
self.last_vel_cmd.angular.z * dt, -ang_disp_lmt,
ang_disp_lmt)
self.last_vel_cmd = None
if chassis_pos_delta is not None \
and not self._check_if_twist_msg_is_zero(chassis_pos_delta, linear_threshold=0.005, angular_threshold=0.01):
# Get chassis position transformation
chassis_pos_rot = transforms.euler_matrix(
chassis_pos_delta.angular.x, chassis_pos_delta.angular.y,
chassis_pos_delta.angular.z)[:3, :3]
rospy.loginfo("chassis_pos_rot: %s", chassis_pos_rot)
chassis_pos_trans = np.zeros([3])
chassis_pos_trans[0] = chassis_pos_delta.linear.x
chassis_pos_trans[1] = chassis_pos_delta.linear.y
chassis_pos_trans[2] = chassis_pos_delta.linear.z
chassis_translation = np.dot(chassis_pos_trans,
chassis_pos_rot)
rospy.loginfo("chassis_translation: %s", chassis_translation)
leg_target_eff_translation = [[]] * LEGS
# Get leg base positions relative to chassis
leg_base_positions = self._get_base_to_leg_base_fk()
for i, leg_base_pos in enumerate(leg_base_positions):
leg_base_pos_arr = np.array(leg_base_pos).reshape(3, 1)
leg_base_pos_arr_new = np.dot(chassis_pos_rot,
leg_base_pos_arr)
leg_base_pos_trans_4 = np.ones(4).reshape(4, 1)
leg_base_pos_trans_4[:3, :] = leg_base_pos_arr_new
# get leg base translations relative to leg_base coordinate frame
relative_trans = np.dot(
np.linalg.inv(self.kdl_fk_base_to_leg_base[i].forward(
[])), leg_base_pos_trans_4)
relative_trans = relative_trans.reshape(1,
4).tolist()[0][:3]
leg_target_eff_translation[i] = relative_trans
# Get leg target end-effector translations
for i, q in enumerate(self.leg_jt_home_pos):
base_to_leg_base_rot = self.kdl_fk_base_to_leg_base[
i].forward([])[:3, :3]
leg_target_eff_trans = np.dot(
np.linalg.inv(base_to_leg_base_rot),
chassis_translation).tolist()[0]
leg_target_eff_translation[i] = [
x + y for x, y in zip(leg_target_eff_translation[i],
leg_target_eff_trans)
] # TODO: FIXME: Technically incorrect
# 1: side_alpha legs lift, plant to +transformation
rospy.loginfo(
"1: side_alpha legs lift, plant to +transformation")
if self._odd_starts:
active_legs = [1, 2, 5]
else: # even starts
active_legs = [0, 3, 4]
init_wp = WaypointMsg()
lift_wp = WaypointMsg()
end_wp = WaypointMsg()
legs_jt_init_pos = self._get_joint_angles()
leg_eff_cur_pos = self._get_leg_base_to_eff_fk(
legs_jt_init_pos)
for i in range(LEGS):
motor_names = [name for name in self.hebi_mapping[i]]
# INITIAL POSITION
init_wp.names.extend(motor_names)
init_wp.positions.extend(legs_jt_init_pos[i])
init_wp.velocities.extend([0.0] * ACTUATORS_PER_LEG)
init_wp.accelerations.extend([0.0] * ACTUATORS_PER_LEG)
# LIFT
lift_wp.names.extend(motor_names)
if i in active_legs:
# apply translation
leg_lift_eff_target_pos = [
(x + y + z) / 2.0 for x, y, z in zip(
leg_eff_cur_pos[i], self.leg_eff_home_pos[i],
leg_target_eff_translation[i])
]
leg_lift_eff_target_pos = [
x + y for x, y in zip(leg_lift_eff_target_pos,
self.leg_eff_step_height[i])
]
# get ik
leg_lift_jt_target_pos = self._get_pos_ik(
self.trac_ik_leg_base_to_end[i],
legs_jt_init_pos[i],
leg_lift_eff_target_pos,
seed_xyz=self.leg_eff_home_pos[i])
lift_wp.positions.extend(leg_lift_jt_target_pos)
lift_wp.velocities.extend([NAN] * ACTUATORS_PER_LEG)
lift_wp.accelerations.extend([NAN] * ACTUATORS_PER_LEG)
else:
lift_wp.positions.extend(legs_jt_init_pos[i])
lift_wp.velocities.extend([0.0] * ACTUATORS_PER_LEG)
lift_wp.accelerations.extend([0.0] * ACTUATORS_PER_LEG)
# PLANT
end_wp.names.extend(motor_names)
if i in active_legs:
# apply translation
leg_plant_eff_target_pos = [
x + y
for x, y in zip(self.leg_eff_home_pos[i],
leg_target_eff_translation[i])
]
leg_plant_eff_target_pos[2] = self.leg_eff_home_pos[i][
2] # end eff z-position should match home z-position
# get ik
leg_plant_jt_target_pos = self._get_pos_ik(
self.trac_ik_leg_base_to_end[i],
leg_lift_jt_target_pos,
leg_plant_eff_target_pos,
seed_xyz=leg_lift_eff_target_pos)
end_wp.positions.extend(leg_plant_jt_target_pos)
end_wp.velocities.extend([0.0] * ACTUATORS_PER_LEG)
end_wp.accelerations.extend([0.0] * ACTUATORS_PER_LEG)
else:
end_wp.positions.extend(legs_jt_init_pos[i])
end_wp.velocities.extend([0.0] * ACTUATORS_PER_LEG)
end_wp.accelerations.extend([0.0] * ACTUATORS_PER_LEG)
goal = TrajectoryGoal()
goal.waypoints.append(init_wp)
goal.waypoints.append(lift_wp)
goal.waypoints.append(end_wp)
goal.times.extend([0.0, 0.4, 0.8])
self.release_pos([1, 2, 3, 4, 5, 6])
self.trajectory_action_client.send_goal(goal)
self.trajectory_action_client.wait_for_result()
self.hold_pos([1, 2, 3, 4, 5, 6])
# 2: side_alpha legs push to new home positions; side_beta legs push to -transformation
rospy.loginfo(
"2: side_alpha legs push to new home positions; side_beta legs push to -transformation"
)
if self._odd_starts:
active_legs = [0, 3, 4]
else: # even starts
active_legs = [1, 2, 5]
init_wp = WaypointMsg()
end_wp = WaypointMsg()
legs_jt_init_pos = self._get_joint_angles()
for i in range(LEGS):
motor_names = [name for name in self.hebi_mapping[i]]
# INITIAL POSITION
init_wp.names.extend(motor_names)
init_wp.positions.extend(legs_jt_init_pos[i])
init_wp.velocities.extend([0.0] * ACTUATORS_PER_LEG)
init_wp.accelerations.extend([0.0] * ACTUATORS_PER_LEG)
# PUSH
end_wp.names.extend(motor_names)
if i in active_legs:
# apply -translation
leg_plant_eff_target_pos = [
x + y for x, y in zip(self.leg_eff_home_pos[i], [
-val for val in leg_target_eff_translation[i]
])
]
leg_plant_eff_target_pos[2] = self.leg_eff_home_pos[i][
2] # end eff z-position should match home z-position
# get ik
leg_plant_jt_target_pos = self._get_pos_ik(
self.trac_ik_leg_base_to_end[i],
legs_jt_init_pos[i],
leg_plant_eff_target_pos,
seed_xyz=self.leg_eff_home_pos[i])
end_wp.positions.extend(leg_plant_jt_target_pos)
end_wp.velocities.extend([0.0] * ACTUATORS_PER_LEG)
end_wp.accelerations.extend([0.0] * ACTUATORS_PER_LEG)
else:
end_wp.positions.extend(self.leg_jt_home_pos[i])
end_wp.velocities.extend([0.0] * ACTUATORS_PER_LEG)
end_wp.accelerations.extend([0.0] * ACTUATORS_PER_LEG)
goal = TrajectoryGoal()
goal.waypoints.append(init_wp)
goal.waypoints.append(end_wp)
goal.times.extend([0.0, 0.4])
self.release_pos([1, 2, 3, 4, 5, 6])
self.trajectory_action_client.send_goal(goal)
self.trajectory_action_client.wait_for_result()
self.hold_pos([1, 2, 3, 4, 5, 6])
self._odd_starts = not self._odd_starts
self._loop_rate.sleep(
) # FIXME: Doesn't make sense to use this unless re-planning trajectories
def main():
# Initialize ROS node
rospy.init_node("example_04_trajectory_node", disable_signals=True)
rate = rospy.Rate(200)
group_name = "my_group"
num_joints = 3
num_waypoints = 5
# Create a client which uses the service to create a group
add_group_client = rospy.ServiceProxy("hebiros/add_group_from_names", AddGroupFromNamesSrv)
# Create a subscriber to receive feedback from a group
# Register feedback callback which runs when feedback is received
feedback_subscriber = rospy.Subscriber("/hebiros/"+group_name+"/feedback/joint_state", JointState, feedback_callback, queue_size=100)
# Construct a group using 3 known modules
rospy.wait_for_service("hebiros/add_group_from_names")
req_group_name = group_name
req_names = ["base", "shoulder", "elbow"]
req_families = ["HEBI"]
add_group_client(req_group_name, req_names, req_families)
# Create an action client for executing a trajectory
client = SimpleActionClient("/hebiros/"+group_name+"/trajectory", TrajectoryAction)
# Wait for the action server corresponding to the action client
client.wait_for_server()
user_input = raw_input("Press Enter/Return to execute Trajectory")
# Construct a trajectory to be sent as an action goal
goal = TrajectoryGoal()
# Set the times to reach each waypoint in seconds
times = [0, 5, 10, 15, 20]
names = ["HEBI/base", "HEBI/shoulder", "HEBI/elbow"]
# Wait for feedback from actuators
while not rospy.is_shutdown() and feedback.position is not None and len(feedback.position) < len(names):
rate.sleep()
# Set positions, velocities, and accelerations for each waypoint and each joint
# The following vectors have one joint per row and one waypoint per column
positions = [[feedback.position[0], 0, M_PI_2, 0, 0],
[feedback.position[1], 0, M_PI_2, M_PI_2, 0],
[feedback.position[2], 0, 0, M_PI_2, 0]]
velocities = [[0, NAN, NAN, NAN, 0],
[0, NAN, NAN, NAN, 0],
[0, NAN, NAN, NAN, 0]]
accelerations = [[0, NAN, NAN, NAN, 0],
[0, NAN, NAN, NAN, 0],
[0, NAN, NAN, NAN, 0]]
# Construct the goal using the TrajectoryGoal format
for i in range(num_waypoints):
waypoint = WaypointMsg()
waypoint.names = names
waypoint.positions = [joint[i] for joint in positions]
waypoint.velocities = [joint[i] for joint in velocities]
waypoint.accelerations = [joint[i] for joint in accelerations]
goal.waypoints.append(waypoint)
goal.times.append(times[i])
# Send the goal, executing the trajectory
client.send_goal(goal, trajectory_done, trajectory_active, trajectory_feedback)
while not rospy.is_shutdown():
rate.sleep()
def move_to_pose(self, target_pose, mode, move_duration=0):
"""
:param target_pose: [x,y,z,R,P,Y] or Pose msg
:type target_pose: list or Pose
:param mode: "trajectory" or "command"
:type mode: str
:param move_duration: time in seconds to complete move
:type move_duration: float
:return:
"""
xyz_bounds = [0.01, 0.01, 0.01]
wxyz_bounds = [0.05, 0.05, 0.05]
if not isinstance(target_pose, Pose):
# xyz
target_xyz = [0, 0, 0]
for i in range(3):
if target_pose[i] in ("", " "):
xyz_bounds[i] = 1e5
else:
target_xyz[i] = float(target_pose[i])
# RPY
target_rpy = [0, 0, 0] # Tait-Byran Extrinsic xyz Euler angles
for i in range(3):
if target_pose[3 + i] in ("", " "):
wxyz_bounds[i] = 1e5
else:
target_rpy[i] = float(target_pose[3 + i])
# wxyz
target_wxyz = transforms.quaternion_from_euler(
target_rpy[0], target_rpy[1], target_rpy[2],
axes='sxyz').tolist()
else:
target_xyz = [
target_pose.position.x, target_pose.position.y,
target_pose.position.z
]
target_wxyz = [
target_pose.orientation.w, target_pose.orientation.x,
target_pose.orientation.y, target_pose.orientation.z
]
print("Seed angles: {}".format(
[round(ang, 4) for ang in self.hebi_wrap.get_joint_positions()]))
print("Target End-Effector Pose...")
print(" xyz: {}".format(target_xyz))
print(" wxyz: {}".format(target_wxyz))
print("Bounds...")
print(" xyz: {}".format(xyz_bounds))
print(" wxyz: {}".format(wxyz_bounds))
# Get target joint angles
target_jt_angles = self.trac_ik.get_ik(
self.hebi_wrap.get_joint_positions(), target_xyz[0], target_xyz[1],
target_xyz[2], target_wxyz[0], target_wxyz[1], target_wxyz[2],
target_wxyz[3], xyz_bounds[0], xyz_bounds[1], xyz_bounds[2],
wxyz_bounds[0], wxyz_bounds[1], wxyz_bounds[2])
if target_jt_angles is None:
print("NO JOINT SOLUTION FOUND FOR END-EFFECTOR POSE "
"\n(x: {}, y: {}, z: {}, W: {}, X: {}, Y: {}, Z: {})"
"\nPLEASE TRY AGAIN.".format(*target_xyz + target_wxyz))
return False
else:
if mode == "trajectory":
goal = TrajectoryGoal()
start_wp = WaypointMsg()
start_wp.names = self.hebi_mapping
start_wp.positions = self.hebi_wrap.get_joint_positions()
start_wp.velocities = [0] * len(self.hebi_mapping)
start_wp.accelerations = [0] * len(self.hebi_mapping)
goal.waypoints.append(start_wp)
end_wp = WaypointMsg()
end_wp.names = self.hebi_mapping
end_wp.positions = target_jt_angles
end_wp.velocities = [0] * len(self.hebi_mapping)
end_wp.accelerations = [0] * len(self.hebi_mapping)
goal.waypoints.append(end_wp)
goal.times.extend([0, move_duration])
self._hold_position = False
self.hebi_wrap.trajectory_action_client.send_goal_and_wait(
goal)
self._hold_joint_angles = self.hebi_wrap.get_joint_positions()
self._hold_position = True
else: # mode == "command"
cmd = JointState()
cmd.name = self.hebi_mapping
cmd.position = target_jt_angles
cmd.velocity = [0] * len(self.hebi_mapping)
cmd.effort = [0] * len(self.hebi_mapping)
self._hold_position = False
self.hebi_wrap.joint_state_publisher.publish(cmd)
self._hold_joint_angles = target_jt_angles
self._hold_position = True
return True