def cart_error(self, ep_actual, ep_desired):
pos_act, rot_act = PoseConv.to_pos_rot(ep_actual)
pos_des, rot_des = PoseConv.to_pos_rot(ep_desired)
err = np.mat(np.zeros((6, 1)))
err[:3,0] = pos_act - pos_des
err[3:6,0] = np.mat(trans.euler_from_matrix(rot_des.T * rot_act)).T
return err
def command_move_cb(self, msg):
if self.arm is None:
rospy.logwarn(
"[cartesian_manager] Cartesian controller not enabled.")
return
self.cart_ctrl.stop_moving(wait=True)
if msg.header.frame_id == "":
msg.header.frame_id = "torso_lift_link"
if self.kin is None or msg.header.frame_id not in self.kin.get_link_names(
):
self.kin = create_joint_kin("torso_lift_link", msg.header.frame_id)
torso_pos_ep, torso_rot_ep = PoseConv.to_pos_rot(self.arm.get_ep())
torso_B_ref = self.kin.forward(base_link="torso_lift_link",
end_link=msg.header.frame_id)
_, torso_rot_ref = PoseConv.to_pos_rot(torso_B_ref)
torso_rot_ref *= self.frame_rot
ref_pos_off, ref_rot_off = PoseConv.to_pos_rot(msg)
change_pos = torso_rot_ep.T * torso_rot_ref * ref_pos_off
change_pos_xyz = change_pos.T.A[0]
ep_rot_ref = torso_rot_ep.T * torso_rot_ref
change_rot = ep_rot_ref * ref_rot_off * ep_rot_ref.T
_, change_rot_rpy = PoseConv.to_pos_euler(
np.mat([0] * 3).T, change_rot)
rospy.loginfo("[cartesian_manager] Command relative movement." +
str((change_pos_xyz, change_rot_rpy)))
self.cart_ctrl.execute_cart_move((change_pos_xyz, change_rot_rpy),
((0, 0, 0), 0),
velocity=self.velocity,
blocking=True)
def create_ellipsoidal_path(self,
start_ell_pos,
start_ell_quat,
end_ell_pos,
end_ell_quat,
velocity=0.001,
min_jerk=True):
print "Start rot (%s):\r\n%s" % (type(start_ell_quat), start_ell_quat)
print "End rot (%s):\r\n%s" % (type(end_ell_quat), end_ell_quat)
_, start_ell_rot = PoseConv.to_pos_rot((start_ell_pos, start_ell_quat))
_, end_ell_rot = PoseConv.to_pos_rot((end_ell_pos, end_ell_quat))
rpy = trans.euler_from_matrix(
start_ell_rot.T *
end_ell_rot) # get roll, pitch, yaw of angle diff
end_ell_pos[1] = np.mod(end_ell_pos[1],
2 * np.pi) # wrap longitude value
ell_init = np.mat(start_ell_pos).T
ell_final = np.mat(end_ell_pos).T
# find the closest longitude angle to interpolate to
if np.fabs(2 * np.pi + ell_final[1, 0] - ell_init[1, 0]) < np.pi:
ell_final[1, 0] += 2 * np.pi
elif np.fabs(-2 * np.pi + ell_final[1, 0] - ell_init[1, 0]) < np.pi:
ell_final[1, 0] -= 2 * np.pi
if np.any(np.isnan(ell_init)) or np.any(np.isnan(ell_final)):
rospy.logerr("[ellipsoid_space] Nan values in ellipsoid. " +
"ell_init: %f, %f, %f; " %
(ell_init[0, 0], ell_init[1, 0], ell_init[2, 0]) +
"ell_final: %f, %f, %f; " %
(ell_final[0, 0], ell_final[1, 0], ell_final[2, 0]))
return None
num_samps = np.max([
2,
int(np.linalg.norm(ell_final - ell_init) / velocity),
int(np.linalg.norm(rpy) / velocity)
])
if min_jerk:
t_vals = min_jerk_traj(num_samps)
else:
t_vals = np.linspace(0, 1, num_samps)
# smoothly interpolate from init to final
ell_lat_traj = np.interp(t_vals, (0, 1),
(start_ell_pos[0], end_ell_pos[0]))
ell_lon_traj = np.interp(t_vals, (0, 1),
(start_ell_pos[1], end_ell_pos[1]))
ell_height_traj = np.interp(t_vals, (0, 1),
(start_ell_pos[2], end_ell_pos[2]))
ell_pos_traj = np.vstack((ell_lat_traj, ell_lon_traj, ell_height_traj))
ell_quat_traj = [
trans.quaternion_slerp(start_ell_quat, end_ell_quat, t)
for t in t_vals
]
return [(ell_pos_traj[:, i], ell_quat_traj[i])
for i in xrange(num_samps)]
def pose_to_ellipsoidal(self, pose):
pose_pos, pose_rot = PoseConv.to_pos_rot(pose)
lat, lon, height = self.pos_to_ellipsoidal(pose_pos[0, 0],
pose_pos[1, 0], pose_pos[2,
0])
_, ell_rot = PoseConv.to_pos_rot(
self.normal_to_ellipse(lat, lon, height))
_, quat_rot = PoseConv.to_pos_quat(
np.mat([0] * 3).T, ell_rot.T * pose_rot)
return [lat, lon, height], quat_rot
def inverse(self, pose, q_guess=None, min_joints=None, max_joints=None):
pos, rot = PoseConv.to_pos_rot(pose)
pos_kdl = kdl.Vector(pos[0,0], pos[1,0], pos[2,0])
rot_kdl = kdl.Rotation(rot[0,0], rot[0,1], rot[0,2],
rot[1,0], rot[1,1], rot[1,2],
rot[2,0], rot[2,1], rot[2,2])
frame_kdl = kdl.Frame(rot_kdl, pos_kdl)
if min_joints is None:
min_joints = self.joint_safety_lower
if max_joints is None:
max_joints = self.joint_safety_upper
mins_kdl = joint_list_to_kdl(min_joints)
maxs_kdl = joint_list_to_kdl(max_joints)
ik_p_kdl = kdl.ChainIkSolverPos_NR_JL(self.chain, mins_kdl, maxs_kdl,
self._fk_kdl, self._ik_v_kdl)
if q_guess == None:
# use the midpoint of the joint limits as the guess
lower_lim = np.where(np.isfinite(min_joints), min_joints, 0.)
upper_lim = np.where(np.isfinite(max_joints), max_joints, 0.)
q_guess = (lower_lim + upper_lim) / 2.0
q_guess = np.where(np.isnan(q_guess), [0.]*len(q_guess), q_guess)
q_kdl = kdl.JntArray(self.num_joints)
q_guess_kdl = joint_list_to_kdl(q_guess)
if ik_p_kdl.CartToJnt(q_guess_kdl, frame_kdl, q_kdl) >= 0:
return np.array(joint_kdl_to_list(q_kdl))
else:
return None
def command_absolute_cb(self, msg):
if self.arm is None:
rospy.logwarn(
"[cartesian_manager] Cartesian controller not enabled.")
return
self.cart_ctrl.stop_moving(wait=True)
if msg.header.frame_id == "":
msg.header.frame_id = "torso_lift_link"
if self.kin is None or msg.header.frame_id not in self.kin.get_link_names(
):
self.kin = create_joint_kin("torso_lift_link", msg.header.frame_id)
torso_pos_ep, torso_rot_ep = self.arm.get_ep()
torso_B_ref = self.kin.forward(base_link="torso_lift_link",
end_link=msg.header.frame_id)
ref_B_goal = PoseConv.to_homo_mat(msg)
torso_B_goal = torso_B_ref * ref_B_goal
change_pos, change_rot = PoseConv.to_pos_rot(torso_B_goal)
change_pos_xyz = change_pos.T.A[0]
rospy.loginfo("[cartesian_manager] Command absolute movement." +
str((change_pos_xyz, change_rot)))
self.cart_ctrl.execute_cart_move((change_pos_xyz, change_rot),
((1, 1, 1), 1),
velocity=self.velocity,
blocking=True)
def inverse(self, pose, q_guess=None, min_joints=None, max_joints=None):
pos, rot = PoseConv.to_pos_rot(pose)
pos_kdl = kdl.Vector(pos[0, 0], pos[1, 0], pos[2, 0])
rot_kdl = kdl.Rotation(rot[0, 0], rot[0, 1], rot[0, 2], rot[1, 0],
rot[1, 1], rot[1, 2], rot[2, 0], rot[2, 1],
rot[2, 2])
frame_kdl = kdl.Frame(rot_kdl, pos_kdl)
if min_joints is None:
min_joints = self.joint_safety_lower
if max_joints is None:
max_joints = self.joint_safety_upper
mins_kdl = joint_list_to_kdl(min_joints)
maxs_kdl = joint_list_to_kdl(max_joints)
ik_p_kdl = kdl.ChainIkSolverPos_NR_JL(self.chain, mins_kdl, maxs_kdl,
self._fk_kdl, self._ik_v_kdl)
if q_guess == None:
# use the midpoint of the joint limits as the guess
lower_lim = np.where(np.isfinite(min_joints), min_joints, 0.)
upper_lim = np.where(np.isfinite(max_joints), max_joints, 0.)
q_guess = (lower_lim + upper_lim) / 2.0
q_guess = np.where(np.isnan(q_guess), [0.] * len(q_guess), q_guess)
q_kdl = kdl.JntArray(self.num_joints)
q_guess_kdl = joint_list_to_kdl(q_guess)
if ik_p_kdl.CartToJnt(q_guess_kdl, frame_kdl, q_kdl) >= 0:
return np.array(joint_kdl_to_list(q_kdl))
else:
return None
def robot_ellipsoidal_pose(self, lat, lon, height, orient_quat, kinect_frame_mat=None):
if kinect_frame_mat is None:
kinect_frame_mat = self.get_ell_frame()
pos, quat = self.ell_space.ellipsoidal_to_pose(lat, lon, height)
quat_rotated = trans.quaternion_multiply(quat, orient_quat)
ell_pose_mat = PoseConv.to_homo_mat(pos, quat_rotated)
return PoseConv.to_pos_rot(kinect_frame_mat * ell_pose_mat)
def inverse_biased(self,
pose,
q_init,
q_bias,
q_bias_weights,
rot_weight=1.,
bias_vel=0.01,
num_iter=100):
# This code is potentially volitile
q_out = np.mat(self.inverse_search(pose)).T
for i in range(num_iter):
pos_fk, rot_fk = PoseConv.to_pos_rot(self.forward(q_out))
delta_twist = np.mat(np.zeros((6, 1)))
pos_delta = pos - pos_fk
delta_twist[:3, 0] = pos_delta
rot_delta = np.mat(np.eye(4))
rot_delta[:3, :3] = rot * rot_fk.T
rot_delta_angles = np.mat(trans.euler_from_matrix(rot_delta)).T
delta_twist[3:6, 0] = rot_delta_angles
J = self.jacobian(q_out)
J[3:6, :] *= np.sqrt(rot_weight)
delta_twist[3:6, 0] *= np.sqrt(rot_weight)
J_tinv = np.linalg.inv(J.T * J + np.diag(q_bias_weights) *
np.eye(len(q_init))) * J.T
q_bias_diff = q_bias - q_out
q_bias_diff_normed = q_bias_diff * bias_vel / np.linalg.norm(
q_bias_diff)
delta_q = q_bias_diff_normed + J_tinv * (delta_twist -
J * q_bias_diff_normed)
q_out += delta_q
q_out = np.mat(self.clip_joints_safe(q_out.T.A[0])).T
return q_out
def get_head_pose(self, name, gripper_rot=0.):
lat, lon, height = head_poses[name][0]
roll, pitch, yaw = head_poses[name][1]
pos, rot = PoseConv.to_pos_rot(
self.ell_space.ellipsoidal_to_pose(lat, lon, height))
rot = rot * tf_trans.euler_matrix(yaw, pitch, roll + gripper_rot,
'rzyx')[:3, :3]
return pos, rot
def get_head_pose(self, ell_coords_rot, gripper_rot=0.):
lat, lon, height = ell_coords_rot[0]
roll, pitch, yaw = ell_coords_rot[1]
pos, rot = PoseConv.to_pos_rot(
self.ell_space.ellipsoidal_to_pose(lat, lon, height))
rot = rot * trans.euler_matrix(yaw, pitch, roll + gripper_rot,
'szyx')[:3, :3]
return pos, rot
def FK(self, q, link_number=None):
if link_number is not None:
end_link = self.get_link_names(fixed=False)[link_number]
else:
end_link = None
homo_mat = self.forward(q, end_link)
pos, rot = PoseConv.to_pos_rot(homo_mat)
return pos, rot
def FK(self, q, link_number=None):
if link_number is not None:
end_link = self.get_link_names(fixed=False)[link_number]
else:
end_link = None
homo_mat = self.forward(q, end_link)
pos, rot = PoseConv.to_pos_rot(homo_mat)
return pos, rot
def enable_controller_cb(req):
if req.enable:
_, frame_rot = PoseConv.to_pos_rot([0]*3,
[req.frame_rot.x, req.frame_rot.y, req.frame_rot.z])
if req.velocity == 0:
req.velocity = 0.03
success = self.enable_controller(req.end_link, req.ctrl_params, req.ctrl_name,
frame_rot, velocity=req.velocity)
else:
success = self.disable_controller()
return EnableCartControllerResponse(success)
def _ctrl_state_cb(self, ctrl_state):
self._save_ep(PoseConv.to_homo_mat(ctrl_state.x_desi_filtered))
with self.ctrl_state_lock:
self.ctrl_state_dict["frame"] = ctrl_state.header.frame_id
self.ctrl_state_dict["x_desi"] = PoseConv.to_pos_rot(ctrl_state.x_desi)
self.ctrl_state_dict["xd_desi"] = extract_twist(ctrl_state.xd_desi)
self.ctrl_state_dict["x_act"] = PoseConv.to_pos_rot(ctrl_state.x)
self.ctrl_state_dict["xd_act"] = extract_twist(ctrl_state.xd)
self.ctrl_state_dict["x_desi_filt"] = PoseConv.to_pos_rot(
ctrl_state.x_desi_filtered)
self.ctrl_state_dict["x_err"] = extract_twist(ctrl_state.x_err)
self.ctrl_state_dict["tau_pose"] = np.array(ctrl_state.tau_pose)
self.ctrl_state_dict["tau_posture"] = np.array(ctrl_state.tau_posture)
self.ctrl_state_dict["tau"] = np.array(ctrl_state.tau)
self.ctrl_state_dict["F"] = np.array([ctrl_state.F.force.x,
ctrl_state.F.force.y,
ctrl_state.F.force.z,
ctrl_state.F.torque.x,
ctrl_state.F.torque.y,
ctrl_state.F.torque.z])
def execute_cart_move(self, change_ep, abs_sel, velocity=0.001,
num_samps=None, blocking=True):
if not self.cmd_lock.acquire(False):
return False
cur_pos, cur_rot = PoseConv.to_pos_rot(self.arm.get_ep())
change_pos_ep, change_rot_ep = change_ep
abs_cart_ep_sel, is_abs_rot = abs_sel
pos_f = np.where(abs_cart_ep_sel, change_pos_ep,
np.array(cur_pos + cur_rot * np.mat(change_pos_ep).T).T[0])
if is_abs_rot:
rot_mat_f = change_rot_ep
else:
rpy = change_rot_ep
_, cur_rot = PoseConv.to_pos_rot(self.arm.get_ep())
rot_mat = np.mat(trans.euler_matrix(*rpy))[:3,:3]
rot_mat_f = cur_rot * rot_mat
traj = self._create_cart_trajectory(pos_f, rot_mat_f, velocity, num_samps)
retval = self._run_traj(traj, blocking=blocking)
self.cmd_lock.release()
return retval
def create_tool_arrow():
arrows = MarkerArray()
color = ColorRGBA(0., 0., 1., 1.)
for i, param in enumerate(params):
ell_pos, ell_rot = params[param]
_, ell_rot_mat = PoseConv.to_pos_rot([0]*3, ell_rot)
cart_pose = PoseConv.to_homo_mat(ell_space.ellipsoidal_to_pose(*ell_pos))
cart_pose[:3,:3] = cart_pose[:3,:3] * ell_rot_mat
arrow = create_arrow_marker(cart_pose, i, color)
arrow.header.stamp = rospy.Time.now()
arrows.markers.append(arrow)
return arrows
def robot_ellipsoidal_pose(self,
lat,
lon,
height,
orient_quat,
kinect_frame_mat=None):
if kinect_frame_mat is None:
kinect_frame_mat = self.get_ell_frame()
pos, quat = self.ell_space.ellipsoidal_to_pose(lat, lon, height)
quat_rotated = trans.quaternion_multiply(quat, orient_quat)
ell_pose_mat = PoseConv.to_homo_mat(pos, quat_rotated)
return PoseConv.to_pos_rot(kinect_frame_mat * ell_pose_mat)
def create_ellipsoidal_path(self, start_ell_pos, start_ell_quat,
end_ell_pos, end_ell_quat,
velocity=0.001, min_jerk=True):
print "Start rot (%s):\r\n%s" %(type(start_ell_quat),start_ell_quat)
print "End rot (%s):\r\n%s" %(type(end_ell_quat),end_ell_quat)
_, start_ell_rot = PoseConv.to_pos_rot((start_ell_pos,start_ell_quat))
_, end_ell_rot = PoseConv.to_pos_rot((end_ell_pos,end_ell_quat))
rpy = trans.euler_from_matrix(start_ell_rot.T * end_ell_rot) # get roll, pitch, yaw of angle diff
end_ell_pos[1] = np.mod(end_ell_pos[1], 2 * np.pi) # wrap longitude value
ell_init = np.mat(start_ell_pos).T
ell_final = np.mat(end_ell_pos).T
# find the closest longitude angle to interpolate to
if np.fabs(2 * np.pi + ell_final[1,0] - ell_init[1,0]) < np.pi:
ell_final[1,0] += 2 * np.pi
elif np.fabs(-2 * np.pi + ell_final[1,0] - ell_init[1,0]) < np.pi:
ell_final[1,0] -= 2 * np.pi
if np.any(np.isnan(ell_init)) or np.any(np.isnan(ell_final)):
rospy.logerr("[ellipsoid_space] Nan values in ellipsoid. " +
"ell_init: %f, %f, %f; " % (ell_init[0,0], ell_init[1,0], ell_init[2,0]) +
"ell_final: %f, %f, %f; " % (ell_final[0,0], ell_final[1,0], ell_final[2,0]))
return None
num_samps = np.max([2, int(np.linalg.norm(ell_final - ell_init) / velocity),
int(np.linalg.norm(rpy) / velocity)])
if min_jerk:
t_vals = min_jerk_traj(num_samps)
else:
t_vals = np.linspace(0,1,num_samps)
# smoothly interpolate from init to final
ell_lat_traj = np.interp(t_vals, (0,1),(start_ell_pos[0], end_ell_pos[0]))
ell_lon_traj = np.interp(t_vals, (0,1),(start_ell_pos[1], end_ell_pos[1]))
ell_height_traj = np.interp(t_vals, (0,1),(start_ell_pos[2], end_ell_pos[2]))
ell_pos_traj = np.vstack((ell_lat_traj, ell_lon_traj, ell_height_traj))
ell_quat_traj = [trans.quaternion_slerp(start_ell_quat, end_ell_quat, t) for t in t_vals]
return [(ell_pos_traj[:,i], ell_quat_traj[i]) for i in xrange(num_samps)]
def _create_cart_trajectory(self, pos_f, rot_mat_f, velocity=0.001, num_samps=None):
cur_pos, cur_rot = PoseConv.to_pos_rot(self.arm.get_ep())
rpy = trans.euler_from_matrix(cur_rot.T * rot_mat_f) # get roll, pitch, yaw of angle diff
if num_samps is None:
num_samps = np.max([2, int(np.linalg.norm(pos_f - cur_pos) / velocity),
int(np.linalg.norm(rpy) / velocity)])
traj = self.arm.interpolate_ep([cur_pos, cur_rot],
[np.mat(pos_f).T, rot_mat_f],
min_jerk_traj(num_samps))
return traj
def command_move_cb(self, msg):
if self.arm is None:
rospy.logwarn("[cartesian_manager] Cartesian controller not enabled.")
return
self.cart_ctrl.stop_moving(wait=True)
if msg.header.frame_id == "":
msg.header.frame_id = "torso_lift_link"
if self.kin is None or msg.header.frame_id not in self.kin.get_link_names():
self.kin = create_joint_kin("torso_lift_link", msg.header.frame_id)
torso_pos_ep, torso_rot_ep = PoseConv.to_pos_rot(self.arm.get_ep())
torso_B_ref = self.kin.forward(base_link="torso_lift_link",
end_link=msg.header.frame_id)
_, torso_rot_ref = PoseConv.to_pos_rot(torso_B_ref)
torso_rot_ref *= self.frame_rot
ref_pos_off, ref_rot_off = PoseConv.to_pos_rot(msg)
change_pos = torso_rot_ep.T * torso_rot_ref * ref_pos_off
change_pos_xyz = change_pos.T.A[0]
ep_rot_ref = torso_rot_ep.T * torso_rot_ref
change_rot = ep_rot_ref * ref_rot_off * ep_rot_ref.T
_, change_rot_rpy = PoseConv.to_pos_euler(np.mat([0]*3).T, change_rot)
rospy.loginfo("[cartesian_manager] Command relative movement." +
str((change_pos_xyz, change_rot_rpy)))
self.cart_ctrl.execute_cart_move((change_pos_xyz, change_rot_rpy), ((0, 0, 0), 0),
velocity=self.velocity, blocking=True)
def enable_controller_cb(req):
if req.enable:
_, frame_rot = PoseConv.to_pos_rot(
[0] * 3,
[req.frame_rot.x, req.frame_rot.y, req.frame_rot.z])
if req.velocity == 0:
req.velocity = 0.03
success = self.enable_controller(req.end_link,
req.ctrl_params,
req.ctrl_name,
frame_rot,
velocity=req.velocity)
else:
success = self.disable_controller()
return EnableCartControllerResponse(success)
def _create_ell_trajectory(self, ell_f, rot_mat_f, orient_quat=[0., 0., 0., 1.], velocity=0.001):
_, cur_rot = PoseConv.to_pos_rot(self.arm.get_ep())
rpy = trans.euler_from_matrix(cur_rot.T * rot_mat_f) # get roll, pitch, yaw of angle diff
ell_f[1] = np.mod(ell_f[1], 2 * np.pi) # wrap longitude value
# get the current ellipsoidal location of the end effector
ell_init = np.mat(self.get_ell_ep()).T
ell_final = np.mat(ell_f).T
# find the closest longitude angle to interpolate to
if np.fabs(2 * np.pi + ell_final[1,0] - ell_init[1,0]) < np.pi:
ell_final[1,0] += 2 * np.pi
elif np.fabs(-2 * np.pi + ell_final[1,0] - ell_init[1,0]) < np.pi:
ell_final[1,0] -= 2 * np.pi
if np.any(np.isnan(ell_init)) or np.any(np.isnan(ell_final)):
rospy.logerr("[ellipsoid_controller] Nan values in ellipsoid EPs. " +
"ell_init: %f, %f, %f; " % (ell_init[0,0], ell_init[1,0], ell_init[2,0]) +
"ell_final: %f, %f, %f; " % (ell_final[0,0], ell_final[1,0], ell_final[2,0]))
return None
num_samps = np.max([2, int(np.linalg.norm(ell_final - ell_init) / velocity),
int(np.linalg.norm(rpy) / velocity)])
t_vals = min_jerk_traj(num_samps) # makes movement smooth
# smoothly interpolate from init to final
ell_traj = np.array(ell_init) + np.array(np.tile(ell_final - ell_init,
(1, num_samps))) * np.array(t_vals)
ell_frame_mat = self.ell_server.get_ell_frame()
ell_pose_traj = [self.ell_server.robot_ellipsoidal_pose(
ell_traj[0,i], ell_traj[1,i], ell_traj[2,i], orient_quat, ell_frame_mat)
for i in range(ell_traj.shape[1])]
# modify rotation of trajectory
_, ell_init_rot = self.ell_server.robot_ellipsoidal_pose(
ell_init[0,0], ell_init[1,0], ell_init[2,0], orient_quat, ell_frame_mat)
rot_adjust_traj = self.arm.interpolate_ep([np.mat([0]*3).T, cur_rot],
[np.mat([0]*3).T, rot_mat_f],
min_jerk_traj(num_samps))
ell_pose_traj = [(ell_pose_traj[i][0],
ell_pose_traj[i][1] * ell_init_rot.T * rot_adjust_traj[i][1])
for i in range(num_samps)]
return ell_pose_traj
def _run_traj(self, traj, blocking=True):
self.start_pub.publish(
PoseConv.to_pose_stamped_msg("/torso_lift_link", traj[0]))
self.end_pub.publish(
PoseConv.to_pose_stamped_msg("/torso_lift_link", traj[-1]))
# make sure traj beginning is close to current end effector position
init_pos_tolerance = rospy.get_param("~init_pos_tolerance", 0.05)
init_rot_tolerance = rospy.get_param("~init_rot_tolerance", np.pi/12)
ee_pos, ee_rot = PoseConv.to_pos_rot(self.arm.get_end_effector_pose())
_, rot_diff = PoseConv.to_pos_euler((ee_pos, ee_rot * traj[0][1].T))
pos_diff = np.linalg.norm(ee_pos - traj[0][0])
if pos_diff > init_pos_tolerance:
rospy.logwarn("[controller_base] End effector too far from current position. " +
"Pos diff: %.3f, Tolerance: %.3f" % (pos_diff, init_pos_tolerance))
return False
if np.linalg.norm(rot_diff) > init_rot_tolerance:
rospy.logwarn("[controller_base] End effector too far from current rotation. " +
"Rot diff: %.3f, Tolerance: %.3f" % (np.linalg.norm(rot_diff),
init_rot_tolerance))
return False
return self.execute_cart_traj(self.arm, traj, self.time_step, blocking=blocking)
def command_absolute_cb(self, msg):
if self.arm is None:
rospy.logwarn("[cartesian_manager] Cartesian controller not enabled.")
return
self.cart_ctrl.stop_moving(wait=True)
if msg.header.frame_id == "":
msg.header.frame_id = "torso_lift_link"
if self.kin is None or msg.header.frame_id not in self.kin.get_link_names():
self.kin = create_joint_kin("torso_lift_link", msg.header.frame_id)
torso_pos_ep, torso_rot_ep = self.arm.get_ep()
torso_B_ref = self.kin.forward(base_link="torso_lift_link",
end_link=msg.header.frame_id)
ref_B_goal = PoseConv.to_homo_mat(msg)
torso_B_goal = torso_B_ref * ref_B_goal
change_pos, change_rot = PoseConv.to_pos_rot(torso_B_goal)
change_pos_xyz = change_pos.T.A[0]
rospy.loginfo("[cartesian_manager] Command absolute movement." +
str((change_pos_xyz, change_rot)))
self.cart_ctrl.execute_cart_move((change_pos_xyz, change_rot), ((1, 1, 1), 1),
velocity=self.velocity, blocking=True)
def inverse_biased(self, pose, q_init, q_bias, q_bias_weights, rot_weight=1.0, bias_vel=0.01, num_iter=100):
# This code is potentially volitile
q_out = np.mat(self.inverse_search(pose)).T
for i in range(num_iter):
pos_fk, rot_fk = PoseConv.to_pos_rot(self.forward(q_out))
delta_twist = np.mat(np.zeros((6, 1)))
pos_delta = pos - pos_fk
delta_twist[:3, 0] = pos_delta
rot_delta = np.mat(np.eye(4))
rot_delta[:3, :3] = rot * rot_fk.T
rot_delta_angles = np.mat(trans.euler_from_matrix(rot_delta)).T
delta_twist[3:6, 0] = rot_delta_angles
J = self.jacobian(q_out)
J[3:6, :] *= np.sqrt(rot_weight)
delta_twist[3:6, 0] *= np.sqrt(rot_weight)
J_tinv = np.linalg.inv(J.T * J + np.diag(q_bias_weights) * np.eye(len(q_init))) * J.T
q_bias_diff = q_bias - q_out
q_bias_diff_normed = q_bias_diff * bias_vel / np.linalg.norm(q_bias_diff)
delta_q = q_bias_diff_normed + J_tinv * (delta_twist - J * q_bias_diff_normed)
q_out += delta_q
q_out = np.mat(self.clip_joints_safe(q_out.T.A[0])).T
return q_out
def _parse_ell_move(self, change_ep, abs_sel, orient_quat):
#change_ell_ep = [lat, lon, height], change_rot_ep = Rotation in matrix or quat form (3x3 rotation mat, 1x4 quat)
#Provides new position and orientation goals (in relative or abs manner, determined next).
change_ell_ep, change_rot_ep = change_ep
#abs_ell_ep_sel = 1x3 list of 0/1, is_abs_rot = 0/1 (Bool)
#abs_ell_ep_sel: selects lat, long, height absolute vs relative position command
#change_rot_ep: selects abs vs relative use of orientation.
abs_ell_ep_sel, is_abs_rot = abs_sel
ell_f = np.where(abs_ell_ep_sel,
change_ell_ep,
np.array(self.get_ell_ep()) + np.array(change_ell_ep))
print "old", ell_f
if ell_f[0] > np.pi:
ell_f[0] = 2 * np.pi - ell_f[0]
ell_f[1] *= -1
if ell_f[0] < 0.:
ell_f[0] *= -1
ell_f[1] *= -1
ell_f[1] = np.mod(ell_f[1], 2 * np.pi)
ell_f = self._clip_ell_ep(ell_f)
print "new", ell_f
if is_abs_rot:
#Apply rotation from change_rot_ep as final orientation of pose
#Expects rotation matrix in change_rot_ep
rot_change_mat = change_rot_ep
_, ell_final_rot = self.ell_server.robot_ellipsoidal_pose(ell_f[0], ell_f[1], ell_f[2],
orient_quat)
rot_mat_f = ell_final_rot * rot_change_mat
else:
#Apply rotation from change_rot_ep as change to current rotation
#Expects quaternion in change_rot_ep
quat = change_rot_ep
_, cur_rot = PoseConv.to_pos_rot(self.arm.get_ep())
rot_mat = np.mat(trans.quaternion_matrix(quat))[:3,:3]
rot_mat_f = cur_rot * rot_mat
return ell_f, rot_mat_f
def _parse_ell_move(self, change_ep, abs_sel, orient_quat):
#change_ell_ep = [lat, lon, height], change_rot_ep = Rotation in matrix or quat form (3x3 rotation mat, 1x4 quat)
#Provides new position and orientation goals (in relative or abs manner, determined next).
change_ell_ep, change_rot_ep = change_ep
#abs_ell_ep_sel = 1x3 list of 0/1, is_abs_rot = 0/1 (Bool)
#abs_ell_ep_sel: selects lat, long, height absolute vs relative position command
#change_rot_ep: selects abs vs relative use of orientation.
abs_ell_ep_sel, is_abs_rot = abs_sel
ell_f = np.where(abs_ell_ep_sel, change_ell_ep,
np.array(self.get_ell_ep()) + np.array(change_ell_ep))
print "old", ell_f
if ell_f[0] > np.pi:
ell_f[0] = 2 * np.pi - ell_f[0]
ell_f[1] *= -1
if ell_f[0] < 0.:
ell_f[0] *= -1
ell_f[1] *= -1
ell_f[1] = np.mod(ell_f[1], 2 * np.pi)
ell_f = self._clip_ell_ep(ell_f)
print "new", ell_f
if is_abs_rot:
#Apply rotation from change_rot_ep as final orientation of pose
#Expects rotation matrix in change_rot_ep
rot_change_mat = change_rot_ep
_, ell_final_rot = self.ell_server.robot_ellipsoidal_pose(
ell_f[0], ell_f[1], ell_f[2], orient_quat)
rot_mat_f = ell_final_rot * rot_change_mat
else:
#Apply rotation from change_rot_ep as change to current rotation
#Expects quaternion in change_rot_ep
quat = change_rot_ep
_, cur_rot = PoseConv.to_pos_rot(self.arm.get_ep())
rot_mat = np.mat(trans.quaternion_matrix(quat))[:3, :3]
rot_mat_f = cur_rot * rot_mat
return ell_f, rot_mat_f
roslib.load_manifest("hrl_face_adls")
import rospy
from std_msgs.msg import Bool, Float32
from geometry_msgs.msg import TwistStamped, PoseStamped
from hrl_ellipsoidal_control.controller_base import CartesianStepController
from pykdl_utils.joint_kinematics import create_joint_kin
from hrl_geom.pose_converter import PoseConv
from hrl_pr2_arms.pr2_arm_jt_task import create_ep_arm, PR2ArmJTransposeTask
from hrl_pr2_arms.pr2_controller_switcher import ControllerSwitcher
from face_adls_manager import async_call
from hrl_face_adls.srv import EnableCartController, EnableCartControllerResponse
VELOCITY = 0.03
_, FLIP_PERSPECTIVE_ROT = PoseConv.to_pos_rot([0] * 3, [0, 0, np.pi])
class CartesianControllerManager(object):
def __init__(self, arm_char):
self.arm_char = arm_char
self.arm = None
self.kin = None
self.cart_ctrl = CartesianStepController()
self.ctrl_switcher = ControllerSwitcher()
self.command_move_sub = rospy.Subscriber(
"/face_adls/%s_cart_move" % arm_char, TwistStamped,
async_call(self.command_move_cb))
self.command_absolute_sub = rospy.Subscriber(
"/face_adls/%s_cart_absolute" % arm_char, PoseStamped,
async_call(self.command_absolute_cb))
#!/usr/bin/python
import numpy as np
import roslib
roslib.load_manifest("hrl_geom")
import rospy
from hrl_geom.pose_converter import PoseConv
if __name__ == "__main__":
rospy.init_node("test_poseconv")
homo_mat = PoseConv.to_homo_mat([0., 1., 2.], [0., 0., np.pi/2])
pose_msg = PoseConv.to_pose_msg(homo_mat)
pos, quat = PoseConv.to_pos_quat(pose_msg)
pose_stamped_msg = PoseConv.to_pose_stamped_msg("/base_link", pos, quat)
pose_stamped_msg2 = PoseConv.to_pose_stamped_msg("/base_link", [pos, quat])
tf_stamped = PoseConv.to_tf_stamped_msg("/new_link", pose_stamped_msg)
p, R = PoseConv.to_pos_rot("/new_link", tf_stamped)
for name in ["homo_mat", "pose_msg", "pos", "quat", "pose_stamped_msg",
"pose_stamped_msg2", "tf_stamped", "p", "R"]:
print "%s: \n" % name, locals()[name]
def get_head_pose(self, ell_coords_rot, gripper_rot=0.):
lat, lon, height = ell_coords_rot[0]
roll, pitch, yaw = ell_coords_rot[1]
pos, rot = PoseConv.to_pos_rot(self.ell_space.ellipsoidal_to_pose(lat, lon, height))
rot = rot * trans.euler_matrix(yaw, pitch, roll + gripper_rot, 'szyx')[:3, :3]
return pos, rot
def link_trans():
pub = rospy.Publisher('coodinator_msgs_to_gui', String, queue_size=1000)
rospy.init_node('link_trans_gp8', anonymous=True)
rate = rospy.Rate(100)
rospy.sleep(1)
rospack = rospkg.RosPack()
rospack.list()
# Read in robot description from parameter srv
# and build kinematic model
robot = URDF.from_parameter_server()
base_link = robot.get_root()
end_link = robot.link_map.keys()[len(robot.link_map) - 1]
print "Root link: %s; End link: %s" % (base_link, end_link)
kdl_kin = KDLKinematics(robot, base_link, end_link)
print "Robot ik model built...\n"
print len(kdl_kin.get_link_names())
#
link_files = []
for i in range(8):
link_files.append(
rospack.get_path('motoman_app') + "/joint_data/link_" + str(i) +
"_data.csv")
print link_files[i]
joint_filename = rospack.get_path(
'motoman_app') + "/joint_data/joint_value_gp8_data.csv"
print "joint_filename"
home_pose_file = rospack.get_path(
'motoman_app') + "/joint_data/home_pose.csv"
in_value = np.genfromtxt(joint_filename, delimiter=',') # n by 12 array
print "Data stored"
(row, col) = in_value.shape
print "Data length: ", row
#
q_init = np.array([0.0, 0.0, 0.0, 0.0, 0.0, 0.0])
tf2 = np.array([])
for i in range(8):
pose = kdl_kin.forward(q_init,
kdl_kin.get_link_names()[i],
kdl_kin.get_link_names()[0])
pos, rot = PoseConv.to_pos_rot(pose)
tf_temp = np.array([
pos[0, 0], pos[1, 0], pos[2, 0], rot[0, 0], rot[1, 0], rot[2, 0],
rot[0, 1], rot[1, 1], rot[2, 1], rot[0, 2], rot[1, 2], rot[2, 2]
])
if len(tf2) == 0:
tf2 = tf_temp
else:
tf2 = np.vstack([tf2, tf_temp])
print "home_pose done"
np.savetxt(home_pose_file, tf2, fmt='%f', delimiter=",")
for i in range(8):
tf = np.array([])
for j in range(row):
q = in_value[j]
# pose=kdl_kin._do_kdl_fk(q, i)
pose = kdl_kin.forward(q,
kdl_kin.get_link_names()[i],
kdl_kin.get_link_names()[0])
pos, rot = PoseConv.to_pos_rot(pose)
tf_temp = np.array([
pos[0, 0], pos[1, 0], pos[2, 0], rot[0, 0], rot[1, 0],
rot[2, 0], rot[0, 1], rot[1, 1], rot[2, 1], rot[0, 2],
rot[1, 2], rot[2, 2]
])
if j == 0:
tf = tf_temp
else:
tf = np.vstack([tf, tf_temp])
print "link", i, " done"
(length, width) = tf.shape
np.savetxt(link_files[i], tf, fmt='%f', delimiter=",")
print "link", i, "saved"
result_str = "link_saved"
pub.publish(result_str)
def pose_to_ellipsoidal(self, pose):
pose_pos, pose_rot = PoseConv.to_pos_rot(pose)
lat, lon, height = self.pos_to_ellipsoidal(pose_pos[0,0], pose_pos[1,0], pose_pos[2,0])
_, ell_rot = PoseConv.to_pos_rot(self.normal_to_ellipse(lat, lon, height))
_, quat_rot = PoseConv.to_pos_quat(np.mat([0]*3).T, ell_rot.T * pose_rot)
return [lat, lon, height], quat_rot
def _create_ell_trajectory(self,
ell_f,
rot_mat_f,
orient_quat=[0., 0., 0., 1.],
velocity=0.001):
_, cur_rot = PoseConv.to_pos_rot(self.arm.get_ep())
rpy = trans.euler_from_matrix(
cur_rot.T * rot_mat_f) # get roll, pitch, yaw of angle diff
ell_f[1] = np.mod(ell_f[1], 2 * np.pi) # wrap longitude value
# get the current ellipsoidal location of the end effector
ell_init = np.mat(self.get_ell_ep()).T
ell_final = np.mat(ell_f).T
# find the closest longitude angle to interpolate to
if np.fabs(2 * np.pi + ell_final[1, 0] - ell_init[1, 0]) < np.pi:
ell_final[1, 0] += 2 * np.pi
elif np.fabs(-2 * np.pi + ell_final[1, 0] - ell_init[1, 0]) < np.pi:
ell_final[1, 0] -= 2 * np.pi
if np.any(np.isnan(ell_init)) or np.any(np.isnan(ell_final)):
rospy.logerr(
"[ellipsoid_controller] Nan values in ellipsoid EPs. " +
"ell_init: %f, %f, %f; " %
(ell_init[0, 0], ell_init[1, 0], ell_init[2, 0]) +
"ell_final: %f, %f, %f; " %
(ell_final[0, 0], ell_final[1, 0], ell_final[2, 0]))
return None
num_samps = np.max([
2,
int(np.linalg.norm(ell_final - ell_init) / velocity),
int(np.linalg.norm(rpy) / velocity)
])
t_vals = min_jerk_traj(num_samps) # makes movement smooth
# smoothly interpolate from init to final
ell_traj = np.array(ell_init) + np.array(
np.tile(ell_final - ell_init, (1, num_samps))) * np.array(t_vals)
ell_frame_mat = self.ell_server.get_ell_frame()
ell_pose_traj = [
self.ell_server.robot_ellipsoidal_pose(ell_traj[0, i], ell_traj[1,
i],
ell_traj[2, i], orient_quat,
ell_frame_mat)
for i in range(ell_traj.shape[1])
]
# modify rotation of trajectory
_, ell_init_rot = self.ell_server.robot_ellipsoidal_pose(
ell_init[0, 0], ell_init[1, 0], ell_init[2, 0], orient_quat,
ell_frame_mat)
rot_adjust_traj = self.arm.interpolate_ep(
[np.mat([0] * 3).T, cur_rot], [np.mat([0] * 3).T, rot_mat_f],
min_jerk_traj(num_samps))
ell_pose_traj = [
(ell_pose_traj[i][0],
ell_pose_traj[i][1] * ell_init_rot.T * rot_adjust_traj[i][1])
for i in range(num_samps)
]
return ell_pose_traj
roslib.load_manifest("hrl_face_adls")
import rospy
from std_msgs.msg import Bool, Float32
from geometry_msgs.msg import TwistStamped, PoseStamped
from hrl_ellipsoidal_control.controller_base import CartesianStepController
from pykdl_utils.joint_kinematics import create_joint_kin
from hrl_geom.pose_converter import PoseConv
from hrl_pr2_arms.pr2_arm_jt_task import create_ep_arm, PR2ArmJTransposeTask
from hrl_pr2_arms.pr2_controller_switcher import ControllerSwitcher
from face_adls_manager import async_call
from hrl_face_adls.srv import EnableCartController, EnableCartControllerResponse
VELOCITY = 0.03
_, FLIP_PERSPECTIVE_ROT = PoseConv.to_pos_rot([0]*3, [0, 0, np.pi])
class CartesianControllerManager(object):
def __init__(self, arm_char):
self.arm_char = arm_char
self.arm = None
self.kin = None
self.cart_ctrl = CartesianStepController()
self.ctrl_switcher = ControllerSwitcher()
self.command_move_sub = rospy.Subscriber("/face_adls/%s_cart_move" % arm_char, TwistStamped,
async_call(self.command_move_cb))
self.command_absolute_sub = rospy.Subscriber("/face_adls/%s_cart_absolute" % arm_char,
PoseStamped, async_call(self.command_absolute_cb))
self.adjust_posture_sub = rospy.Subscriber("/face_adls/%s_cart_posture" % arm_char,
Float32, self.adjust_posture_cb)
def enable_controller_cb(req):