def get_obstacle_pose(
cap_front,
cap_rear,
obs_front,
obs_rear,
obs_gps_to_centroid,
velodyne_to_front,
cap_yaw_error_rad=0,
cap_pitch_error_rad=0):
# calculate capture yaw in ENU frame and setup correction rotation
cap_front_v = dict_to_vect(cap_front)
cap_rear_v = dict_to_vect(cap_rear)
cap_yaw = get_yaw(cap_front_v, cap_rear_v)
cap_yaw += cap_yaw_error_rad
rot_cap = kd.Rotation.EulerZYX(-cap_yaw, -cap_pitch_error_rad, 0)
obs_rear_v = dict_to_vect(obs_rear)
if obs_front:
obs_front_v = dict_to_vect(obs_front)
obs_yaw = get_yaw(obs_front_v, obs_rear_v)
# use the front gps as the obstacle reference point if it exists as it's closers
# to the centroid and mounting metadata seems more reliable
cap_to_obs = obs_front_v - cap_front_v
else:
cap_to_obs = obs_rear_v - cap_front_v
# transform capture car to obstacle vector into capture car velodyne lidar frame
res = rot_cap * cap_to_obs
res += list_to_vect(velodyne_to_front)
# obs_gps_to_centroid is offset for front gps if it exists, otherwise rear
obs_gps_to_centroid_v = list_to_vect(obs_gps_to_centroid)
if obs_front:
# if we have both front + rear RTK calculate an obstacle yaw and use it for centroid offset
obs_rot_z = kd.Rotation.RotZ(obs_yaw - cap_yaw)
centroid_offset = obs_rot_z * obs_gps_to_centroid_v
else:
# if no obstacle yaw calculation possible, treat rear RTK as centroid and offset in Z only
obs_rot_z = kd.Rotation()
centroid_offset = kd.Vector(0, 0, obs_gps_to_centroid_v[2])
res += centroid_offset
return frame_to_dict(kd.Frame(obs_rot_z, res))
def read_data(bag, arm):
'''
Returns tuple (time, theta, theta_ref, angular_vel, angular_vel_ref,
angular_acc, angular_acc_ref, s, gripper_pos_command, gripper_vel_command, gripper_pos)
'''
global alpha
time = np.array([])
frame = PyKDL.Frame(PyKDL.Rotation.RPY(0.0, 0.0, 1.22),
PyKDL.Vector(0.0, 0.0, 0.0))
force_x = np.array([])
force_x_t = np.array([])
force_y = np.array([])
force_y_t = np.array([])
force_z = np.array([])
torque_x = np.array([])
torque_x_t = np.array([])
torque_y = np.array([])
torque_y_t = np.array([])
torque_z = np.array([])
for topic, msg, t in bag.read_messages(
topics=['/ft/' + arm + '_gripper_motor']):
time = np.append(time, msg.header.stamp.to_sec())
force_x = np.append(force_x, msg.wrench.force.x)
force_y = np.append(force_y, msg.wrench.force.y)
#aligned_Fx = msg.wrench.force.x*math.cos(alpha) - msg.wrench.force.y*math.sin(alpha)
#aligned_Fy = msg.wrench.force.x*math.sin(alpha) + msg.wrench.force.y*math.cos(alpha)
#force_x_t = np.append(force_x_t, aligned_Fx)
#force_y_t = np.append(force_y_t, aligned_Fy)
#force_z = np.append(force_z, msg.wrench.force.z)
## torque_x = np.append(torque_x, msg.wrench.torque.x)
## torque_y = np.append(torque_y, msg.wrench.torque.y)
## aligned_Tx = msg.wrench.torque.y*math.sin(alpha) + msg.wrench.torque.x*math.cos(alpha)
## aligned_Ty = msg.wrench.torque.y*math.cos(alpha) - msg.wrench.torque.x*math.sin(alpha)
## torque_x_t = np.append(torque_x_t, aligned_Tx)
## torque_y_t = np.append(torque_y_t, aligned_Ty)
#torque_z = np.append(torque_z, msg.wrench.torque.z)
#return time, force_x, force_y, force_z, torque_x, torque_y, torque_z
return time, force_x, force_y, #force_x_t, force_y_t, torque_x, torque_y, torque_x_t, torque_y_t
def getGraspingAxis(bin_frame, obj_frame, object_name, simtrackUsed):
'''
this function assumes everything is represented in the quaternions in the /base_link frame
'''
if object_name.endswith('_scan'):
object_name = object_name[:-5]
dictObj = objDict()
objSpec = dictObj.getEntry(object_name)
F_bin_frame = posemath.fromTf(bin_frame)
F_obj_frame = posemath.fromTf(obj_frame)
objRed = F_obj_frame.M * kdl.Vector(1.0, 0.0, 0.0)
objGreen = F_obj_frame.M * kdl.Vector(0.0, 1.0, 0.0)
objBlue = F_obj_frame.M * kdl.Vector(0.0, 0.0, 1.0)
binRed = F_bin_frame.M * kdl.Vector(1.0, 0.0, 0.0)
binGreen = F_bin_frame.M * kdl.Vector(0.0, 1.0, 0.0)
binBlue = F_bin_frame.M * kdl.Vector(0.0, 0.0, 1.0)
rRProj = kdl.dot(objRed, binRed)
gRProj = kdl.dot(objGreen, binRed)
bRProj = kdl.dot(objBlue, binRed)
tmpApproach1 = [abs(rRProj), abs(gRProj), abs(bRProj)]
if simtrackUsed:
for i in range(3):
if i in objSpec.invalidApproachAxis:
tmpApproach1[i] = 0
tmpApproach2 = [rRProj, gRProj, bRProj]
axisApproach = tmpApproach1.index(max(tmpApproach1))
objAxes = [objRed, objGreen, objBlue]
tmpGrasp1 = []
for i in range(3):
if simtrackUsed:
if i == axisApproach or i in objSpec.invalidGraspAxis:
tmpGrasp1.append(0)
continue
tmpGrasp1.append(kdl.dot(objAxes[i], binBlue))
tmpGrasp2 = [abs(t) for t in tmpGrasp1]
axisGrasp = tmpGrasp2.index(max(tmpGrasp2))
return axisApproach, tmpApproach2[axisApproach] / abs(
tmpApproach2[axisApproach]), axisGrasp, tmpGrasp1[axisGrasp] / abs(
tmpGrasp1[axisGrasp])
def __init__(
self,
objs,
bases,
pos,
rot,
pos_tol,
):
self.objs = objs
self.bases = bases
self.pos_tol = pos_tol
self.pos = pos
self.rot = rot
# Compute the relative transformation from the "base" where we hope to
# find the object (or, you know, where we're afraid to find it...)
pg = kdl.Vector(*self.pos)
Rg = kdl.Rotation.Quaternion(*self.rot)
self.T = kdl.Frame(Rg, pg)
def getActualPosition(self):
reply = yarp.Bottle()
reply.clear()
request = yarp.Bottle()
request.clear()
request.addString('get actual_position')
self.rpc.write(request, reply)
position = kdl.Vector()
if reply.size() != 3:
raise Exception('Unexpected reply size')
for i in xrange(3):
position[i] = reply.get(i).asDouble()
return position
def fromStr(self, line):
tab = line.split()
self.successful = bool(tab[0])
contacts_count = int(tab[1])
self.contacts = []
for idx in range(0, contacts_count):
f_idx = int(tab[2 + idx * 8 + 0])
px = float(tab[2 + idx * 8 + 1])
py = float(tab[2 + idx * 8 + 2])
pz = float(tab[2 + idx * 8 + 3])
qx = float(tab[2 + idx * 8 + 4])
qy = float(tab[2 + idx * 8 + 5])
qz = float(tab[2 + idx * 8 + 6])
qw = float(tab[2 + idx * 8 + 7])
self.contacts.append([
f_idx,
PyKDL.Frame(PyKDL.Rotation.Quaternion(qx, qy, qz, qw),
PyKDL.Vector(px, py, pz))
])
def skeleton_handler(self, msg):
for joint in msg.name:
#self.skeleton['confidence'][joint] = msg.confidence[msg.name.index(joint)]
self.skeleton['position'][joint] = KDL.Vector(
msg.position[msg.name.index(joint)].x,
msg.position[msg.name.index(joint)].y,
msg.position[msg.name.index(joint)].z)
if joint == 'right_hand':
if self.skeleton['position'][joint][0] < -0.15:
keyboard.press(Key.left)
time.sleep(abs(self.skeleton['position'][joint][0]))
keyboard.release(Key.left)
elif self.skeleton['position'][joint][0] > 0.15:
keyboard.press(Key.right)
time.sleep(abs(self.skeleton['position'][joint][0]))
keyboard.release(Key.right)
else:
keyboard.release(Key.right)
keyboard.release(Key.left)
def look_at_segmentation_area(robot, entity, volume=None):
""" Has a robot look at a certain object and possibly a volume
:param robot: robot object
:param entity: entity to look at
:param volume: string indicating the specific volume to look at (e.g., 'on_top_on' or 'shelf3')
"""
# Determine the height of the head target
# Start with a default
# Check if we have areas: use these
if volume in entity.volumes:
search_volume = entity.volumes[volume]
x_obj = 0.5 * (search_volume.min_corner.x() + search_volume.max_corner.x())
y_obj = 0.5 * (search_volume.min_corner.y() + search_volume.max_corner.y())
z_obj = search_volume.min_corner.z()
lookat_pos_map = entity.pose.frame * kdl.Vector(x_obj, y_obj, z_obj)
x = lookat_pos_map.x()
y = lookat_pos_map.y()
z = lookat_pos_map.z()
else:
# Look at the top of the entity to inspect
pos = entity.pose.frame.p
x = pos.x()
y = pos.y()
z = pos.z() + entity.shape.z_max
# Point the head at the right direction
robot.head.look_at_point(VectorStamped(x, y, z, "/map"), timeout=0)
# Make sure the spindle is at the appropriate height if we are AMIGO
if robot.robot_name == "amigo":
# Send spindle goal to bring head to a suitable location
# Correction for standard height: with a table heigt of 0.76 a spindle position
# of 0.35 is desired, hence offset = 0.76-0.35 = 0.41
# Minimum: 0.15 (to avoid crushing the arms), maximum 0.4
spindle_target = max(0.15, min(z - 0.41, robot.torso.upper_limit[0]))
robot.torso._send_goal([spindle_target], timeout=0)
robot.torso.wait_for_motion_done()
robot.head.wait_for_motion_done()
def move_tip(self, x=0., y=0., z=0., roll=0., pitch=0., yaw=0.):
"""
Moves the tooltip in the world frame by the given x,y,z / roll,pitch,yaw.
@return True on success
"""
transform = PyKDL.Frame(PyKDL.Rotation.RPY(pitch, roll, yaw),
PyKDL.Vector(-x, -y, -z))
tip_pose = self.get_tip_pose()
tip_pose_kdl = posemath.fromMsg(tip_pose)
final_pose = toMsg(tip_pose_kdl * transform)
self.arm_commander.set_start_state_to_current_state()
self.arm_commander.set_pose_targets([final_pose])
plan = self.arm_commander.plan()
if not self.arm_commander.execute(plan):
return False
return True
def __init__(self,
pos,
rot,
gripper,
goal=None,
cartesian_vel=0.35,
angular_vel=0.5):
self.pos = pos
self.rot = [r * np.pi for r in rot]
self.goal = goal
self.gripper = gripper
self.cartesian_vel = cartesian_vel
self.angular_vel = angular_vel
print(self.pos, self.rot)
pg = kdl.Vector(*self.pos)
Rg = kdl.Rotation.RPY(*self.rot)
self.T = kdl.Frame(Rg, pg)
def _loadFromFile(self):
self.data = np.genfromtxt(self.getGTName(),
delimiter=self.delimiter,
dtype=None,
names=True)
self.objs = {}
for row in self.data:
name = row[0]
if 'fake' in name:
continue
size = [row[1], row[2], row[3]]
p = PyKDL.Vector(row[4], row[5], row[6])
rot = PyKDL.Rotation.Quaternion(row[7], row[8], row[9], row[10])
color = np.array([row[11], row[12], row[13]])
self.objs[name] = {
"vobj": VirtualObject(frame=PyKDL.Frame(rot, p), size=size),
"color": color
}
def orientation_feature(feature):
eps = 1e-10 # some small number for alignment test
direction = msg2vec(feature.direction)
scale = direction.Norm()
axis = kdl.Vector(0,0,1) * direction
angle = direction.z()
laxis = axis.Norm()
if laxis > eps and scale > eps:
l = sqrt((1 - angle / scale) / 2) / laxis
qu = sqrt((1 + angle / scale) / 2)
q = [axis.x()*l, axis.y()*l, axis.z()*l, qu]
else:
# aligned with x/z-axis or zero-length: no rotation needed
q = [0, 0, 0, 1]
return q
def computeTSRsForSphere(radius, angleStep=30*numpy.pi/180.0):
radial_vector = kdl.Vector(0.0, 0.0, 1.0) * radius
x_angle_grid = numpy.linspace(0.0, 2*numpy.pi, int(2*numpy.pi/angleStep))
y_angle_grid = numpy.copy(x_angle_grid)
tsrs = []
for x_angle in x_angle_grid:
for y_angle in y_angle_grid:
R = kdl.Rotation.RPY(x_angle, y_angle, 0.0)
pos = R * radial_vector
rot = R * kdl.Rotation.RotX(numpy.pi)
tsrs.append(kdlFrameToBoxTSR(kdl.Frame(rot, pos)))
return tsrs
def pose_msg_to_kdl_frame(pose):
"""
Convert a geometry_msgs.msg.Pose message to a PyKDL.Frame object
:param pose: Pose to be converted
:type pose: geometry_msgs.msg.Pose
:rtype: PyKDL.Frame
>>> pose = gm.Pose(gm.Point(1, 2, 3), gm.Quaternion(1, 0, 0, 0))
>>> frame = pose_msg_to_kdl_frame(pose)
>>> frame.p
[ 1, 2, 3]
>>> frame.M.GetQuaternion()
(1.0, 0.0, 0.0, 0.0)
"""
rot = kdl.Rotation.Quaternion(pose.orientation.x, pose.orientation.y,
pose.orientation.z, pose.orientation.w)
trans = kdl.Vector(pose.position.x, pose.position.y, pose.position.z)
return kdl.Frame(rot, trans)
def test_transform(self):
b = tf2_ros.Buffer()
t = TransformStamped()
t.transform.translation.x = 1.0
t.transform.rotation = Quaternion(w=0.0, x=1.0, y=0.0, z=0.0)
t.header.stamp = builtin_interfaces.msg.Time(sec=2)
t.header.frame_id = 'a'
t.child_frame_id = 'b'
b.set_transform(t, 'eitan_rocks')
out = b.lookup_transform('a', 'b', rclpy.time.Time(seconds=2),
rclpy.time.Duration(seconds=2))
self.assertEqual(out.transform.translation.x, 1)
self.assertEqual(out.transform.rotation.x, 1)
self.assertEqual(out.header.frame_id, 'a')
self.assertEqual(out.child_frame_id, 'b')
v = PyKDL.Vector(1, 2, 3)
out = b.transform(
tf2_ros.Stamped(v, builtin_interfaces.msg.Time(sec=2), 'a'), 'b')
self.assertEqual(out.x(), 0)
self.assertEqual(out.y(), -2)
self.assertEqual(out.z(), -3)
f = PyKDL.Frame(PyKDL.Rotation.RPY(1, 2, 3), PyKDL.Vector(1, 2, 3))
out = b.transform(
tf2_ros.Stamped(f, builtin_interfaces.msg.Time(sec=2), 'a'), 'b')
self.assertEqual(out.p.x(), 0)
self.assertEqual(out.p.y(), -2)
self.assertEqual(out.p.z(), -3)
# TODO(tfoote) check values of rotation
t = PyKDL.Twist(PyKDL.Vector(1, 2, 3), PyKDL.Vector(4, 5, 6))
out = b.transform(
tf2_ros.Stamped(t, builtin_interfaces.msg.Time(sec=2), 'a'), 'b')
self.assertEqual(out.vel.x(), 1)
self.assertEqual(out.vel.y(), -8)
self.assertEqual(out.vel.z(), 2)
self.assertEqual(out.rot.x(), 4)
self.assertEqual(out.rot.y(), -5)
self.assertEqual(out.rot.z(), -6)
w = PyKDL.Wrench(PyKDL.Vector(1, 2, 3), PyKDL.Vector(4, 5, 6))
out = b.transform(
tf2_ros.Stamped(w, builtin_interfaces.msg.Time(sec=2), 'a'), 'b')
self.assertEqual(out.force.x(), 1)
self.assertEqual(out.force.y(), -2)
self.assertEqual(out.force.z(), -3)
self.assertEqual(out.torque.x(), 4)
self.assertEqual(out.torque.y(), -8)
self.assertEqual(out.torque.z(), -4)
def generate(self):
for ori_idx in range(len(orientations)):
T_H_Hd = orientations[ori_idx]
T_H_Od = T_H_Hd * self.T_H_O
for surf_pt_idx in range(len(self.surface_points_obj)):
surf_pt = self.surface_points_obj[surf_pt_idx]
if not surf_pt.allowed:
continue
pt = T_H_Od * surf_pt.pos
vol_idx = self.getVolIndex(pt)
if vol_idx != None:
if not ori_idx in self.vol_samples[vol_idx[0]][vol_idx[1]][vol_idx[2]]:
self.vol_samples[vol_idx[0]][vol_idx[1]][vol_idx[2]][ori_idx] = [surf_pt.id]
else:
self.vol_samples[vol_idx[0]][vol_idx[1]][vol_idx[2]][ori_idx].append(surf_pt.id)
print "transforming the volumetric map..."
for xi in range(self.vol_samples_count):
print xi
for yi in range(self.vol_samples_count):
for zi in range(self.vol_samples_count):
for ori in self.vol_samples[xi][yi][zi]:
planes = 0
edges = 0
points = 0
norm = PyKDL.Vector()
for pt_id in self.vol_samples[xi][yi][zi][ori]:
norm += self.surface_points_obj[pt_id].normal
if self.surface_points_obj[pt_id].is_plane:
planes += 1
if self.surface_points_obj[pt_id].is_edge:
edges += 1
if self.surface_points_obj[pt_id].is_point:
points += 1
norm.Normalize()
if planes >= edges and planes >= points:
self.vol_samples[xi][yi][zi][ori] = (norm, 0)
elif edges >= planes and edges >= points:
self.vol_samples[xi][yi][zi][ori] = (norm, 1)
else:
self.vol_samples[xi][yi][zi][ori] = (norm, 2)
def intersect(self, ray_dir_frame, **kwargs):
if 'normal' in kwargs and kwargs['normal']:
normal = kwargs['normal']
else:
normal = self._normal
if 'point' in kwargs and kwargs['point']:
point = kwargs['point']
else:
point = self._point
if not (normal and point):
return None
u = ray_dir_frame.p - ray_dir_frame * kdl.Vector(1.0, 0.0, 0.0)
# normal.Normalize()
n = normal
w = ray_dir_frame.p - point # - n * self._distance
D = kdl.dot(n, u)
N = -kdl.dot(n, w)
# Check if the vector is parallel to the plane
if math.fabs(D) > sys.float_info.epsilon:
sI = N / D
# Is the intersection on the back?
if sI >= 0.0:
return None
# Point on the plane
p = ray_dir_frame.p + sI * u
yaw = math.atan2(p.y(), p.x())
# Turn along the pointed direction in horizontal plane
q = kdl.Rotation.RPY(0.0, 0.0, yaw)
return kdl.Frame(q, p)
return None
def inverseKinematics(robotChain, pos, rot, qGuess=None, minJoints=None, maxJoints=None):
"""
Perform inverse kinematics
Args:
robotChain: robot's chain object
pos: 1 x 3 or 3 x 1 array of the end effector position.
rot: 3 x 3 array of the end effector rotation
qGuess: guess values for the joint positions
minJoints: minimum value of the position for each joint
maxJoints: maximum value of the position for each joint
Returns:
list of joint positions or None (no solution)
"""
# print("inside inverse: ", pos, " ; ", rot)
posKdl = kdl.Vector(pos[0], pos[1], pos[2])
rotKdl = kdl.Rotation(rot[0, 0], rot[0, 1], rot[0, 2],
rot[1, 0], rot[1, 1], rot[1, 2],
rot[2, 0], rot[2, 1], rot[2, 2])
frameKdl = kdl.Frame(rotKdl, posKdl)
numJoints = robotChain.getNrOfJoints()
minJoints = -np.pi * np.ones(numJoints) if minJoints is None else minJoints
maxJoints = np.pi * np.ones(numJoints) if maxJoints is None else maxJoints
minsKdl = jointListToKdl(minJoints)
maxsKdl = jointListToKdl(maxJoints)
fkKdl = kdl.ChainFkSolverPos_recursive(robotChain)
ikVKdl = kdl.ChainIkSolverVel_pinv(robotChain)
ikPKdl = kdl.ChainIkSolverPos_NR_JL(robotChain, minsKdl, maxsKdl,
fkKdl, ikVKdl)
if qGuess is None:
# use the midpoint of the joint limits as the guess
lowerLim = np.where(np.isfinite(minJoints), minJoints, 0.)
upperLim = np.where(np.isfinite(maxJoints), maxJoints, 0.)
qGuess = (lowerLim + upperLim) / 2.0
qGuess = np.where(np.isnan(qGuess), [0.]*len(qGuess), qGuess)
qKdl = kdl.JntArray(numJoints)
qGuessKdl = jointListToKdl(qGuess)
if ikPKdl.CartToJnt(qGuessKdl, frameKdl, qKdl) >= 0:
return jointKdlToList(qKdl)
else:
return None
def zero_forces(PSM, epsilon):
home = False
Kp = 0.005
Kd = 0.003
F_old = force_feedback
F_array = npm.repmat(force_feedback, 10, 1)
while home == False:
Fx = force_feedback[0]
Fy = force_feedback[1]
Fz = force_feedback[2]
Fx_d = Fx - F_old[0]
Fy_d = Fy - F_old[1]
Fz_d = Fz - F_old[2]
F_old = [Fx, Fy, Fz]
F_array[0, :] = force_feedback
F_array[1, :] = F_old
F_array[2:-1, :] = F_array[1:-2, :]
#F_average = (np.array(force_feedback) + np.array(F_old)+ np.array(F1)+np.array(F2)+np.array(F3)+np.array(F4))/6
F_median = np.median(F_array, 0)
F_average = np.mean(F_array, 0)
if np.linalg.norm(F_median) > epsilon:
#if np.abs(Fx) > 0.05:
p2.dmove(
PyKDL.Vector(Kp * Fx + Kd * Fx_d, Kp * Fy + Kd * Fy_d,
Kp * Fz + Kd * Fz_d))
#p2.dmove(PyKDL.Vector(Kp*Fx+Kd*Fx_d, 0, 0))
#print(np.linalg.norm(F_median))
#print(np.linalg.norm(force_feedback))
#print(force_feedback)
#print(str(Kp*Fx+Kd*Fx_d) + ',' +str(Kp*Fy+Kd*Fy_d) + ',' +str(-(Kp*Fz+Kd*Fz_d)))
else:
print(F_median)
print(F_average)
home = True
def create_right_chain(self):
ch = kdl.Chain()
self.right_arm_base_offset_from_torso_lift_link = np.matrix([0., -0.188, 0.]).T
# shoulder pan
ch.addSegment(kdl.Segment(kdl.Joint(kdl.Joint.RotZ),kdl.Frame(kdl.Vector(0.1,0.,0.))))
# shoulder lift
ch.addSegment(kdl.Segment(kdl.Joint(kdl.Joint.RotY),kdl.Frame(kdl.Vector(0.,0.,0.))))
# upper arm roll
ch.addSegment(kdl.Segment(kdl.Joint(kdl.Joint.RotX),kdl.Frame(kdl.Vector(0.4,0.,0.))))
# elbox flex
ch.addSegment(kdl.Segment(kdl.Joint(kdl.Joint.RotY),kdl.Frame(kdl.Vector(0.0,0.,0.))))
# forearm roll
ch.addSegment(kdl.Segment(kdl.Joint(kdl.Joint.RotX),kdl.Frame(kdl.Vector(0.321,0.,0.))))
# wrist flex
ch.addSegment(kdl.Segment(kdl.Joint(kdl.Joint.RotY),kdl.Frame(kdl.Vector(0.,0.,0.))))
# wrist roll
ch.addSegment(kdl.Segment(kdl.Joint(kdl.Joint.RotX),kdl.Frame(kdl.Vector(0.,0.,0.))))
return ch
def is_at_landing_spot(self):
rp = kdl.Vector(self.robot_current_pose.pose.position.x,
self.robot_current_pose.pose.position.y,
0.0)
ls = self.landing_spot
d = (rp - ls).Norm()
if math.isnan(d):
rospy.loginfo('Landing anywhere')
return True
# rospy.loginfo('Robot pose: {}'.format(rp))
if d < (self.landing_tolerance * 1.5):
rospy.loginfo('Distance to landing spot: {}'.format(d))
if d < self.landing_tolerance:
return True
else:
return False
def _get_goal(self):
if self._command is None or rospy.get_time() - self._last_reference_update > 0.1:
return self._last_goal
# Compute the step from the input velocity
step_frame = PyKDL.Frame(
PyKDL.Rotation.RPY(self._command[3],
self._command[4],
self._command[5]),
PyKDL.Vector(self._command[0],
self._command[1],
self._command[2]))
next_goal = self._last_goal * step_frame
g_pos = [next_goal.p.x(), next_goal.p.y(), next_goal.p.z()]
g_quat = next_goal.M.GetQuaternion()
if self._arm_interface.inverse_kinematics(g_pos, g_quat) is not None:
return next_goal
else:
return self._last_goal
def getFrameFromPose(pose):
"""
Return a PyKDL.Frame object from a Pose().
@type pose: geometry_msgs/Pose
@param pose: Pose.
@rtype: PyKDL.Frame
"""
V = kdl.Vector(pose.position.x, pose.position.y, pose.position.z)
x = pose.orientation.x
y = pose.orientation.y
z = pose.orientation.z
w = pose.orientation.w
qmag = math.sqrt(x**2 + y**2 + z**2 + w**2)
R = kdl.Rotation.Quaternion(pose.orientation.x / qmag,
pose.orientation.y / qmag,
pose.orientation.z / qmag,
pose.orientation.w / qmag)
return kdl.Frame(R, V)
def pubVirtualBowlcenPose(self):
f = PyKDL.Frame.Identity()
f.p = PyKDL.Vector(0.5, 0.2, -0.2)
f.M = PyKDL.Rotation.Quaternion(0, 0, 0, 1)
# frame pub --------------------------------------
ps = PoseStamped()
ps.header.frame_id = 'torso_lift_link'
ps.header.stamp = rospy.Time.now()
ps.pose.position.x = f.p[0]
ps.pose.position.y = f.p[1]
ps.pose.position.z = f.p[2]
ps.pose.orientation.x = f.M.GetQuaternion()[0]
ps.pose.orientation.y = f.M.GetQuaternion()[1]
ps.pose.orientation.z = f.M.GetQuaternion()[2]
ps.pose.orientation.w = f.M.GetQuaternion()[3]
self.bowl_cen_pose_pub.publish(ps)
def moveToHandle():
print "Ruch do klamki..."
DoorR = velma.getTf("B", "right_door") #getTf(frame_from, frame_to); "B"-robot base frame (torso_base); returns: PyKDL.Frame transformation from base frame to target frame
doorR_x, doorR_y, doorR_z = DoorR.p
DoorL = velma.getTf("B", "left_door") #getTf(frame_from, frame_to); "B"-robot base frame (torso_base); returns: PyKDL.Frame transformation from base frame to target frame
doorL_x, doorL_y, doorL_z = DoorL.p
position = velma.getLastJointState()
Goal = PyKDL.Vector(doorL_x,doorL_y,doorR_z+0.1)
T_B_Door = PyKDL.Frame(PyKDL.Rotation.RotZ(alfa), Goal)
if not velma.moveCartImpRight([T_B_Door], [10.0], None, None, None, None, PyKDL.Wrench(PyKDL.Vector(100,100,0.7), PyKDL.Vector(0.7,0.7,0.7)), start_time=0.5, path_tol=PyKDL.Twist(PyKDL.Vector(0.2,0.2,1),PyKDL.Vector(1,1,1))):
exitError(8)
if velma.waitForEffectorRight() != 0:
print "Velma: Dotykam klamki :p"
rospy.sleep(0.5)
resetPoseTol()
rospy.sleep(0.5)
def __init__(self, robot_name="", params_dict={}):
self.robot_name = robot_name
self.is_active = False
self.done = False
# Controller Parameters
if params_dict == {} or params_dict == None:
params_dict = self.DEFAULT_PARAMETERS
self.params = params_dict
self.damp_force_threshold = self._param("damp_force_threshold",
params_dict)
self.damp_magnitude = self._param("damp_magnitude",
params_dict)
self.initial_velocity = self._param("velocity",
params_dict)
self.velocity = 0
self.axis = PyKDL.Vector()
self.last_force_msg = Twist()
self.force_limit = [2, 2, 2]
def move_table(userdata=None):
""" 'Locks' a locking designator """
# For now, don't do anything
return "done"
# Move away the cabinet
robot.ed.update_entity(id="cabinet",
frame_stamped=FrameStamped(frame=kdl.Frame(kdl.Rotation(),
kdl.Vector(12.0, 0, 0)),
frame_id="map"))
# Determine where to perform the challenge
robot_pose = robot.base.get_location()
ENTITY_POSES.sort(key=lambda tup: (tup[0].frame.p - robot_pose.frame.p).Norm())
# Update the world model
robot.ed.update_entity(id=CABINET, frame_stamped=ENTITY_POSES[0][0])
robot.ed.update_entity(id=TABLE, frame_stamped=ENTITY_POSES[0][1])
return "done"
def get_pointer(self, ws_shape, ray_kdl_frame, pass_point=None, cache=False):
robot_kdl_frame = self.frame_from_tf(self.human_frame, self.robot_frame)
if not pass_point:
if not ws_shape.point:
if robot_kdl_frame:
# Initialize with robot's current position
ws_shape.point = copy.deepcopy(robot_kdl_frame.p)
else:
return None
pass_point = ws_shape.point
ray_origin_kdl_frame = self.frame_from_tf(self.human_frame, self.ray_origin_frame)
shape_origin_kdl_frame = self.frame_from_tf(self.human_frame, ws_shape.frame_id)
pointer_pose = None
if pass_point and ray_origin_kdl_frame and shape_origin_kdl_frame:
actual_pass_point = pass_point - copy.deepcopy(shape_origin_kdl_frame.p)
if cache: # or not ws_shape.point:
ws_shape.point = pass_point
if isinstance(ws_shape, Plane):
normal = None
if ws_shape is self.vis_plane:
tmp_point = copy.deepcopy(ray_origin_kdl_frame.p)
robot_dir = actual_pass_point - tmp_point
normal = kdl.Vector(robot_dir.x(), robot_dir.y(), 0.0)
if cache:
ws_shape.normal = normal
pointer_pose = ws_shape.ray_to_location(ray_kdl_frame, normal=normal, point=actual_pass_point)
else:
pointer_pose = ws_shape.ray_to_location(ray_kdl_frame, point=actual_pass_point)
return pointer_pose
def transform_frame_to_map(self, cloud, tf):
rospy.loginfo("creating transformer")
if (tf is None):
self.listener = tf.TransformListener()
else:
self.listener = TransformationStore().msg_to_transformer(tf)
rospy.sleep(5)
self.child_camera_frame = cloud.header.frame_id
rospy.loginfo("doing conversion")
t = self.listener.getLatestCommonTime("map", self.child_camera_frame)
tr_r = self.listener.lookupTransform("map", self.child_camera_frame, t)
tr = Transform()
tr.translation = Vector3(tr_r[0][0], tr_r[0][1], tr_r[0][2])
tr.rotation = Quaternion(tr_r[1][0], tr_r[1][1], tr_r[1][2],
tr_r[1][3])
#self.transform_frame_to_map = tr
tr_s = TransformStamped()
tr_s.header = std_msgs.msg.Header()
tr_s.header.stamp = rospy.Time.now()
tr_s.header.frame_id = 'map'
tr_s.child_frame_id = self.child_camera_frame
tr_s.transform = tr
t_kdl = self.transform_to_kdl(tr_s)
points_out = []
for p_in in pc2.read_points(cloud, field_names=["x", "y", "z", "rgb"]):
p_out = t_kdl * PyKDL.Vector(p_in[0], p_in[1], p_in[2])
points_out.append([p_out[0], p_out[1], p_out[2], p_in[3]])
fil_fields = []
for x in cloud.fields:
if (x.name in "x" or x.name in "y" or x.name in "z"
or x.name in "rgb"):
fil_fields.append(x)
res = pc2.create_cloud(cloud.header, fil_fields, points_out)
return res
def __init__(self):
# Get the node params
self.linear_multiplier = rospy.get_param('~linear_multiplier')
self.angular_multiplier = rospy.get_param('~angular_multiplier')
self.broadcast_rate = rospy.get_param('~broadcast_rate')
self.body_fixed = rospy.get_param('~body_fixed')
# Initialize the frame we're going to publish
xyzw = [float(f) for f in rospy.get_param('~xyzw', '0 0 0 1').split()]
xyzw_offset = [
float(f)
for f in rospy.get_param('~xyzw_offset', '0 0 0 1').split()
]
xyz = [float(f) for f in rospy.get_param('~xyz', '0 0 0').split()]
rospy.logwarn("xyzw: " + str(xyzw))
rospy.logwarn("xyz: " + str(xyz))
self.transform = PyKDL.Frame(
PyKDL.Rotation.Quaternion(*xyzw) *
PyKDL.Rotation.Quaternion(*xyzw_offset), PyKDL.Vector(*xyz))
self.transform_out = PyKDL.Frame()
self.rotation = self.transform.M.GetQuaternion()
self.time = rospy.Time.now()
self.frame_id = rospy.get_param('~frame_id')
self.child_frame_id = rospy.get_param('~child_frame_id')
# Synchronization
self.broadcast_lock = threading.Lock()
# Initialze TF structures
self.broadcaster = tf.TransformBroadcaster()
# Subscribe to twist inputs
self.twist_sub = rospy.Subscriber("twist", geometry_msgs.Twist,
self.twist_cb)
self.twiststamped_sub = rospy.Subscriber("twist_stamped",
geometry_msgs.TwistStamped,
self.twiststamped_cb)
# Broadcast the frame at a given rate
self.broadcast_thread = threading.Thread(target=self.broadcast_loop)
self.broadcast_thread.start()
