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 tag_callback(self, msg_tag):
# Listen for the transform of the tag in the world
avg = PoseAverage.PoseAverage()
for tag in msg_tag.detections:
try:
Tt_w = self.tfbuf.lookup_transform(self.world_frame, "tag_{id}".format(id=tag.id), rospy.Time(), rospy.Duration(1))
Mtbase_w=self.transform_to_matrix(Tt_w.transform)
Mt_tbase = tr.concatenate_matrices(tr.translation_matrix((0,0,0.17)), tr.euler_matrix(0,0,np.pi))
Mt_w = tr.concatenate_matrices(Mtbase_w,Mt_tbase)
Mt_r=self.pose_to_matrix(tag.pose)
Mr_t=np.linalg.inv(Mt_r)
Mr_w=np.dot(Mt_w,Mr_t)
Tr_w = self.matrix_to_transform(Mr_w)
avg.add_pose(Tr_w)
self.publish_sign_highlight(tag.id)
except(tf2_ros.LookupException, tf2_ros.ConnectivityException, tf2_ros.ExtrapolationException) as ex:
rospy.logwarn("Error looking up transform for tag_%s", tag.id)
rospy.logwarn(ex.message)
Tr_w = avg.get_average() # Average of the opinions
# Broadcast the robot transform
if Tr_w is not None:
# Set the z translation, and x and y rotations to 0
Tr_w.translation.z = 0
rot = Tr_w.rotation
rotz=tr.euler_from_quaternion((rot.x, rot.y, rot.z, rot.w))[2]
(rot.x, rot.y, rot.z, rot.w) = tr.quaternion_from_euler(0, 0, rotz)
T = TransformStamped()
T.transform = Tr_w
T.header.frame_id = self.world_frame
T.header.stamp = rospy.Time.now()
T.child_frame_id = self.duckiebot_frame
self.pub_tf.publish(TFMessage([T]))
self.lifetimer = rospy.Time.now()
def space_from_joints(joint_angles):
T01, T12, T23, T34, T4e = direct_kinematics(joint_angles)
T = tfs.concatenate_matrices(T01, T12, T23, T34, T4e)
rz1, ry, rz2 = tfs.euler_from_matrix(T, 'szyz')
trans = tfs.translation_from_matrix(T)
S = np.append(trans, rz2)
return S
def transformation_matrix(rot_angle, rot_dir, trans):
# print "rot_angle: ", rot_angle
# print "rot_dir: ", rot_dir
# print "trans: ", trans
return tfs.concatenate_matrices(
tfs.rotation_matrix(rot_angle, rot_dir),
tfs.translation_matrix(trans))
def space_from_joints_small(joint_angles):
T01, T12, T23, T34, T45, T5e, Tec, Tem = direct_kinematics(joint_angles)
T = tfs.concatenate_matrices(T01, T12, T23, T34, T45, T5e, Tem)
rx, ry, rz = tfs.euler_from_matrix(T, 'sxyz')
trans = tfs.translation_from_matrix(T)
S = np.concatenate((trans, [rz, ry]), axis=1)
return S
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 flipOrbToNDT (orbPose):
qOrb = [orbPose.qx, orbPose.qy, orbPose.qz, orbPose.qw]
orbFlip = trafo.concatenate_matrices(
trafo.quaternion_matrix(qOrb),
trafo.rotation_matrix(np.pi/2, (1,0,0)),
trafo.rotation_matrix(np.pi/2, (0,0,1))
)
return trafo.quaternion_from_matrix(orbFlip)
def matrix_from_StampedTransform(msg):
T = msg.transform
trans = [T.translation.x, T.translation.y, T.translation.z]
quat = [T.rotation.x, T.rotation.y, T.rotation.z, T.rotation.w]
return tfs.concatenate_matrices(
tfs.translation_matrix(trans),
tfs.quaternion_matrix(quat))
def _makeGraspPath(self, preGraspGoal):
'''Given a pregrasp MoveArmGoal message, generate a MoveArmGoal message to perform the final
approach to the object to grasp it.'''
graspGoal = MoveArmGoal()
graspGoal.planner_service_name = self.plannerServiceName
motion_plan_request = MotionPlanRequest()
motion_plan_request.group_name = self.armGroupName
motion_plan_request.num_planning_attempts = 25
motion_plan_request.planner_id = ""
motion_plan_request.allowed_planning_time = rospy.Duration(5,0)
#Orientation constraint is the same as for pregrasp
motion_plan_request.goal_constraints.orientation_constraints = copy.deepcopy(preGraspGoal.motion_plan_request.goal_constraints.orientation_constraints)
#motion_plan_request.goal_constraints.orientation_constraints[0].orientation = Quaternion(0.656778, 0.261999, 0.648401, -0.282093) #stow orientation for debug
graspGoal.motion_plan_request = motion_plan_request
#Translate from pregrasp position to final position in a roughly straight line
o = motion_plan_request.goal_constraints.orientation_constraints[0].orientation
p = preGraspGoal.motion_plan_request.goal_constraints.position_constraints[0].position
preGraspMat = transformations.quaternion_matrix([o.x,o.y,o.z,o.w])
preGraspMat[:3, 3] = [p.x,p.y,p.z]
distance = self.preGraspDistance * .5 + self.gripperFingerLength * .5
graspTransMat = transformations.translation_matrix([0,0,distance])
graspMat = transformations.concatenate_matrices(preGraspMat, graspTransMat)
#print preGraspMat
#print graspTransMat
#print graspMat
p = transformations.translation_from_matrix(graspMat)
#Publish grasp transform for visualization
self._tf_broadcaster.sendTransform(
(p[0],p[1],p[2]),
(o.x, o.y, o.x, o.w),
motion_plan_request.goal_constraints.orientation_constraints[0].header.stamp,
"grasp",
motion_plan_request.goal_constraints.orientation_constraints[0].header.frame_id)
pos_constraint = PositionConstraint()
pos_constraint.header = motion_plan_request.goal_constraints.orientation_constraints[0].header
pos_constraint.link_name = self.toolLinkName
pos_constraint.position = Point(p[0],p[1],p[2])
#pos_constraint.position = Point(-0.0644721, 0.609922, 0) #Stow position for debug
pos_constraint.constraint_region_shape.type = Shape.SPHERE
pos_constraint.constraint_region_shape.dimensions = [0.01]
pos_constraint.weight = 1
motion_plan_request.goal_constraints.position_constraints.append(pos_constraint)
#TODO: Add path constraint to require a (roughly) cartesian move
#Turn off collision operations between the gripper and all objects
for collisionName in self.gripperCollisionNames:
collisionOperation = CollisionOperation(collisionName,
CollisionOperation.COLLISION_SET_ALL,
0.0,
CollisionOperation.DISABLE)
graspGoal.operations.collision_operations.append(collisionOperation)
return graspGoal
def get_trans_to_trans(trans1, rot1, trans2, rot2):
quat_mat1 = transformations.quaternion_matrix(rot1)
quat_mat2 = transformations.quaternion_matrix(rot2)
trans_mat1 = transformations.translation_matrix(trans1)
trans_mat2 = transformations.translation_matrix(trans2)
mat1 = transformations.concatenate_matrices(trans_mat1, quat_mat1)
mat2 = transformations.concatenate_matrices(trans_mat2, quat_mat2)
rel_mat = numpy.linalg.inv(mat1).dot(mat2)
rel_trans = transformations.translation_from_matrix(rel_mat)
rel_rot = transformations.quaternion_from_matrix(rel_mat)
rel_rot = [rel_rot[3], rel_rot[0], rel_rot[1], rel_rot[2]]
print "rel_pose: [%f, %f, %f, %f, %f, %f, %f]" % (
rel_trans[0],
rel_trans[1],
rel_trans[2],
rel_rot[0],
rel_rot[1],
rel_rot[2],
rel_rot[3],
)
def _makeMove(self):
'''Move forward a small distance'''
print "Making move goal"
graspGoal = MoveArmGoal()
graspGoal.planner_service_name = self.plannerServiceName
motion_plan_request = MotionPlanRequest()
motion_plan_request.group_name = self.armGroupName
motion_plan_request.num_planning_attempts = 5
motion_plan_request.planner_id = ""
motion_plan_request.allowed_planning_time = rospy.Duration(5,0)
#Orientation constraint is the same as for previous
#Create Orientation constraint object
o_constraint = OrientationConstraint()
o_constraint.header.frame_id = self.frameID
o_constraint.header.stamp = rospy.Time.now()
o_constraint.link_name = self.toolLinkName
o_constraint.orientation = Quaternion(0.656788, 0.261971, 0.648416, -0.282059)
o_constraint.absolute_roll_tolerance = 0.04
o_constraint.absolute_pitch_tolerance = 0.04
o_constraint.absolute_yaw_tolerance = 0.04
o_constraint.weight = 1
motion_plan_request.goal_constraints.orientation_constraints.append(o_constraint)
#Translate from pregrasp position to final position in a roughly straight line
o = o_constraint.orientation
p = Point(-0.064433, 0.609915, 0)
preGraspMat = transformations.quaternion_matrix([o.x,o.y,o.z,o.w])
preGraspMat[:3, 3] = [p.x,p.y,p.z]
distance = .3#self.preGraspDistance + self.gripperFingerLength/2
graspTransMat = transformations.translation_matrix([0,0,distance])
graspMat = transformations.concatenate_matrices(preGraspMat, graspTransMat)
#print preGraspMat
#print graspTransMat
#print graspMat
p = transformations.translation_from_matrix(graspMat)
#Publish grasp transform for visualization
self._tf_broadcaster.sendTransform(
(p[0],p[1],p[2]),
(o.x, o.y, o.x, o.w),
o_constraint.header.stamp,
"grasp",
o_constraint.header.frame_id)
pos_constraint = PositionConstraint()
pos_constraint.header = o_constraint.header
pos_constraint.link_name = self.toolLinkName
pos_constraint.position = Point(p[0],p[1],p[2])
pos_constraint.constraint_region_shape.type = Shape.SPHERE
pos_constraint.constraint_region_shape.dimensions = [0.01]#[0.01, 0.01, 0.01]
pos_constraint.weight = 1
motion_plan_request.goal_constraints.position_constraints.append(pos_constraint)
graspGoal.motion_plan_request = motion_plan_request
return graspGoal
def convert_pose_inverse_transform(self, pose):
""" This is a helper method to invert a transform (this is built into
the tf C++ classes, but ommitted from Python) """
transform = t.concatenate_matrices(
t.translation_matrix(
[pose.position.x, pose.position.y, pose.position.z]),
t.quaternion_matrix([
pose.orientation.x, pose.orientation.y, pose.orientation.z,
pose.orientation.w
]))
inverse_transform_matrix = t.inverse_matrix(transform)
return (t.translation_from_matrix(inverse_transform_matrix),
t.quaternion_from_matrix(inverse_transform_matrix))
def update_real_const_pose(self):
if self.real_pose_mat is None:
#rospy.logwarn("Real pose of '{}' is not detected!".format(self.referencePart))
pass
else:
for const_name in self.assemConsts.keys():
const = self.assemConsts[const_name]
const_end_pose = const["endCoordinate"]
real_const_end_pose = tf.concatenate_matrices(
self.real_pose_mat, const_end_pose)
const["real_endCoordinate"] = real_const_end_pose
const_quat = tf.quaternion_from_matrix(real_const_end_pose)
const["real_zAxis"] = utils.get_transformed_zAxis(const_quat)
def odomVisualPublisher(v, th, time):
global last_time, voX, voY, voT
current_time = time
dt = (current_time.to_sec() - last_time.to_sec())
voX += v * math.cos(voT) * dt
voY += v * math.sin(voT) * dt
voT += th * dt
msg = Odometry()
msg.header.stamp = current_time
msg.pose.pose.position = Point(voX, voY, 0.0)
tx, ty, tz, tw = tf.transformations.quaternion_from_euler(0.0, 0.0, voT)
msg.pose.pose.orientation = Quaternion(tx, ty, tz, tw)
pub1.publish(msg)
pos = msg.pose.pose
inversed_transform = t.concatenate_matrices(
t.translation_matrix((pos.position.x, pos.position.y, pos.position.z)),
t.quaternion_matrix((pos.orientation.x, pos.orientation.y,
pos.orientation.z, pos.orientation.w)))
inversed_transform = t.inverse_matrix(inversed_transform)
(position1,
orientation1) = tfListener.lookupTransform("/odom", "/base_footprint",
rospy.Time(0))
inversed_transform_1 = t.concatenate_matrices(
t.translation_matrix(position1), t.quaternion_matrix(orientation1))
inversed_transform_1 = t.inverse_matrix(inversed_transform_1)
multiply = np.dot(inversed_transform, inversed_transform_1)
tran = t.translation_from_matrix(multiply)
rot = t.quaternion_from_matrix(multiply)
antena.sendTransform(tran, rot, current_time, 'odom_visual',
'base_footprint')
last_time = current_time
def get_object_pose_as_matrix(object_name):
global object_poses
obj = object_poses[object_name]
obj_trans = (obj.pose.pose.pose.position.x, obj.pose.pose.pose.position.y, obj.pose.pose.pose.position.z)
obj_quat = (
obj.pose.pose.pose.orientation.x,
obj.pose.pose.pose.orientation.y,
obj.pose.pose.pose.orientation.z,
obj.pose.pose.pose.orientation.w,
)
obj_quat_mat = transformations.quaternion_matrix(obj_quat)
obj_trans_mat = transformations.translation_matrix(obj_trans)
obj_mat = transformations.concatenate_matrices(obj_trans_mat, obj_quat_mat)
return obj_mat
def speed_bump_y(point):
# Publish a rotated plane using a numpy transform matrix
radian = 0.523599 # 30도
sin = 0.5
cos = 0.866
depth = 0.3
width = 2.0
R_x_1 = transformations.rotation_matrix(radian, (1,0,0)) # Rotate around y-axis by 0.3 radians
T0_1 = transformations.translation_matrix((point.x, point.y-0.5*cos*depth,0.5*sin*depth))
T_1 = transformations.concatenate_matrices(T0_1, R_x_1)
markers.publishPlane(T_1, width, depth, 'yellow', 5.0) # pose, width, depth, color, lifetime
R_x_2 = transformations.rotation_matrix(-1*radian, (1,0,0)) # Rotate around y-axis by 0.3 radians
T0_2 = transformations.translation_matrix((point.x,point.y+ 0.5*cos*depth,0.5*sin*depth))
T_2 = transformations.concatenate_matrices(T0_2, R_x_2)
markers.publishPlane(T_2, width, depth, 'yellow', 5.0) # pose, width, depth, color, lifetime
center = Point(point.x, point.y, sin*depth + 0.1)
P = Pose(center,Quaternion(0,0,0,1))
scale = Vector3(0.15,0.15,0.2)
markers.publishText(P, 'Speed Bump', 'white', scale, 5.0) # pose, text, color, scale, lifetime
def get_twisted_zAxis(self, rot):
initial_zAxis = np.array([0, 0, 1])
if len(rot) == 3:
rot_mat = tf.euler_matrix(rot[0], rot[0], rot[0])
elif len(rot) == 4:
rot_mat = tf.quaternion_matrix(rot)
else:
print("Given rot component's number is wrong!")
return TypeError
initial_tf = tf.translation_matrix(initial_zAxis)
twisted_tf = tf.concatenate_matrices(rot_mat, initial_tf)
twisted_tf[np.abs(twisted_tf)<1e-5] = 0
return twisted_tf[:3, 3]
def odom_cb(self, msg):
stamp = msg.header.stamp
try:
self._tfl.waitForTransform('base_footprint', 'odom', stamp, timeout=self._timeout)
o2b_p, o2b_q = self._tfl.lookupTransform('odom', 'base_footprint', stamp)#'base_footprint', 'odom', stamp)
except tf.Exception as e:
rospy.loginfo('w2o tf error : {}'.format(e))
return
o2b_T = tx.concatenate_matrices(
tx.translation_matrix(o2b_p), tx.quaternion_matrix(o2b_q)
)
o2b_T_i = tx.inverse_matrix(o2b_T)
w2b = msg.pose.pose
w2b_T = pm.toMatrix(pm.fromMsg(w2b))
w2o_T = tx.concatenate_matrices(w2b_T, o2b_T_i)
txn, rxn = pm.toTf(pm.fromMatrix(w2o_T))
self._tfb.sendTransform(txn, rxn, stamp, 'odom', 'world')
def calculate_bridge_and_base_pose(self, table_state, phi):
# Calculate bridge pose from cue position and phi
phi_radians = math.pi * phi / 180.0
cue_x, cue_y = self.get_cue_ball_pos(table_state)
bridge_x, bridge_y = self.get_bridge_pos(cue_x, cue_y, phi)
(qx, qy, qz, qw) = transformations.quaternion_about_axis(phi_radians, (0, 0, 1))
bridge_pos = transformations.translation_matrix([bridge_x, bridge_y, 0.0])
bridge_orient = transformations.quaternion_matrix([qx, qy, qz, qw])
bridge_pose = transformations.concatenate_matrices(bridge_pos, bridge_orient)
bridge_pose_pos = transformations.translation_from_matrix(bridge_pose)
bridge_pose_orient = transformations.quaternion_from_matrix(bridge_pose)
# Produce base pose via fixed transform from bridge pose
bridge_to_base_trans = transformations.translation_matrix([Constants.BRIDGE_IN_BASE_X, Constants.BRIDGE_IN_BASE_Y, Constants.BRIDGE_IN_BASE_Z])
bridge_to_base_rot = transformations.quaternion_matrix([Constants.BRIDGE_IN_BASE_QX, Constants.BRIDGE_IN_BASE_QY, Constants.BRIDGE_IN_BASE_QZ, Constants.BRIDGE_IN_BASE_QW])
bridge_to_base = transformations.concatenate_matrices(bridge_to_base_trans, bridge_to_base_rot)
base_to_bridge = transformations.inverse_matrix(bridge_to_base)
base_pose = transformations.concatenate_matrices(bridge_pose, base_to_bridge) # correct order?
base_pose_pos = transformations.translation_from_matrix(base_pose)
base_pose_orient = transformations.quaternion_from_matrix(base_pose)
print "BASE_POSE", base_pose, "BRIDGE_POSE", bridge_pose
return ((bridge_pose_pos, bridge_pose_orient), (base_pose_pos, base_pose_orient))
def set_relative_pose_object(self,
workspace,
x_rel,
y_rel,
yaw_rel,
yaw_center=None):
"""
Updates the transform base_link -> object_base in local tfBuffer
:param workspace: reference workspace object
:param x_rel: object base x position relative to workspace
:param y_rel: object base y position relative to workspace
:param yaw_rel: object base rotation on z relative to workspace
:param yaw_center: Avoid over rotation
"""
position = np.dot(workspace.position_matrix,
np.array([x_rel, y_rel, 1]))
camera_rotation = transformations.euler_matrix(0, 0, yaw_rel)
# Here we correct the object orientation to be similar to base_link if
# the object in on the ground. Not neccessarily needed to be honest...
convention_rotation = np.array([[0, -1, 0, 0], [-1, 0, 0, 0],
[0, 0, -1, 0], [0, 0, 0, 1]])
object_rotation = transformations.concatenate_matrices(
workspace.rotation_matrix, camera_rotation, convention_rotation)
roll, pitch, yaw = transformations.euler_from_matrix(object_rotation)
# Correcting yaw to avoid out of reach targets
if yaw_center is not None:
if yaw < yaw_center - np.pi / 2:
yaw += np.pi
elif yaw > yaw_center + np.pi / 2:
yaw -= np.pi
q = transformations.quaternion_from_euler(roll, pitch, yaw)
t = TransformStamped()
t.transform.translation.x = position[0]
t.transform.translation.y = position[1]
t.transform.translation.z = position[2]
t.transform.rotation.x = q[0]
t.transform.rotation.y = q[1]
t.transform.rotation.z = q[2]
t.transform.rotation.w = q[3]
t.header.frame_id = "base_link"
t.child_frame_id = "object_base"
self.__tf_buffer.set_transform(t, "default_authority")
def main():
#quaternion = numpy.empty((4, ), dtype=numpy.float64)
print qx
quaternion = [qx, qy, qz, qw]
euler = t.euler_from_quaternion(t.quaternion_inverse(quaternion))
transform = t.concatenate_matrices(t.translation_matrix([-x, -y, -z]),
t.quaternion_matrix(quaternion))
inversed_transform = t.inverse_matrix(transform)
print "Inversed tf: "
print inversed_transform
print "HOW TO GO ON with that????? Just get the simple transform which might be wrong"
print "Trans (m): "
print[-x, -y, -z]
print "Rot (rad): "
print euler
def updateCoilPoseFromTF():
global transListener, leftCoilPose
now = rospy.Time.now()
# Change left coil position into world position
try:
transListener.waitForTransform('minefield', 'left_coil', now, rospy.Duration(3.0))
(trans,rot) = transListener.lookupTransform('minefield', 'left_coil', now)
except:
return
tr = transformations.concatenate_matrices(transformations.translation_matrix(trans), transformations.quaternion_matrix(rot))
leftCoilPose = PoseStamped()
leftCoilPose.pose = pose_msg_from_matrix(tr)
def effector_target_pose(self, target_pose, offset):
T = tfs.concatenate_matrices(
self.listener.fromTranslationRotation(
(target_pose.pose.position.x, target_pose.pose.position.y,
target_pose.pose.position.z),
(target_pose.pose.orientation.x,
target_pose.pose.orientation.y,
target_pose.pose.orientation.z,
target_pose.pose.orientation.w)),
self.listener.fromTranslationRotation(
offset, tfs.quaternion_from_euler(0, radians(90), 0)))
pose = gmsg.PoseStamped()
pose.header.frame_id = target_pose.header.frame_id
pose.pose = gmsg.Pose(gmsg.Point(*tfs.translation_from_matrix(T)),
gmsg.Quaternion(*tfs.quaternion_from_matrix(T)))
return pose
def updateRobotPose():
global robotPose
# This function does not get the true robot pose, but only the pose of 'base_link' in the TF
# you should replace it by the robot pose resulting from a good localization process
robotPose = PoseStamped()
now = rospy.Time.now()
# Get left coil position in relation to robot
try:
transListener.waitForTransform('minefield', 'base_link', now, rospy.Duration(2.0))
(trans,rot) = transListener.lookupTransform('minefield', 'base_link', now)
except:
return
tr2 = transformations.concatenate_matrices(transformations.translation_matrix(trans), transformations.quaternion_matrix(rot))
robotPose.pose = pose_msg_from_matrix(tr2)
def send_reset_to_rovio(self):
rospy.wait_for_service('rovio/reset_to_pose')
try:
rovio_reset_srv = rospy.ServiceProxy('rovio/reset_to_pose',
SrvResetToPose)
# compute pose from local ENU (East-North-Up frame) to
# IMU frame of the ViSensor (== C frame, according to MSF)
T_Enu_C = tf.concatenate_matrices(self._T_Enu_I, self._T_I_C)
q_Enu_C = tf.quaternion_from_matrix(T_Enu_C)
# set new Sensor IMU position and orientation respect to World frame
# (which is now aligned to local ENU)
self._pose_world_imu_msg.position.x = T_Enu_C[0, 3]
self._pose_world_imu_msg.position.y = T_Enu_C[1, 3]
self._pose_world_imu_msg.position.z = T_Enu_C[2, 3]
self._pose_world_imu_msg.orientation.w = q_Enu_C[3]
self._pose_world_imu_msg.orientation.x = q_Enu_C[0]
self._pose_world_imu_msg.orientation.y = q_Enu_C[1]
self._pose_world_imu_msg.orientation.z = q_Enu_C[2]
rovio_reset_srv(self._pose_world_imu_msg)
if self._verbose:
q_Enu_I = tf.quaternion_from_matrix(self._T_Enu_I)
(yaw, pitch, roll) = tf.euler_from_quaternion(q_Enu_I, 'rzyx')
rospy.loginfo(rospy.get_name() +
": body frame of MAV assumed with " +
str(math.degrees(roll)) + " (deg) roll, " +
str(math.degrees(pitch)) + " (deg) pitch, " +
str(math.degrees(yaw)) +
" (deg) yaw from local ENU (local axis, ZYX)")
self.create_rovio_init_info()
success = True
message = "Service call to reset Rovio internal state sent"
return (success, message)
except rospy.ServiceException, e:
success = False
message = "Service call to reset Rovio internal state failed: %s" % e
return (success, message)
def get_inverse_trans_rot(pose):
# Car transform
transT_car = (pose.pose.position.x, pose.pose.position.y,
pose.pose.position.z)
rotT_car = (pose.pose.orientation.x, pose.pose.orientation.y,
pose.pose.orientation.z, pose.pose.orientation.w)
# World transform
transform = t.concatenate_matrices(t.translation_matrix(transT_car),
t.quaternion_matrix(rotT_car))
inversed_transform = t.inverse_matrix(transform)
translation = t.translation_from_matrix(inversed_transform)
quaternion = t.quaternion_from_matrix(inversed_transform)
# rospy.loginfo('transT_world = {}'.format(translation))
# rospy.loginfo('rotT_world = {}'.format(quaternion))
# transT = translation
# rotT = quaternion
return translation, quaternion
def callback(msg): global distance_z,distance_y,marker_yaw,flag,count markerID = 'marker_'+str(msg.id) try: (trans,rot) = listener.lookupTransform( markerID, 'camera', rospy.Time(0)) transform = t.concatenate_matrices(t.translation_matrix(trans), t.quaternion_matrix(rot)) inv_tf = t.inverse_matrix(transform) distance_z = trans[2] distance_y = trans[1] marker_yaw = math.atan(inv_tf[0][2]/inv_tf[2][2]) camera2base() if(distance_z<0.75): if(flag==1): if( msg.id <= 214 and msg.id>= 200): for i in database: if(i[0] == msg.id): substance = i[1] break else: substance = 'OBSTACLE' prev_site = (soi_visited[-1][1],soi_visited[-1][2]) for obj in SOI: if(obj[0]==msg.id): serial_no = SOI.index(obj) current_site = (obj[1],obj[2]) soi_visited.append(obj) break if(msg.id==219): msg.id = 189 print str(serial_no)+':'+str(prev_site)+':'+str(current_site)+':'+str(msg.id)+':'+substance count+=1 if(count==5): print 'Mission Accomplished' pub.publish() flag = 0 localize(msg.id) except (tf.LookupException, tf.ConnectivityException, tf.ExtrapolationException): pass
def read_aruco_transformation():
with open(
'/home/roya/catkin_ws/src/pam_manipulation_planning/config/aruco_camera.yaml'
) as f:
aruco = yaml.safe_load(f)
kinect_T_aruco_rot = quaternion_matrix([
aruco["kinect_orientation_x"], aruco["kinect_orientation_y"],
aruco["kinect_orientation_z"], aruco["kinect_orientation_w"]
])
kinect_T_aruco_trans = translation_matrix([
aruco["kinect_position_x"], aruco["kinect_position_y"],
aruco["kinect_position_z"]
])
transform = t.concatenate_matrices(kinect_T_aruco_trans,
kinect_T_aruco_rot)
return transform
def updateCoilPoseFromTF():
global transListener, leftCoilPose
now = rospy.Time.now()
# Change left coil position into world position
try:
transListener.waitForTransform('minefield', 'left_coil', now,
rospy.Duration(3.0))
(trans, rot) = transListener.lookupTransform('minefield', 'left_coil',
now)
except:
return
tr = transformations.concatenate_matrices(
transformations.translation_matrix(trans),
transformations.quaternion_matrix(rot))
leftCoilPose = PoseStamped()
leftCoilPose.pose = pose_msg_from_matrix(tr)
def updateRobotPose():
global robotPose
# This function does not get the true robot pose, but only the pose of 'base_link' in the TF
# you should replace it by the robot pose resulting from a good localization process
robotPose = PoseStamped()
now = rospy.Time.now()
# Get left coil position in relation to robot
try:
transListener.waitForTransform('minefield', 'base_link', now,
rospy.Duration(2.0))
(trans, rot) = transListener.lookupTransform('minefield', 'base_link',
now)
except:
return
tr2 = transformations.concatenate_matrices(
transformations.translation_matrix(trans),
transformations.quaternion_matrix(rot))
robotPose.pose = pose_msg_from_matrix(tr2)
def updateCoilPoseManually(referencePose):
global transListener, leftCoilPose
now = rospy.Time.now()
# Get left coil pose in relation to robot
try:
transListener.waitForTransform('base_link', 'left_coil', now, rospy.Duration(2.0))
(trans,rot) = transListener.lookupTransform('base_link', 'left_coil', now)
except:
return
localCoil_Mat = transformations.concatenate_matrices(transformations.translation_matrix(trans), transformations.quaternion_matrix(rot))
# Use reference robot pose
robot_Mat = matrix_from_pose_msg(referencePose)
# Compute corrected coil pose
corrected_Mat = np.dot(robot_Mat, localCoil_Mat)
leftCoilPose = PoseStamped()
leftCoilPose.pose = pose_msg_from_matrix(corrected_Mat)
def callback(msg):
global distance_z, distance_y, marker_yaw, localized, Marker_ID
markerID = 'marker_' + str(msg.id)
try:
(trans, rot) = listener.lookupTransform(markerID, 'camera',
rospy.Time(0))
transform = t.concatenate_matrices(t.translation_matrix(trans),
t.quaternion_matrix(rot))
inv_tf = t.inverse_matrix(transform)
distance_z = trans[2]
distance_y = trans[1]
marker_yaw = math.atan(inv_tf[0][2] / inv_tf[2][2])
#convert readings to base_link
camera2base()
#If the detected marker is within 50 cm of the robot then it will do three things:
# 1. Localize
# 2. Tell the node goal_pt to cancel the goal because ArUco is visible from here.
# eyes.publish() sends an Empty ROS message to do this.
# 3. Finally it will display the output.
# The step 2 is carried out only if the ArUco was not previously detected and the ID has been added to SOI list.
# This is done by the if statement checking the variable 'localized'. It is set to 1 in the localize() function
# if ID has been successfully added to the SOI list.
if (distance_z < 0.5):
localize(msg.id)
if (not visited.has_key(msg.id)) and localized:
eyes.publish()
localized = 0
Marker_ID = msg.id
print Marker_ID
display_output(msg.id)
except (tf.LookupException, tf.ConnectivityException,
tf.ExtrapolationException):
pass
def find_error_to_target(self):
## finding the difference in distance between target point and drone using transformations
try:
(trans_drone, rot_drone) = self.listener.lookupTransform(
'/end_effector', '/camera_optical', rospy.Time(0))
x, y = self.pixel_to_meter()
print("x", x)
print("y", y)
trans_target = [x, y, self.trans[2]]
rot_target = [0.11320321, 0, 0, 0.99357186]
euler_target = euler_from_quaternion(rot_target)
euler_drone = euler_from_quaternion(rot_drone)
matrix_target = compose_matrix(None, None, euler_target,
trans_target, None)
matrix_drone = compose_matrix(None, None, euler_drone, trans_drone,
None)
matrix_result = concatenate_matrices(matrix_drone, matrix_target)
trans_result = translation_from_matrix(matrix_result)
rot_result = quaternion_from_matrix(matrix_result)
roll_result, pitch_result, yaw_result = euler_from_quaternion(
rot_result)
#for the .csv file
self.array_x.append(trans_result[0])
self.array_y.append(trans_result[1])
self.array_z.append(trans_result[2])
self.img_x.append(self.trans[0] + C_MID[0])
self.img_y.append(self.trans[1] + C_MID[1])
return True, trans_result
except (tf.LookupException, tf.ConnectivityException,
tf.ExtrapolationException) as e:
print(e)
return False, None
def attach_obj(self, move_obj, rel_tr, rel_quat, subassembly_name):
self.attach_cm(move_obj.cm, move_obj.assem_mass, rel_tr, rel_quat)
self.attach_consts(move_obj.assemConsts, rel_tr, rel_quat)
self.attach_dict(move_obj.assemDict, rel_tr, rel_quat, move_obj.referencePart)
self.assemXML = xmltodict.unparse(self.assemDict)
self.assem_mass += move_obj.assem_mass
rel_tf = utils.get_tf_matrix(rel_tr, rel_quat)
for comp_name, comp in move_obj.components.items():
comp_tr = comp["tr"]
comp_quat = comp["quat"]
comp_mass = comp["mass"]
comp_tf = utils.get_tf_matrix(comp_tr, comp_quat)
new_tf = tf.concatenate_matrices(rel_tf,comp_tf)
new_tr = tf.translation_from_matrix(new_tf)
new_quat = tf.quaternion_from_matrix(new_tf)
self.components[comp_name] = {"tr": new_tr, "quat": new_quat, "mass": comp_mass}
self.subassem_components[self.obj_name]["mass"] = self.assem_mass
self.subassem_components[move_obj.assem_name] = {"tr": rel_tr, "quat": rel_quat, "mass": move_obj.assem_mass}
self.assem_name = subassembly_name
def get_transform_wrt_odom_frame(self):
try:
(trans,
rot) = self.transformer.lookupTransform("map", "odom",
rospy.Time(0.2))
except:
return None
cur_pose = self.get_cur_ros_pose()
transform = tftrans.concatenate_matrices(
tftrans.translation_matrix(trans), tftrans.quaternion_matrix(rot))
inversed_transform = tftrans.inverse_matrix(transform)
inv_translation = tftrans.translation_from_matrix(inversed_transform)
inv_quaternion = tftrans.quaternion_from_matrix(inversed_transform)
transformStamped = geometry_msgs.msg.TransformStamped()
transformStamped.transform.translation.x = inv_translation[0]
transformStamped.transform.translation.y = inv_translation[1]
transformStamped.transform.translation.z = inv_translation[2]
transformStamped.transform.rotation.x = inv_quaternion[0]
transformStamped.transform.rotation.y = inv_quaternion[1]
transformStamped.transform.rotation.z = inv_quaternion[2]
transformStamped.transform.rotation.w = inv_quaternion[3]
cur_transform_wrt_odom = tf2_geometry_msgs.do_transform_pose(
cur_pose, transformStamped)
translation = cur_transform_wrt_odom.pose.position
quaternion = (cur_transform_wrt_odom.pose.orientation.x,
cur_transform_wrt_odom.pose.orientation.y,
cur_transform_wrt_odom.pose.orientation.z,
cur_transform_wrt_odom.pose.orientation.w)
_, _, yaw = tf.transformations.euler_from_quaternion(quaternion)
return translation, yaw
def sigmoid(p, vertices, gt_rebased):
# Get the current parameter set and build the transform
roll, pitch, yaw = p
q = quaternion_from_rpy(roll, pitch, yaw)
cur_delta = to_transform([0.0, 0.0, 0.0, 0.0, q[0], q[1], q[2], q[3]])
# Compute the quadratic error for the current delta transformation
err = 0.0
for i in range(len(vertices)):
# Compute the corrected ground truth
tf_gt = to_transform(gt_rebased[i])
tf_gt_t = tf.translation_from_matrix(tf_gt)
tf_gt_t = np.array([tf_gt_t[0], tf_gt_t[1], tf_gt_t[2], 1.0])
tf_gt_corrected = tf.concatenate_matrices(cur_delta, tf_gt_t)
tf_gt_corr_vect = [
0.0, tf_gt_corrected[0], tf_gt_corrected[1], tf_gt_corrected[2],
0.0, 0.0, 0.0, 1.0
]
# Compute the error
err += np.power(calc_dist(vertices[i], tf_gt_corr_vect), 2)
return np.sqrt(err)
def _callback(self, fiducial_transforms):
header = fiducial_transforms.header
child_frame_id = self._frame_id
if self._invert:
child_frame_id = header.frame_id
header.frame_id = self._frame_id
obj = filter(lambda a: a.fiducial_id == self._fiducial_id,
fiducial_transforms.transforms)
if len(obj) > 0:
obj = obj[0]
if self._invert:
trans = obj.transform.translation
rot = obj.transform.rotation
trans = [trans.x, trans.y, trans.z]
rot = [rot.x, rot.y, rot.z, rot.w]
m_trans = t.translation_matrix(trans)
m_rot = t.quaternion_matrix(rot)
transform = t.concatenate_matrices(m_trans, m_rot)
inverse_transform = t.inverse_matrix(transform)
trans = t.translation_from_matrix(inverse_transform)
rot = t.quaternion_from_matrix(inverse_transform)
obj.transform.translation = Vector3(x=trans[0],
y=trans[1],
z=trans[2])
obj.transform.rotation = Quaternion(x=rot[0],
y=rot[1],
z=rot[2],
w=rot[3])
self._tf_brd.sendTransformMessage(
TransformStamped(header=header,
child_frame_id=child_frame_id,
transform=obj.transform))
def execute(self, userdata):
target_pos_from_orig_pos_local_frame = tft.translation_matrix(
(self.grid[userdata.search_count][0] * self.grid_scale_x,
self.grid[userdata.search_count][1] * self.grid_scale_y, 0.0))
grasp_yaw_rotation_mat = tft.euler_matrix(0, 0, self.grasping_yaw)
uav_target_coords = tft.concatenate_matrices(
userdata.orig_global_trans, grasp_yaw_rotation_mat,
target_pos_from_orig_pos_local_frame)
uav_target_pos = tft.translation_from_matrix(uav_target_coords)
self.robot.goPosWaitConvergence('global',
[uav_target_pos[0], uav_target_pos[1]],
self.robot.getTargetZ(),
self.robot.getTargetYaw(),
pos_conv_thresh=0.2,
yaw_conv_thresh=0.1,
vel_conv_thresh=0.1,
timeout=self.grid_search_timeout)
userdata.search_count += 1
if userdata.search_count == len(self.grid):
userdata.search_count = 0
userdata.search_failed = True
orig_pos = tft.translation_from_matrix(userdata.orig_global_trans)
rospy.logwarn(self.__class__.__name__ + ": return to original pos")
self.robot.goPosWaitConvergence('global',
[orig_pos[0], orig_pos[1]],
self.robot.getTargetZ(),
self.robot.getTargetYaw(),
pos_conv_thresh=0.1,
yaw_conv_thresh=0.1,
vel_conv_thresh=0.1,
timeout=7)
return 'succeeded'
def matrix_from_trans_rot(trans, rot):
trans_mat = transformations.translation_matrix(trans)
rot_mat = transformations.quaternion_matrix(rot)
return transformations.concatenate_matrices(trans_mat, rot_mat)
def matrix_from_pose_msg(pose):
t = transformations.translation_matrix((pose.position.x, pose.position.y, pose.position.z))
q = [pose.orientation.x, pose.orientation.y, pose.orientation.z, pose.orientation.w]
r = transformations.quaternion_matrix(q)
return transformations.concatenate_matrices(t, r)
def __init__(self, agent_topic_names=['ridgeback2', 'ridgeback1']):
print("")
print("Initializiation starting.\n")
rospy.init_node('transform_publisher', anonymous=True)
self.n_agent_topics = len(agent_topic_names)
self.awaiting_msg_agent = [True] * self.n_agent_topics
self.data_agent = [None] * self.n_agent_topics
for ii in range(self.n_agent_topics):
rospy.Subscriber(
"/vrpn_client_node/" + agent_topic_names[ii] + "/pose",
PoseStamped, self.callback_agent_optitrack, ii)
# Use ridgeback-time stamp for messages
self.ridgeback_stamp = None
self.awaiting_msg_imu = True
rospy.Subscriber("/imu/data", Imu, self.callback_imu) # Get timestamp
self.tf_listener = tf.TransformListener()
self.tf_broadcast = tf2.TransformBroadcaster()
# START LOOP
while ((self.awaiting_msg_imu or np.sum(self.awaiting_msg_agent))
and not rospy.is_shutdown()):
# import pdb; pdb.set_trace() # BREAKPOINT
if self.awaiting_msg_imu:
print("Waiting for imu-stamp message ...")
if np.sum(self.awaiting_msg_agent):
print("Waiting for robot-message ...")
rospy.sleep(0.25)
rate = rospy.Rate(100) # Hz
print("Entering main loop....")
while not rospy.is_shutdown():
# lock.acquire()
use_ridgeback_stamp = True
if use_ridgeback_stamp:
time_stamp = self.ridgeback_stamp
else:
print(
"Warning: A time synchronization problem might occur due to clock asynchronization"
)
print("we advice using the recorded ridgeback time.")
try:
(self.trans_odom2base,
self.rot_odom2base) = self.tf_listener.lookupTransform(
'/base_link', '/odom', rospy.Time(0))
except (tf.LookupException, tf.ConnectivityException,
tf.ExtrapolationException):
continue
transform = trafo.concatenate_matrices(
trafo.translation_matrix(self.trans_odom2base),
trafo.quaternion_matrix(self.rot_odom2base))
inversed_transform = trafo.inverse_matrix(transform)
# TODO inverse in other sence to have optitrack as 'master'
trafo_odom2world_child = TransformStamped()
# trafo_odom2world_child.header.stamp = rospy.Time.now()
trafo_odom2world_child.header.stamp = time_stamp
trafo_odom2world_child.header.frame_id = "base_link_clone"
trafo_odom2world_child.child_frame_id = "odom"
trafo_odom2world_child.transform.translation.x = self.trans_odom2base[
0]
trafo_odom2world_child.transform.translation.y = self.trans_odom2base[
1]
trafo_odom2world_child.transform.translation.z = self.trans_odom2base[
2]
trafo_odom2world_child.transform.rotation.x = self.rot_odom2base[0]
trafo_odom2world_child.transform.rotation.y = self.rot_odom2base[1]
trafo_odom2world_child.transform.rotation.z = self.rot_odom2base[2]
trafo_odom2world_child.transform.rotation.w = self.rot_odom2base[3]
self.tf_broadcast.sendTransform(trafo_odom2world_child)
# Optitrack Data - Smoothened / quaternion is calculated throuh multiple markers
marker_positions = np.zeros((3, self.n_agent_topics))
for ii in range(self.n_agent_topics):
marker_positions[:, ii] = [
self.data_agent[ii].pose.position.x,
self.data_agent[ii].pose.position.y,
self.data_agent[ii].pose.position.z
]
center_position = np.sum(marker_positions,
axis=1) / self.n_agent_topics
if self.n_agent_topics == 1:
quat = [
self.data_agent[0].pose.orientation.w,
self.data_agent[0].pose.orientation.x,
self.data_agent[0].pose.orientation.y,
self.data_agent.pose.orientation.z
]
elif self.n_agent_topics == 2:
y_dir_ridgeback = marker_positions[:, 1] - marker_positions[:,
0]
y_dir_ridgeback[2] = 0 # remove z deviation
# x_direction = np.cross(y_direction, [0, 0, 1])
y_dir_init = np.array([0, 1, 0])
quat = quaternion_from_2vectors(y_dir_init, y_dir_ridgeback)
else:
raise NotImplementedError()
delta_dist = [0.395, 0.06, 0]
delta_dist = quat_vec_multiplication(quat, delta_dist)
trafo_optitrack2base_link_clone = TransformStamped()
trafo_optitrack2base_link_clone.header.stamp = time_stamp # Time delay of clocks - use ridgeback-message stamp to bridge this
# trafo_optitrack2base_link_clone.header.stamp = rospy.Time.now()
trafo_optitrack2base_link_clone.header.frame_id = "world_optitrack"
trafo_optitrack2base_link_clone.child_frame_id = "base_link_clone"
trafo_optitrack2base_link_clone.transform.translation.x = center_position[
0] + delta_dist[0]
trafo_optitrack2base_link_clone.transform.translation.y = center_position[
1] + delta_dist[1]
trafo_optitrack2base_link_clone.transform.translation.z = 0
trafo_optitrack2base_link_clone.transform.rotation.w = quat[0]
trafo_optitrack2base_link_clone.transform.rotation.x = quat[1]
trafo_optitrack2base_link_clone.transform.rotation.y = quat[2]
trafo_optitrack2base_link_clone.transform.rotation.z = quat[3]
self.tf_broadcast.sendTransform(trafo_optitrack2base_link_clone)
# LAB to optitrack [ set values MANUALLY]
trafo_lab2optitrack = TransformStamped()
trafo_lab2optitrack.header.stamp = time_stamp
# trafo_lab2optitrack.header.stamp = rospy.Time.now()
trafo_lab2optitrack.header.frame_id = "world_lab"
trafo_lab2optitrack.child_frame_id = "world_optitrack"
trafo_lab2optitrack.transform.translation.x = 0.0
trafo_lab2optitrack.transform.translation.y = 0.0
trafo_lab2optitrack.transform.translation.z = 0.0
# q = tf.transformations.quaternion_from_euler(0, 0, -8./180*pi)
q = tf.transformations.quaternion_from_euler(0, 0, 0)
trafo_lab2optitrack.transform.rotation.x = q[0]
trafo_lab2optitrack.transform.rotation.y = q[1]
trafo_lab2optitrack.transform.rotation.z = q[2]
trafo_lab2optitrack.transform.rotation.w = q[3]
self.tf_broadcast.sendTransform(trafo_lab2optitrack)
# Zero TF
# TODO: is this really needed?
trafo_0 = TransformStamped()
trafo_0.header.stamp = time_stamp
trafo_0.header.frame_id = "world_lab"
trafo_0.child_frame_id = "world"
trafo_0.transform.rotation.w = 1
self.tf_broadcast.sendTransform(trafo_0)
# lock.release()
# return True
rate.sleep()
def poseToTransform(self, pose):
# Convert the pose to a 4x4 homogeneous transform
pos = (pose.position.x, pose.position.y, pose.position.z)
rot = (pose.orientation.x, pose.orientation.y, pose.orientation.z, pose.orientation.w)
trans = tr.concatenate_matrices(tr.translation_matrix(pos), tr.quaternion_matrix(rot))
return trans
def create_surface_grasp(object_frame, support_surface_frame, handarm_params,
object_type):
# Get the relevant parameters for hand object combination
print(object_type)
if (object_type in handarm_params['surface_grasp']):
params = handarm_params['surface_grasp'][object_type]
else:
params = handarm_params['surface_grasp']['object']
hand_transform = params['hand_transform']
pregrasp_transform = params['pregrasp_transform']
post_grasp_transform = params['post_grasp_transform'] # TODO: USE THIS!!!
drop_off_config = params['drop_off_config']
downward_force = params['downward_force']
hand_closing_time = params['hand_closing_duration']
hand_synergy = params['hand_closing_synergy']
down_speed = params['down_speed']
up_speed = params['up_speed']
# Set the initial pose above the object
goal_ = np.copy(object_frame) #TODO: this should be support_surface_frame
goal_[:3, 3] = tra.translation_from_matrix(object_frame)
goal_ = goal_.dot(hand_transform)
#the grasp frame is symmetrical - check which side is nicer to reach
#this is a hacky first version for our WAM
zflip_transform = tra.rotation_matrix(math.radians(180.0), [0, 0, 1])
if goal_[0][0] < 0:
goal_ = goal_.dot(zflip_transform)
# hand pose above object
pre_grasp_pose = goal_.dot(pregrasp_transform)
# Set the directions to use TRIK controller with
dirDown = tra.translation_matrix([0, 0, -down_speed])
dirUp = tra.translation_matrix([0, 0, up_speed])
# Set the frames to visualize with RViz
rviz_frames = []
#rviz_frames.append(object_frame)
#rviz_frames.append(goal_)
#rviz_frames.append(pre_grasp_pose)
# assemble controller sequence
control_sequence = []
# # 1. Go above the object - Pregrasp
# control_sequence.append(
# ha.InterpolatedHTransformControlMode(pre_grasp_pose, controller_name='GoAboveIFCO', goal_is_relative='0',
# name='Pre_preGrasp'))
#
# # 1b. Switch when hand reaches the goal pose
# control_sequence.append(ha.FramePoseSwitch('Pre_preGrasp', 'Pregrasp', controller='GoAboveIFCO', epsilon='0.01'))
#Joao test
#control_sequence.append(
# ha.JointControlMode(np.zeros(7), goal_is_relative='0', completion_times=np.array([10, 10, 10, 10, 10, 10, 10]), controller_name='GoToDropJointConfig1', name='GoDropOff1'))
#
# # 7.b Switch when joint is reached
# #control_sequence.append(ha.TimeSwitch('GoDropOff1', 'Pregrasp', duration = hand_closing_time))
#control_sequence.append(ha.JointConfigurationSwitch('GoDropOff1', 'Pregrasp', controller='GoToDropJointConfig1',epsilon=str(math.radians(17.))))
#
# control_sequence.append(
# ha.JointControlMode(np.array([1.1, 1.2, 0.3, 0.4, 0.5, 1.6, 0.7]), goal_is_relative='0', completion_times=np.array([10, 10, 10, 10, 10, 10, 10]),
# controller_name='GoToDropJointConfig2', name='GoDropOff2'))
#
# control_sequence.append(ha.JointConfigurationSwitch('GoDropOff2', 'Pregrasp', controller='GoToDropJointConfig2',epsilon=str(math.radians(7.))))
# 2. Go above the object - Pregrasp
control_sequence.append(
ha.InterpolatedHTransformControlMode(tra.translation_matrix(
[0, 0, 1.0]),
controller_name='GoStart',
goal_is_relative='0',
name='Initial'))
control_sequence.append(
ha.FramePoseSwitch('Initial',
'Pregrasp',
controller='GoStart',
epsilon='0.05'))
print(tra.translation_matrix([0, 0, 1.0]))
#rviz_frames.append(object_frame)
pregrasp_frame = tra.concatenate_matrices(
object_frame, tra.inverse_matrix(pregrasp_transform),
tra.inverse_matrix(hand_transform))
print(pregrasp_frame)
import IPython
#IPython.embed()
rviz_frames.append(tra.concatenate_matrices(pregrasp_frame,
hand_transform))
#rviz_frames.append(pregrasp_frame)
publish_rviz_markers(rviz_frames, 'world', handarm_params)
control_sequence.append(
ha.InterpolatedHTransformControlMode(pregrasp_frame,
controller_name='GoAboveObject',
goal_is_relative='0',
name='Pregrasp'))
#
# # 2b. Switch when hand reaches the goal pose
control_sequence.append(
ha.FramePoseSwitch('Pregrasp',
'finished',
controller='GoAboveObject',
epsilon='0.01'))
#
# # 3. Go down onto the object (relative in world frame) - Godown
# control_sequence.append(
# ha.InterpolatedHTransformControlMode(dirDown, controller_name='GoDown', goal_is_relative='1', name="GoDown",
# reference_frame="world"))
#
# force = np.array([0, 0, 0.5*downward_force, 0, 0, 0])
# # 3b. Switch when goal is reached
# #JOAO CHANGED
# #control_sequence.append(ha.ForceTorqueSwitch('GoDown', 'softhand_close', goal = force,norm_weights = np.array([0, 0, 1, 0, 0, 0]), jump_criterion = "THRESH_UPPER_BOUND", goal_is_relative = '1', frame_id = 'world', port = '2'))
# control_sequence.append(ha.FramePoseSwitch('Pregrasp', 'GoDown', controller = 'GoAboveObject', epsilon = '0.01'))
#
#
# # 4. Maintain the position
# desired_displacement = np.array([[1.0, 0.0, 0.0, 0.0], [0.0, 1.0, 0.0, 0.0 ], [0.0, 0.0, 1.0, 0.0], [0.0, 0.0, 0.0, 1.0]])
# force_gradient = np.array([[1.0, 0.0, 0.0, 0.0], [0.0, 1.0, 0.0, 0.0 ], [0.0, 0.0, 1.0, 0.005], [0.0, 0.0, 0.0, 1.0]])
# desired_force_dimension = np.array([0.0, 0.0, 1.0, 0.0, 0.0, 0.0])
#
# if handarm_params['isForceControllerAvailable']:
# control_sequence.append(ha.HandControlMode_ForceHT(name = 'softhand_close', synergy = hand_synergy,
# desired_displacement = desired_displacement,
# force_gradient = force_gradient,
# desired_force_dimension = desired_force_dimension))
# else:
# # if hand is not RBO then create general hand closing mode?
# control_sequence.append(ha.SimpleRBOHandControlMode(goal = np.array([1])))
#
#
# # 4b. Switch when hand closing time ends
# # joao changed
# #control_sequence.append(ha.TimeSwitch('softhand_close', 'PostGraspRotate', duration = hand_closing_time))
# control_sequence.append(ha.FramePoseSwitch('Pregrasp', 'GoDown', controller = 'GoAboveObject', epsilon = '0.01'))
#
#
# # 5. Rotate hand after closing and before lifting it up
# # relative to current hand pose
# control_sequence.append(
# ha.HTransformControlMode(post_grasp_transform, controller_name='PostGraspRotate', name='PostGraspRotate', goal_is_relative='1', ))
#
# # 5b. Switch when hand rotated
# control_sequence.append(ha.FramePoseSwitch('PostGraspRotate', 'GoUp', controller='PostGraspRotate', epsilon='0.01', goal_is_relative='1', reference_frame = 'EE'))
#
# # 6. Lift upwards
# control_sequence.append(ha.InterpolatedHTransformControlMode(dirUp, controller_name = 'GoUpHTransform', name = 'GoUp', goal_is_relative='1', reference_frame="world"))
#
# # 6b. Switch when joint is reached
# control_sequence.append(ha.FramePoseSwitch('GoUp', 'GoDropOff', controller = 'GoUpHTransform', epsilon = '0.01', goal_is_relative='1', reference_frame="world"))
#
# # 7. Go to dropOFF
# control_sequence.append(ha.JointControlMode(drop_off_config, controller_name = 'GoToDropJointConfig', name = 'GoDropOff'))
#
# # 7.b Switch when joint is reached
# control_sequence.append(ha.JointConfigurationSwitch('GoDropOff', 'finished', controller = 'GoToDropJointConfig', epsilon = str(math.radians(7.))))
# 8. Block joints to finish motion and hold object in air
finishedMode = ha.ControlMode(name='finished')
finishedSet = ha.ControlSet()
finishedSet.add(
ha.Controller(name='JointSpaceController',
type='InterpolatedJointController',
goal=np.ones(7) * 0.2,
goal_is_relative=1,
v_max='[0,0]',
a_max='[0,0]'))
finishedMode.set(finishedSet)
control_sequence.append(finishedMode)
return cookbook.sequence_of_modes_and_switches_with_saftyOn(
control_sequence), rviz_frames
def callback(self, msg):
if not self.active:
return
#print msg
tag_infos = []
trans_x_buf = 0.0
trans_y_buf = 0.0
trans_z_buf = 0.0
rot_z_buf = 0.0
yaw_buf = 0.0
# Load tag detections message
for detection in msg.detections:
tag_id = int(detection.id)
self.checkProtocol(tag_id)
# Define the transforms
veh_t_camxout = tr.translation_matrix(
(self.camera_x, self.camera_y, self.camera_z))
veh_R_camxout = tr.euler_matrix(0, self.camera_theta * np.pi / 180,
0, 'rxyz')
veh_T_camxout = tr.concatenate_matrices(
veh_t_camxout,
veh_R_camxout) # 4x4 Homogeneous Transform Matrix
camxout_T_camzout = tr.euler_matrix(-np.pi / 2, 0, -np.pi / 2,
'rzyx')
veh_T_camzout = tr.concatenate_matrices(veh_T_camxout,
camxout_T_camzout)
tagzout_T_tagxout = tr.euler_matrix(-np.pi / 2, 0, np.pi / 2,
'rxyz')
#Load translation
trans = detection.pose.pose.position
rot = detection.pose.pose.orientation
camzout_t_tagzout = tr.translation_matrix(
(trans.x * self.scale_x, trans.y * self.scale_y,
trans.z * self.scale_z))
camzout_R_tagzout = tr.quaternion_matrix(
(rot.x, rot.y, rot.z, rot.w))
camzout_T_tagzout = tr.concatenate_matrices(
camzout_t_tagzout, camzout_R_tagzout)
veh_T_tagxout = tr.concatenate_matrices(veh_T_camzout,
camzout_T_tagzout,
tagzout_T_tagxout)
# Overwrite transformed value
(trans.x, trans.y,
trans.z) = tr.translation_from_matrix(veh_T_tagxout)
(rot.x, rot.y, rot.z,
rot.w) = tr.quaternion_from_matrix(veh_T_tagxout)
frame_name = '/tag' + str(tag_id)
try:
(trans_tf, rot_tf) = self.listener.lookupTransform(
'/ducky1', frame_name, rospy.Time(0))
except (tf.LookupException, tf.ConnectivityException,
tf.ExtrapolationException):
continue
rot_tf_rpy = tf.transformations.euler_from_quaternion(rot_tf)
yaw = rot_tf_rpy[2] / np.pi
# Check phase
if tag_id == 3:
# Back of the objet, phase confusion
if rot.z < 0.0:
# turn around the coordinate frame to avoid confusion
yaw = -yaw
# Publish tag id
tag_id_msg = Int8()
tag_id_msg.data = tag_id
self.pub_tagId.publish(tag_id_msg)
# Publish ack
ack_msg = BoolStamped()
if trans.x < 0.7:
ack_msg.data = True
else:
ack_msg.data = False
self.pub_ack.publish(ack_msg)
#print detection
print detection #trans.x, trans.y, trans.z
#self.checkProtocol(tag_id)
#print rot.z - yaw
#print tag_id, trans, rot
rot_z_buf += rot.z
yaw_buf += yaw
if len(msg.detections) > 0:
# Broadcast tranform
self.broadcaster.sendTransform(
(trans.x, trans.y, trans.z),
tf.transformations.quaternion_from_euler(
0, 0,
np.pi *
(rot_z_buf - yaw_buf) / float(len(msg.detections))),
rospy.Time.now(), "ducky1", "camera")
width = 0.02
markers.publishPath(path, 'orange', width, 5.0) # path, color, width, lifetime
# Plane / Rectangle:
# Publish a rectangle between two points (thin, planar surface)
# If the z-values are different, this will produce a cuboid
point1 = Point(-1,0,0)
point2 = Point(-2,-1,0)
markers.publishRectangle(point1, point2, 'blue', 5.0)
# Publish a rotated plane using a numpy transform matrix
R_y = transformations.rotation_matrix(0.3, (0,1,0)) # Rotate around y-axis by 0.3 radians
T0 = transformations.translation_matrix((-3,-1.5,0))
T = transformations.concatenate_matrices(T0, R_y)
depth = 1.1
width = 1.5
markers.publishPlane(T, depth, width, 'purple', 5.0) # pose, depth, width, color, lifetime
# Publish a plane using a ROS Pose Msg
P = Pose(Point(-3,0,0),Quaternion(0,0,0,1))
depth = 1.3
width = 1.3
markers.publishPlane(P, depth, width, 'brown', 5.0) # pose, depth, width, color, lifetime
# Polygon:
# Publish a polygon using a ROS Polygon Msg
polygon = Polygon()
def space_coordinates(self, joints):
T = tfs.identity_matrix()
for idx in range(0, self.dof):
T = tfs.concatenate_matrices(T, self.transformations[idx](joints[idx]))
return self.space_from_matrix(T)
def callback(self, msg):
tag_infos = []
# Load tag detections message
for detection in msg.detections:
# ------ start tag info processing
new_info = TagInfo()
new_info.id = int(detection.id)
id_info = self.tags_dict[new_info.id]
# Check yaml file to fill in ID-specific information
new_info.tag_type = self.sign_types[id_info['tag_type']]
if new_info.tag_type == self.info.S_NAME:
new_info.street_name = id_info['street_name']
elif new_info.tag_type == self.info.SIGN:
new_info.traffic_sign_type = self.traffic_sign_types[id_info['traffic_sign_type']]
elif new_info.tag_type == self.info.VEHICLE:
new_info.vehicle_name = id_info['vehicle_name']
# TODO: Implement location more than just a float like it is now.
# location is now 0.0 if no location is set which is probably not that smart
if self.loc == 226:
l = (id_info['location_226'])
if l is not None:
new_info.location = l
elif self.loc == 316:
l = (id_info['location_316'])
if l is not None:
new_info.location = l
tag_infos.append(new_info)
# --- end tag info processing
# Define the transforms
veh_t_camxout = tr.translation_matrix((self.camera_x, self.camera_y, self.camera_z))
veh_R_camxout = tr.euler_matrix(0, self.camera_theta*np.pi/180, 0, 'rxyz')
veh_T_camxout = tr.concatenate_matrices(veh_t_camxout, veh_R_camxout) # 4x4 Homogeneous Transform Matrix
camxout_T_camzout = tr.euler_matrix(-np.pi/2,0,-np.pi/2,'rzyx')
veh_T_camzout = tr.concatenate_matrices(veh_T_camxout, camxout_T_camzout)
tagzout_T_tagxout = tr.euler_matrix(-np.pi/2, 0, np.pi/2, 'rxyz')
#Load translation
trans = detection.pose.pose.position
rot = detection.pose.pose.orientation
camzout_t_tagzout = tr.translation_matrix((trans.x*self.scale_x, trans.y*self.scale_y, trans.z*self.scale_z))
camzout_R_tagzout = tr.quaternion_matrix((rot.x, rot.y, rot.z, rot.w))
camzout_T_tagzout = tr.concatenate_matrices(camzout_t_tagzout, camzout_R_tagzout)
veh_T_tagxout = tr.concatenate_matrices(veh_T_camzout, camzout_T_tagzout, tagzout_T_tagxout)
# Overwrite transformed value
(trans.x, trans.y, trans.z) = tr.translation_from_matrix(veh_T_tagxout)
(rot.x, rot.y, rot.z, rot.w) = tr.quaternion_from_matrix(veh_T_tagxout)
detection.pose.pose.position = trans
detection.pose.pose.orientation = rot
new_tag_data = AprilTagsWithInfos()
new_tag_data.detections = msg.detections
new_tag_data.infos = tag_infos
# Publish Message
self.pub_postPros.publish(new_tag_data)
objects_path = objects_path[0]
# Load the kitchen environment
kitchen_file = os.path.join(objects_path, 'pr_kitchen.env.xml')
env.Load(kitchen_file)
# Remove the wall kinbody
env.Remove(env.GetKinBody('walls'))
# Move the robot near the fridge
Rot_z = transformations.rotation_matrix(numpy.pi/4.0, (0,0,1))
trans = numpy.array([[1., 0., 0., 0.2],
[0., 1., 0., 0.5],
[0., 0., 1., 0.0],
[0., 0., 0., 1.]])
robot_pose = transformations.concatenate_matrices(trans, Rot_z)
robot.SetTransform(robot_pose)
# Get transform of the lower handle link,
# this is actually at the origin of the fridge
fridge = env.GetRobot('refrigerator')
fridge_handle = fridge.GetLink('lower_handle')
T_lower_handle = fridge_handle.GetTransform()
# Translate the lower handle axis to be at the actual handle
height = 0.925
forward = 0.427
sideways = -0.308
T_handle_offset = numpy.eye(4)
T_handle_offset[0:3,3] = numpy.array([forward, sideways, height])
T_handle = transformations.concatenate_matrices(T_lower_handle, T_handle_offset)
def transform_from_quat(pos, quat):
return tfms.concatenate_matrices(tfms.translation_matrix(pos),
tfms.quaternion_matrix(quat))
def _makePreGrasp(self, box, objectName):
'''Given a bounding box, identify an
approach vector, and designate a pregrasp position along that vector. Returns a MoveArmGoal
message to move to the preGrasp position'''
useX = not(box.box_dims.x > self.gripperOpenWidth or box.box_dims.x < self.gripperClosedWidth)
useY = not(box.box_dims.y > self.gripperOpenWidth or box.box_dims.y < self.gripperClosedWidth)
#Do all geometry in robot's base_link frame
#Turn box pose into a tf for visualization
self._tf_broadcaster.sendTransform(
(box.pose.pose.position.x,box.pose.pose.position.y,box.pose.pose.position.z),
(box.pose.pose.orientation.x,box.pose.pose.orientation.y,box.pose.pose.orientation.z,box.pose.pose.orientation.w),
box.pose.header.stamp,
objectName,
box.pose.header.frame_id)
#Turn box pose into a tf matrix in the proper frame
boxPose = self._tf_listener.transformPose(self.frameID, box.pose)
boxMat = transformations.quaternion_matrix([boxPose.pose.orientation.x,boxPose.pose.orientation.y,boxPose.pose.orientation.z,boxPose.pose.orientation.w])
boxMat[:3, 3] = [boxPose.pose.position.x,boxPose.pose.position.y,boxPose.pose.position.z+.01] #Lift 1 cm above box CoM
#Approach Vector is in the principal plane of the box most closely aligned with robot XZ. This plane will be tool YZ
#Vector is at a downward 30 degree angle
#Orientation of box should be (approximately) a pure z rotation from the robot base_link
#Therefore the angle of rotation about the z axis is appoximately
# 2 * acos(q_w)
boxPose_base = self._tf_listener.transformPose('/base_link', boxPose)
theta = 2 * math.acos(boxPose_base.pose.orientation.w)
rospy.logdebug('Angle is %f',theta)
#Determine what to rotate the pose quaternion by to get the vector quaternion
#Start with a 30 degree downward rotation
if useX and not useY:
rospy.loginfo("Can only grab box along x axis")
width = box.box_dims.x
if theta >= pi/2 and theta <= 3*pi/2:
rotationMatrix = transformations.euler_matrix(0, 5*pi/6, 0, 'rzyz')
else:
rotationMatrix = transformations.euler_matrix(pi, 5*pi/6, 0, 'rzyz')
elif useY and not useX:
rospy.loginfo("Can only grab box along y axis")
width = box.box_dims.y
if theta >= pi/4 and theta <= 3*pi/4:
rospy.loginfo("Grabbing box along -y axis")
rotationMatrix = transformations.euler_matrix(pi/2, 7*pi/6, -pi/2, 'rzyz')
else:
rospy.loginfo("Grabbing box along y axis")
rotationMatrix = transformations.euler_matrix(pi/2, self._makePreGraspPose(boxMat, 1), -pi/2, 'rzyz')
else:
#Can use either face for pickup, so pick the one best aligned to the robot
#TODO only -x and -y are currently correct
rospy.loginfo("Theta is %f",theta)
if theta >= pi/4 and theta <= 3*pi/4:
rospy.loginfo("Grabbing box along -y axis")
width = box.box_dims.x
rotationMatrix = transformations.euler_matrix(pi/2, 7*pi/6, -pi/2, 'rzyz')
elif theta >= 3*pi/4 and theta <= 5*pi/4:
rospy.loginfo("Grabbing box along -x axis")
width = box.box_dims.y
rotationMatrix = transformations.euler_matrix(pi, 5*pi/6, pi/2, 'rzyz')
elif theta >= 5*pi/4 and theta <= 7*pi/4:
rospy.loginfo("Grabbing box along y axis")
width = box.box_dims.x
rotationMatrix = transformations.euler_matrix(pi/2, self._makePreGraspPose(boxMat, 1), -pi/2, 'rzyz')
else:
rospy.loginfo("Grabbing box along x axis")
width = box.box_dims.y
rotationMatrix = transformations.euler_matrix(0, 5*pi/6, pi/2, 'rzyz')
#Rotated TF for visualization
self._tf_broadcaster.sendTransform(
(0,0,0),
transformations.quaternion_from_matrix(rotationMatrix),
boxPose.header.stamp,
objectName+'_rotated',
objectName)
#Pregrasp TF is rotated box TF translated back along the z axis
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)
self._tf_broadcaster.sendTransform(
p,
q,
boxPose.header.stamp,
objectName+'_pregrasp',
self.frameID)
#Create Orientation constraint object
o_constraint = OrientationConstraint()
o_constraint.header.frame_id = self.frameID
o_constraint.header.stamp = rospy.Time.now()
o_constraint.link_name = self.toolLinkName
o_constraint.orientation = Quaternion(q[0],q[1],q[2],q[3])
o_constraint.absolute_roll_tolerance = 0.04
o_constraint.absolute_pitch_tolerance = 0.04
o_constraint.absolute_yaw_tolerance = 0.04
#Determine position and tolerance from vector and box size
pos_constraint = PositionConstraint()
pos_constraint.header = o_constraint.header
pos_constraint.link_name = self.toolLinkName
pos_constraint.position = Point(p[0],p[1],p[2])
#Position tolerance (in final tool frame) is:
# x = gripper open width - box x width
# y = object height * precision multiplier (<=1)
# z = 1 cm
#Note that this is probably in the wrong frame, negating the advantage of all of the thinking in the preceding 4 lines
#TODO: If you're feeling ambitious, turn this into a mesh and make it the right shape
pos_constraint.constraint_region_shape.type = Shape.SPHERE
pos_constraint.constraint_region_shape.dimensions = [0.01]
# self.gripperOpenWidth - width,
# box.box_dims.z*0.2,
# 0.01]
preGraspGoal = MoveArmGoal()
preGraspGoal.planner_service_name = self.plannerServiceName
motion_plan_request = MotionPlanRequest()
motion_plan_request.group_name = self.armGroupName
motion_plan_request.num_planning_attempts = 10
motion_plan_request.planner_id = ""
motion_plan_request.allowed_planning_time = rospy.Duration(5,0)
pos_constraint.weight = 1
motion_plan_request.goal_constraints.position_constraints.append(pos_constraint)
o_constraint.weight = 1
motion_plan_request.goal_constraints.orientation_constraints.append(o_constraint)
preGraspGoal.motion_plan_request = motion_plan_request
#Turn off collision operations between the gripper and all objects
for collisionName in self.gripperCollisionNames:
collisionOperation = CollisionOperation(collisionName,
CollisionOperation.COLLISION_SET_ALL,
0.0,
CollisionOperation.DISABLE)
preGraspGoal.operations.collision_operations.append(collisionOperation)
return preGraspGoal
def generate_grasps(box,
num_grasps=4,
desired_approach_distance = 0.10,
min_approach_distance = 0.05,
effort = 50):
"""
Generates grasps
Parameters:
box: bounding box to genereate grasps for
num_grasps: number of grasps to generate, spaced equally around the box
desired_approach_distance: how far the pre-grasp should ideally be away from the grasp
min_approach_distance: how much distance between pre-grasp and grasp must actually be feasible for the grasp not to be rejected
Return: a list of grasps
"""
rospy.loginfo("Generating grasps")
grasps = []
for i in range(num_grasps):
g = Grasp()
# align z-axis rotation to box
euler = transformations.euler_from_quaternion([
box.pose.pose.orientation.x,
box.pose.pose.orientation.y,
box.pose.pose.orientation.z,
box.pose.pose.orientation.w])
rot_ang = euler[2]
# apply transformations based on rotations
rot_ang += i * (2 * math.pi) / num_grasps
t1 = transformations.translation_matrix([
box.pose.pose.position.x,
box.pose.pose.position.y,
box.pose.pose.position.z + \
box.box_dims.z * 0.05])
r1 = transformations.rotation_matrix(rot_ang, [0, 0, 1])
box_width = max(box.box_dims.x, box.box_dims.y)
t2 = transformations.translation_matrix([-(box_width/2 + 0.12), 0, 0])
mat = transformations.concatenate_matrices(t1, r1, t2)
translation = transformations.translation_from_matrix(mat)
g.grasp_pose.position.x = translation[0]
g.grasp_pose.position.y = translation[1]
g.grasp_pose.position.z = translation[2]
q = transformations.quaternion_from_matrix(mat)
g.grasp_pose.orientation.x = q[0]
g.grasp_pose.orientation.y = q[1]
g.grasp_pose.orientation.z = q[2]
g.grasp_pose.orientation.w = q[3]
g.desired_approach_distance = desired_approach_distance
g.min_approach_distance = min_approach_distance
pre_grasp_posture = JointState()
pre_grasp_posture.header.stamp = rospy.Time.now()
pre_grasp_posture.header.frame_id = 'base_link'
pre_grasp_posture.name = ['r_wrist_flex_link']
pre_grasp_posture.position = [0.8]
pre_grasp_posture.effort = [effort]
g.pre_grasp_posture = pre_grasp_posture
grasp_posture = JointState()
grasp_posture.header.stamp = rospy.Time.now()
grasp_posture.header.frame_id = 'base_link'
grasp_posture.name = ['r_wrist_flex_link']
grasp_posture.position = [0.45]
grasp_posture.effort = [effort]
g.grasp_posture = grasp_posture
grasps.append(g)
np.random.shuffle(grasps)
return grasps
def transformBack(tf_xyzquat, pose):
T_mat = tfm.concatenate_matrices( tfm.translation_matrix(tf_xyzquat[0:3]), tfm.quaternion_matrix(tf_xyzquat[3:7]))
pose_mat = tfm.concatenate_matrices( tfm.translation_matrix(pose[0:3]), tfm.quaternion_matrix(pose[3:7]) )
new_pose_mat = np.dot(pose_mat, tfm.inverse_matrix(T_mat))
return tfm.translation_from_matrix(new_pose_mat).tolist() + tfm.quaternion_from_matrix(new_pose_mat).tolist()
def PathCalculations(bag):
# here we get the tranformation between map and world reference and robot poses according to world coordinates
tf_static = bag.read_messages(topics=['/tf_static', 'tf2_msgs/TFMessage'])
vrpn_robot = bag.read_messages(
topics=['/vrpn_client_node/husky/pose', 'geometry_msgs/PoseStamped'])
for topic, msg, t in tf_static:
# we invert the matrix to get world to map
rot_w2m = msg.transforms[0].transform.rotation
tras_w2m = msg.transforms[0].transform.translation
m2w = tff.concatenate_matrices(
tff.translation_matrix([tras_w2m.x, tras_w2m.y, tras_w2m.z]),
tff.quaternion_matrix([rot_w2m.x, rot_w2m.y, rot_w2m.z, rot_w2m.w]))
w2m = tff.inverse_matrix(m2w)
world2map = TransformStamped()
world2map.transform.rotation = rot_w2m
world2map.transform.rotation.w = -1 * rot_w2m.w # inverse rotation matrix just using quaternions
world2map.transform.translation.x = w2m[0][3]
world2map.transform.translation.y = w2m[1][3]
world2map.transform.translation.z = w2m[2][3]
t_old = rospy.Time(0)
init = True
dist = 0
deltax = 0
dfx_old = 0
dfx = 0
deltay = 0
dfy_old = 0
dfy = 0
bte = 0
global obs_dist_path
global all_k
print "start processing like crazy"
for topic, msg, t in vrpn_robot:
#sys.stdout.flush()
robot_w = msg
robot_m = tf2_geometry_msgs.do_transform_pose(
robot_w,
world2map) #transform robot pose from world ref to map ref
robot_px_m = robot_m.pose.position.x / map_scale # robot pose in map image coordinates
robot_py_m = robot_m.pose.position.y / map_scale
robot_px_m_dist = int(robot_px_m + map_w / 2)
robot_py_m_dist = int(robot_py_m + map_h / 2)
robot_px_mc = (
robot_px_m + dx / 2 - cx
) #*map_scale # robot pose in map cropped image coordinates [meters]
robot_py_mc = (robot_py_m + dy / 2 - cy) #*map_scale
dt = abs((t.secs + t.nsecs / 10**9) -
(t_old.secs + t_old.nsecs / 10**9))
if t > rospy.Time(t_stop):
break
if t < rospy.Time(t_start):
pass
else:
# calculating total traveled distance and curvature
if init:
init = False
else:
#deltax = robot_m.pose.position.x - x_old
deltax = robot_px_mc - x_old
#deltay = robot_m.pose.position.y - y_old
deltay = robot_py_mc - y_old
dist = dist + math.sqrt(
deltax**2 + deltay**2) # distance traveled
if dt > 0.0:
dfx = deltax / dt # first derivate for the curvature
dfy = deltay / dt
ddfx = (dfx - dfx_old) / dt # second derivate
ddfy = (dfy - dfy_old) / dt
if abs(dfx) > 0.0 and abs(dfy) > 0.0:
k_num = abs(
dfx * ddfy -
dfy * ddfx) # wikipedia s formula for curvature
k_den = (dfx**2 + dfy**2)**(1.5)
k = k_num / k_den
# print k_num , k_den
else:
k = 0
all_k = np.vstack([all_k, [k, t]])
bte = bte + k**2
print robot_px_mc, robot_py_mc, t
x_old = robot_px_mc
y_old = robot_py_mc
dfx_old = dfx
dfy_old = dfy
t_old = t
# calculating minimum distance from obstacles with a sampling time
'''
if dt >= sample_time:
obs_dist = calcMinDistFromObstacles(objectmap,robot_px_m_dist,robot_py_m_dist)
obs_dist_path = np.vstack([obs_dist_path,[obs_dist,t]])
t_old = t
'''
#min_obs_dist = np.amin(obs_dist_path[1:,0])
#min_obs_dist_index = np.where(obs_dist_path[1:,0] == np.amin(obs_dist_path[1:,0]))
#index_min = min_obs_dist_index + np.ones(len(min_obs_dist_index) ,np.uint8)
#time_obs_dist = obs_dist_path[index_min,1]
#print "Absolute minimum distance ", min_obs_dist , " at " , time_obs_dist
print "Total distance traveled ", dist
print "Total bending energy ", bte
max_k = np.amax([all_k[:, 0]])
print "Max curvature ", max_k
with open('Estimation.txt', 'w') as file:
file.write(
'1 - obs_dist_path\n2 - dist\n3 - all_k\n with a sample time of ')
file.write(str(sample_time))
file.write('\n\n\n')
#file.write(str(obs_dist_path[:,:]))
#file.write('\n \n \n')
file.write(str(dist))
file.write('\n \n \n')
file.write(str(all_k[:, :]))
print "All finished. Check ./Estimation.txt file for details"
# Add a table to the environment
table_file = os.path.join(objects_path, 'table.kinbody.xml')
table = env.ReadKinBodyXMLFile(table_file)
if table == None:
print('Failed to load table kinbody')
sys.exit()
env.AddKinBody(table)
# Rotate so table surface is horizontal
Rot_x = transformations.rotation_matrix(numpy.pi/2.0, (1,0,0))
Rot_z = transformations.rotation_matrix(numpy.pi/2.0, (0,0,1))
trans = numpy.array([[1., 0., 0., 0.0], # along base frame y-axis
[0., 1., 0., 0.0], # vertical in base frame
[0., 0., 1., 0.8], # along base frame x-axis
[0., 0., 0., 1.]])
table_pose = transformations.concatenate_matrices(Rot_z, Rot_x, trans)
table.SetTransform(table_pose)
# Set the Segway base to be static, so it doesn't fall over by gravity
robot.GetLink('segway').SetStatic(True)
with env:
# Initialize the physics engine
physics = openravepy.RaveCreatePhysicsEngine(env,'ode')
env.SetPhysicsEngine(physics)
physics.SetGravity(numpy.array((0,0,-9.81)))
# Enable physics simulator
env.StopSimulation()
env.StartSimulation(timestep=0.001)
def callback(self, msg):
detect_msg = BoolStamped()
detect_msg.data = True
self.pub_detected.publish(detect_msg)
tag_infos = []
# Load tag detections message
for detection in msg.detections:
# ------ start tag info processing
new_info = TagInfo()
new_info.id = int(detection.id)
id_info = self.tags_dict[new_info.id]
if new_info.id == 1:
self.turnright = True
self.turnleft = False
self.turnaround = False
#print "---------Tag 1---------Turn Right-----------"
elif new_info.id == 2:
self.turnright = False
self.turnleft = True
self.turnaround = False
#print "---------Tag 2---------Turn LEFT-----------"
elif new_info.id == 3:
self.turnright = False
self.turnleft = False
self.turnaround = True
print "---------Tag 3---------Turn around-----------"
# Check yaml file to fill in ID-specific information
new_info.tag_type = self.sign_types[id_info['tag_type']]
if new_info.tag_type == self.info.S_NAME:
new_info.street_name = id_info['street_name']
elif new_info.tag_type == self.info.SIGN:
new_info.traffic_sign_type = self.traffic_sign_types[
id_info['traffic_sign_type']]
elif new_info.tag_type == self.info.VEHICLE:
new_info.vehicle_name = id_info['vehicle_name']
# TODO: Implement location more than just a float like it is now.
# location is now 0.0 if no location is set which is probably not that smart
if self.loc == 226:
l = (id_info['location_226'])
if l is not None:
new_info.location = l
elif self.loc == 316:
l = (id_info['location_316'])
if l is not None:
new_info.location = l
tag_infos.append(new_info)
# --- end tag info processing
# Define the transforms
veh_t_camxout = tr.translation_matrix(
(self.camera_x, self.camera_y, self.camera_z))
veh_R_camxout = tr.euler_matrix(0, self.camera_theta * np.pi / 180,
0, 'rxyz')
veh_T_camxout = tr.concatenate_matrices(
veh_t_camxout,
veh_R_camxout) # 4x4 Homogeneous Transform Matrix
camxout_T_camzout = tr.euler_matrix(-np.pi / 2, 0, -np.pi / 2,
'rzyx')
veh_T_camzout = tr.concatenate_matrices(veh_T_camxout,
camxout_T_camzout)
tagzout_T_tagxout = tr.euler_matrix(-np.pi / 2, 0, np.pi / 2,
'rxyz')
#Load translation
trans = detection.pose.pose.position
rot = detection.pose.pose.orientation
camzout_t_tagzout = tr.translation_matrix(
(trans.x * self.scale_x, trans.y * self.scale_y,
trans.z * self.scale_z))
camzout_R_tagzout = tr.quaternion_matrix(
(rot.x, rot.y, rot.z, rot.w))
camzout_T_tagzout = tr.concatenate_matrices(
camzout_t_tagzout, camzout_R_tagzout)
veh_T_tagxout = tr.concatenate_matrices(veh_T_camzout,
camzout_T_tagzout,
tagzout_T_tagxout)
# Overwrite transformed value
(trans.x, trans.y,
trans.z) = tr.translation_from_matrix(veh_T_tagxout)
(rot.x, rot.y, rot.z,
rot.w) = tr.quaternion_from_matrix(veh_T_tagxout)
detection.pose.pose.position = trans
detection.pose.pose.orientation = rot
new_tag_data = AprilTagsWithInfos()
new_tag_data.detections = msg.detections
new_tag_data.infos = tag_infos
# Publish Message
self.pub_postPros.publish(new_tag_data)
def callback(self, msg):
tag_infos = []
new_tag_data = AprilTagsWithInfos()
# Load tag detections message
for detection in msg.detections:
# ------ start tag info processing
new_info = TagInfo()
#Can use id 1 as long as no bundles are used
new_info.id = int(detection.id[0])
id_info = self.tags_dict[new_info.id]
# Check yaml file to fill in ID-specific information
new_info.tag_type = self.sign_types[id_info['tag_type']]
if new_info.tag_type == self.info.S_NAME:
new_info.street_name = id_info['street_name']
elif new_info.tag_type == self.info.SIGN:
new_info.traffic_sign_type = self.traffic_sign_types[
id_info['traffic_sign_type']]
# publish for FSM
# parking apriltag event
msg_parking = BoolStamped()
msg_parking.header.stamp = rospy.Time(0)
if new_info.traffic_sign_type == TagInfo.PARKING:
msg_parking.data = True
else:
msg_parking.data = False
self.pub_postPros_parking.publish(msg_parking)
# intersection apriltag event
msg_intersection = BoolStamped()
msg_intersection.header.stamp = rospy.Time(0)
if (new_info.traffic_sign_type == TagInfo.FOUR_WAY) or (
new_info.traffic_sign_type == TagInfo.RIGHT_T_INTERSECT
) or (new_info.traffic_sign_type == TagInfo.LEFT_T_INTERSECT
) or (new_info.traffic_sign_type
== TagInfo.T_INTERSECTION):
msg_intersection.data = True
else:
msg_intersection.data = False
self.pub_postPros_intersection.publish(msg_intersection)
elif new_info.tag_type == self.info.VEHICLE:
new_info.vehicle_name = id_info['vehicle_name']
# TODO: Implement location more than just a float like it is now.
# location is now 0.0 if no location is set which is probably not that smart
if self.loc == 226:
l = (id_info['location_226'])
if l is not None:
new_info.location = l
elif self.loc == 316:
l = (id_info['location_316'])
if l is not None:
new_info.location = l
tag_infos.append(new_info)
# --- end tag info processing
# Define the transforms
veh_t_camxout = tr.translation_matrix(
(self.camera_x, self.camera_y, self.camera_z))
veh_R_camxout = tr.euler_matrix(0, self.camera_theta * np.pi / 180,
0, 'rxyz')
veh_T_camxout = tr.concatenate_matrices(
veh_t_camxout,
veh_R_camxout) # 4x4 Homogeneous Transform Matrix
camxout_T_camzout = tr.euler_matrix(-np.pi / 2, 0, -np.pi / 2,
'rzyx')
veh_T_camzout = tr.concatenate_matrices(veh_T_camxout,
camxout_T_camzout)
tagzout_T_tagxout = tr.euler_matrix(-np.pi / 2, 0, np.pi / 2,
'rxyz')
#Load translation
trans = detection.pose.pose.pose.position
rot = detection.pose.pose.pose.orientation
header = detection.pose.header
camzout_t_tagzout = tr.translation_matrix(
(trans.x * self.scale_x, trans.y * self.scale_y,
trans.z * self.scale_z))
camzout_R_tagzout = tr.quaternion_matrix(
(rot.x, rot.y, rot.z, rot.w))
camzout_T_tagzout = tr.concatenate_matrices(
camzout_t_tagzout, camzout_R_tagzout)
veh_T_tagxout = tr.concatenate_matrices(veh_T_camzout,
camzout_T_tagzout,
tagzout_T_tagxout)
# Overwrite transformed value
(trans.x, trans.y,
trans.z) = tr.translation_from_matrix(veh_T_tagxout)
(rot.x, rot.y, rot.z,
rot.w) = tr.quaternion_from_matrix(veh_T_tagxout)
new_tag_data.detections.append(
AprilTagDetection(int(detection.id[0]),
float(detection.size[0]),
PoseStamped(header, Pose(trans, rot))))
new_tag_data.infos = tag_infos
# Publish Message
self.pub_postPros.publish(new_tag_data)
def homogeneous_matrix(trans, quat):
return TR.concatenate_matrices(TR.translation_matrix(trans),
TR.quaternion_matrix(quat))
def homogeneous_matrix(trans, rot_ang, rot_dir):
T = tfs.translation_matrix(trans)
R = tfs.rotation_matrix(rot_ang, rot_dir)
return tfs.concatenate_matrices(T, R)