def msg_to_kdl(msg):
if isinstance(msg, TransformStamped):
return transform_to_kdl(msg)
elif isinstance(msg, PoseStamped):
return pose_to_kdl(msg.pose)
elif isinstance(msg, Pose):
return pose_to_kdl(msg)
elif isinstance(msg, PointStamped):
return point_to_kdl(msg.point)
elif isinstance(msg, Point):
return point_to_kdl(msg)
elif isinstance(msg, QuaternionStamped):
return quaternion_to_kdl(msg.quaternion)
elif isinstance(msg, Quaternion):
return quaternion_to_kdl(msg)
elif isinstance(msg, Twist):
return twist_to_kdl(msg)
elif isinstance(msg, TwistStamped):
return twist_to_kdl(msg.twist)
elif isinstance(msg, Vector3Stamped):
return point_to_kdl(msg.vector)
elif isinstance(msg, Vector3):
return point_to_kdl(msg)
else:
raise TypeError(u'can\'t convert {} to kdl'.format(type(msg)))
def get_waypoints(self):
start = None
ti = 0
if self.current_trajectory is not None:
ti = self.get_trajectory_index(self.current_trajectory, offset=1.0)
if ti is None or ti == 0:
ti = 0
try:
trans = self.tfBuffer.lookup_transform(
self.reference_frame, self.own_frame,
rclpy.time.Time().to_msg())
frame = tf2_kdl.transform_to_kdl(trans)
start = (frame.p.x(), frame.p.y(), frame.M.GetRPY()[2])
except Exception as e:
self.get_logger().info(f"Exception in tf transformations\n{e}")
else:
start = (self.current_trajectory.poses[ti].pose.position.x,
self.current_trajectory.poses[ti].pose.position.y,
yaw_from_orientation(
self.current_trajectory.poses[ti].pose.orientation))
goal = (self.goal.pose.position.x, self.goal.pose.position.y,
yaw_from_orientation(self.goal.pose.orientation))
return [start, goal], ti
def get_initial_position(self, robot):
if robot.simulation:
robot_to_gob = (0.04, 0.08)
else:
robot_to_gob = robot.actuators.PUMPS.get(self.pump_id).get("pos")
gob_to_robot = TransformStamped()
gob_to_robot.transform.translation.x = -robot_to_gob[0]
gob_to_robot.transform.translation.y = -robot_to_gob[1]
goal_pose = TransformStamped()
goal_pose.transform.translation.x = self.position[0]
goal_pose.transform.translation.y = self.position[1]
robot_pose = PoseStamped()
robot_pose.pose.position.x = robot.get_position()[0]
robot_pose.pose.position.y = robot.get_position()[1]
angle = self.get_initial_orientation(robot)
rot = Rotation.RotZ(angle)
q = rot.GetQuaternion()
goal_pose.transform.rotation.x = q[0]
goal_pose.transform.rotation.y = q[1]
goal_pose.transform.rotation.z = q[2]
goal_pose.transform.rotation.w = q[3]
gob_to_robot_kdl = transform_to_kdl(gob_to_robot)
robot_goal_pose_kdl = do_transform_frame(gob_to_robot_kdl, goal_pose)
return (robot_goal_pose_kdl.p.x(), robot_goal_pose_kdl.p.y())
def get_transfrom(reference_frame, target_frame):
listener = tf.TransformListener()
try:
listener.waitForTransform(reference_frame,
target_frame,
rospy.Time(0),
timeout=rospy.Duration(1))
translation_rotation = listener.lookupTransform(
reference_frame, target_frame, rospy.Time())
except Exception as e1:
try:
tf_buffer = tf2_ros.Buffer()
tf2_listener = tf2_ros.TransformListener(tf_buffer)
transform_stamped = tf_buffer.lookup_transform(
reference_frame,
target_frame,
rospy.Time(0),
timeout=rospy.Duration(1))
translation_rotation = tf_conversions.toTf(
tf2_kdl.transform_to_kdl(transform_stamped))
except Exception as e2:
rospy.logerr("get_transfrom::\n " +
"Failed to find transform from %s to %s" % (
reference_frame,
target_frame,
))
return translation_rotation
def _publish_tf(self, stamp):
# check that we know which frames we need to publish from
if self.map is None or self.base_frame is None:
rospy.loginfo('Not publishing until map and odometry frames found')
return
# calculate the mean position
x = np.mean([p.x for p in self.particles])
y = np.mean([p.y for p in self.particles])
theta = np.mean([p.theta for p in self.particles]) #TODO - wraparound
# map to base_link
map2base_frame = PyKDL.Frame(
PyKDL.Rotation.Quaternion(*transformations.quaternion_from_euler(0, 0, theta)),
PyKDL.Vector(x,y,0)
)
odom2base_frame = tf2_kdl.transform_to_kdl(self.tf_buffer.lookup_transform(
target_frame=self.odom_frame,
source_frame=self.base_frame,
time=stamp,
timeout=rospy.Duration(4.0)
))
# derive frame according to REP105
map2odom = map2base_frame * odom2base_frame.Inverse()
br = tf2_ros.TransformBroadcaster()
t = TransformStamped()
t.header.stamp = stamp
t.header.frame_id = self.map.frame
t.child_frame_id = self.odom_frame
t.transform.translation = Vector3(*map2odom.p)
t.transform.rotation = Quaternion(*map2odom.M.GetQuaternion())
br.sendTransform(t)
# for Debugging
if False:
t.header.stamp = stamp
t.header.frame_id = self.map.frame
t.child_frame_id = self.base_frame+"_old"
t.transform.translation = Vector3(*map2base_frame.p)
t.transform.rotation = Quaternion(*map2base_frame.M.GetQuaternion())
br.sendTransform(t)
if self.publish_cloud:
msg = PoseArray(
header=Header(stamp=stamp, frame_id=self.map.frame),
poses=[
Pose(
position=Point(p.x, p.y, 0),
orientation=Quaternion(*transformations.quaternion_from_euler(0, 0, p.theta))
)
for p in self.particles
]
)
self.pose_array.publish(msg)
def _publish_tf(self, stamp):
# check that we know which frames we need to publish from
if self.map is None or self.base_frame is None:
rospy.loginfo('Not publishing until map and odometry frames found')
return
# calculate the mean position
x = np.mean([p.x for p in self.particles])
y = np.mean([p.y for p in self.particles])
theta = np.mean([p.theta for p in self.particles]) #TODO - wraparound
# map to base_link
map2base_frame = PyKDL.Frame(
PyKDL.Rotation.Quaternion(
*transformations.quaternion_from_euler(0, 0, theta)),
PyKDL.Vector(x, y, 0))
odom2base_frame = tf2_kdl.transform_to_kdl(
self.tf_buffer.lookup_transform(target_frame=self.odom_frame,
source_frame=self.base_frame,
time=stamp,
timeout=rospy.Duration(4.0)))
# derive frame according to REP105
map2odom = map2base_frame * odom2base_frame.Inverse()
br = tf2_ros.TransformBroadcaster()
t = TransformStamped()
t.header.stamp = stamp
t.header.frame_id = self.map.frame
t.child_frame_id = self.odom_frame
t.transform.translation = Vector3(*map2odom.p)
t.transform.rotation = Quaternion(*map2odom.M.GetQuaternion())
br.sendTransform(t)
# for Debugging
if False:
t.header.stamp = stamp
t.header.frame_id = self.map.frame
t.child_frame_id = self.base_frame + "_old"
t.transform.translation = Vector3(*map2base_frame.p)
t.transform.rotation = Quaternion(
*map2base_frame.M.GetQuaternion())
br.sendTransform(t)
if self.publish_cloud:
msg = PoseArray(
header=Header(stamp=stamp, frame_id=self.map.frame),
poses=[
Pose(position=Point(p.x, p.y, 0),
orientation=Quaternion(
*transformations.quaternion_from_euler(
0, 0, p.theta))) for p in self.particles
])
self.pose_array.publish(msg)
def calculate_ik(base_link, tip_link, seed_joint_state,
goal_transform_geometry_msg):
"""
Calculates the Inverse Kinematics from base_link to tip_link according to the given
goal_transform_geometry_msg. The initial joint states would be considered from seed_joint_state.
Returns the result joint states and the success status.
base_link eg. - "triangle_base_link" or "calib_left_arm_base_link" or "calib_right_arm_base_link"
tip_link eg. - "left_arm_7_link" or "right_arm_7_link"
"""
robot_urdf_string = rospy.get_param('robot_description')
urdf_obj = urdf_parser_py.urdf.URDF.from_xml_string(robot_urdf_string)
_, kdl_tree = kdl_parser_py.urdf.treeFromUrdfModel(urdf_obj)
kdl_chain = kdl_tree.getChain(base_link, tip_link)
num_joints = kdl_chain.getNrOfJoints()
print "number of joints: " + str(num_joints)
# Get Joint limits
kdl_joint_limits_min, kdl_joint_limits_max = get_kdl_joint_limit_arrays(
kdl_chain, urdf_obj)
fk_solver = PyKDL.ChainFkSolverPos_recursive(kdl_chain)
velocity_ik = PyKDL.ChainIkSolverVel_pinv(kdl_chain)
# ik_solver = PyKDL.ChainIkSolverPos_LMA(kdl_chain, 1e-5, 1000, 1e-15)
ik_solver = PyKDL.ChainIkSolverPos_NR_JL(kdl_chain, kdl_joint_limits_min,
kdl_joint_limits_max, fk_solver,
velocity_ik)
# Getting the goal frame and seed state
goal_frame_kdl = tf2_kdl.transform_to_kdl(goal_transform_geometry_msg)
seed_joint_state_kdl = get_kdl_jnt_array_from_list(num_joints,
seed_joint_state)
# Solving IK
result_joint_state_kdl = solve_ik(ik_solver, num_joints,
seed_joint_state_kdl, goal_frame_kdl)
# check if calculated joint state results in the correct end-effector position using FK
goal_pose_reached = check_ik_result_using_fk(fk_solver,
result_joint_state_kdl,
goal_frame_kdl)
# check if calculated joint state is within joint limits
joints_within_limits = check_result_joints_are_within_limits(
num_joints, result_joint_state_kdl, kdl_joint_limits_min,
kdl_joint_limits_max)
print "Result Joint State Within Limits: " + str(joints_within_limits)
print "Can Reach Goal Pose With Solution: " + str(goal_pose_reached)
result_joint_state_vector = get_list_from_kdl_jnt_array(
num_joints, result_joint_state_kdl)
goal_pose_reached_successfully = goal_pose_reached and joints_within_limits
return result_joint_state_vector, goal_pose_reached_successfully
def get_transform(self, reference_frame, target_frame):
try:
transform_stamped = self.tfBuffer.lookup_transform(
reference_frame, target_frame, rospy.Time(0),
rospy.Duration(1.0))
translation_rotation = tf_conversions.toTf(
tf2_kdl.transform_to_kdl(transform_stamped))
except (tf2_ros.LookupException, tf2_ros.ConnectivityException,
tf2_ros.ExtrapolationException), e:
print(e)
rospy.logerr('FAILED TO GET TRANSFORM FROM %s to %s' %
(reference_frame, target_frame))
return None
def get_waypoints(self):
start = Pose2D()
try:
trans = self.tfBuffer.lookup_transform(
self.reference_frame,
self.own_frame,
rclpy.time.Time().to_msg(),
timeout=rclpy.time.Duration(seconds=3.0))
frame = tf2_kdl.transform_to_kdl(trans)
start.theta = frame.M.GetRPY()[2]
start.x = frame.p.x()
start.y = frame.p.y()
#self.get_logger().debug(f"got the following for ego position: {start}")
except Exception as e:
self.get_logger().info(f"Exception in tf transformations\n{e}")
goal = Pose2D()
goal.x = self.goal.pose.position.x
goal.y = self.goal.pose.position.y
goal.theta = yaw_from_orientation(self.goal.pose.orientation)
return [start, goal]
def __init__(self):
self.object_names = rospy.get_param(
'object_list', ['RedCylinder', 'BlueCuboid', 'GreenCube'])
self.trans_noise = rospy.get_param('trans_noise', 0.0)
self.rotation_noise = rospy.get_param('rotation_noise', 0.0)
self.camera_link_name = rospy.get_param('camera_link_name',
'camera_link')
self.object_publisher = rospy.Publisher('/recognized_objects_array',
RecognizedObjectArray,
queue_size=10)
# Get camera link in reference to the world through tf
self.tf_buffer = tf2_ros.Buffer()
self.tf_listener = tf2_ros.TransformListener(self.tf_buffer)
# tf publisher
self.tf_pub = tf2_ros.TransformBroadcaster()
self.matched_object_list = []
self.matched_object_lock = threading.Lock()
rospy.sleep(5)
cam_trans_msg = None
while cam_trans_msg is None:
try:
cam_trans_msg = self.tf_buffer.lookup_transform(
self.camera_link_name, 'world', rospy.Time.now())
finally:
if cam_trans_msg is not None:
break
self.cam_trans = tf2_kdl.transform_to_kdl(cam_trans_msg)
self.gazebo_subscriber = rospy.Subscriber('/gazebo/model_states',
ModelStates,
self.calback_gazebo_state,
queue_size=30)
def odom_callback(self, msg):
"""Odom callback, computes the new pose of the robot relative to map,
send this new pose on odom_map_relative topic and start vlx routine
if this pose is near walls"""
robot_tf = TransformStamped()
robot_tf.transform.translation.x = msg.pose.pose.position.x
robot_tf.transform.translation.y = msg.pose.pose.position.y
robot_tf.transform.rotation = msg.pose.pose.orientation
robot_frame = transform_to_kdl(robot_tf)
new_robot_pose_kdl = do_transform_frame(robot_frame, self._initial_tf)
self.robot_pose.pose.position.x = new_robot_pose_kdl.p.x()
self.robot_pose.pose.position.y = new_robot_pose_kdl.p.y()
q = new_robot_pose_kdl.M.GetQuaternion()
self.robot_pose.header.stamp = msg.header.stamp
self.robot_pose.header.frame_id = "map"
self.robot_pose.pose.orientation = Quaternion(x=q[0], y=q[1], z=q[2], w=q[3])
self.odom_map_relative_publisher_.publish(self.robot_pose)
if self.is_near_walls(self.robot_pose.pose.position):
if self.vlx.continuous_sampling == 2:
self.vlx.near_wall_routine(self.robot_pose)
else:
self.vlx.start_near_wall_routine(self.robot_pose)
elif self.vlx.continuous_sampling == 2:
self.vlx.stop_near_wall_routine()
for topic, msg, t in bag.read_messages(topics=['desired_pose']):
trans_list.append(msg)
for trans in trans_list:
# Publish the desired tf
trans.header.stamp = rospy.Time.now()
broadcaster.sendTransform(trans)
# Find your current cartesian pose
youbot.forward_kinematics(youbot.current_joint_position,
youbot.current_pose)
youbot.broadcast_pose(youbot.current_pose)
# Get next desired pose from rosbag
frame = tf2_kdl.transform_to_kdl(trans)
# Ikine
jointkdl = youbot.inverse_kinematics_closed(frame)
joint_array = []
for pos in jointkdl:
joint_array.append(pos)
print("publishing joint: {}".format(joint_array))
rospy.sleep(0.2)
youbot.publish_joint_trajectory(joint_array)
rospy.sleep(1)
raw_input('Press enter for next pose\n')
def get_frame(parent, frame, time):
return tf2_kdl.transform_to_kdl(
tf2_buffer.lookup_transform(parent, frame, time))
frames = []
while not rospy.is_shutdown():
try:
key = raw_input('Press Enter to read next frame ')
if key == 'q':
break
trans_true = buffer.lookup_transform('tool_tracker', 'world',
rospy.Time())
trans_vive = buffer.lookup_transform('world_vive',
'tracker_LHR_0DBB917B',
rospy.Time())
frame_true = tf2_kdl.transform_to_kdl(trans_true)
frame_vive = tf2_kdl.transform_to_kdl(trans_vive)
world_vive = frame_vive * frame_true
print('Current:', world_vive.p, world_vive.M.GetRPY())
frames.append(world_vive)
xyz = [(f.p.x(), f.p.y(), f.p.z()) for f in frames]
rpy = [f.M.GetRPY() for f in frames]
print('Mean:',
np.mean(xyz, axis=0).tolist(),
np.mean(rpy, axis=0).tolist())
print('Std:',
np.std(xyz, axis=0).tolist(),
np.std(rpy, axis=0).tolist())