def pub_head_registration(ud):
cheek_pose_base_link = self.tf_list.transformPose(
"/base_link", ud.cheek_pose)
# find the center of the ellipse given a cheek click
cheek_transformation = np.mat(
tf_trans.euler_matrix(2.6 * np.pi / 6, 0, 0, 'szyx'))
cheek_transformation[0:3, 3] = np.mat([-0.08, -0.04, 0]).T
cheek_pose = PoseConverter.to_homo_mat(cheek_pose_base_link)
#b_B_c[0:3,0:3] = np.eye(3)
norm_xy = cheek_pose[0:2, 2] / np.linalg.norm(cheek_pose[0:2, 2])
head_rot = np.arctan2(norm_xy[1], norm_xy[0])
cheek_pose[0:3,
0:3] = tf_trans.euler_matrix(0, 0, head_rot,
'sxyz')[0:3, 0:3]
self.cheek_pub.publish(
PoseConverter.to_pose_stamped_msg("/base_link", cheek_pose))
ell_center = cheek_pose * cheek_transformation
self.ell_center_pub.publish(
PoseConverter.to_pose_stamped_msg("/base_link", ell_center))
# create an ellipsoid msg and command it
ep = EllipsoidParams()
ep.e_frame.transform = PoseConverter.to_tf_msg(ell_center)
ep.height = 0.924
ep.E = 0.086
self.ep_pub.publish(ep)
return 'succeeded'
def motion_model(self, f0, f1, stamp, use_kalman=False):
if not use_kalman:
# simple `repetition` model
txn0, rxn0 = f0['pose'][L_POS], f0['pose'][A_POS]
txn1, rxn1 = f1['pose'][L_POS], f1['pose'][A_POS]
R0 = tx.euler_matrix(*rxn0)
R1 = tx.euler_matrix(*rxn1)
T0 = tx.compose_matrix(angles=rxn0, translate=txn0)
T1 = tx.compose_matrix(angles=rxn1, translate=txn1)
Tv = np.dot(T1, vm.inv(T0)) # Tv * T0 = T1
T2 = np.dot(Tv, T1)
txn = tx.translation_from_matrix(T2)
rxn = tx.euler_from_matrix(T2)
x = f1['pose'].copy()
P = f1['cov'].copy()
x[0:3] = txn
x[9:12] = rxn
return x, P
else:
# dt MUST NOT BE None
self.kf_.x = f0['pose']
self.kf_.P = f0['cov']
dt = (f1['stamp'] - f0['stamp'])
self.kf_.predict(dt)
txn, rxn = f1['pose'][L_POS], f1['pose'][A_POS]
z = np.concatenate([txn, rxn])
self.kf_.update(z)
dt = (stamp - f1['stamp'])
self.kf_.predict(dt)
return self.kf_.x.copy(), self.kf_.P.copy()
def __init__(self):
super(PisaHand, self).__init__()
self[
'mesh_file'] = "package://ec_grasp_planner/data/pisa_hand_simple.dae"
self['mesh_file_scale'] = 0.001
#self['surface_grasp']['object']['hand_transform'] = tra.concatenate_matrices(tra.translation_matrix([0.0, -0.05, 0.3]),tra.concatenate_matrices(tra.rotation_matrix(math.radians(90.), [0, 0, 1]),tra.rotation_matrix(math.radians(180.), [1, 0, 0])))
self['surface_grasp']['object'][
'hand_transform'] = tra.concatenate_matrices(
tra.translation_matrix([0.010, 0.007, 0.115]))
#self['surface_grasp']['object']['pregrasp_pose'] = tra.concatenate_matrices(tra.translation_matrix([0, 0, 0.5]),tra.euler_matrix(0, math.pi / 2, 0))
#self['surface_grasp']['object']['pregrasp_pose'] = tra.concatenate_matrices(tra.translation_matrix([0.4, 0.0, 0.0]),tra.euler_matrix(0, math.pi / 2, 0))
self['surface_grasp']['object']['hand_pose'] = tra.translation_matrix(
[0.010, 0.007, 0.115])
self['surface_grasp']['object']['downward_force'] = 7.
self['surface_grasp']['object'][
'pregrasp_transform'] = tra.concatenate_matrices(
tra.translation_matrix([0.3, -0.03, 0.05]),
tra.euler_matrix(-math.pi / 2, 0, -math.pi / 2),
tra.euler_matrix(0, 0, -0.5))
self['surface_grasp']['object'][
'post_grasp_transform'] = tra.concatenate_matrices(
tra.translation_matrix([0, 0, 0.0]),
tra.rotation_matrix(math.radians(-30.0), [0, 1, 0]))
self['surface_grasp']['object']['hand_closing_duration'] = 5
self['surface_grasp']['object']['hand_closing_synergy'] = 5
def fwd_kin(self, joint_values): # Expects a 7 x 1 values for joint_values
joint_values = np.insert(
joint_values, len(joint_values), 0.0,
axis=0) # added 0 at the end for the fixed joint
T = np.eye(4)
T_joint = np.zeros([self.numJoints + 1, 4,
4]) # +1 for the fixed joint at the end
# As per Craigs convention, T = Rotx * Trans x * Rotz * Trans z; Franka follows this
for i in range(self.numJoints + 1):
Tx = tf_tran.translation_matrix((self.a[i], 0, 0)) # x translation
Rx = tf_tran.euler_matrix(self.alpha[i], 0, 0,
axes='sxyz') # x rotation
aa = tf_tran.concatenate_matrices(Rx, Tx)
Tz = tf_tran.translation_matrix((0, 0, self.d[i])) # z translation
Rz = tf_tran.euler_matrix(0, 0, joint_values[i],
axes='sxyz') # z rotation
ab = tf_tran.concatenate_matrices(Rz, Tz)
ac = tf_tran.concatenate_matrices(aa, ab)
T = tf_tran.concatenate_matrices(T, ac)
T_joint[
i, :, :] = T # gives the transformation of each joint wrt base CS
return T, T_joint
def publish_transforms():
T_obj = euler_matrix(0.79, 0.0, 0.79).dot(translation_matrix((0, 1, 1)))
T_robot = euler_matrix(0, 0.0, 1.5).dot(translation_matrix((0, -1, 0)))
camera_offset = [0, 0.1, 0.1]
T_obj_camera = np.linalg.inv(T_robot.dot(
translation_matrix(camera_offset))).dot(T_obj)
R_camera = lookAt(translation_from_matrix(T_obj_camera))
T_camera = translation_matrix(camera_offset).dot(R_camera)
obj = geometry_msgs.msg.TransformStamped()
obj.transform.rotation = Quaternion(*quaternion_from_matrix(T_obj))
obj.transform.translation = Vector3(*translation_from_matrix(T_obj))
obj.header.stamp = rospy.Time.now()
obj.header.frame_id = "base_frame"
obj.child_frame_id = "object_frame"
br.sendTransform(obj)
robot = geometry_msgs.msg.TransformStamped()
robot.transform.rotation = Quaternion(*quaternion_from_matrix(T_robot))
robot.transform.translation = Vector3(*translation_from_matrix(T_robot))
robot.header.stamp = rospy.Time.now()
robot.header.frame_id = "base_frame"
robot.child_frame_id = "robot_frame"
br.sendTransform(robot)
camera = geometry_msgs.msg.TransformStamped()
camera.transform.rotation = Quaternion(*quaternion_from_matrix(T_camera))
camera.transform.translation = Vector3(*translation_from_matrix(T_camera))
camera.header.stamp = rospy.Time.now()
camera.header.frame_id = "robot_frame"
camera.child_frame_id = "camera_frame"
br.sendTransform(camera)
def sendTransform(out_pose):
"""发送TF(topic)
Args:
out_pose: ros输出格式数据
"""
pub.publish(out_pose.fuse_tf)
mat44_map_laser = tft.euler_matrix(0, 0, out_pose.fuse_tf.map_theta)
mat44_map_laser[0][3] = out_pose.fuse_tf.map_x
mat44_map_laser[1][3] = out_pose.fuse_tf.map_y
mat44_laser_map = la.inv(mat44_map_laser)
mat44_odom_laser = tft.euler_matrix(0, 0, out_pose.fuse_tf.odom_theta)
mat44_odom_laser[0][3] = out_pose.fuse_tf.odom_x
mat44_odom_laser[1][3] = out_pose.fuse_tf.odom_y
mat44_map_odom = la.inv(np.dot(mat44_odom_laser, mat44_laser_map))
q = tft.quaternion_from_matrix(mat44_map_odom)
br = tf2_ros.TransformBroadcaster()
t = TransformStamped()
t.header.stamp = rospy.Time.now()
t.header.frame_id = "map"
t.child_frame_id = "odom"
t.transform.translation.x = mat44_map_odom[0][3]
t.transform.translation.y = mat44_map_odom[1][3]
t.transform.translation.z = 0.0
t.transform.rotation.x = q[0]
t.transform.rotation.y = q[1]
t.transform.rotation.z = q[2]
t.transform.rotation.w = q[3]
br.sendTransform(t)
def get_size_and_build_walls(self):
# Get size of the ogrid ==============================================
# Get some useful vectors
between_vector = self.left_f - self.right_f
mid_point = self.right_f + between_vector / 2
target_vector = self.target - mid_point
self.mid_point = mid_point
# For rotations of the `between_vector` and the enu x-axis
b_theta = np.arctan2(between_vector[1], between_vector[0])
b_rot_mat = trns.euler_matrix(0, 0, b_theta)[:3, :3]
# Make the endpoints
rot_buffer = b_rot_mat.dot([20, 0, 0])
endpoint_left_f = self.left_f + rot_buffer
endpoint_right_f = self.right_f - rot_buffer
# Now lets build some wall points ======================================
if self.left_b is None:
# For rotations of the `target_vector` and the enu x-axis
t_theta = np.arctan2(target_vector[1], target_vector[0])
t_rot_mat = trns.euler_matrix(0, 0, t_theta)[:3, :3]
rot_channel = t_rot_mat.dot([self.channel_length, 0, 0])
self.left_b = self.left_f + rot_channel
self.right_b = self.right_f + rot_channel
# Now draw contours from the boat to the start gate ====================
# Get vector from boat to mid_point
mid_point_vector = self.boat_pos - self.mid_point
b_theta = np.arctan2(mid_point_vector[1], mid_point_vector[0])
b_rot_mat = trns.euler_matrix(0, 0, b_theta)[:3, :3]
rot_buffer = b_rot_mat.dot(
[np.linalg.norm(mid_point_vector) * 1.5, 0, 0])
left_cone_point = self.left_f + rot_buffer
right_cone_point = self.right_f + rot_buffer
# Define bounds for the grid
self.x_lower = min(left_cone_point[0], right_cone_point[0],
endpoint_left_f[0], endpoint_right_f[0],
self.target[0]) - self.edge_buffer
self.x_upper = max(left_cone_point[0], right_cone_point[0],
endpoint_left_f[0], endpoint_right_f[0],
self.target[0]) + self.edge_buffer
self.y_lower = min(left_cone_point[1], right_cone_point[1],
endpoint_left_f[1], endpoint_right_f[1],
self.target[1]) - self.edge_buffer
self.y_upper = max(left_cone_point[1], right_cone_point[1],
endpoint_left_f[1], endpoint_right_f[1],
self.target[1]) + self.edge_buffer
self.walls = [
self.left_b, self.left_f, left_cone_point, right_cone_point,
self.right_f, self.right_b
]
print self.walls
def matrix(self):
if self.mode == 0:
R = transformations.euler_matrix(self.phi, self.th, 0)
else:
R = transformations.euler_matrix(self.phi, 0, self.th)
T = transformations.translation_matrix([self.x, self.y, self.z])
M = numpy.dot(T, R)
return M
def __init__(self):
self.subscriber = rospy.Subscriber("input_odom",
Odometry,
self.callback,
queue_size=1)
self.publisher = rospy.Publisher("output_odom", Odometry, queue_size=1)
self.T_NED_V = TR.euler_matrix(pi, 0, 0, 'rxyz')
self.T_XAV_UAV = TR.euler_matrix(pi, 0, 0, 'rxyz')
self.T_XAV_V = TR.identity_matrix() # the output transform
def __init__(self, **kwargs):
super(RBOHand2Kuka, self).__init__()
#old parameters below - must be updated to new convention!
self['surface_grasp']['initial_goal'] = np.array([
-0.05864322834179703, 0.4118988657714642, -0.05864200146127985,
-1.6887810963180838, -0.11728653060066829, -0.8237944986945402, 0
])
self['surface_grasp']['pregrasp_pose'] = tra.translation_matrix(
[0, 0, -0.2])
self['surface_grasp']['hand_pose'] = tra.concatenate_matrices(
tra.translation_matrix([0, 0, 0]),
tra.rotation_matrix(math.radians(0.), [0, 0, 1]))
self['surface_grasp']['downward_force'] = 7.
self['surface_grasp']['valve_pattern'] = (np.array([[0.,
4.1], [0., 0.1],
[0., 5.], [0., 5.],
[0., 2.],
[0., 3.5]]),
np.array([[1, 0]] * 6))
self['wall_grasp']['pregrasp_pose'] = tra.translation_matrix(
[0.05, 0, -0.2])
self['wall_grasp']['table_force'] = 7.0
self['wall_grasp']['sliding_speed'] = 0.1
self['wall_grasp']['up_speed'] = 0.1
self['wall_grasp']['down_speed'] = 0.1
self['wall_grasp']['wall_force'] = 10.0
self['wall_grasp']['angle_of_attack'] = 1.0 #radians
self['wall_grasp']['object_lift_time'] = 4.5
self['edge_grasp']['initial_goal'] = np.array([
-0.05864322834179703, 0.4118988657714642, -0.05864200146127985,
-1.6887810963180838, -0.11728653060066829, -0.8237944986945402, 0
])
self['edge_grasp']['pregrasp_pose'] = tra.translation_matrix(
[0.2, 0, 0.4])
self['edge_grasp']['hand_object_pose'] = tra.concatenate_matrices(
tra.translation_matrix([0, 0, 0.05]),
tra.rotation_matrix(math.radians(10.), [1, 0, 0]),
tra.euler_matrix(0, 0, -math.pi / 2.))
self['edge_grasp']['grasp_pose'] = tra.concatenate_matrices(
tra.translation_matrix([0, -0.05, 0]),
tra.rotation_matrix(math.radians(10.), [1, 0, 0]),
tra.euler_matrix(0, 0, -math.pi / 2.))
self['edge_grasp']['postgrasp_pose'] = tra.translation_matrix(
[0, 0, -0.1])
self['edge_grasp']['downward_force'] = 4.0
self['edge_grasp']['sliding_speed'] = 0.04
self['edge_grasp']['valve_pattern'] = (np.array([[0, 0], [0,
0], [1, 0],
[1, 0], [1, 0],
[1, 0]]),
np.array([[0, 3.0]] * 6))
def set_tf_apriltag_camera(self, R, t):
# detected tf: camera-apriltag, camera frame has wrong orientation
tf_detected = np.zeros((4, 4), dtype=np.float64)
tf_detected[0:3, 0:3] = R
tf_detected[0:3, 3] = t[:, 0]
tf_detected[3, 3] = 1
# correct camera frame and apriltag orientation to conform with specifications
camera_rot = transformations.euler_matrix(np.pi/2, -np.pi/2, 0, axes="ryzx") # rotate C to C'
apriltag_rot = transformations.euler_matrix(np.pi/2, -np.pi/2, 0, axes="rxzy") # rotate A' to A
tf_camera_apriltag = camera_rot @ tf_detected @ apriltag_rot # Tcc' * Tc'a' * Ta'a = Tca
self.tf_apriltag_camera = np.linalg.inv(tf_camera_apriltag) # Tac
def get_size_and_build_walls(self):
# Get size of the ogrid ==============================================
# Get some useful vectors
between_vector = self.left_f - self.right_f
mid_point = self.right_f + between_vector / 2
target_vector = self.target - mid_point
self.mid_point = mid_point
# For rotations of the `between_vector` and the enu x-axis
b_theta = np.arctan2(between_vector[1], between_vector[0])
b_rot_mat = trns.euler_matrix(0, 0, b_theta)[:3, :3]
# Make the endpoints
rot_buffer = b_rot_mat.dot([20, 0, 0])
endpoint_left_f = self.left_f + rot_buffer
endpoint_right_f = self.right_f - rot_buffer
# Now lets build some wall points ======================================
if self.left_b is None:
# For rotations of the `target_vector` and the enu x-axis
t_theta = np.arctan2(target_vector[1], target_vector[0])
t_rot_mat = trns.euler_matrix(0, 0, t_theta)[:3, :3]
rot_channel = t_rot_mat.dot([self.channel_length, 0, 0])
self.left_b = self.left_f + rot_channel
self.right_b = self.right_f + rot_channel
# Now draw contours from the boat to the start gate ====================
# Get vector from boat to mid_point
mid_point_vector = self.boat_pos - self.mid_point
b_theta = np.arctan2(mid_point_vector[1], mid_point_vector[0])
b_rot_mat = trns.euler_matrix(0, 0, b_theta)[:3, :3]
rot_buffer = b_rot_mat.dot([np.linalg.norm(mid_point_vector) * 1.5, 0, 0])
left_cone_point = self.left_f + rot_buffer
right_cone_point = self.right_f + rot_buffer
# Define bounds for the grid
self.x_lower = min(left_cone_point[0], right_cone_point[0],
endpoint_left_f[0], endpoint_right_f[0], self.target[0]) - self.edge_buffer
self.x_upper = max(left_cone_point[0], right_cone_point[0],
endpoint_left_f[0], endpoint_right_f[0], self.target[0]) + self.edge_buffer
self.y_lower = min(left_cone_point[1], right_cone_point[1],
endpoint_left_f[1], endpoint_right_f[1], self.target[1]) - self.edge_buffer
self.y_upper = max(left_cone_point[1], right_cone_point[1],
endpoint_left_f[1], endpoint_right_f[1], self.target[1]) + self.edge_buffer
self.walls = [self.left_b, self.left_f, left_cone_point, right_cone_point, self.right_f, self.right_b]
def __init__(self):
self.subscriber = rospy.Subscriber("vicon",
PoseStamped,
self.callback,
queue_size=1)
self.reference_pub = rospy.Publisher("reference",
Odometry,
queue_size=1)
self.ext_att_pub = rospy.Publisher("external_attitude",
Quaternion,
queue_size=1)
self.T_V_NED = TR.euler_matrix(pi, 0, 0, 'rxyz')
# self.T_XAV_V = TR.identity_matrix() # from subscriber callback
self.T_UAV_XAV = TR.euler_matrix(pi, 0, 0, 'rxyz')
self.T_UAV_NED = TR.identity_matrix() # the output transform
def pull_obj2robot(pose, angle, delta_spring, object):
mesh, vertices = object.transform_object(pose, type="PoseStamped")
pose2d, placement = get2dpose_object(pose, object)
ripi = pose2d[0:2]
#1. define nominal orientation of gripper for pullin
T_gripper_nominal_world = tfm.euler_matrix(0, np.pi / 2, np.pi / 2, 'rxyz')
#2. rotate gripper by desired angle of gripper(sampling different solutions) + orientation of object about z axis
T_transform = tfm.euler_matrix(0, 0, angle + pose2d[2], 'rxyz')
gripper_nominal_pose_world = pose_from_matrix(T_gripper_nominal_world,
"PoseStamped", "yumi_body")
pose_transform = pose_from_matrix(T_transform, "PoseStamped", "yumi_body")
gripper_final_pose_world = transform_pose(gripper_nominal_pose_world,
pose_transform)
gripper_final_pose_world.pose.position.x = ripi[0]
gripper_final_pose_world.pose.position.y = ripi[1]
gripper_final_pose_world.pose.position.z = mesh.max_[2] - mesh.min_[
2] + delta_spring
#3. move gripper closer to edge (to get tactile imprint)
gripper_poses = [gripper_final_pose_world, gripper_final_pose_world]
poses_tip_world = deepcopy(gripper_poses)
pose2d, stable_placement = get2dpose_object(pose, object)
pose_proposals_base = proposals_base_from_object_pose(object, pose)
pose_transform_frameB = unit_pose()
if placement == 0:
pose_transform_frameB.pose.position.x -= 0.09 / 2
elif placement == 2:
pose_transform_frameB.pose.position.z -= 0.02
elif placement == 3:
pose_transform_frameB.pose.position.z += 0.03
elif placement == 1:
pose_transform_frameB.pose.position.x += 0.02
pose_transform_frameB.pose.position.z += 0.0
elif placement == 4:
pose_transform_frameB.pose.position.z += 0.02
gripper_final_pose_world = transform_pose_intermediate_frame(
gripper_final_pose_world, pose, pose_transform_frameB)
#4 pull at corner
# face_id, dict_faces = get_object_convex_face_id(gripper_poses,
# object,
# stable_placement,
# pose_proposals_base,
# arm="r")
# corner_point_list = dict_faces['faces'][face_id]
# gripper_final_pose_world.pose.position.x = corner_point_list[0][0]
# gripper_final_pose_world.pose.position.y = corner_point_list[0][1]
return gripper_final_pose_world
def test_move_out_of_collision(self):
env = orpy.Environment()
env.Load('data/pumablocks.env.xml')
body = env.GetKinBody('lego0')
Tinit = body.GetTransform()
def move_until_collision(step, direction, timeout=1.0):
start_time = time.time()
Tcollision = body.GetTransform()
while not env.CheckCollision(body):
Tcollision[:3,3] += step*np.array(direction)
body.SetTransform(Tcollision)
if time.time()-start_time > timeout:
return False
return True
# Move down into collision with the table
body.SetTransform(Tinit)
self.assertTrue( move_until_collision(0.005, [0, -1, 0]) )
result = criros.raveutils.move_out_of_collision(env, body, max_displacement=0.006)
self.assertTrue(result)
body.SetTransform(Tinit)
self.assertTrue( move_until_collision(0.005, [0, -1, 0]) )
result = criros.raveutils.move_out_of_collision(env, body, max_displacement=0.001)
self.assertFalse(result)
# Show we can cope with tilted objects
body.SetTransform(Tinit)
move_until_collision(0.005, [0, -1, 0])
Toffset = tr.euler_matrix(np.deg2rad(10), 0, 0)
Tbody = body.GetTransform()
Tnew = np.dot(Tbody, Toffset)
body.SetTransform(Tnew)
result = criros.raveutils.move_out_of_collision(env, body, max_displacement=0.015)
self.assertTrue(result)
def to_pose2d(pos):
# pos = N?x3x4 transformation matrix
# 2d rotation requires (-y) component
# 2d translation requires (z, -x) component
# == test ==
# -- opt 1 : einsum --
T_pre = tx.euler_matrix(-np.pi/2,0,-np.pi/2) #4x4
T_0i = tx.inverse_matrix(np.concatenate((pos[0], [[0,0,0,1]]), axis=0)) #4x4
T_cam = np.stack([np.concatenate((p, [[0,0,0,1]]), axis=0) for p in pos], axis=0)
pos_n = np.einsum('ij,jk,nkl,lm->nim', T_pre, T_0i, T_cam, T_pre.T) # normalized position
res = []
for p in pos_n:
t = tx.translation_from_matrix(p)
q = tx.euler_from_matrix(p)
res.append([ t[0], t[1], q[-1] ])
res = np.stack(res, axis=0)
return res
# --------------------
# -- opt 2 : no einsum --
#T_pre = tx.euler_matrix(-np.pi/2,0,-np.pi/2) #4x4
#T_0i = tx.inverse_matrix(np.concatenate((pos[0], [[0,0,0,1]]), axis=0)) #4x4
#T_cam = np.stack([np.concatenate((p, [[0,0,0,1]]), axis=0) for p in pos], axis=0)
#res = []
#for p in pos:
# T_cam = np.concatenate((p, [[0,0,0,1]]), axis=0)
# T_base = T_pre.dot(T_0i).dot(T_cam).dot(T_pre.T)
# t = tx.translation_from_matrix(T_base)
# q = tx.euler_from_matrix(T_base)
# res.append([t[0], t[1], q[-1]])
#res = np.stack(res, axis=0)
return res
def lookup_tag(tag_number):
""" Returns the AR tag position in world coordinates
Parameters
----------
tag_number : int
AR tag number
Returns
-------
:obj:`autolab_core.RigidTransform` AR tag position in world coordinates
"""
to_frame = 'ar_marker_{}'.format(tag_number)
tag_pos = None
while not tag_pos:
try:
t = listener.getLatestCommonTime(from_frame, to_frame)
# print(rospy.Time.now())
# t = rospy.Time.now()
tag_pos, tag_rot = listener.lookupTransform(from_frame, to_frame, t)
except:
continue;
# if not listener.frameExists(from_frame) or not listener.frameExists(to_frame):
# print 'Frames not found'
# print 'Did you place AR marker {} within view of the baxter left hand camera?'.format(tag_number)
# exit(0)
angles = tfs.euler_from_quaternion(tag_rot)
# print(angles)
trans = tfs.euler_matrix(*angles)
print(RigidTransform(trans[:3, :3], tag_pos))
return RigidTransform(trans[:3, :3], tag_pos)
def Aruco_marker_detector(self):
""" Use the build in python library to detect the aruco marker and its coordinates """
img = self.frame.copy()
# Detect the markers in the image
markerCorners, markerIds, rejectedCandidates = cv.aruco.detectMarkers(img, self.dictionary, parameters=self.parameters)
# Get the transformation matrix to the marker
if markerCorners:
markerSize = 2.0
axisLength = 3.0
rvecs, tvecs, _ = cv.aruco.estimatePoseSingleMarkers(markerCorners, markerSize, self.K, self.distCoeffs)
# Draw the axis on the marker
frame_out = cv.aruco.drawAxis( img, self.K, self.distCoeffs, rvecs, tvecs, axisLength)
rotMat = tr.euler_matrix(np.pi / 2.0, 0, 0)
rotMat = rotMat[0:3, 0:3]
tvecs = np.matmul(rotMat, tvecs[0][0].T)
rotMat, _ = cv.Rodrigues(rvecs)
eul = tr.euler_from_matrix(rotMat)
yaw_angle = self.pos[3] - eul[2]
# Publish the position of the marker
marker_pos = PT()
marker_pos.position.x = tvecs[0]
marker_pos.position.y = - tvecs[2]
marker_pos.position.z = tvecs[1]
marker_pos.yaw = eul[2] * np.pi / 180.0
self.aruco_marker_pos_pub.publish(marker_pos)
# Publish the image with the detected marker
self.aruco_marker_img_pub.publish(self.bridge.cv2_to_imgmsg(frame_out, "bgr8"))
def d_spiral_point(self,
point,
radius,
granularity=8,
revolutions=1,
direction='ccw',
theta_offset=0,
meters_per_rev=0):
"""
Sprials a point using discrete moves
This produces a generator
"""
point = np.array(point)
if direction == 'ccw':
angle_incrment = 2 * np.pi / granularity
else:
angle_incrment = -2 * np.pi / granularity
sprinkles = transformations.euler_matrix(0, 0, angle_incrment)[:3, :3]
# Find first point to go to using boat rotation
next_point = np.append(normalize(self.position[:2] - point[:2]),
0) # Doing this in 2d
radius_increment = meters_per_rev / granularity
for i in range(granularity * revolutions + 1):
new = point + radius * next_point
radius += radius_increment
yield self.set_position(new).look_at(point).yaw_left(theta_offset)
next_point = sprinkles.dot(next_point)
yield self.set_position(new).look_at(point).yaw_left(theta_offset)
def prepare_trajectory(trajectory: np.ndarray, robot_pose: Pose,
robot_model: rtb.DHRobot,
model_pose: np.ndarray) -> np.ndarray:
robot_orientation = Quaternion(robot_pose.orientation[0],
robot_pose.orientation[1],
robot_pose.orientation[2],
robot_pose.orientation[3])
robot_position = Point(robot_pose.position[0], robot_pose.position[1],
robot_pose.position[2])
robot_euler = T.euler_from_quaternion(
(robot_orientation.x, robot_orientation.y, robot_orientation.z,
robot_orientation.w))
robot_transformation = T.euler_matrix(robot_euler[0], robot_euler[1],
robot_euler[2])
robot_transformation[0, 3] = robot_position.x
robot_transformation[1, 3] = robot_position.y
robot_transformation[2, 3] = robot_position.z
robot_transformation = np.linalg.inv(robot_transformation)
transformed_trajectory = []
for i in range(len(trajectory)):
projected_point = robot_transformation.dot(
np.concatenate((trajectory[i], 1), axis=None)) / 1000
projected_point = model_pose.dot(projected_point)[:-1]
ik_result, fail, err = robot_model.ikine(
sm.SE3(projected_point[0], projected_point[1], projected_point[2]))
transformed_trajectory += [ik_result]
return np.array(transformed_trajectory)
def eulerAnglesToRotationMatrix(theta):
"""Calculates rotation matrix given euler angles in 'xyz' sequence
:param theta: list of 3 euler angles
:return: 3x3 rotation matrix equivalent to the given euler angles
"""
return euler_matrix(theta[0], theta[1], theta[2], axes="sxyz")[:3, :3]
def generate_ep(self):
cart_cur = self.cart_arm.get_end_effector_pose()
if self.step_ind < self.num_steps:
self.cart_cur_goal = self.trajectory[self.step_ind]
self.step_ind += 1
else:
self.cart_cur_goal = self.cart_goal
self.cart_err = self.cart_arm.ep_error(cart_cur, self.cart_cur_goal)
#self.cart_err[np.where(np.fabs(self.cart_err) < self.err_goal_thresh * 0.6)] = 0.0
cart_control = np.array([
pidc.update_state(cart_err_i)
for pidc, cart_err_i in zip(self.controllers, self.cart_err)
])
cep_pos_cur, cep_rot_cur = self.cart_arm.get_ep()
cep_pos_new = cep_pos_cur - cart_control[:3, 0]
cart_rot_control = np.mat(
tf_trans.euler_matrix(cart_control[3, 0], cart_control[4, 0],
cart_control[5, 0]))[:3, :3]
cep_rot_new = cep_rot_cur * cart_rot_control.T
ep_new = (cep_pos_new, cep_rot_new)
if self.debug:
print "=" * 50
print "cart_cur", cart_cur
print "-" * 50
print "cur goal", self.cart_cur_goal
print "-" * 50
print "err", self.cart_err
print "-" * 50
print "cart_control", cart_control
return EPStopConditions.CONTINUE, ep_new
def plot_frame(self,
frame_num=None,
mark_num=None,
xyz_rotation=(0, 0, 0),
azimuth=60,
elevation=30):
"""Plots the location of each marker in the specified frame
"""
#Get the frame
if frame_num is not None:
frame = self._get_frames()[:, :, frame_num]
else:
frame = self.read()[0][:, :, 0]
# Apply any desired rotation
rot_matrix = transformations.euler_matrix(*xyz_rotation)[0:3, 0:3]
frame = rot_matrix.dot(frame.T).T
xs = frame[:, 0]
ys = frame[:, 1]
zs = frame[:, 2]
#Make the plot
figure = plt.figure(figsize=(11, 10))
axes = figure.add_subplot(111, projection='3d')
markers = ['r'] * self.get_num_points()
if mark_num is not None:
markers[mark_num] = 'b'
axes.scatter(xs, ys, zs, c=markers, marker='o', s=60)
axes.auto_scale_xyz([-0.35, 0.35], [-0.35, 0.35], [-0.35, 0.35])
axes.set_xlabel('X (m)')
axes.set_ylabel('Y (m)')
axes.set_zlabel('Z (m)')
def main():
rospy.init_node("arm_cart_vel_control")
if True:
ctrl_switcher = ControllerSwitcher()
ctrl_switcher.carefree_switch(
'r', '%s_arm_controller',
'$(find hrl_pr2_arms)/params/joint_traj_params_electric.yaml')
rospy.sleep(0.5)
ctrl_switcher.carefree_switch(
'r', '%s_joint_controller_low',
'$(find hrl_pr2_arms)/params/joint_traj_params_electric_low.yaml')
r_arm_js = create_pr2_arm('r',
PR2ArmJointTrajectory,
controller_name='%s_joint_controller_low')
q = [
-0.34781704, 0.27341079, -1.75392154, -2.08626393, -3.43756314,
-1.82146607, -1.85187734
]
r_arm_js.set_ep(q, 3)
rospy.sleep(6)
ctrl_switcher.carefree_switch(
'r', '%s_cart_low_rfh',
'$(find kelsey_sandbox)/params/j_transpose_low_rfh.yaml')
r_arm = create_pr2_arm('r', PR2ArmCartesianPostureBase)
r_arm.set_posture()
rospy.sleep(0.2)
vel_ctrl = PR2ArmCartVelocityController(r_arm)
#vel_frame = tf_trans.euler_matrix(0, -np.pi/2, 0)[:3,:3]
#vel_ctrl.move_velocity(velocity_rot=vel_frame, velocity=0.005, is_translation=False)
vel_frame = tf_trans.euler_matrix(0, 0, np.pi / 2)[:3, :3]
vel_ctrl.move_velocity(velocity_rot=vel_frame,
velocity=0.10,
is_translation=False)
def circle_point(self, point, radius, granularity=8, theta_offset=0):
'''
Circle a point whilst looking at it
This produces a generator, so for use:
circle = navigator.move.circle_point([1,2,0], 5)
for p in circle:
yield p.go()
'''
point = np.array(point)
angle_incrment = 2 * np.pi / granularity
sprinkles = transformations.euler_matrix(0, 0, angle_incrment)[:3, :3]
# Find first point to go to using boat rotation
next_point = np.append(normalize(self.nav.pose[0][:2] - point[:2]),
0) # Doing this in 2d
for i in range(granularity + 1):
new = point + radius * next_point
yield self.set_position(new).look_at(point).yaw_left(
theta_offset
) # Remember this is a generator - not a twisted yield
next_point = sprinkles.dot(next_point)
yield self.set_position(new).look_at(point).yaw_left(theta_offset)
def test_frame3_rpy(self, x, y, z, roll, pitch, yaw):
r2 = euler_matrix(roll, pitch, yaw)
r2[0, 3] = x
r2[1, 3] = y
r2[2, 3] = z
np.testing.assert_array_almost_equal(w.compile_and_execute(w.frame_rpy, [x, y, z, roll, pitch, yaw]),
r2)
def place(self, x, y, z, alpha, beta, gamma, approach):
# find pre-place pose
T_grasp = euler_matrix(radians(alpha),
radians(beta),
radians(gamma),
axes="sxyz")
T_grasp[:3, 3] = np.array([x, y, z])
T_trans = np.identity(4)
T_trans[2, 3] = -approach
T_pre_grasp = np.dot(T_grasp, T_trans)
alpha_pre, beta_pre, gamma_pre = euler_from_matrix(T_pre_grasp)
alpha_pre, beta_pre, gamma_pre = (degrees(alpha_pre),
degrees(beta_pre),
degrees(gamma_pre))
x_pre, y_pre, z_pre = T_pre_grasp[:3, 3]
# move to pre-place
if (self.move(x_pre, y_pre, z_pre, alpha_pre, beta_pre, gamma_pre) and
# move to place
self.move(x, y, z, alpha, beta, gamma) and
# open gripper
self.open_gripper() and
# move back to pre-grasp
self.move(x_pre, y_pre, z_pre, alpha_pre, beta_pre,
gamma_pre)):
rospy.loginfo("Place finished")
return True
else:
rospy.logerr("Place failed")
self.clear_error()
return False
def transform_frame(vec, orient, att, frame_id):
vec = numpy.array(vec)
if frame_id == 'aruco_map':
# from 'aruco_map' to 'main_camera'
rmat, j = Rodrigues(orient)
vec.transpose()
vec = rmat.dot(vec)
vec.transpose()
elif frame_id == 'main_camera':
pass
# from 'main_camera' to 'fcu'
a = camera_angle
vec[0], vec[1], vec[2] = vec[1] * math.cos(a) + vec[2] * math.sin(
a), -vec[0], vec[2] * math.cos(a) - vec[1] * math.sin(a)
# from 'fcu' to 'fcu_horiz'
roll, pitch = att[0], att[1]
rmat = euler_matrix(roll, pitch, 0, 'rxyz')
rmat = rmat[0:3, 0:3]
vec.transpose()
vec = rmat.dot(vec)
vec.transpose()
return list(vec)
def _extract_twist_msg(twist_msg):
pos = [twist_msg.linear.x, twist_msg.linear.y, twist_msg.linear.z]
rot_euler = [twist_msg.angular.x, twist_msg.angular.y, twist_msg.angular.z]
quat = tf_trans.quaternion_from_euler(*rot_euler, axes='sxyz')
homo_mat = np.mat(tf_trans.euler_matrix(*rot_euler))
homo_mat[:3,3] = np.mat([pos]).T
return homo_mat, quat, rot_euler
def calc_transformation_matrix(x, y, z, rotx, roty, rotz):
# return transformation matrix based on translation and rotation components
r_mat = transformations.euler_matrix(radians(rotz), radians(rotx),
radians(roty), 'rzxy')
t_mat = transformations.translation_matrix((x, y, z))
return numpy.dot(t_mat, r_mat)
def generate_ep(self):
cart_cur = self.cart_arm.get_end_effector_pose()
if self.step_ind < self.num_steps:
self.cart_cur_goal = self.trajectory[self.step_ind]
self.step_ind += 1
else:
self.cart_cur_goal = self.cart_goal
self.cart_err = self.cart_arm.ep_error(cart_cur, self.cart_cur_goal)
#self.cart_err[np.where(np.fabs(self.cart_err) < self.err_goal_thresh * 0.6)] = 0.0
cart_control = np.array([pidc.update_state(cart_err_i) for pidc, cart_err_i in
zip(self.controllers, self.cart_err)])
cep_pos_cur, cep_rot_cur = self.cart_arm.get_ep()
cep_pos_new = cep_pos_cur - cart_control[:3,0]
cart_rot_control = np.mat(tf_trans.euler_matrix(cart_control[3,0], cart_control[4,0],
cart_control[5,0]))[:3,:3]
cep_rot_new = cep_rot_cur * cart_rot_control.T
ep_new = (cep_pos_new, cep_rot_new)
if self.debug:
print "="*50
print "cart_cur", cart_cur
print "-"*50
print "cur goal", self.cart_cur_goal
print "-"*50
print "err", self.cart_err
print "-"*50
print "cart_control", cart_control
return EPStopConditions.CONTINUE, ep_new
def publish_state(self, t):
state_msg = TransformStamped()
t_ned_imu = tft.translation_matrix(self.model.get_position())
R_ned_imu = tft.quaternion_matrix(self.model.get_orientation())
T_ned_imu = tft.concatenate_matrices(t_ned_imu, R_ned_imu)
#rospy.loginfo("T_ned_imu = \n" + str(T_ned_imu))
if self.T_imu_vicon is None:
# grab the static transform from imu to vicon frame from param server:
frames = rospy.get_param("frames", None)
ypr = frames['vicon_to_imu']['rotation']
q_vicon_imu = tft.quaternion_from_euler(*[radians(x) for x in ypr], axes='rzyx') # xyzw
R_vicon_imu = tft.quaternion_matrix(q_vicon_imu)
t_vicon_imu = tft.translation_matrix(frames['vicon_to_imu']['translation'])
# rospy.loginfo(str(R_vicon_imu))
# rospy.loginfo(str(t_vicon_imu))
self.T_vicon_imu = tft.concatenate_matrices(t_vicon_imu, R_vicon_imu)
self.T_imu_vicon = tft.inverse_matrix(self.T_vicon_imu)
self.T_enu_ned = tft.euler_matrix(radians(90), 0, radians(180), 'rzyx')
rospy.loginfo(str(self.T_enu_ned))
rospy.loginfo(str(self.T_imu_vicon))
#rospy.loginfo(str(T_vicon_imu))
# we have /ned -> /imu, need to output /enu -> /vicon:
T_enu_vicon = tft.concatenate_matrices(self.T_enu_ned, T_ned_imu, self.T_imu_vicon )
state_msg.header.stamp = t
state_msg.header.frame_id = "/enu"
state_msg.child_frame_id = "/simflyer1/flyer_vicon"
stt = state_msg.transform.translation
stt.x, stt.y, stt.z = T_enu_vicon[:3,3]
stro = state_msg.transform.rotation
stro.x, stro.y, stro.z, stro.w = tft.quaternion_from_matrix(T_enu_vicon)
self.pub_state.publish(state_msg)
def _makePreGraspPose(self, boxMat, axis):
if axis==0: #x axis
alpha = 0
gamma = 0
else: #y axis
alpha = pi/2
gamma = -pi/2
ik_request = PositionIKRequest()
ik_request.ik_link_name = self.toolLinkName
with self._joint_state_lock:
ik_request.ik_seed_state.joint_state = copy.deepcopy(self._joint_states)
ik_request.pose_stamped = PoseStamped()
ik_request.pose_stamped.header.stamp = rospy.Time.now()
ik_request.pose_stamped.header.frame_id = self.frameID
beta = pi/2
while beta < 3*pi/2:
rotationMatrix = transformations.euler_matrix(alpha, beta, gamma, 'rzyz')
distance = self.preGraspDistance + self.gripperFingerLength
preGraspMat = transformations.translation_matrix([0,0,-distance])
fullMat = transformations.concatenate_matrices(boxMat, rotationMatrix, preGraspMat)
p = transformations.translation_from_matrix(fullMat)
q = transformations.quaternion_from_matrix(fullMat)
print "trying {} radians".format(beta)
try:
self._ik_server(ik_request, Constraints(), rospy.Duration(5.0))
except rospy.service.ServiceException:
beta += 0.1
else:
if ik_resp.error_code.val > 0:
return beta
else:
#print ik_resp.error_code.val
beta += 0.01
rospy.logerr('No way to pick this up. All IK solutions failed.')
return 7 * pi / 6
def test_frame3_rpy(self, x, y, z, roll, pitch, yaw):
r1 = np.array(spw.frame_rpy(x, y, z, roll, pitch, yaw)).astype(float)
r2 = euler_matrix(roll, pitch, yaw)
r2[0, 3] = x
r2[1, 3] = y
r2[2, 3] = z
self.assertTrue(np.isclose(r1, r2).all(), msg='\n{} != \n{}'.format(r1, r2))
def calculate_joints(self, position):
self.joint_angle_matricies = []
for angle in position:
self.joint_angle_matricies.append(transformations.euler_matrix(0, 0, angle))
T_c = [np.identity(4)]
tf_msg = TFMessage()
for index in xrange(len(self.urdf_transform_matricies)):
urdf_transform_matrix = self.urdf_transform_matricies[index]
joint_angle_matrix = self.joint_angle_matricies[index]
transform_matrix = np.dot(urdf_transform_matrix, joint_angle_matrix)
tf_stamp = TransformStamped()
tf_stamp.child_frame_id = self.urdf_transforms[index].child_frame_id
tf_stamp.header.frame_id = self.urdf_transforms[index].header.frame_id
if index in (8, 10): #sets parent of fingers to link6
index = 6
T_c.append(np.dot(T_c[index], transform_matrix))
tf_stamp.transform = msgify(Transform, T_c[-1])
#tf_stamp.transform = msgify(Transform, transform_matrix)
tf_msg.transforms.append(tf_stamp)
#print transforms.header
#print link_states.name[-1]
#print link_states.pose[-1]
#print '-----------------------------------------------'
return tf_msg
def __init__(self, MPC1):
self.lock = threading.Lock()
self.MPC = MPC1
self.RATE = rospy.get_param('/rate', 50)
self.dt = 0.0
self.time_start = 0.0
self.end = False
self.pose_init = [0.0, 0.0, 0.0]
self.flag = True
"Desired values setup"
# rotation matrix [4x4] from `world` frame to `body`
self.bTw = tftr.euler_matrix(-np.pi, 0.0, 0.0, 'rxyz')
# # in `world` frame
# self.A = rospy.get_param('/A', 90.0) # [degrees]
# self.pose_des = rospy.get_param('/pose_des', [0.5, 0.0, 2.0])
# # in 'body' frame
# self.pose_des = self.transform_pose(self.pose_des)
#print(self.pose_des.T)
self.rot_z_des = 0.0
"ROS stuff"
self.pub_cmd_vel = rospy.Publisher("/cmd_vel",
Twist,
queue_size=1,
latch=False)
self.sub_odom = rospy.Subscriber("/odom", Odometry,
self.odometry_callback)
def calc_transformation(self, name, relative_to=None):
calc_from_joint = False
if relative_to:
if relative_to in self.urdf.link_map:
parent_link_name = relative_to
elif relative_to in self.urdf.joint_map:
parent_link_name = self.urdf.joint_map[name].parent
calc_from_joint = True
else:
parent_link_name = self.urdf.get_root()
calc_to_joint = False
if name in self.urdf.link_map:
child_link_name = name
elif name in self.urdf.joint_map:
child_link_name = self.urdf.joint_map[name].child
calc_to_joint = True
chains = self.urdf.get_chain(parent_link_name, child_link_name,
joints=True, links=True)
if calc_from_joint:
chains = chains[1:]
if calc_to_joint:
chains = chains[:-1]
poses = []
for name in chains:
if name in self.urdf.joint_map:
joint = self.urdf.joint_map[name]
if joint.origin is not None:
poses.append(joint.origin)
elif name in self.urdf.link_map:
link = self.urdf.link_map[name]
if link.visual is not None and link.visual.origin is not None:
poses.append(link.visual.origin)
m = np.dot(T.translation_matrix((0,0,0)),
T.euler_matrix(0,0,0))
for p in poses:
n = np.dot(T.translation_matrix(tuple(p.xyz)),
T.euler_matrix(*tuple(p.rpy)))
m = np.dot(m, n)
t = T.translation_from_matrix(m)
q = T.quaternion_from_matrix(m)
return tuple(t), (q[3], q[0], q[1], q[2])
def get_path(buoy_l, buoy_r):
# Vector between the two buoys
between_vector = buoy_r.position - buoy_l.position
# Rotate that vector to point through the buoys
r = trns.euler_matrix(0, 0, np.radians(90))[:3, :3]
direction_vector = r.dot(between_vector)
direction_vector /= np.linalg.norm(direction_vector)
return between_vector, direction_vector
def fromROSPose2DModel(self, model): self.type = model.type self.pose = LocalPose() matrix = euler_matrix(0.0, 0.0, model.pose.theta) matrix[0][3] = model.pose.x matrix[1][3] = model.pose.y self.pose.pose = fromMatrix(matrix) self.geometry_type = 'POSE2D' geometry_string = 'POINT(%f %f %f)' % (model.pose.x, model.pose.y, 0.0) self.geometry = WKTElement(geometry_string)
def pose_to_matrix(self, ps):
trans = np.matrix([-ps.pose.position.x,
-ps.pose.position.y,
ps.pose.position.z]).T
r, p, y = euler_from_quaternion([ps.pose.orientation.x,
ps.pose.orientation.y,
ps.pose.orientation.z,
ps.pose.orientation.w])
rot = np.matrix(euler_matrix(-r, -p, -y)[:3, :3]).T
return rot, trans
def test_to_tf(self): tb = tb_angles(45,-5,24) self.assertTrue(tb.to_tf().flags.writeable) for yaw in self.yaw: for pitch in self.pitch: for roll in self.roll: tb = tb_angles(yaw,pitch,roll) m = tb.to_tf() m2 = tft.euler_matrix(yaw * pi / 180, pitch * pi / 180, roll * pi / 180, 'rzyx') self.assertTrue(np.allclose(m, m2),msg='%s and %s not close!' % (m,m2))
def MsgToPose(msg):
#Parse the ROS message to a 4x4 pose format
#Get translation and rotation (from Euler angles)
pose = transformations.euler_matrix(msg.roll,msg.pitch,msg.yaw)
pose[0,3] = msg.pt.x
pose[1,3] = msg.pt.y
pose[2,3] = msg.pt.z
return pose
def get_rot_matrix(self, rev=False):
frm, to = 'ardrone/odom', 'ardrone/ardrone_base_bottomcam'
if rev:
frm, to = to, frm
self.tf.waitForTransform(frm, to, rospy.Time(0), rospy.Duration(3))
trans, rot = self.tf.lookupTransform(frm, to, rospy.Time(0))
x, y, z = euler_from_quaternion(rot)
yaw = euler_from_quaternion(rot, axes='szxy')[0]
return yaw, np.array(euler_matrix(-x, -y, z))
def pub_head_registration(ud):
cheek_pose_base_link = self.tf_list.transformPose("/base_link", ud.cheek_pose)
# find the center of the ellipse given a cheek click
cheek_transformation = np.mat(tf_trans.euler_matrix(2.6 * np.pi/6, 0, 0, 'szyx'))
cheek_transformation[0:3, 3] = np.mat([-0.08, -0.04, 0]).T
cheek_pose = PoseConverter.to_homo_mat(cheek_pose_base_link)
#b_B_c[0:3,0:3] = np.eye(3)
norm_xy = cheek_pose[0:2, 2] / np.linalg.norm(cheek_pose[0:2, 2])
head_rot = np.arctan2(norm_xy[1], norm_xy[0])
cheek_pose[0:3,0:3] = tf_trans.euler_matrix(0, 0, head_rot, 'sxyz')[0:3,0:3]
self.cheek_pub.publish(PoseConverter.to_pose_stamped_msg("/base_link", cheek_pose))
ell_center = cheek_pose * cheek_transformation
self.ell_center_pub.publish(PoseConverter.to_pose_stamped_msg("/base_link", ell_center))
# create an ellipsoid msg and command it
ep = EllipsoidParams()
ep.e_frame.transform = PoseConverter.to_tf_msg(ell_center)
ep.height = 0.924
ep.E = 0.086
self.ep_pub.publish(ep)
return 'succeeded'
def place_generator(self):
place_pose = PoseStamped()
place_pose.header.frame_id = REFERENCE_FRAME
place_pose.pose.position.x = 0.57
place_pose.pose.position.y = 0.16
place_pose.pose.position.z = 0.56
place_pose.pose.orientation.w = 1.0
P = transformations.quaternion_matrix(
[
place_pose.pose.orientation.x,
place_pose.pose.orientation.y,
place_pose.pose.orientation.z,
place_pose.pose.orientation.w,
]
)
P[0, 3] = place_pose.pose.position.x
P[1, 3] = place_pose.pose.position.y
P[2, 3] = place_pose.pose.position.z
places = []
yaw_angles = [0, 1, 57, -1, 57, 3, 14]
x_vals = [0, 0.05, 0.1, 0.15]
z_vals = [0.05, 0.1, 0.15]
for y in yaw_angles:
G = transformations.euler_matrix(0, 0, y)
G[0, 3] = 0.0 # offset about x
G[1, 3] = 0.0 # about y
G[2, 3] = 0.0 # about z
for z in z_vals:
for x in x_vals:
TM = np.dot(P, G)
pl = deepcopy(place_pose)
pl.pose.position.x = TM[0, 3] + x
pl.pose.position.y = TM[1, 3]
pl.pose.position.z = TM[2, 3] + z
quat = transformations.quaternion_from_matrix(TM)
pl.pose.orientation.x = quat[0]
pl.pose.orientation.y = quat[1]
pl.pose.orientation.z = quat[2]
pl.pose.orientation.w = quat[3]
pl.header.frame_id = REFERENCE_FRAME
places.append(deepcopy(pl))
return places
def orient_division(self, grid, point, angle_offset_mult=1):
""" Returns a position to go to on the other side of the line """
angle_offset = 1 * angle_offset_mult # rads
point = np.array(point)
r_pos = self.navigator.pose[0][:2] - point[:2]
angle = np.arctan2(*r_pos[::-1]) - angle_offset
center = np.array(grid.shape) / 2
line_pt2 = trns.euler_matrix(0, 0, angle)[:3, :3].dot(np.array([1E3, 0, 0]))
cv2.line(grid, tuple(center), tuple(line_pt2[:2].astype(np.int32)), 255, 2)
# Let's put ourselves on the other side of that there line
return self.generate_target(angle, r_pos) + point[:2]
def _MsgToPose(msg):
"""
Parse the ROS message to a 4x4 pose format
@param msg The ros message containing a pose
@return A 4x4 transformation matrix containing the pose
as read from the message
"""
# Get translation and rotation (from Euler angles)
pose = transformations.euler_matrix(msg.roll * 0.0, msg.pitch * 0.0, msg.yaw * 0.0)
pose[0, 3] = msg.pt.x
pose[1, 3] = msg.pt.y
pose[2, 3] = msg.pt.z
return pose
def __init__(self):
rospy.Subscriber("/mavros/local_position/pose", PoseStamped, self.localCb)
rospy.Subscriber("/itech_ros/mavros_offboard/enu", PoseStamped, self.enuCb)
self.localEnuPub = rospy.Publisher(util.topicName("mavros_local_pose_fusion", "local_enu"), PoseStamped, queue_size=10)
self.offsetPub = rospy.Publisher(util.topicName("mavros_local_pose_fusion", "offset"), PointStamped, queue_size=10)
self.offset = None
self.localPose = PoseModel()
self.localEnuPose = PoseModel()
#tick posee are the poses at the time that we get positioning data.
self.enuTickPose = PoseModel()
self.localTickPose = PoseModel()
self.r = euler_matrix(0, 0, math.pi/-2)
def pose_relative_rot(pose, r=0., p=0., y=0., degrees=True):
"""Return a pose rotated relative to a given pose."""
ps = deepcopy(pose)
if degrees:
r = math.radians(r)
p = math.radians(p)
y = math.radians(y)
des_rot_mat = tft.euler_matrix(r,p,y)
q_ps = [ps.pose.orientation.x,
ps.pose.orientation.y,
ps.pose.orientation.z,
ps.pose.orientation.w]
state_rot_mat = tft.quaternion_matrix(q_ps)
final_rot_mat = np.dot(state_rot_mat, des_rot_mat)
ps.pose.orientation = Quaternion(
*tft.quaternion_from_matrix(final_rot_mat))
return ps
def post_grasp(self, new_pose, obj_idx, fl):
######### GRASP OBJECT/ REMOVE FROM SCENE ######.
if fl == "true":
self.close_gripper()
self.aro = obj_idx
else:
self.open_gripper()
self.aro = None
rospy.sleep(2)
### POST GRASP RETREAT ###
M1 = transformations.quaternion_matrix(
[
new_pose.pose.orientation.x,
new_pose.pose.orientation.y,
new_pose.pose.orientation.z,
new_pose.pose.orientation.w,
]
)
M1[0, 3] = new_pose.pose.position.x
M1[1, 3] = new_pose.pose.position.y
M1[2, 3] = new_pose.pose.position.z
M2 = transformations.euler_matrix(0, 0, 0)
M2[0, 3] = 0.0 # offset about x
M2[1, 3] = 0.0 # about y
M2[2, 3] = 0.25 # about z
T1 = np.dot(M2, M1)
npo = deepcopy(new_pose)
npo.pose.position.x = T1[0, 3]
npo.pose.position.y = T1[1, 3]
npo.pose.position.z = T1[2, 3]
quat = transformations.quaternion_from_matrix(T1)
npo.pose.orientation.x = quat[0]
npo.pose.orientation.y = quat[1]
npo.pose.orientation.z = quat[2]
npo.pose.orientation.w = quat[3]
npo.header.frame_id = REFERENCE_FRAME
self.right_arm.plan(npo.pose)
self.right_arm.go(wait=True)
def test_create_matrix(self): for yaw in self.yaw: for pitch in self.pitch: for roll in self.roll: m = tft.euler_matrix(yaw * pi / 180., pitch * pi / 180., roll * pi / 180., 'rzyx') tb = tb_angles(m) self.assertTbEqual(tb, yaw, pitch, roll) m2 = m[0:3,0:3] tb = tb_angles(m2) self.assertTbEqual(tb, yaw, pitch, roll) m3 = np.mat(m) tb = tb_angles(m3) self.assertTbEqual(tb, yaw, pitch, roll) m4 = m.tolist() tb = tb_angles(m4) self.assertTbEqual(tb, yaw, pitch, roll)
def test_to_mat(self): tb = tb_angles(45,-5,24) m1 = tb.matrix self.assertFalse(m1.flags.writeable) m1a = tb.matrix self.assertIs(m1, m1a) m2 = tb.to_matrix() self.assertTrue(m2.flags.writeable) self.assertIsNot(m1,m2) for yaw in self.yaw: for pitch in self.pitch: for roll in self.roll: tb = tb_angles(yaw,pitch,roll) m = tb.matrix self.assertAlmostEqual(np.linalg.det(m), 1) m2 = tft.euler_matrix(yaw * pi / 180, pitch * pi / 180, roll * pi / 180, 'rzyx')[0:3,0:3] self.assertTrue(np.allclose(m, m2),msg='%s and %s not close!' % (m,m2))
def publish_pointestimates(servoang):
global X_points, Kc, MSG
# MSG[9]=726
# servoang=0
t = np.mat("-1.6991; 51.3061; -83.5694")
t[2, :] = t[2, :] + MSG[9]
# print t
R = euler_matrix(0.0022, 0.003 + ((servoang) / 180.0 * pi), -0.2652, "rzyx")
R = R[0:3, 0:3].T
# print R
# print t
t = R * t
# print t
T = np.bmat("R t")
x = Kc * T * X_points
x[0, :] = x[0, :] / x[2, :]
x[1, :] = x[1, :] / x[2, :]
x[2, :] = x[2, :] / x[2, :]
# print x
return x
def publish_pose(self, header, results, img):
msg = PoseArray(header=header)
for r in results:
pose = r["face_origin"]
pose_2d = r['face_2d_origin']
ori = r["pose"]
mat = T.euler_matrix(-ori[0], -ori[1], -ori[2])
rotmat = mat[:3, :3]
quat = T.quaternion_from_matrix(mat)
quat = T.quaternion_multiply(
[0.5, 0.5, -0.5, 0.5], quat)
pose.orientation.x = quat[0]
pose.orientation.y = quat[1]
pose.orientation.z = quat[2]
pose.orientation.w = quat[3]
msg.poses.append(pose)
if self.visualize:
zvec = np.array([0, 0, 1], np.float32)
yvec = np.array([0, 1, 0], np.float32)
xvec = np.array([1, 0, 0], np.float32)
zvec = _project_plane_yz(rotmat.dot(zvec)) * 50.0
yvec = _project_plane_yz(rotmat.dot(yvec)) * 50.0
xvec = _project_plane_yz(rotmat.dot(xvec)) * 50.0
face_2d_center = np.array([pose_2d.position.x, pose_2d.position.y])
img = _draw_line(img, face_2d_center,
face_2d_center + zvec, (255, 0, 0), 3)
img = _draw_line(img, face_2d_center,
face_2d_center + yvec, (0, 255, 0), 3)
img = _draw_line(img, face_2d_center,
face_2d_center + xvec, (0, 0, 255), 3)
self.pub_pose.publish(msg)
if self.visualize:
img_msg = self.cv_bridge.cv2_to_imgmsg(img, "bgr8")
img_msg.header = header
self.pub_debug_image.publish(img_msg)
def d_spiral_point(self, point, radius, granularity=8, revolutions=1, direction='ccw', theta_offset=0, meters_per_rev=0):
"""
Sprials a point using discrete moves
This produces a generator
"""
point = np.array(point)
if direction == 'ccw':
angle_incrment = 2 * np.pi / granularity
else:
angle_incrment = -2 * np.pi / granularity
sprinkles = transformations.euler_matrix(0, 0, angle_incrment)[:3, :3]
# Find first point to go to using boat rotation
next_point = np.append(normalize(self.position[:2] - point[:2]), 0) # Doing this in 2d
radius_increment = meters_per_rev / granularity
for i in range(granularity * revolutions + 1):
new = point + radius * next_point
radius += radius_increment
yield self.set_position(new).look_at(point).yaw_left(theta_offset)
next_point = sprinkles.dot(next_point)
yield self.set_position(new).look_at(point).yaw_left(theta_offset)
def circle_point(self, point, radius, granularity=8, theta_offset=0):
'''
Circle a point whilst looking at it
This produces a generator, so for use:
circle = navigator.move.circle_point([1,2,0], 5)
for p in circle:
yield p.go()
'''
point = np.array(point)
angle_incrment = 2 * np.pi / granularity
sprinkles = transformations.euler_matrix(0, 0, angle_incrment)[:3, :3]
# Find first point to go to using boat rotation
next_point = np.append(normalize(self.nav.pose[0][:2] - point[:2]), 0) # Doing this in 2d
for i in range(granularity + 1):
new = point + radius * next_point
yield self.set_position(new).look_at(point).yaw_left(theta_offset) # Remember this is a generator - not a twisted yield
next_point = sprinkles.dot(next_point)
yield self.set_position(new).look_at(point).yaw_left(theta_offset)
def search_IK(self, pose, q_guess, sigma=0.6, sample_size=10,
search_penalties=[10, 10, 8, 5, 2, 2, 1]):
q_samples = []
q_diffs = []
for i in range(sample_size):
if i != 0:
rot_vals = np.random.normal(0, sigma, 3)
else:
rot_vals = [0]*3
rot_vals = np.clip(rot_vals, -np.pi, np.pi)
rand_rot = tf_trans.euler_matrix(*rot_vals)
new_pose = pose.copy()
new_pose[:3,:3] = rand_rot[:3,:3] * new_pose[:3,:3]
cur_time = rospy.Time.now().to_sec()
q_sample = self.IK(new_pose, q_guess)
if q_sample is None:
continue
q_samples.append(q_sample)
q_diffs.append(np.sum(np.array(search_penalties) *
self.angle_difference(q_guess, q_sample)))
if len(q_samples) == 0:
return None
return q_samples[np.argmin(q_diffs)]
def main():
rospy.init_node("arm_cart_vel_control")
if True:
ctrl_switcher = ControllerSwitcher()
ctrl_switcher.carefree_switch('r', '%s_arm_controller',
'$(find hrl_pr2_arms)/params/joint_traj_params_electric.yaml')
rospy.sleep(0.5)
ctrl_switcher.carefree_switch('r', '%s_joint_controller_low',
'$(find hrl_pr2_arms)/params/joint_traj_params_electric_low.yaml')
r_arm_js = create_pr2_arm('r', PR2ArmJointTrajectory, controller_name='%s_joint_controller_low')
q = [-0.34781704, 0.27341079, -1.75392154, -2.08626393, -3.43756314, -1.82146607, -1.85187734]
r_arm_js.set_ep(q, 3)
rospy.sleep(6)
ctrl_switcher.carefree_switch('r', '%s_cart_low_rfh',
'$(find kelsey_sandbox)/params/j_transpose_low_rfh.yaml')
r_arm = create_pr2_arm('r', PR2ArmCartesianPostureBase)
r_arm.set_posture()
rospy.sleep(0.2)
vel_ctrl = PR2ArmCartVelocityController(r_arm)
#vel_frame = tf_trans.euler_matrix(0, -np.pi/2, 0)[:3,:3]
#vel_ctrl.move_velocity(velocity_rot=vel_frame, velocity=0.005, is_translation=False)
vel_frame = tf_trans.euler_matrix(0, 0, np.pi/2)[:3,:3]
vel_ctrl.move_velocity(velocity_rot=vel_frame, velocity=0.10, is_translation=False)
def update(self, dt):
# The following model is completely arbitrary and should not be taken to be representative of
# real vehicle performance!
# But, it should be good enough to test out control modes etc.
yaw_cmd, pitch_cmd, roll_cmd, alt_cmd, motor_enable = self.u
cur_yaw, cur_pitch, cur_roll = tft.euler_from_quaternion(self.get_orientation(), 'rzyx')
# Feedback control: attitude angles track inputs exactly; altitude is input to
# PID controller for thrust
if motor_enable:
thrust = self.thrust_cmd
# accelerations due to pitch/roll in body frame:
pitch_clipped, pwas_clipped = clip(pitch_cmd, -50, 50)
roll_clipped, rwas_clipped = clip(roll_cmd, -50, 50)
yaw_cmd = yaw_cmd
else:
thrust = 0.0
yaw_cmd = degrees(cur_yaw)
pitch_clipped = degrees(cur_pitch)
roll_clipped = degrees(cur_roll)
pwas_clipped = rwas_clipped = False
# Propagate dynamics
# Velocity
accel_thrust = thrust*THRUST_SCALING/MASS
if pwas_clipped or rwas_clipped:
rospy.logwarn('Pitch and/or roll clipped! Pre-clip values %f %f' % (pitch_cmd, roll_cmd))
accel_body = np.array([-sin(radians(pitch_clipped))*accel_thrust,
sin(radians(roll_clipped))*accel_thrust,
0.0]) # vertical accel will be added in inertial frame
# rotate body frame accelerations into inertial frame, and add drag and vertical forces:
Ryaw = tft.euler_matrix(cur_yaw, 0, 0, 'rzyx')
accel_inertial_lateral = np.dot(Ryaw[:3,:3], accel_body)
cur_velocity = self.get_velocity()
accel_inertial = accel_inertial_lateral + \
np.array([- LINEAR_DRAG*cur_velocity[0]
- QUADRATIC_DRAG*np.sign(cur_velocity[0])*(cur_velocity[0]**2),
- LINEAR_DRAG*cur_velocity[1]
- QUADRATIC_DRAG*np.sign(cur_velocity[1])*(cur_velocity[1]**2),
GRAVITY - accel_thrust*cos(radians(pitch_clipped))*cos(radians(roll_clipped))])
velocity = cur_velocity + accel_inertial*dt
# Position
position = self.get_position() + velocity*dt
position[2] = min(0, position[2]) # can't go lower than the ground
# Angles
if position[2] == 0.0 and velocity[2] > 0:
# On the ground..
velocity = np.zeros(3)
orientation = tft.quaternion_from_euler(radians(yaw_cmd), 0, 0, 'rzyx')
else:
# assume perfect yaw rate, and pitch and roll tracking:
orientation = tft.quaternion_from_euler(radians(yaw_cmd), radians(pitch_clipped), radians(roll_clipped), 'rzyx')
# Angular rate
# from J. Diebel, "Representing attitude: Euler angles, unit quaternions, and rotation vectors," 2006
q_cur = np.array((orientation[3],) + tuple(orientation[:3])) # convert to wxyz
q_prev = np.array((self.prev_orientation[3],) + tuple(self.prev_orientation[:3])) # convert to wxyz
if dt > EPS:
q_dot = (q_cur - q_prev)/dt
else:
q_dot = np.zeros(4)
Wprime = np.array([[-q_cur[1], q_cur[0], q_cur[3], -q_cur[2]],
[-q_cur[2], -q_cur[3], q_cur[0], q_cur[1]],
[-q_cur[3], q_cur[2], -q_cur[1], q_cur[0]]])
ang_vel_body = 2*np.dot(Wprime,q_dot)
# Assemble new state vector
self.x = np.concatenate([position, velocity, q_cur, ang_vel_body])
# Save for future finite differences
self.prev_orientation = orientation[:] # be sure to copy not reference...
def homo_mat_from_2d(x, y, rot):
mat2d = np.mat(tf_trans.euler_matrix(0, 0, rot))
mat2d[0,3] = x
mat2d[1,3] = y
return mat2d
