; LLVM IR from erwincoumans/tiny-differentiable-simulator :: third_party/rbdl/addons/urdfreader (urdfreader)

; Unoptimized IR (clang -disable-llvm-passes):
target datalayout = "e-m:e-p270:32:32-p271:32:32-p272:64:64-i64:64-i128:128-f80:128-n8:16:32:64-S128"
target triple = "x86_64-unknown-linux-gnu"

%"class.std::basic_ostream" = type { ptr, %"class.std::basic_ios" }
%"class.std::basic_ios" = type { %"class.std::ios_base", ptr, i8, i8, ptr, ptr, ptr, ptr }
%"class.std::ios_base" = type { ptr, i64, i64, i32, i32, i32, ptr, %"struct.std::ios_base::_Words", [8 x %"struct.std::ios_base::_Words"], i32, ptr, %"class.std::locale" }
%"struct.std::ios_base::_Words" = type { ptr, i64 }
%"class.std::locale" = type { ptr }
%class.my_shared_ptr.1 = type { ptr, ptr }
%"class.std::map" = type { %"class.std::_Rb_tree" }
%"class.std::_Rb_tree" = type { %"struct.std::_Rb_tree<std::__cxx11::basic_string<char>, std::pair<const std::__cxx11::basic_string<char>, my_shared_ptr<urdf::Link>>, std::_Select1st<std::pair<const std::__cxx11::basic_string<char>, my_shared_ptr<urdf::Link>>>, std::less<std::__cxx11::basic_string<char>>>::_Rb_tree_impl" }
%"struct.std::_Rb_tree<std::__cxx11::basic_string<char>, std::pair<const std::__cxx11::basic_string<char>, my_shared_ptr<urdf::Link>>, std::_Select1st<std::pair<const std::__cxx11::basic_string<char>, my_shared_ptr<urdf::Link>>>, std::less<std::__cxx11::basic_string<char>>>::_Rb_tree_impl" = type { [8 x i8], %"struct.std::_Rb_tree_header" }
%"struct.std::_Rb_tree_header" = type { %"struct.std::_Rb_tree_node_base", i64 }
%"struct.std::_Rb_tree_node_base" = type { i32, ptr, ptr, ptr }
%"class.std::map.2" = type { %"class.std::_Rb_tree.3" }
%"class.std::_Rb_tree.3" = type { %"struct.std::_Rb_tree<std::__cxx11::basic_string<char>, std::pair<const std::__cxx11::basic_string<char>, my_shared_ptr<urdf::Joint>>, std::_Select1st<std::pair<const std::__cxx11::basic_string<char>, my_shared_ptr<urdf::Joint>>>, std::less<std::__cxx11::basic_string<char>>>::_Rb_tree_impl" }
%"struct.std::_Rb_tree<std::__cxx11::basic_string<char>, std::pair<const std::__cxx11::basic_string<char>, my_shared_ptr<urdf::Joint>>, std::_Select1st<std::pair<const std::__cxx11::basic_string<char>, my_shared_ptr<urdf::Joint>>>, std::less<std::__cxx11::basic_string<char>>>::_Rb_tree_impl" = type { [8 x i8], %"struct.std::_Rb_tree_header" }
%"class.std::vector" = type { %"struct.std::_Vector_base" }
%"struct.std::_Vector_base" = type { %"struct.std::_Vector_base<std::__cxx11::basic_string<char>, std::allocator<std::__cxx11::basic_string<char>>>::_Vector_impl" }
%"struct.std::_Vector_base<std::__cxx11::basic_string<char>, std::allocator<std::__cxx11::basic_string<char>>>::_Vector_impl" = type { %"struct.std::_Vector_base<std::__cxx11::basic_string<char>, std::allocator<std::__cxx11::basic_string<char>>>::_Vector_impl_data" }
%"struct.std::_Vector_base<std::__cxx11::basic_string<char>, std::allocator<std::__cxx11::basic_string<char>>>::_Vector_impl_data" = type { ptr, ptr, ptr }
%"class.std::stack" = type { %"class.std::deque" }
%"class.std::deque" = type { %"class.std::_Deque_base" }
%"class.std::_Deque_base" = type { %"struct.std::_Deque_base<my_shared_ptr<urdf::Link>, std::allocator<my_shared_ptr<urdf::Link>>>::_Deque_impl" }
%"struct.std::_Deque_base<my_shared_ptr<urdf::Link>, std::allocator<my_shared_ptr<urdf::Link>>>::_Deque_impl" = type { %"struct.std::_Deque_base<my_shared_ptr<urdf::Link>, std::allocator<my_shared_ptr<urdf::Link>>>::_Deque_impl_data" }
%"struct.std::_Deque_base<my_shared_ptr<urdf::Link>, std::allocator<my_shared_ptr<urdf::Link>>>::_Deque_impl_data" = type { ptr, i64, %"struct.std::_Deque_iterator", %"struct.std::_Deque_iterator" }
%"struct.std::_Deque_iterator" = type { ptr, ptr, ptr, ptr }
%"class.std::stack.21" = type { %"class.std::deque.22" }
%"class.std::deque.22" = type { %"class.std::_Deque_base.23" }
%"class.std::_Deque_base.23" = type { %"struct.std::_Deque_base<int, std::allocator<int>>::_Deque_impl" }
%"struct.std::_Deque_base<int, std::allocator<int>>::_Deque_impl" = type { %"struct.std::_Deque_base<int, std::allocator<int>>::_Deque_impl_data" }
%"struct.std::_Deque_base<int, std::allocator<int>>::_Deque_impl_data" = type { ptr, i64, %"struct.std::_Deque_iterator.27", %"struct.std::_Deque_iterator.27" }
%"struct.std::_Deque_iterator.27" = type { ptr, ptr, ptr, ptr }
%class.my_shared_ptr.28 = type { ptr, ptr }
%class.Vector3_t = type { %"class.Eigen::Matrix" }
%"class.Eigen::Matrix" = type { %"class.Eigen::PlainObjectBase" }
%"class.Eigen::PlainObjectBase" = type { %"class.Eigen::DenseStorage" }
%"class.Eigen::DenseStorage" = type { %"struct.Eigen::internal::plain_array" }
%"struct.Eigen::internal::plain_array" = type { [3 x double] }
%class.Matrix3_t = type { %"class.Eigen::Matrix.52" }
%"class.Eigen::Matrix.52" = type { %"class.Eigen::PlainObjectBase.53" }
%"class.Eigen::PlainObjectBase.53" = type { %"class.Eigen::DenseStorage.60" }
%"class.Eigen::DenseStorage.60" = type { %"struct.Eigen::internal::plain_array.61" }
%"struct.Eigen::internal::plain_array.61" = type { [9 x double] }
%"struct.RigidBodyDynamics::Body" = type <{ double, %class.Vector3_t, %class.Matrix3_t, i8, [7 x i8] }>
%"struct.RigidBodyDynamics::Joint" = type { ptr, i32, i32, i32, i32 }
%"struct.RigidBodyDynamics::Math::SpatialTransform" = type { %class.Matrix3_t, %class.Vector3_t }
%"class.std::__cxx11::basic_string" = type { %"struct.std::__cxx11::basic_string<char>::_Alloc_hider", i64, %union.anon }
%"struct.std::__cxx11::basic_string<char>::_Alloc_hider" = type { ptr }
%union.anon = type { i64, [8 x i8] }
%class.my_shared_ptr.42 = type { ptr, ptr }
%class.SpatialVector_t = type { %"class.Eigen::Matrix.161" }
%"class.Eigen::Matrix.161" = type { %"class.Eigen::PlainObjectBase.162" }
%"class.Eigen::PlainObjectBase.162" = type { %"class.Eigen::DenseStorage.169" }
%"class.Eigen::DenseStorage.169" = type { %"struct.Eigen::internal::plain_array.170" }
%"struct.Eigen::internal::plain_array.170" = type { [6 x double] }
%"class.Eigen::CwiseNullaryOp" = type { [8 x i8], %"struct.Eigen::internal::scalar_constant_op" }
%"struct.Eigen::internal::scalar_constant_op" = type { double }
%"class.Eigen::Transpose" = type { ptr }
%"class.Eigen::CwiseNullaryOp.181" = type { [8 x i8], %"struct.Eigen::internal::scalar_constant_op" }
%"class.urdf::ModelInterface" = type { %"class.std::map", %"class.std::map.2", %"class.std::map.7", %"class.std::__cxx11::basic_string", %class.my_shared_ptr.1, i32, i32 }
%"class.std::map.7" = type { %"class.std::_Rb_tree.8" }
%"class.std::_Rb_tree.8" = type { %"struct.std::_Rb_tree<std::__cxx11::basic_string<char>, std::pair<const std::__cxx11::basic_string<char>, my_shared_ptr<urdf::Material>>, std::_Select1st<std::pair<const std::__cxx11::basic_string<char>, my_shared_ptr<urdf::Material>>>, std::less<std::__cxx11::basic_string<char>>>::_Rb_tree_impl" }
%"struct.std::_Rb_tree<std::__cxx11::basic_string<char>, std::pair<const std::__cxx11::basic_string<char>, my_shared_ptr<urdf::Material>>, std::_Select1st<std::pair<const std::__cxx11::basic_string<char>, my_shared_ptr<urdf::Material>>>, std::less<std::__cxx11::basic_string<char>>>::_Rb_tree_impl" = type { [8 x i8], %"struct.std::_Rb_tree_header" }
%"class.urdf::Link" = type { %"class.std::__cxx11::basic_string", %class.my_shared_ptr.29, %class.my_shared_ptr.30, %class.my_shared_ptr.31, %"class.std::vector.32", %"class.std::vector.37", %class.my_shared_ptr.42, %"class.std::vector.43", %"class.std::vector.48", i32, ptr }
%class.my_shared_ptr.29 = type { ptr, ptr }
%class.my_shared_ptr.30 = type { ptr, ptr }
%class.my_shared_ptr.31 = type { ptr, ptr }
%"class.std::vector.32" = type { %"struct.std::_Vector_base.33" }
%"struct.std::_Vector_base.33" = type { %"struct.std::_Vector_base<my_shared_ptr<urdf::Collision>, std::allocator<my_shared_ptr<urdf::Collision>>>::_Vector_impl" }
%"struct.std::_Vector_base<my_shared_ptr<urdf::Collision>, std::allocator<my_shared_ptr<urdf::Collision>>>::_Vector_impl" = type { %"struct.std::_Vector_base<my_shared_ptr<urdf::Collision>, std::allocator<my_shared_ptr<urdf::Collision>>>::_Vector_impl_data" }
%"struct.std::_Vector_base<my_shared_ptr<urdf::Collision>, std::allocator<my_shared_ptr<urdf::Collision>>>::_Vector_impl_data" = type { ptr, ptr, ptr }
%"class.std::vector.37" = type { %"struct.std::_Vector_base.38" }
%"struct.std::_Vector_base.38" = type { %"struct.std::_Vector_base<my_shared_ptr<urdf::Visual>, std::allocator<my_shared_ptr<urdf::Visual>>>::_Vector_impl" }
%"struct.std::_Vector_base<my_shared_ptr<urdf::Visual>, std::allocator<my_shared_ptr<urdf::Visual>>>::_Vector_impl" = type { %"struct.std::_Vector_base<my_shared_ptr<urdf::Visual>, std::allocator<my_shared_ptr<urdf::Visual>>>::_Vector_impl_data" }
%"struct.std::_Vector_base<my_shared_ptr<urdf::Visual>, std::allocator<my_shared_ptr<urdf::Visual>>>::_Vector_impl_data" = type { ptr, ptr, ptr }
%"class.std::vector.43" = type { %"struct.std::_Vector_base.44" }
%"struct.std::_Vector_base.44" = type { %"struct.std::_Vector_base<my_shared_ptr<urdf::Joint>, std::allocator<my_shared_ptr<urdf::Joint>>>::_Vector_impl" }
%"struct.std::_Vector_base<my_shared_ptr<urdf::Joint>, std::allocator<my_shared_ptr<urdf::Joint>>>::_Vector_impl" = type { %"struct.std::_Vector_base<my_shared_ptr<urdf::Joint>, std::allocator<my_shared_ptr<urdf::Joint>>>::_Vector_impl_data" }
%"struct.std::_Vector_base<my_shared_ptr<urdf::Joint>, std::allocator<my_shared_ptr<urdf::Joint>>>::_Vector_impl_data" = type { ptr, ptr, ptr }
%"class.std::vector.48" = type { %"struct.std::_Vector_base.49" }
%"struct.std::_Vector_base.49" = type { %"struct.std::_Vector_base<my_shared_ptr<urdf::Link>, std::allocator<my_shared_ptr<urdf::Link>>>::_Vector_impl" }
%"struct.std::_Vector_base<my_shared_ptr<urdf::Link>, std::allocator<my_shared_ptr<urdf::Link>>>::_Vector_impl" = type { %"struct.std::_Vector_base<my_shared_ptr<urdf::Link>, std::allocator<my_shared_ptr<urdf::Link>>>::_Vector_impl_data" }
%"struct.std::_Vector_base<my_shared_ptr<urdf::Link>, std::allocator<my_shared_ptr<urdf::Link>>>::_Vector_impl_data" = type { ptr, ptr, ptr }
%"class.urdf::Inertial" = type { %"class.urdf::Pose", double, double, double, double, double, double, double }
%"class.urdf::Pose" = type { %"class.urdf::Vector3", %"class.urdf::Rotation" }
%"class.urdf::Vector3" = type { double, double, double }
%"class.urdf::Rotation" = type { double, double, double, double }
%"class.urdf::Joint" = type { %"class.std::__cxx11::basic_string", i32, %"class.urdf::Vector3", %"class.std::__cxx11::basic_string", %"class.std::__cxx11::basic_string", %"class.urdf::Pose", %class.my_shared_ptr.156, %class.my_shared_ptr.157, %class.my_shared_ptr.158, %class.my_shared_ptr.159, %class.my_shared_ptr.160 }
%class.my_shared_ptr.156 = type { ptr, ptr }
%class.my_shared_ptr.157 = type { ptr, ptr }
%class.my_shared_ptr.158 = type { ptr, ptr }
%class.my_shared_ptr.159 = type { ptr, ptr }
%class.my_shared_ptr.160 = type { ptr, ptr }

@_ZSt4cout = external global %"class.std::basic_ostream", align 8
@.str = external hidden unnamed_addr constant [20 x i8], align 1
@.str.4 = external hidden unnamed_addr constant [16 x i8], align 1
@.str.5 = external hidden unnamed_addr constant [24 x i8], align 1
@.str.6 = external hidden unnamed_addr constant [21 x i8], align 1
@.str.7 = external hidden unnamed_addr constant [17 x i8], align 1
@.str.8 = external hidden unnamed_addr constant [17 x i8], align 1
@.str.9 = external hidden unnamed_addr constant [17 x i8], align 1
@.str.10 = external hidden unnamed_addr constant [3 x i8], align 1
@.str.11 = external hidden unnamed_addr constant [8 x i8], align 1
@.str.12 = external hidden unnamed_addr constant [15 x i8], align 1
@.str.13 = external hidden unnamed_addr constant [10 x i8], align 1
@_ZSt4cerr = external global %"class.std::basic_ostream", align 8
@.str.14 = external hidden unnamed_addr constant [31 x i8], align 1
@.str.15 = external hidden unnamed_addr constant [17 x i8], align 1
@.str.16 = external hidden unnamed_addr constant [22 x i8], align 1
@.str.17 = external hidden unnamed_addr constant [36 x i8], align 1
@.str.18 = external hidden unnamed_addr constant [30 x i8], align 1
@.str.19 = external hidden unnamed_addr constant [85 x i8], align 1
@.str.20 = external hidden unnamed_addr constant [16 x i8], align 1
@.str.21 = external hidden unnamed_addr constant [16 x i8], align 1
@.str.22 = external hidden unnamed_addr constant [16 x i8], align 1
@.str.23 = external hidden unnamed_addr constant [5 x i8], align 1
@.str.24 = external hidden unnamed_addr constant [3 x i8], align 1
@.str.25 = external hidden unnamed_addr constant [11 x i8], align 1

; Function Attrs: mustprogress uwtable
define dso_local noundef zeroext i1 @_ZN17RigidBodyDynamics6Addons15construct_modelEPNS_5ModelE13my_shared_ptrIN4urdf14ModelInterfaceEEbb(ptr noundef %0, ptr noundef %1, i1 noundef zeroext %2, i1 noundef zeroext %3) #0 personality ptr @__gxx_personality_v0 {
  %5 = alloca i1, align 1
  %6 = alloca ptr, align 8
  %7 = alloca ptr, align 8
  %8 = alloca i8, align 1
  %9 = alloca i8, align 1
  %10 = alloca %class.my_shared_ptr.1, align 8
  %11 = alloca %"class.std::map", align 8
  %12 = alloca ptr, align 8
  %13 = alloca i32, align 4
  %14 = alloca %"class.std::map.2", align 8
  %15 = alloca %"class.std::vector", align 8
  %16 = alloca %"class.std::stack", align 8
  %17 = alloca %"class.std::stack.21", align 8
  %18 = alloca %class.my_shared_ptr.28, align 8
  %19 = alloca ptr, align 8
  %20 = alloca %class.my_shared_ptr.28, align 8
  %21 = alloca %class.Vector3_t, align 8
  %22 = alloca %class.Vector3_t, align 8
  %23 = alloca %class.Matrix3_t, align 8
  %24 = alloca double, align 8
  %25 = alloca %"struct.RigidBodyDynamics::Body", align 8
  %26 = alloca %"struct.RigidBodyDynamics::Joint", align 8
  %27 = alloca %"struct.RigidBodyDynamics::Joint", align 8
  %28 = alloca %"struct.RigidBodyDynamics::Math::SpatialTransform", align 8
  %29 = alloca %"class.std::__cxx11::basic_string", align 8
  %30 = alloca i32, align 4
  %31 = alloca %class.my_shared_ptr.1, align 8
  %32 = alloca i32, align 4
  %33 = alloca %class.my_shared_ptr.42, align 8
  %34 = alloca i32, align 4
  %35 = alloca i32, align 4
  %36 = alloca i32, align 4
  %37 = alloca i32, align 4
  %38 = alloca %class.my_shared_ptr.42, align 8
  %39 = alloca %class.my_shared_ptr.1, align 8
  %40 = alloca %class.my_shared_ptr.1, align 8
  %41 = alloca i32, align 4
  %42 = alloca %"struct.RigidBodyDynamics::Joint", align 8
  %43 = alloca %class.Vector3_t, align 8
  %44 = alloca %class.Vector3_t, align 8
  %45 = alloca double, align 8
  %46 = alloca double, align 8
  %47 = alloca double, align 8
  %48 = alloca %"struct.RigidBodyDynamics::Joint", align 8
  %49 = alloca %class.Vector3_t, align 8
  %50 = alloca double, align 8
  %51 = alloca double, align 8
  %52 = alloca double, align 8
  %53 = alloca %"struct.RigidBodyDynamics::Joint", align 8
  %54 = alloca %class.Vector3_t, align 8
  %55 = alloca double, align 8
  %56 = alloca double, align 8
  %57 = alloca double, align 8
  %58 = alloca %"struct.RigidBodyDynamics::Joint", align 8
  %59 = alloca %"struct.RigidBodyDynamics::Joint", align 8
  %60 = alloca %class.Vector3_t, align 8
  %61 = alloca %"struct.RigidBodyDynamics::Joint", align 8
  %62 = alloca %"struct.RigidBodyDynamics::Joint", align 8
  %63 = alloca %"struct.RigidBodyDynamics::Joint", align 8
  %64 = alloca %class.SpatialVector_t, align 16
  %65 = alloca double, align 8
  %66 = alloca double, align 8
  %67 = alloca double, align 8
  %68 = alloca double, align 8
  %69 = alloca double, align 8
  %70 = alloca double, align 8
  %71 = alloca %class.SpatialVector_t, align 16
  %72 = alloca double, align 8
  %73 = alloca double, align 8
  %74 = alloca double, align 8
  %75 = alloca double, align 8
  %76 = alloca double, align 8
  %77 = alloca double, align 8
  %78 = alloca %class.SpatialVector_t, align 16
  %79 = alloca double, align 8
  %80 = alloca double, align 8
  %81 = alloca double, align 8
  %82 = alloca double, align 8
  %83 = alloca double, align 8
  %84 = alloca double, align 8
  %85 = alloca %class.SpatialVector_t, align 16
  %86 = alloca double, align 8
  %87 = alloca double, align 8
  %88 = alloca double, align 8
  %89 = alloca double, align 8
  %90 = alloca double, align 8
  %91 = alloca double, align 8
  %92 = alloca %class.SpatialVector_t, align 16
  %93 = alloca double, align 8
  %94 = alloca double, align 8
  %95 = alloca double, align 8
  %96 = alloca double, align 8
  %97 = alloca double, align 8
  %98 = alloca double, align 8
  %99 = alloca %class.SpatialVector_t, align 16
  %100 = alloca double, align 8
  %101 = alloca double, align 8
  %102 = alloca double, align 8
  %103 = alloca double, align 8
  %104 = alloca double, align 8
  %105 = alloca double, align 8
  %106 = alloca i32, align 4
  %107 = alloca %class.Vector3_t, align 8
  %108 = alloca %class.Vector3_t, align 8
  %109 = alloca %"struct.RigidBodyDynamics::Math::SpatialTransform", align 8
  %110 = alloca %"struct.RigidBodyDynamics::Math::SpatialTransform", align 8
  %111 = alloca %"struct.RigidBodyDynamics::Math::SpatialTransform", align 8
  %112 = alloca %"struct.RigidBodyDynamics::Math::SpatialTransform", align 8
  %113 = alloca %class.Vector3_t, align 8
  %114 = alloca double, align 8
  %115 = alloca double, align 8
  %116 = alloca double, align 8
  %117 = alloca %"struct.RigidBodyDynamics::Math::SpatialTransform", align 8
  %118 = alloca %class.Vector3_t, align 8
  %119 = alloca double, align 8
  %120 = alloca double, align 8
  %121 = alloca double, align 8
  %122 = alloca %"struct.RigidBodyDynamics::Math::SpatialTransform", align 8
  %123 = alloca %class.Vector3_t, align 8
  %124 = alloca double, align 8
  %125 = alloca double, align 8
  %126 = alloca double, align 8
  %127 = alloca %"struct.RigidBodyDynamics::Math::SpatialTransform", align 8
  %128 = alloca %class.Vector3_t, align 8
  %129 = alloca %class.Vector3_t, align 8
  %130 = alloca %class.Vector3_t, align 8
  %131 = alloca %class.Matrix3_t, align 8
  %132 = alloca %"class.Eigen::CwiseNullaryOp", align 8
  %133 = alloca double, align 8
  %134 = alloca %class.Vector3_t, align 8
  %135 = alloca double, align 8
  %136 = alloca double, align 8
  %137 = alloca double, align 8
  %138 = alloca %"struct.RigidBodyDynamics::Body", align 8
  %139 = alloca i32, align 4
  %140 = alloca %"class.Eigen::Transpose", align 8
  %141 = alloca %class.Matrix3_t, align 8
  %142 = alloca %"class.Eigen::CwiseNullaryOp", align 8
  %143 = alloca %"struct.RigidBodyDynamics::Body", align 8
  %144 = alloca double, align 8
  %145 = alloca %class.Vector3_t, align 8
  %146 = alloca %"class.Eigen::CwiseNullaryOp.181", align 8
  %147 = alloca %"struct.RigidBodyDynamics::Joint", align 8
  %148 = alloca %"class.std::__cxx11::basic_string", align 8
  %149 = alloca %"class.std::__cxx11::basic_string", align 8
  %150 = alloca %"struct.RigidBodyDynamics::Joint", align 8
  %151 = alloca %"struct.RigidBodyDynamics::Math::SpatialTransform", align 8
  %152 = alloca %"class.std::__cxx11::basic_string", align 8
  %153 = alloca %"class.std::__cxx11::basic_string", align 8
  store ptr %0, ptr %6, align 8, !tbaa !5
  store ptr %1, ptr %7, align 8, !tbaa !5
  %154 = zext i1 %2 to i8
  store i8 %154, ptr %8, align 1, !tbaa !9
  %155 = zext i1 %3 to i8
  store i8 %155, ptr %9, align 1, !tbaa !9
  call void @llvm.lifetime.start.p0(i64 16, ptr %10) #7
  call void @_ZN13my_shared_ptrIN4urdf4LinkEEC2Ev(ptr noundef nonnull align 8 dereferenceable(16) %10)
  call void @llvm.lifetime.start.p0(i64 48, ptr %11) #7
  call void @_ZNSt3mapINSt7__cxx1112basic_stringIcSt11char_traitsIcESaIcEEE13my_shared_ptrIN4urdf4LinkEESt4lessIS5_ESaISt4pairIKS5_S9_EEEC2Ev(ptr noundef nonnull align 8 dereferenceable(48) %11) #7
  %156 = invoke noundef ptr @_ZN13my_shared_ptrIN4urdf14ModelInterfaceEEptEv(ptr noundef nonnull align 8 dereferenceable(16) %1)
          to label %157 unwind label %339

157:                                              ; preds = %4
  %158 = getelementptr inbounds %"class.urdf::ModelInterface", ptr %156, i32 0, i32 0
  %159 = invoke noundef nonnull align 8 dereferenceable(48) ptr @_ZNSt3mapINSt7__cxx1112basic_stringIcSt11char_traitsIcESaIcEEE13my_shared_ptrIN4urdf4LinkEESt4lessIS5_ESaISt4pairIKS5_S9_EEEaSERKSG_(ptr noundef nonnull align 8 dereferenceable(48) %11, ptr noundef nonnull align 8 dereferenceable(48) %158)
          to label %160 unwind label %339

160:                                              ; preds = %157
  call void @llvm.lifetime.start.p0(i64 48, ptr %14) #7
  call void @_ZNSt3mapINSt7__cxx1112basic_stringIcSt11char_traitsIcESaIcEEE13my_shared_ptrIN4urdf5JointEESt4lessIS5_ESaISt4pairIKS5_S9_EEEC2Ev(ptr noundef nonnull align 8 dereferenceable(48) %14) #7
  %161 = invoke noundef ptr @_ZN13my_shared_ptrIN4urdf14ModelInterfaceEEptEv(ptr noundef nonnull align 8 dereferenceable(16) %1)
          to label %162 unwind label %343

162:                                              ; preds = %160
  %163 = getelementptr inbounds %"class.urdf::ModelInterface", ptr %161, i32 0, i32 1
  %164 = invoke noundef nonnull align 8 dereferenceable(48) ptr @_ZNSt3mapINSt7__cxx1112basic_stringIcSt11char_traitsIcESaIcEEE13my_shared_ptrIN4urdf5JointEESt4lessIS5_ESaISt4pairIKS5_S9_EEEaSERKSG_(ptr noundef nonnull align 8 dereferenceable(48) %14, ptr noundef nonnull align 8 dereferenceable(48) %163)
          to label %165 unwind label %343

165:                                              ; preds = %162
  call void @llvm.lifetime.start.p0(i64 24, ptr %15) #7
  call void @_ZNSt6vectorINSt7__cxx1112basic_stringIcSt11char_traitsIcESaIcEEESaIS5_EEC2Ev(ptr noundef nonnull align 8 dereferenceable(24) %15) #7
  call void @llvm.lifetime.start.p0(i64 80, ptr %16) #7
  invoke void @_ZNSt5stackI13my_shared_ptrIN4urdf4LinkEESt5dequeIS3_SaIS3_EEEC2IS6_vEEv(ptr noundef nonnull align 8 dereferenceable(80) %16)
          to label %166 unwind label %347

166:                                              ; preds = %165
  call void @llvm.lifetime.start.p0(i64 80, ptr %17) #7
  invoke void @_ZNSt5stackIiSt5dequeIiSaIiEEEC2IS2_vEEv(ptr noundef nonnull align 8 dereferenceable(80) %17)
          to label %167 unwind label %351

167:                                              ; preds = %166
  call void @llvm.lifetime.start.p0(i64 16, ptr %18) #7
  %168 = invoke noundef ptr @_ZN13my_shared_ptrIN4urdf14ModelInterfaceEEptEv(ptr noundef nonnull align 8 dereferenceable(16) %1)
          to label %169 unwind label %355

169:                                              ; preds = %167
  invoke void @_ZNK4urdf14ModelInterface7getRootEv(ptr dead_on_unwind writable sret(%class.my_shared_ptr.28) align 8 %18, ptr noundef nonnull align 8 dereferenceable(200) %168)
          to label %170 unwind label %355

170:                                              ; preds = %169
  %171 = invoke noundef ptr @_ZN13my_shared_ptrIKN4urdf4LinkEEptEv(ptr noundef nonnull align 8 dereferenceable(16) %18)
          to label %172 unwind label %359

172:                                              ; preds = %170
  %173 = getelementptr inbounds %"class.urdf::Link", ptr %171, i32 0, i32 0
  %174 = invoke noundef nonnull align 8 dereferenceable(16) ptr @_ZNSt3mapINSt7__cxx1112basic_stringIcSt11char_traitsIcESaIcEEE13my_shared_ptrIN4urdf4LinkEESt4lessIS5_ESaISt4pairIKS5_S9_EEEixERSD_(ptr noundef nonnull align 8 dereferenceable(48) %11, ptr noundef nonnull align 8 dereferenceable(32) %173)
          to label %175 unwind label %359

175:                                              ; preds = %172
  invoke void @_ZNSt5stackI13my_shared_ptrIN4urdf4LinkEESt5dequeIS3_SaIS3_EEE4pushERKS3_(ptr noundef nonnull align 8 dereferenceable(80) %16, ptr noundef nonnull align 8 dereferenceable(16) %174)
          to label %176 unwind label %359

176:                                              ; preds = %175
  call void @_ZN13my_shared_ptrIKN4urdf4LinkEED2Ev(ptr noundef nonnull align 8 dereferenceable(16) %18) #7
  call void @llvm.lifetime.end.p0(i64 16, ptr %18) #7
  call void @llvm.lifetime.start.p0(i64 8, ptr %19) #7
  call void @llvm.lifetime.start.p0(i64 16, ptr %20) #7
  %177 = invoke noundef ptr @_ZN13my_shared_ptrIN4urdf14ModelInterfaceEEptEv(ptr noundef nonnull align 8 dereferenceable(16) %1)
          to label %178 unwind label %364

178:                                              ; preds = %176
  invoke void @_ZNK4urdf14ModelInterface7getRootEv(ptr dead_on_unwind writable sret(%class.my_shared_ptr.28) align 8 %20, ptr noundef nonnull align 8 dereferenceable(200) %177)
          to label %179 unwind label %364

179:                                              ; preds = %178
  store ptr %20, ptr %19, align 8, !tbaa !5
  call void @llvm.lifetime.start.p0(i64 24, ptr %21) #7
  invoke void @_ZN9Vector3_tC2Ev(ptr noundef nonnull align 8 dereferenceable(24) %21)
          to label %180 unwind label %368

180:                                              ; preds = %179
  call void @llvm.lifetime.start.p0(i64 24, ptr %22) #7
  invoke void @_ZN9Vector3_tC2Ev(ptr noundef nonnull align 8 dereferenceable(24) %22)
          to label %181 unwind label %372

181:                                              ; preds = %180
  call void @llvm.lifetime.start.p0(i64 72, ptr %23) #7
  invoke void @_ZN9Matrix3_tC2Ev(ptr noundef nonnull align 8 dereferenceable(72) %23)
          to label %182 unwind label %376

182:                                              ; preds = %181
  call void @llvm.lifetime.start.p0(i64 8, ptr %24) #7
  %183 = load ptr, ptr %19, align 8, !tbaa !5
  %184 = invoke noundef ptr @_ZNK13my_shared_ptrIKN4urdf4LinkEEptEv(ptr noundef nonnull align 8 dereferenceable(16) %183)
          to label %185 unwind label %380

185:                                              ; preds = %182
  %186 = getelementptr inbounds %"class.urdf::Link", ptr %184, i32 0, i32 1
  %187 = invoke noundef zeroext i1 @_ZNK13my_shared_ptrIN4urdf8InertialEEcvbEv(ptr noundef nonnull align 8 dereferenceable(16) %186)
          to label %188 unwind label %380

188:                                              ; preds = %185
  br i1 %187, label %189, label %477

189:                                              ; preds = %188
  %190 = load ptr, ptr %19, align 8, !tbaa !5
  %191 = invoke noundef ptr @_ZNK13my_shared_ptrIKN4urdf4LinkEEptEv(ptr noundef nonnull align 8 dereferenceable(16) %190)
          to label %192 unwind label %380

192:                                              ; preds = %189
  %193 = getelementptr inbounds %"class.urdf::Link", ptr %191, i32 0, i32 1
  %194 = invoke noundef ptr @_ZNK13my_shared_ptrIN4urdf8InertialEEptEv(ptr noundef nonnull align 8 dereferenceable(16) %193)
          to label %195 unwind label %380

195:                                              ; preds = %192
  %196 = getelementptr inbounds %"class.urdf::Inertial", ptr %194, i32 0, i32 1
  %197 = load double, ptr %196, align 8, !tbaa !11
  store double %197, ptr %24, align 8, !tbaa !17
  %198 = load ptr, ptr %19, align 8, !tbaa !5
  %199 = invoke noundef ptr @_ZNK13my_shared_ptrIKN4urdf4LinkEEptEv(ptr noundef nonnull align 8 dereferenceable(16) %198)
          to label %200 unwind label %380

200:                                              ; preds = %195
  %201 = getelementptr inbounds %"class.urdf::Link", ptr %199, i32 0, i32 1
  %202 = invoke noundef ptr @_ZNK13my_shared_ptrIN4urdf8InertialEEptEv(ptr noundef nonnull align 8 dereferenceable(16) %201)
          to label %203 unwind label %380

203:                                              ; preds = %200
  %204 = getelementptr inbounds %"class.urdf::Inertial", ptr %202, i32 0, i32 0
  %205 = getelementptr inbounds %"class.urdf::Pose", ptr %204, i32 0, i32 0
  %206 = getelementptr inbounds %"class.urdf::Vector3", ptr %205, i32 0, i32 0
  %207 = load ptr, ptr %19, align 8, !tbaa !5
  %208 = invoke noundef ptr @_ZNK13my_shared_ptrIKN4urdf4LinkEEptEv(ptr noundef nonnull align 8 dereferenceable(16) %207)
          to label %209 unwind label %380

209:                                              ; preds = %203
  %210 = getelementptr inbounds %"class.urdf::Link", ptr %208, i32 0, i32 1
  %211 = invoke noundef ptr @_ZNK13my_shared_ptrIN4urdf8InertialEEptEv(ptr noundef nonnull align 8 dereferenceable(16) %210)
          to label %212 unwind label %380

212:                                              ; preds = %209
  %213 = getelementptr inbounds %"class.urdf::Inertial", ptr %211, i32 0, i32 0
  %214 = getelementptr inbounds %"class.urdf::Pose", ptr %213, i32 0, i32 0
  %215 = getelementptr inbounds %"class.urdf::Vector3", ptr %214, i32 0, i32 1
  %216 = load ptr, ptr %19, align 8, !tbaa !5
  %217 = invoke noundef ptr @_ZNK13my_shared_ptrIKN4urdf4LinkEEptEv(ptr noundef nonnull align 8 dereferenceable(16) %216)
          to label %218 unwind label %380

218:                                              ; preds = %212
  %219 = getelementptr inbounds %"class.urdf::Link", ptr %217, i32 0, i32 1
  %220 = invoke noundef ptr @_ZNK13my_shared_ptrIN4urdf8InertialEEptEv(ptr noundef nonnull align 8 dereferenceable(16) %219)
          to label %221 unwind label %380

221:                                              ; preds = %218
  %222 = getelementptr inbounds %"class.urdf::Inertial", ptr %220, i32 0, i32 0
  %223 = getelementptr inbounds %"class.urdf::Pose", ptr %222, i32 0, i32 0
  %224 = getelementptr inbounds %"class.urdf::Vector3", ptr %223, i32 0, i32 2
  invoke void @_ZN9Vector3_t3setERKdS1_S1_(ptr noundef nonnull align 8 dereferenceable(24) %22, ptr noundef nonnull align 8 dereferenceable(8) %206, ptr noundef nonnull align 8 dereferenceable(8) %215, ptr noundef nonnull align 8 dereferenceable(8) %224)
          to label %225 unwind label %380

225:                                              ; preds = %221
  %226 = load ptr, ptr %19, align 8, !tbaa !5
  %227 = invoke noundef ptr @_ZNK13my_shared_ptrIKN4urdf4LinkEEptEv(ptr noundef nonnull align 8 dereferenceable(16) %226)
          to label %228 unwind label %380

228:                                              ; preds = %225
  %229 = getelementptr inbounds %"class.urdf::Link", ptr %227, i32 0, i32 1
  %230 = invoke noundef ptr @_ZNK13my_shared_ptrIN4urdf8InertialEEptEv(ptr noundef nonnull align 8 dereferenceable(16) %229)
          to label %231 unwind label %380

231:                                              ; preds = %228
  %232 = getelementptr inbounds %"class.urdf::Inertial", ptr %230, i32 0, i32 2
  %233 = load double, ptr %232, align 8, !tbaa !18
  %234 = invoke noundef nonnull align 8 dereferenceable(8) ptr @_ZN5Eigen15DenseCoeffsBaseINS_6MatrixIdLi3ELi3ELi0ELi3ELi3EEELi1EEclEll(ptr noundef nonnull align 1 dereferenceable(1) %23, i64 noundef 0, i64 noundef 0)
          to label %235 unwind label %380

235:                                              ; preds = %231
  store double %233, ptr %234, align 8, !tbaa !17
  %236 = load ptr, ptr %19, align 8, !tbaa !5
  %237 = invoke noundef ptr @_ZNK13my_shared_ptrIKN4urdf4LinkEEptEv(ptr noundef nonnull align 8 dereferenceable(16) %236)
          to label %238 unwind label %380

238:                                              ; preds = %235
  %239 = getelementptr inbounds %"class.urdf::Link", ptr %237, i32 0, i32 1
  %240 = invoke noundef ptr @_ZNK13my_shared_ptrIN4urdf8InertialEEptEv(ptr noundef nonnull align 8 dereferenceable(16) %239)
          to label %241 unwind label %380

241:                                              ; preds = %238
  %242 = getelementptr inbounds %"class.urdf::Inertial", ptr %240, i32 0, i32 3
  %243 = load double, ptr %242, align 8, !tbaa !19
  %244 = invoke noundef nonnull align 8 dereferenceable(8) ptr @_ZN5Eigen15DenseCoeffsBaseINS_6MatrixIdLi3ELi3ELi0ELi3ELi3EEELi1EEclEll(ptr noundef nonnull align 1 dereferenceable(1) %23, i64 noundef 0, i64 noundef 1)
          to label %245 unwind label %380

245:                                              ; preds = %241
  store double %243, ptr %244, align 8, !tbaa !17
  %246 = load ptr, ptr %19, align 8, !tbaa !5
  %247 = invoke noundef ptr @_ZNK13my_shared_ptrIKN4urdf4LinkEEptEv(ptr noundef nonnull align 8 dereferenceable(16) %246)
          to label %248 unwind label %380

248:                                              ; preds = %245
  %249 = getelementptr inbounds %"class.urdf::Link", ptr %247, i32 0, i32 1
  %250 = invoke noundef ptr @_ZNK13my_shared_ptrIN4urdf8InertialEEptEv(ptr noundef nonnull align 8 dereferenceable(16) %249)
          to label %251 unwind label %380

251:                                              ; preds = %248
  %252 = getelementptr inbounds %"class.urdf::Inertial", ptr %250, i32 0, i32 4
  %253 = load double, ptr %252, align 8, !tbaa !20
  %254 = invoke noundef nonnull align 8 dereferenceable(8) ptr @_ZN5Eigen15DenseCoeffsBaseINS_6MatrixIdLi3ELi3ELi0ELi3ELi3EEELi1EEclEll(ptr noundef nonnull align 1 dereferenceable(1) %23, i64 noundef 0, i64 noundef 2)
          to label %255 unwind label %380

255:                                              ; preds = %251
  store double %253, ptr %254, align 8, !tbaa !17
  %256 = load ptr, ptr %19, align 8, !tbaa !5
  %257 = invoke noundef ptr @_ZNK13my_shared_ptrIKN4urdf4LinkEEptEv(ptr noundef nonnull align 8 dereferenceable(16) %256)
          to label %258 unwind label %380

258:                                              ; preds = %255
  %259 = getelementptr inbounds %"class.urdf::Link", ptr %257, i32 0, i32 1
  %260 = invoke noundef ptr @_ZNK13my_shared_ptrIN4urdf8InertialEEptEv(ptr noundef nonnull align 8 dereferenceable(16) %259)
          to label %261 unwind label %380

261:                                              ; preds = %258
  %262 = getelementptr inbounds %"class.urdf::Inertial", ptr %260, i32 0, i32 3
  %263 = load double, ptr %262, align 8, !tbaa !19
  %264 = invoke noundef nonnull align 8 dereferenceable(8) ptr @_ZN5Eigen15DenseCoeffsBaseINS_6MatrixIdLi3ELi3ELi0ELi3ELi3EEELi1EEclEll(ptr noundef nonnull align 1 dereferenceable(1) %23, i64 noundef 1, i64 noundef 0)
          to label %265 unwind label %380

265:                                              ; preds = %261
  store double %263, ptr %264, align 8, !tbaa !17
  %266 = load ptr, ptr %19, align 8, !tbaa !5
  %267 = invoke noundef ptr @_ZNK13my_shared_ptrIKN4urdf4LinkEEptEv(ptr noundef nonnull align 8 dereferenceable(16) %266)
          to label %268 unwind label %380

268:                                              ; preds = %265
  %269 = getelementptr inbounds %"class.urdf::Link", ptr %267, i32 0, i32 1
  %270 = invoke noundef ptr @_ZNK13my_shared_ptrIN4urdf8InertialEEptEv(ptr noundef nonnull align 8 dereferenceable(16) %269)
          to label %271 unwind label %380

271:                                              ; preds = %268
  %272 = getelementptr inbounds %"class.urdf::Inertial", ptr %270, i32 0, i32 5
  %273 = load double, ptr %272, align 8, !tbaa !21
  %274 = invoke noundef nonnull align 8 dereferenceable(8) ptr @_ZN5Eigen15DenseCoeffsBaseINS_6MatrixIdLi3ELi3ELi0ELi3ELi3EEELi1EEclEll(ptr noundef nonnull align 1 dereferenceable(1) %23, i64 noundef 1, i64 noundef 1)
          to label %275 unwind label %380

275:                                              ; preds = %271
  store double %273, ptr %274, align 8, !tbaa !17
  %276 = load ptr, ptr %19, align 8, !tbaa !5
  %277 = invoke noundef ptr @_ZNK13my_shared_ptrIKN4urdf4LinkEEptEv(ptr noundef nonnull align 8 dereferenceable(16) %276)
          to label %278 unwind label %380

278:                                              ; preds = %275
  %279 = getelementptr inbounds %"class.urdf::Link", ptr %277, i32 0, i32 1
  %280 = invoke noundef ptr @_ZNK13my_shared_ptrIN4urdf8InertialEEptEv(ptr noundef nonnull align 8 dereferenceable(16) %279)
          to label %281 unwind label %380

281:                                              ; preds = %278
  %282 = getelementptr inbounds %"class.urdf::Inertial", ptr %280, i32 0, i32 6
  %283 = load double, ptr %282, align 8, !tbaa !22
  %284 = invoke noundef nonnull align 8 dereferenceable(8) ptr @_ZN5Eigen15DenseCoeffsBaseINS_6MatrixIdLi3ELi3ELi0ELi3ELi3EEELi1EEclEll(ptr noundef nonnull align 1 dereferenceable(1) %23, i64 noundef 1, i64 noundef 2)
          to label %285 unwind label %380

285:                                              ; preds = %281
  store double %283, ptr %284, align 8, !tbaa !17
  %286 = load ptr, ptr %19, align 8, !tbaa !5
  %287 = invoke noundef ptr @_ZNK13my_shared_ptrIKN4urdf4LinkEEptEv(ptr noundef nonnull align 8 dereferenceable(16) %286)
          to label %288 unwind label %380

288:                                              ; preds = %285
  %289 = getelementptr inbounds %"class.urdf::Link", ptr %287, i32 0, i32 1
  %290 = invoke noundef ptr @_ZNK13my_shared_ptrIN4urdf8InertialEEptEv(ptr noundef nonnull align 8 dereferenceable(16) %289)
          to label %291 unwind label %380

291:                                              ; preds = %288
  %292 = getelementptr inbounds %"class.urdf::Inertial", ptr %290, i32 0, i32 4
  %293 = load double, ptr %292, align 8, !tbaa !20
  %294 = invoke noundef nonnull align 8 dereferenceable(8) ptr @_ZN5Eigen15DenseCoeffsBaseINS_6MatrixIdLi3ELi3ELi0ELi3ELi3EEELi1EEclEll(ptr noundef nonnull align 1 dereferenceable(1) %23, i64 noundef 2, i64 noundef 0)
          to label %295 unwind label %380

295:                                              ; preds = %291
  store double %293, ptr %294, align 8, !tbaa !17
  %296 = load ptr, ptr %19, align 8, !tbaa !5
  %297 = invoke noundef ptr @_ZNK13my_shared_ptrIKN4urdf4LinkEEptEv(ptr noundef nonnull align 8 dereferenceable(16) %296)
          to label %298 unwind label %380

298:                                              ; preds = %295
  %299 = getelementptr inbounds %"class.urdf::Link", ptr %297, i32 0, i32 1
  %300 = invoke noundef ptr @_ZNK13my_shared_ptrIN4urdf8InertialEEptEv(ptr noundef nonnull align 8 dereferenceable(16) %299)
          to label %301 unwind label %380

301:                                              ; preds = %298
  %302 = getelementptr inbounds %"class.urdf::Inertial", ptr %300, i32 0, i32 6
  %303 = load double, ptr %302, align 8, !tbaa !22
  %304 = invoke noundef nonnull align 8 dereferenceable(8) ptr @_ZN5Eigen15DenseCoeffsBaseINS_6MatrixIdLi3ELi3ELi0ELi3ELi3EEELi1EEclEll(ptr noundef nonnull align 1 dereferenceable(1) %23, i64 noundef 2, i64 noundef 1)
          to label %305 unwind label %380

305:                                              ; preds = %301
  store double %303, ptr %304, align 8, !tbaa !17
  %306 = load ptr, ptr %19, align 8, !tbaa !5
  %307 = invoke noundef ptr @_ZNK13my_shared_ptrIKN4urdf4LinkEEptEv(ptr noundef nonnull align 8 dereferenceable(16) %306)
          to label %308 unwind label %380

308:                                              ; preds = %305
  %309 = getelementptr inbounds %"class.urdf::Link", ptr %307, i32 0, i32 1
  %310 = invoke noundef ptr @_ZNK13my_shared_ptrIN4urdf8InertialEEptEv(ptr noundef nonnull align 8 dereferenceable(16) %309)
          to label %311 unwind label %380

311:                                              ; preds = %308
  %312 = getelementptr inbounds %"class.urdf::Inertial", ptr %310, i32 0, i32 7
  %313 = load double, ptr %312, align 8, !tbaa !23
  %314 = invoke noundef nonnull align 8 dereferenceable(8) ptr @_ZN5Eigen15DenseCoeffsBaseINS_6MatrixIdLi3ELi3ELi0ELi3ELi3EEELi1EEclEll(ptr noundef nonnull align 1 dereferenceable(1) %23, i64 noundef 2, i64 noundef 2)
          to label %315 unwind label %380

315:                                              ; preds = %311
  store double %313, ptr %314, align 8, !tbaa !17
  %316 = load ptr, ptr %19, align 8, !tbaa !5
  %317 = invoke noundef ptr @_ZNK13my_shared_ptrIKN4urdf4LinkEEptEv(ptr noundef nonnull align 8 dereferenceable(16) %316)
          to label %318 unwind label %380

318:                                              ; preds = %315
  %319 = getelementptr inbounds %"class.urdf::Link", ptr %317, i32 0, i32 1
  %320 = invoke noundef ptr @_ZNK13my_shared_ptrIN4urdf8InertialEEptEv(ptr noundef nonnull align 8 dereferenceable(16) %319)
          to label %321 unwind label %380

321:                                              ; preds = %318
  %322 = getelementptr inbounds %"class.urdf::Inertial", ptr %320, i32 0, i32 0
  %323 = getelementptr inbounds %"class.urdf::Pose", ptr %322, i32 0, i32 1
  %324 = invoke noundef nonnull align 8 dereferenceable(8) ptr @_ZN5Eigen15DenseCoeffsBaseINS_6MatrixIdLi3ELi1ELi0ELi3ELi1EEELi1EEixEl(ptr noundef nonnull align 1 dereferenceable(1) %21, i64 noundef 0)
          to label %325 unwind label %380

325:                                              ; preds = %321
  %326 = invoke noundef nonnull align 8 dereferenceable(8) ptr @_ZN5Eigen15DenseCoeffsBaseINS_6MatrixIdLi3ELi1ELi0ELi3ELi1EEELi1EEixEl(ptr noundef nonnull align 1 dereferenceable(1) %21, i64 noundef 1)
          to label %327 unwind label %380

327:                                              ; preds = %325
  %328 = invoke noundef nonnull align 8 dereferenceable(8) ptr @_ZN5Eigen15DenseCoeffsBaseINS_6MatrixIdLi3ELi1ELi0ELi3ELi1EEELi1EEixEl(ptr noundef nonnull align 1 dereferenceable(1) %21, i64 noundef 2)
          to label %329 unwind label %380

329:                                              ; preds = %327
  invoke void @_ZNK4urdf8Rotation6getRPYERdS1_S1_(ptr noundef nonnull align 8 dereferenceable(32) %323, ptr noundef nonnull align 8 dereferenceable(8) %324, ptr noundef nonnull align 8 dereferenceable(8) %326, ptr noundef nonnull align 8 dereferenceable(8) %328)
          to label %330 unwind label %380

330:                                              ; preds = %329
  call void @llvm.lifetime.start.p0(i64 112, ptr %25) #7
  invoke void @_ZN17RigidBodyDynamics4BodyC2ERKdRK9Vector3_tRK9Matrix3_t(ptr noundef nonnull align 8 dereferenceable(105) %25, ptr noundef nonnull align 8 dereferenceable(8) %24, ptr noundef nonnull align 8 dereferenceable(24) %22, ptr noundef nonnull align 8 dereferenceable(72) %23)
          to label %331 unwind label %384

331:                                              ; preds = %330
  call void @llvm.lifetime.start.p0(i64 24, ptr %26) #7
  invoke void @_ZN17RigidBodyDynamics5JointC2ENS_9JointTypeE(ptr noundef nonnull align 8 dereferenceable(24) %26, i32 noundef 13)
          to label %332 unwind label %388

332:                                              ; preds = %331
  %333 = load i8, ptr %8, align 1, !tbaa !9, !range !24, !noundef !25
  %334 = trunc i8 %333 to i1
  br i1 %334, label %335, label %401

335:                                              ; preds = %332
  call void @llvm.lifetime.start.p0(i64 24, ptr %27) #7
  invoke void @_ZN17RigidBodyDynamics5JointC2ENS_9JointTypeE(ptr noundef nonnull align 8 dereferenceable(24) %27, i32 noundef 12)
          to label %336 unwind label %392

336:                                              ; preds = %335
  %337 = invoke noundef nonnull align 8 dereferenceable(24) ptr @_ZN17RigidBodyDynamics5JointaSERKS0_(ptr noundef nonnull align 8 dereferenceable(24) %26, ptr noundef nonnull align 8 dereferenceable(24) %27)
          to label %338 unwind label %396

338:                                              ; preds = %336
  call void @_ZN17RigidBodyDynamics5JointD2Ev(ptr noundef nonnull align 8 dereferenceable(24) %27) #7
  call void @llvm.lifetime.end.p0(i64 24, ptr %27) #7
  br label %401

339:                                              ; preds = %157, %4
  %340 = landingpad { ptr, i32 }
          cleanup
  %341 = extractvalue { ptr, i32 } %340, 0
  store ptr %341, ptr %12, align 8
  %342 = extractvalue { ptr, i32 } %340, 1
  store i32 %342, ptr %13, align 4
  br label %1428

343:                                              ; preds = %162, %160
  %344 = landingpad { ptr, i32 }
          cleanup
  %345 = extractvalue { ptr, i32 } %344, 0
  store ptr %345, ptr %12, align 8
  %346 = extractvalue { ptr, i32 } %344, 1
  store i32 %346, ptr %13, align 4
  br label %1427

347:                                              ; preds = %165
  %348 = landingpad { ptr, i32 }
          cleanup
  %349 = extractvalue { ptr, i32 } %348, 0
  store ptr %349, ptr %12, align 8
  %350 = extractvalue { ptr, i32 } %348, 1
  store i32 %350, ptr %13, align 4
  br label %1426

351:                                              ; preds = %166
  %352 = landingpad { ptr, i32 }
          cleanup
  %353 = extractvalue { ptr, i32 } %352, 0
  store ptr %353, ptr %12, align 8
  %354 = extractvalue { ptr, i32 } %352, 1
  store i32 %354, ptr %13, align 4
  br label %1425

355:                                              ; preds = %169, %167
  %356 = landingpad { ptr, i32 }
          cleanup
  %357 = extractvalue { ptr, i32 } %356, 0
  store ptr %357, ptr %12, align 8
  %358 = extractvalue { ptr, i32 } %356, 1
  store i32 %358, ptr %13, align 4
  br label %363

359:                                              ; preds = %175, %172, %170
  %360 = landingpad { ptr, i32 }
          cleanup
  %361 = extractvalue { ptr, i32 } %360, 0
  store ptr %361, ptr %12, align 8
  %362 = extractvalue { ptr, i32 } %360, 1
  store i32 %362, ptr %13, align 4
  call void @_ZN13my_shared_ptrIKN4urdf4LinkEED2Ev(ptr noundef nonnull align 8 dereferenceable(16) %18) #7
  br label %363

363:                                              ; preds = %359, %355
  call void @llvm.lifetime.end.p0(i64 16, ptr %18) #7
  br label %1424

364:                                              ; preds = %178, %176
  %365 = landingpad { ptr, i32 }
          cleanup
  %366 = extractvalue { ptr, i32 } %365, 0
  store ptr %366, ptr %12, align 8
  %367 = extractvalue { ptr, i32 } %365, 1
  store i32 %367, ptr %13, align 4
  br label %1423

368:                                              ; preds = %179
  %369 = landingpad { ptr, i32 }
          cleanup
  %370 = extractvalue { ptr, i32 } %369, 0
  store ptr %370, ptr %12, align 8
  %371 = extractvalue { ptr, i32 } %369, 1
  store i32 %371, ptr %13, align 4
  br label %1422

372:                                              ; preds = %180
  %373 = landingpad { ptr, i32 }
          cleanup
  %374 = extractvalue { ptr, i32 } %373, 0
  store ptr %374, ptr %12, align 8
  %375 = extractvalue { ptr, i32 } %373, 1
  store i32 %375, ptr %13, align 4
  br label %1421

376:                                              ; preds = %181
  %377 = landingpad { ptr, i32 }
          cleanup
  %378 = extractvalue { ptr, i32 } %377, 0
  store ptr %378, ptr %12, align 8
  %379 = extractvalue { ptr, i32 } %377, 1
  store i32 %379, ptr %13, align 4
  br label %1420

380:                                              ; preds = %479, %329, %327, %325, %321, %318, %315, %311, %308, %305, %301, %298, %295, %291, %288, %285, %281, %278, %275, %271, %268, %265, %261, %258, %255, %251, %248, %245, %241, %238, %235, %231, %228, %225, %221, %218, %212, %209, %203, %200, %195, %192, %189, %185, %182
  %381 = landingpad { ptr, i32 }
          cleanup
  %382 = extractvalue { ptr, i32 } %381, 0
  store ptr %382, ptr %12, align 8
  %383 = extractvalue { ptr, i32 } %381, 1
  store i32 %383, ptr %13, align 4
  br label %1419

384:                                              ; preds = %330
  %385 = landingpad { ptr, i32 }
          cleanup
  %386 = extractvalue { ptr, i32 } %385, 0
  store ptr %386, ptr %12, align 8
  %387 = extractvalue { ptr, i32 } %385, 1
  store i32 %387, ptr %13, align 4
  br label %476

388:                                              ; preds = %331
  %389 = landingpad { ptr, i32 }
          cleanup
  %390 = extractvalue { ptr, i32 } %389, 0
  store ptr %390, ptr %12, align 8
  %391 = extractvalue { ptr, i32 } %389, 1
  store i32 %391, ptr %13, align 4
  br label %475

392:                                              ; preds = %335
  %393 = landingpad { ptr, i32 }
          cleanup
  %394 = extractvalue { ptr, i32 } %393, 0
  store ptr %394, ptr %12, align 8
  %395 = extractvalue { ptr, i32 } %393, 1
  store i32 %395, ptr %13, align 4
  br label %400

396:                                              ; preds = %336
  %397 = landingpad { ptr, i32 }
          cleanup
  %398 = extractvalue { ptr, i32 } %397, 0
  store ptr %398, ptr %12, align 8
  %399 = extractvalue { ptr, i32 } %397, 1
  store i32 %399, ptr %13, align 4
  call void @_ZN17RigidBodyDynamics5JointD2Ev(ptr noundef nonnull align 8 dereferenceable(24) %27) #7
  br label %400

400:                                              ; preds = %396, %392
  call void @llvm.lifetime.end.p0(i64 24, ptr %27) #7
  br label %474

401:                                              ; preds = %338, %332
  call void @llvm.lifetime.start.p0(i64 96, ptr %28) #7
  invoke void @_ZN17RigidBodyDynamics4Math16SpatialTransformC2Ev(ptr noundef nonnull align 8 dereferenceable(96) %28)
          to label %402 unwind label %423

402:                                              ; preds = %401
  %403 = load i8, ptr %9, align 1, !tbaa !9, !range !24, !noundef !25
  %404 = trunc i8 %403 to i1
  br i1 %404, label %405, label %460

405:                                              ; preds = %402
  %406 = invoke noundef nonnull align 8 dereferenceable(8) ptr @_ZStlsISt11char_traitsIcEERSt13basic_ostreamIcT_ES5_PKc(ptr noundef nonnull align 8 dereferenceable(8) @_ZSt4cout, ptr noundef @.str)
          to label %407 unwind label %423

407:                                              ; preds = %405
  %408 = invoke noundef nonnull align 8 dereferenceable(8) ptr @_ZNSolsEPFRSoS_E(ptr noundef nonnull align 8 dereferenceable(8) %406, ptr noundef @_ZSt4endlIcSt11char_traitsIcEERSt13basic_ostreamIT_T0_ES6_)
          to label %409 unwind label %423

409:                                              ; preds = %407
  %410 = invoke noundef nonnull align 8 dereferenceable(8) ptr @_ZStlsISt11char_traitsIcEERSt13basic_ostreamIcT_ES5_PKc(ptr noundef nonnull align 8 dereferenceable(8) @_ZSt4cout, ptr noundef @.str.4)
          to label %411 unwind label %423

411:                                              ; preds = %409
  %412 = invoke noundef nonnull align 8 dereferenceable(8) ptr @_ZN17RigidBodyDynamics4MathlsERSoRKNS0_16SpatialTransformE(ptr noundef nonnull align 8 dereferenceable(8) %410, ptr noundef nonnull align 8 dereferenceable(96) %28)
          to label %413 unwind label %423

413:                                              ; preds = %411
  %414 = invoke noundef nonnull align 8 dereferenceable(8) ptr @_ZNSolsEPFRSoS_E(ptr noundef nonnull align 8 dereferenceable(8) %412, ptr noundef @_ZSt4endlIcSt11char_traitsIcEERSt13basic_ostreamIT_T0_ES6_)
          to label %415 unwind label %423

415:                                              ; preds = %413
  %416 = load i8, ptr %8, align 1, !tbaa !9, !range !24, !noundef !25
  %417 = trunc i8 %416 to i1
  br i1 %417, label %418, label %427

418:                                              ; preds = %415
  %419 = invoke noundef nonnull align 8 dereferenceable(8) ptr @_ZStlsISt11char_traitsIcEERSt13basic_ostreamIcT_ES5_PKc(ptr noundef nonnull align 8 dereferenceable(8) @_ZSt4cout, ptr noundef @.str.5)
          to label %420 unwind label %423

420:                                              ; preds = %418
  %421 = invoke noundef nonnull align 8 dereferenceable(8) ptr @_ZNSolsEPFRSoS_E(ptr noundef nonnull align 8 dereferenceable(8) %419, ptr noundef @_ZSt4endlIcSt11char_traitsIcEERSt13basic_ostreamIT_T0_ES6_)
          to label %422 unwind label %423

422:                                              ; preds = %420
  br label %432

423:                                              ; preds = %464, %460, %457, %454, %451, %449, %447, %443, %441, %439, %436, %434, %432, %429, %427, %420, %418, %413, %411, %409, %407, %405, %401
  %424 = landingpad { ptr, i32 }
          cleanup
  %425 = extractvalue { ptr, i32 } %424, 0
  store ptr %425, ptr %12, align 8
  %426 = extractvalue { ptr, i32 } %424, 1
  store i32 %426, ptr %13, align 4
  br label %473

427:                                              ; preds = %415
  %428 = invoke noundef nonnull align 8 dereferenceable(8) ptr @_ZStlsISt11char_traitsIcEERSt13basic_ostreamIcT_ES5_PKc(ptr noundef nonnull align 8 dereferenceable(8) @_ZSt4cout, ptr noundef @.str.6)
          to label %429 unwind label %423

429:                                              ; preds = %427
  %430 = invoke noundef nonnull align 8 dereferenceable(8) ptr @_ZNSolsEPFRSoS_E(ptr noundef nonnull align 8 dereferenceable(8) %428, ptr noundef @_ZSt4endlIcSt11char_traitsIcEERSt13basic_ostreamIT_T0_ES6_)
          to label %431 unwind label %423

431:                                              ; preds = %429
  br label %432

432:                                              ; preds = %431, %422
  %433 = invoke noundef nonnull align 8 dereferenceable(8) ptr @_ZStlsISt11char_traitsIcEERSt13basic_ostreamIcT_ES5_PKc(ptr noundef nonnull align 8 dereferenceable(8) @_ZSt4cout, ptr noundef @.str.7)
          to label %434 unwind label %423

434:                                              ; preds = %432
  %435 = invoke noundef nonnull align 8 dereferenceable(8) ptr @_ZNSolsEPFRSoS_E(ptr noundef nonnull align 8 dereferenceable(8) %433, ptr noundef @_ZSt4endlIcSt11char_traitsIcEERSt13basic_ostreamIT_T0_ES6_)
          to label %436 unwind label %423

436:                                              ; preds = %434
  %437 = getelementptr inbounds %"struct.RigidBodyDynamics::Body", ptr %25, i32 0, i32 2
  %438 = invoke noundef nonnull align 8 dereferenceable(8) ptr @_ZN5EigenlsINS_6MatrixIdLi3ELi3ELi0ELi3ELi3EEEEERSoS3_RKNS_9DenseBaseIT_EE(ptr noundef nonnull align 8 dereferenceable(8) %435, ptr noundef nonnull align 1 dereferenceable(1) %437)
          to label %439 unwind label %423

439:                                              ; preds = %436
  %440 = invoke noundef nonnull align 8 dereferenceable(8) ptr @_ZNSolsEPFRSoS_E(ptr noundef nonnull align 8 dereferenceable(8) %438, ptr noundef @_ZSt4endlIcSt11char_traitsIcEERSt13basic_ostreamIT_T0_ES6_)
          to label %441 unwind label %423

441:                                              ; preds = %439
  %442 = invoke noundef nonnull align 8 dereferenceable(8) ptr @_ZStlsISt11char_traitsIcEERSt13basic_ostreamIcT_ES5_PKc(ptr noundef nonnull align 8 dereferenceable(8) @_ZSt4cout, ptr noundef @.str.8)
          to label %443 unwind label %423

443:                                              ; preds = %441
  %444 = getelementptr inbounds %"struct.RigidBodyDynamics::Body", ptr %25, i32 0, i32 0
  %445 = load double, ptr %444, align 8, !tbaa !26
  %446 = invoke noundef nonnull align 8 dereferenceable(8) ptr @_ZNSolsEd(ptr noundef nonnull align 8 dereferenceable(8) %442, double noundef %445)
          to label %447 unwind label %423

447:                                              ; preds = %443
  %448 = invoke noundef nonnull align 8 dereferenceable(8) ptr @_ZNSolsEPFRSoS_E(ptr noundef nonnull align 8 dereferenceable(8) %446, ptr noundef @_ZSt4endlIcSt11char_traitsIcEERSt13basic_ostreamIT_T0_ES6_)
          to label %449 unwind label %423

449:                                              ; preds = %447
  %450 = invoke noundef nonnull align 8 dereferenceable(8) ptr @_ZStlsISt11char_traitsIcEERSt13basic_ostreamIcT_ES5_PKc(ptr noundef nonnull align 8 dereferenceable(8) @_ZSt4cout, ptr noundef @.str.9)
          to label %451 unwind label %423

451:                                              ; preds = %449
  %452 = load ptr, ptr %19, align 8, !tbaa !5
  %453 = invoke noundef ptr @_ZNK13my_shared_ptrIKN4urdf4LinkEEptEv(ptr noundef nonnull align 8 dereferenceable(16) %452)
          to label %454 unwind label %423

454:                                              ; preds = %451
  %455 = getelementptr inbounds %"class.urdf::Link", ptr %453, i32 0, i32 0
  %456 = invoke noundef nonnull align 8 dereferenceable(8) ptr @_ZStlsIcSt11char_traitsIcESaIcEERSt13basic_ostreamIT_T0_ES7_RKNSt7__cxx1112basic_stringIS4_S5_T1_EE(ptr noundef nonnull align 8 dereferenceable(8) %450, ptr noundef nonnull align 8 dereferenceable(32) %455)
          to label %457 unwind label %423

457:                                              ; preds = %454
  %458 = invoke noundef nonnull align 8 dereferenceable(8) ptr @_ZNSolsEPFRSoS_E(ptr noundef nonnull align 8 dereferenceable(8) %456, ptr noundef @_ZSt4endlIcSt11char_traitsIcEERSt13basic_ostreamIT_T0_ES6_)
          to label %459 unwind label %423

459:                                              ; preds = %457
  br label %460

460:                                              ; preds = %459, %402
  %461 = load ptr, ptr %6, align 8, !tbaa !5
  %462 = load ptr, ptr %19, align 8, !tbaa !5
  %463 = invoke noundef ptr @_ZNK13my_shared_ptrIKN4urdf4LinkEEptEv(ptr noundef nonnull align 8 dereferenceable(16) %462)
          to label %464 unwind label %423

464:                                              ; preds = %460
  %465 = getelementptr inbounds %"class.urdf::Link", ptr %463, i32 0, i32 0
  invoke void @_ZNSt7__cxx1112basic_stringIcSt11char_traitsIcESaIcEEC2ERKS4_(ptr noundef nonnull align 8 dereferenceable(32) %29, ptr noundef nonnull align 8 dereferenceable(32) %465)
          to label %466 unwind label %423

466:                                              ; preds = %464
  %467 = invoke noundef i32 @_ZN17RigidBodyDynamics5Model10AppendBodyERKNS_4Math16SpatialTransformERKNS_5JointERKNS_4BodyENSt7__cxx1112basic_stringIcSt11char_traitsIcESaIcEEE(ptr noundef nonnull align 8 dereferenceable(872) %461, ptr noundef nonnull align 8 dereferenceable(96) %28, ptr noundef nonnull align 8 dereferenceable(24) %26, ptr noundef nonnull align 8 dereferenceable(105) %25, ptr noundef %29)
          to label %468 unwind label %469

468:                                              ; preds = %466
  call void @_ZNSt7__cxx1112basic_stringIcSt11char_traitsIcESaIcEED2Ev(ptr noundef nonnull align 8 dereferenceable(32) %29) #7
  call void @llvm.lifetime.end.p0(i64 96, ptr %28) #7
  call void @_ZN17RigidBodyDynamics5JointD2Ev(ptr noundef nonnull align 8 dereferenceable(24) %26) #7
  call void @llvm.lifetime.end.p0(i64 24, ptr %26) #7
  call void @_ZN17RigidBodyDynamics4BodyD2Ev(ptr noundef nonnull align 8 dereferenceable(105) %25) #7
  call void @llvm.lifetime.end.p0(i64 112, ptr %25) #7
  br label %477

469:                                              ; preds = %466
  %470 = landingpad { ptr, i32 }
          cleanup
  %471 = extractvalue { ptr, i32 } %470, 0
  store ptr %471, ptr %12, align 8
  %472 = extractvalue { ptr, i32 } %470, 1
  store i32 %472, ptr %13, align 4
  call void @_ZNSt7__cxx1112basic_stringIcSt11char_traitsIcESaIcEED2Ev(ptr noundef nonnull align 8 dereferenceable(32) %29) #7
  br label %473

473:                                              ; preds = %469, %423
  call void @llvm.lifetime.end.p0(i64 96, ptr %28) #7
  br label %474

474:                                              ; preds = %473, %400
  call void @_ZN17RigidBodyDynamics5JointD2Ev(ptr noundef nonnull align 8 dereferenceable(24) %26) #7
  br label %475

475:                                              ; preds = %474, %388
  call void @llvm.lifetime.end.p0(i64 24, ptr %26) #7
  call void @_ZN17RigidBodyDynamics4BodyD2Ev(ptr noundef nonnull align 8 dereferenceable(105) %25) #7
  br label %476

476:                                              ; preds = %475, %384
  call void @llvm.lifetime.end.p0(i64 112, ptr %25) #7
  br label %1419

477:                                              ; preds = %468, %188
  call void @llvm.lifetime.start.p0(i64 4, ptr %30) #7
  store i32 0, ptr %30, align 4, !tbaa !38
  invoke void @_ZNSt5stackIiSt5dequeIiSaIiEEE4pushEOi(ptr noundef nonnull align 8 dereferenceable(80) %17, ptr noundef nonnull align 4 dereferenceable(4) %30)
          to label %478 unwind label %527

478:                                              ; preds = %477
  call void @llvm.lifetime.end.p0(i64 4, ptr %30) #7
  br label %479

479:                                              ; preds = %602, %478
  %480 = invoke noundef i64 @_ZNKSt5stackI13my_shared_ptrIN4urdf4LinkEESt5dequeIS3_SaIS3_EEE4sizeEv(ptr noundef nonnull align 8 dereferenceable(80) %16)
          to label %481 unwind label %380

481:                                              ; preds = %479
  %482 = icmp ugt i64 %480, 0
  br i1 %482, label %483, label %605

483:                                              ; preds = %481
  call void @llvm.lifetime.start.p0(i64 16, ptr %31) #7
  %484 = invoke noundef nonnull align 8 dereferenceable(16) ptr @_ZNSt5stackI13my_shared_ptrIN4urdf4LinkEESt5dequeIS3_SaIS3_EEE3topEv(ptr noundef nonnull align 8 dereferenceable(80) %16)
          to label %485 unwind label %531

485:                                              ; preds = %483
  invoke void @_ZN13my_shared_ptrIN4urdf4LinkEEC2ERKS2_(ptr noundef nonnull align 8 dereferenceable(16) %31, ptr noundef nonnull align 8 dereferenceable(16) %484)
          to label %486 unwind label %531

486:                                              ; preds = %485
  call void @llvm.lifetime.start.p0(i64 4, ptr %32) #7
  %487 = invoke noundef nonnull align 4 dereferenceable(4) ptr @_ZNSt5stackIiSt5dequeIiSaIiEEE3topEv(ptr noundef nonnull align 8 dereferenceable(80) %17)
          to label %488 unwind label %535

488:                                              ; preds = %486
  %489 = load i32, ptr %487, align 4, !tbaa !38
  store i32 %489, ptr %32, align 4, !tbaa !38
  %490 = load i32, ptr %32, align 4, !tbaa !38
  %491 = zext i32 %490 to i64
  %492 = invoke noundef ptr @_ZN13my_shared_ptrIN4urdf4LinkEEptEv(ptr noundef nonnull align 8 dereferenceable(16) %31)
          to label %493 unwind label %535

493:                                              ; preds = %488
  %494 = getelementptr inbounds %"class.urdf::Link", ptr %492, i32 0, i32 7
  %495 = call noundef i64 @_ZNKSt6vectorI13my_shared_ptrIN4urdf5JointEESaIS3_EE4sizeEv(ptr noundef nonnull align 8 dereferenceable(24) %494) #7
  %496 = icmp ult i64 %491, %495
  br i1 %496, label %497, label %599

497:                                              ; preds = %493
  call void @llvm.lifetime.start.p0(i64 16, ptr %33) #7
  %498 = invoke noundef ptr @_ZN13my_shared_ptrIN4urdf4LinkEEptEv(ptr noundef nonnull align 8 dereferenceable(16) %31)
          to label %499 unwind label %539

499:                                              ; preds = %497
  %500 = getelementptr inbounds %"class.urdf::Link", ptr %498, i32 0, i32 7
  %501 = load i32, ptr %32, align 4, !tbaa !38
  %502 = zext i32 %501 to i64
  %503 = call noundef nonnull align 8 dereferenceable(16) ptr @_ZNSt6vectorI13my_shared_ptrIN4urdf5JointEESaIS3_EEixEm(ptr noundef nonnull align 8 dereferenceable(24) %500, i64 noundef %502) #7
  invoke void @_ZN13my_shared_ptrIN4urdf5JointEEC2ERKS2_(ptr noundef nonnull align 8 dereferenceable(16) %33, ptr noundef nonnull align 8 dereferenceable(16) %503)
          to label %504 unwind label %539

504:                                              ; preds = %499
  invoke void @_ZNSt5stackIiSt5dequeIiSaIiEEE3popEv(ptr noundef nonnull align 8 dereferenceable(80) %17)
          to label %505 unwind label %543

505:                                              ; preds = %504
  call void @llvm.lifetime.start.p0(i64 4, ptr %34) #7
  %506 = load i32, ptr %32, align 4, !tbaa !38
  %507 = add i32 %506, 1
  store i32 %507, ptr %34, align 4, !tbaa !38
  invoke void @_ZNSt5stackIiSt5dequeIiSaIiEEE4pushEOi(ptr noundef nonnull align 8 dereferenceable(80) %17, ptr noundef nonnull align 4 dereferenceable(4) %34)
          to label %508 unwind label %547

508:                                              ; preds = %505
  call void @llvm.lifetime.end.p0(i64 4, ptr %34) #7
  %509 = invoke noundef ptr @_ZN13my_shared_ptrIN4urdf5JointEEptEv(ptr noundef nonnull align 8 dereferenceable(16) %33)
          to label %510 unwind label %543

510:                                              ; preds = %508
  %511 = getelementptr inbounds %"class.urdf::Joint", ptr %509, i32 0, i32 3
  %512 = invoke noundef nonnull align 8 dereferenceable(16) ptr @_ZNSt3mapINSt7__cxx1112basic_stringIcSt11char_traitsIcESaIcEEE13my_shared_ptrIN4urdf4LinkEESt4lessIS5_ESaISt4pairIKS5_S9_EEEixERSD_(ptr noundef nonnull align 8 dereferenceable(48) %11, ptr noundef nonnull align 8 dereferenceable(32) %511)
          to label %513 unwind label %543

513:                                              ; preds = %510
  invoke void @_ZNSt5stackI13my_shared_ptrIN4urdf4LinkEESt5dequeIS3_SaIS3_EEE4pushERKS3_(ptr noundef nonnull align 8 dereferenceable(80) %16, ptr noundef nonnull align 8 dereferenceable(16) %512)
          to label %514 unwind label %543

514:                                              ; preds = %513
  call void @llvm.lifetime.start.p0(i64 4, ptr %35) #7
  store i32 0, ptr %35, align 4, !tbaa !38
  invoke void @_ZNSt5stackIiSt5dequeIiSaIiEEE4pushEOi(ptr noundef nonnull align 8 dereferenceable(80) %17, ptr noundef nonnull align 4 dereferenceable(4) %35)
          to label %515 unwind label %551

515:                                              ; preds = %514
  call void @llvm.lifetime.end.p0(i64 4, ptr %35) #7
  %516 = load i8, ptr %9, align 1, !tbaa !9, !range !24, !noundef !25
  %517 = trunc i8 %516 to i1
  br i1 %517, label %518, label %592

518:                                              ; preds = %515
  call void @llvm.lifetime.start.p0(i64 4, ptr %36) #7
  store i32 1, ptr %36, align 4, !tbaa !38
  br label %519

519:                                              ; preds = %562, %518
  %520 = load i32, ptr %36, align 4, !tbaa !38
  %521 = zext i32 %520 to i64
  %522 = invoke noundef i64 @_ZNKSt5stackIiSt5dequeIiSaIiEEE4sizeEv(ptr noundef nonnull align 8 dereferenceable(80) %17)
          to label %523 unwind label %555

523:                                              ; preds = %519
  %524 = sub i64 %522, 1
  %525 = icmp ult i64 %521, %524
  br i1 %525, label %559, label %526

526:                                              ; preds = %523
  call void @llvm.lifetime.end.p0(i64 4, ptr %36) #7
  br label %565

527:                                              ; preds = %477
  %528 = landingpad { ptr, i32 }
          cleanup
  %529 = extractvalue { ptr, i32 } %528, 0
  store ptr %529, ptr %12, align 8
  %530 = extractvalue { ptr, i32 } %528, 1
  store i32 %530, ptr %13, align 4
  call void @llvm.lifetime.end.p0(i64 4, ptr %30) #7
  br label %1419

531:                                              ; preds = %485, %483
  %532 = landingpad { ptr, i32 }
          cleanup
  %533 = extractvalue { ptr, i32 } %532, 0
  store ptr %533, ptr %12, align 8
  %534 = extractvalue { ptr, i32 } %532, 1
  store i32 %534, ptr %13, align 4
  br label %604

535:                                              ; preds = %600, %599, %488, %486
  %536 = landingpad { ptr, i32 }
          cleanup
  %537 = extractvalue { ptr, i32 } %536, 0
  store ptr %537, ptr %12, align 8
  %538 = extractvalue { ptr, i32 } %536, 1
  store i32 %538, ptr %13, align 4
  br label %603

539:                                              ; preds = %499, %497
  %540 = landingpad { ptr, i32 }
          cleanup
  %541 = extractvalue { ptr, i32 } %540, 0
  store ptr %541, ptr %12, align 8
  %542 = extractvalue { ptr, i32 } %540, 1
  store i32 %542, ptr %13, align 4
  br label %598

543:                                              ; preds = %594, %592, %589, %585, %583, %581, %578, %576, %574, %572, %569, %567, %565, %513, %510, %508, %504
  %544 = landingpad { ptr, i32 }
          cleanup
  %545 = extractvalue { ptr, i32 } %544, 0
  store ptr %545, ptr %12, align 8
  %546 = extractvalue { ptr, i32 } %544, 1
  store i32 %546, ptr %13, align 4
  br label %597

547:                                              ; preds = %505
  %548 = landingpad { ptr, i32 }
          cleanup
  %549 = extractvalue { ptr, i32 } %548, 0
  store ptr %549, ptr %12, align 8
  %550 = extractvalue { ptr, i32 } %548, 1
  store i32 %550, ptr %13, align 4
  call void @llvm.lifetime.end.p0(i64 4, ptr %34) #7
  br label %597

551:                                              ; preds = %514
  %552 = landingpad { ptr, i32 }
          cleanup
  %553 = extractvalue { ptr, i32 } %552, 0
  store ptr %553, ptr %12, align 8
  %554 = extractvalue { ptr, i32 } %552, 1
  store i32 %554, ptr %13, align 4
  call void @llvm.lifetime.end.p0(i64 4, ptr %35) #7
  br label %597

555:                                              ; preds = %559, %519
  %556 = landingpad { ptr, i32 }
          cleanup
  %557 = extractvalue { ptr, i32 } %556, 0
  store ptr %557, ptr %12, align 8
  %558 = extractvalue { ptr, i32 } %556, 1
  store i32 %558, ptr %13, align 4
  call void @llvm.lifetime.end.p0(i64 4, ptr %36) #7
  br label %597

559:                                              ; preds = %523
  %560 = invoke noundef nonnull align 8 dereferenceable(8) ptr @_ZStlsISt11char_traitsIcEERSt13basic_ostreamIcT_ES5_PKc(ptr noundef nonnull align 8 dereferenceable(8) @_ZSt4cout, ptr noundef @.str.10)
          to label %561 unwind label %555

561:                                              ; preds = %559
  br label %562

562:                                              ; preds = %561
  %563 = load i32, ptr %36, align 4, !tbaa !38
  %564 = add i32 %563, 1
  store i32 %564, ptr %36, align 4, !tbaa !38
  br label %519, !llvm.loop !40

565:                                              ; preds = %526
  %566 = invoke noundef nonnull align 8 dereferenceable(8) ptr @_ZStlsISt11char_traitsIcEERSt13basic_ostreamIcT_ES5_PKc(ptr noundef nonnull align 8 dereferenceable(8) @_ZSt4cout, ptr noundef @.str.11)
          to label %567 unwind label %543

567:                                              ; preds = %565
  %568 = invoke noundef ptr @_ZN13my_shared_ptrIN4urdf5JointEEptEv(ptr noundef nonnull align 8 dereferenceable(16) %33)
          to label %569 unwind label %543

569:                                              ; preds = %567
  %570 = getelementptr inbounds %"class.urdf::Joint", ptr %568, i32 0, i32 0
  %571 = invoke noundef nonnull align 8 dereferenceable(8) ptr @_ZStlsIcSt11char_traitsIcESaIcEERSt13basic_ostreamIT_T0_ES7_RKNSt7__cxx1112basic_stringIS4_S5_T1_EE(ptr noundef nonnull align 8 dereferenceable(8) %566, ptr noundef nonnull align 8 dereferenceable(32) %570)
          to label %572 unwind label %543

572:                                              ; preds = %569
  %573 = invoke noundef nonnull align 8 dereferenceable(8) ptr @_ZStlsISt11char_traitsIcEERSt13basic_ostreamIcT_ES5_PKc(ptr noundef nonnull align 8 dereferenceable(8) %571, ptr noundef @.str.12)
          to label %574 unwind label %543

574:                                              ; preds = %572
  %575 = invoke noundef nonnull align 8 dereferenceable(16) ptr @_ZNSt5stackI13my_shared_ptrIN4urdf4LinkEESt5dequeIS3_SaIS3_EEE3topEv(ptr noundef nonnull align 8 dereferenceable(80) %16)
          to label %576 unwind label %543

576:                                              ; preds = %574
  %577 = invoke noundef ptr @_ZN13my_shared_ptrIN4urdf4LinkEEptEv(ptr noundef nonnull align 8 dereferenceable(16) %575)
          to label %578 unwind label %543

578:                                              ; preds = %576
  %579 = getelementptr inbounds %"class.urdf::Link", ptr %577, i32 0, i32 0
  %580 = invoke noundef nonnull align 8 dereferenceable(8) ptr @_ZStlsIcSt11char_traitsIcESaIcEERSt13basic_ostreamIT_T0_ES7_RKNSt7__cxx1112basic_stringIS4_S5_T1_EE(ptr noundef nonnull align 8 dereferenceable(8) %573, ptr noundef nonnull align 8 dereferenceable(32) %579)
          to label %581 unwind label %543

581:                                              ; preds = %578
  %582 = invoke noundef nonnull align 8 dereferenceable(8) ptr @_ZStlsISt11char_traitsIcEERSt13basic_ostreamIcT_ES5_PKc(ptr noundef nonnull align 8 dereferenceable(8) %580, ptr noundef @.str.13)
          to label %583 unwind label %543

583:                                              ; preds = %581
  %584 = invoke noundef ptr @_ZN13my_shared_ptrIN4urdf5JointEEptEv(ptr noundef nonnull align 8 dereferenceable(16) %33)
          to label %585 unwind label %543

585:                                              ; preds = %583
  %586 = getelementptr inbounds %"class.urdf::Joint", ptr %584, i32 0, i32 1
  %587 = load i32, ptr %586, align 8, !tbaa !42
  %588 = invoke noundef nonnull align 8 dereferenceable(8) ptr @_ZNSolsEi(ptr noundef nonnull align 8 dereferenceable(8) %582, i32 noundef %587)
          to label %589 unwind label %543

589:                                              ; preds = %585
  %590 = invoke noundef nonnull align 8 dereferenceable(8) ptr @_ZNSolsEPFRSoS_E(ptr noundef nonnull align 8 dereferenceable(8) %588, ptr noundef @_ZSt4endlIcSt11char_traitsIcEERSt13basic_ostreamIT_T0_ES6_)
          to label %591 unwind label %543

591:                                              ; preds = %589
  br label %592

592:                                              ; preds = %591, %515
  %593 = invoke noundef ptr @_ZN13my_shared_ptrIN4urdf5JointEEptEv(ptr noundef nonnull align 8 dereferenceable(16) %33)
          to label %594 unwind label %543

594:                                              ; preds = %592
  %595 = getelementptr inbounds %"class.urdf::Joint", ptr %593, i32 0, i32 0
  invoke void @_ZNSt6vectorINSt7__cxx1112basic_stringIcSt11char_traitsIcESaIcEEESaIS5_EE9push_backERKS5_(ptr noundef nonnull align 8 dereferenceable(24) %15, ptr noundef nonnull align 8 dereferenceable(32) %595)
          to label %596 unwind label %543

596:                                              ; preds = %594
  call void @_ZN13my_shared_ptrIN4urdf5JointEED2Ev(ptr noundef nonnull align 8 dereferenceable(16) %33) #7
  call void @llvm.lifetime.end.p0(i64 16, ptr %33) #7
  br label %602

597:                                              ; preds = %555, %551, %547, %543
  call void @_ZN13my_shared_ptrIN4urdf5JointEED2Ev(ptr noundef nonnull align 8 dereferenceable(16) %33) #7
  br label %598

598:                                              ; preds = %597, %539
  call void @llvm.lifetime.end.p0(i64 16, ptr %33) #7
  br label %603

599:                                              ; preds = %493
  invoke void @_ZNSt5stackI13my_shared_ptrIN4urdf4LinkEESt5dequeIS3_SaIS3_EEE3popEv(ptr noundef nonnull align 8 dereferenceable(80) %16)
          to label %600 unwind label %535

600:                                              ; preds = %599
  invoke void @_ZNSt5stackIiSt5dequeIiSaIiEEE3popEv(ptr noundef nonnull align 8 dereferenceable(80) %17)
          to label %601 unwind label %535

601:                                              ; preds = %600
  br label %602

602:                                              ; preds = %601, %596
  call void @llvm.lifetime.end.p0(i64 4, ptr %32) #7
  call void @_ZN13my_shared_ptrIN4urdf4LinkEED2Ev(ptr noundef nonnull align 8 dereferenceable(16) %31) #7
  call void @llvm.lifetime.end.p0(i64 16, ptr %31) #7
  br label %479, !llvm.loop !53

603:                                              ; preds = %598, %535
  call void @llvm.lifetime.end.p0(i64 4, ptr %32) #7
  call void @_ZN13my_shared_ptrIN4urdf4LinkEED2Ev(ptr noundef nonnull align 8 dereferenceable(16) %31) #7
  br label %604

604:                                              ; preds = %603, %531
  call void @llvm.lifetime.end.p0(i64 16, ptr %31) #7
  br label %1419

605:                                              ; preds = %481
  call void @llvm.lifetime.start.p0(i64 4, ptr %37) #7
  store i32 0, ptr %37, align 4, !tbaa !38
  br label %606

606:                                              ; preds = %1398, %605
  %607 = load i32, ptr %37, align 4, !tbaa !38
  %608 = zext i32 %607 to i64
  %609 = call noundef i64 @_ZNKSt6vectorINSt7__cxx1112basic_stringIcSt11char_traitsIcESaIcEEESaIS5_EE4sizeEv(ptr noundef nonnull align 8 dereferenceable(24) %15) #7
  %610 = icmp ult i64 %608, %609
  br i1 %610, label %611, label %1416

611:                                              ; preds = %606
  call void @llvm.lifetime.start.p0(i64 16, ptr %38) #7
  %612 = load i32, ptr %37, align 4, !tbaa !38
  %613 = zext i32 %612 to i64
  %614 = call noundef nonnull align 8 dereferenceable(32) ptr @_ZNSt6vectorINSt7__cxx1112basic_stringIcSt11char_traitsIcESaIcEEESaIS5_EEixEm(ptr noundef nonnull align 8 dereferenceable(24) %15, i64 noundef %613) #7
  %615 = invoke noundef nonnull align 8 dereferenceable(16) ptr @_ZNSt3mapINSt7__cxx1112basic_stringIcSt11char_traitsIcESaIcEEE13my_shared_ptrIN4urdf5JointEESt4lessIS5_ESaISt4pairIKS5_S9_EEEixERSD_(ptr noundef nonnull align 8 dereferenceable(48) %14, ptr noundef nonnull align 8 dereferenceable(32) %614)
          to label %616 unwind label %659

616:                                              ; preds = %611
  invoke void @_ZN13my_shared_ptrIN4urdf5JointEEC2ERKS2_(ptr noundef nonnull align 8 dereferenceable(16) %38, ptr noundef nonnull align 8 dereferenceable(16) %615)
          to label %617 unwind label %659

617:                                              ; preds = %616
  call void @llvm.lifetime.start.p0(i64 16, ptr %39) #7
  %618 = invoke noundef ptr @_ZN13my_shared_ptrIN4urdf5JointEEptEv(ptr noundef nonnull align 8 dereferenceable(16) %38)
          to label %619 unwind label %663

619:                                              ; preds = %617
  %620 = getelementptr inbounds %"class.urdf::Joint", ptr %618, i32 0, i32 4
  %621 = invoke noundef nonnull align 8 dereferenceable(16) ptr @_ZNSt3mapINSt7__cxx1112basic_stringIcSt11char_traitsIcESaIcEEE13my_shared_ptrIN4urdf4LinkEESt4lessIS5_ESaISt4pairIKS5_S9_EEEixERSD_(ptr noundef nonnull align 8 dereferenceable(48) %11, ptr noundef nonnull align 8 dereferenceable(32) %620)
          to label %622 unwind label %663

622:                                              ; preds = %619
  invoke void @_ZN13my_shared_ptrIN4urdf4LinkEEC2ERKS2_(ptr noundef nonnull align 8 dereferenceable(16) %39, ptr noundef nonnull align 8 dereferenceable(16) %621)
          to label %623 unwind label %663

623:                                              ; preds = %622
  call void @llvm.lifetime.start.p0(i64 16, ptr %40) #7
  %624 = invoke noundef ptr @_ZN13my_shared_ptrIN4urdf5JointEEptEv(ptr noundef nonnull align 8 dereferenceable(16) %38)
          to label %625 unwind label %667

625:                                              ; preds = %623
  %626 = getelementptr inbounds %"class.urdf::Joint", ptr %624, i32 0, i32 3
  %627 = invoke noundef nonnull align 8 dereferenceable(16) ptr @_ZNSt3mapINSt7__cxx1112basic_stringIcSt11char_traitsIcESaIcEEE13my_shared_ptrIN4urdf4LinkEESt4lessIS5_ESaISt4pairIKS5_S9_EEEixERSD_(ptr noundef nonnull align 8 dereferenceable(48) %11, ptr noundef nonnull align 8 dereferenceable(32) %626)
          to label %628 unwind label %667

628:                                              ; preds = %625
  invoke void @_ZN13my_shared_ptrIN4urdf4LinkEEC2ERKS2_(ptr noundef nonnull align 8 dereferenceable(16) %40, ptr noundef nonnull align 8 dereferenceable(16) %627)
          to label %629 unwind label %667

629:                                              ; preds = %628
  call void @llvm.lifetime.start.p0(i64 4, ptr %41) #7
  store i32 0, ptr %41, align 4, !tbaa !38
  %630 = load ptr, ptr %6, align 8, !tbaa !5
  %631 = invoke noundef ptr @_ZN13my_shared_ptrIN4urdf4LinkEEptEv(ptr noundef nonnull align 8 dereferenceable(16) %39)
          to label %632 unwind label %671

632:                                              ; preds = %629
  %633 = getelementptr inbounds %"class.urdf::Link", ptr %631, i32 0, i32 0
  %634 = call noundef ptr @_ZNKSt7__cxx1112basic_stringIcSt11char_traitsIcESaIcEE5c_strEv(ptr noundef nonnull align 8 dereferenceable(32) %633) #7
  %635 = invoke noundef i32 @_ZNK17RigidBodyDynamics5Model9GetBodyIdEPKc(ptr noundef nonnull align 8 dereferenceable(872) %630, ptr noundef %634)
          to label %636 unwind label %671

636:                                              ; preds = %632
  store i32 %635, ptr %41, align 4, !tbaa !38
  %637 = load i32, ptr %41, align 4, !tbaa !38
  %638 = call noundef i32 @_ZNSt14numeric_limitsIjE3maxEv() #7
  %639 = icmp eq i32 %637, %638
  br i1 %639, label %640, label %675

640:                                              ; preds = %636
  %641 = invoke noundef nonnull align 8 dereferenceable(8) ptr @_ZStlsISt11char_traitsIcEERSt13basic_ostreamIcT_ES5_PKc(ptr noundef nonnull align 8 dereferenceable(8) @_ZSt4cerr, ptr noundef @.str.14)
          to label %642 unwind label %671

642:                                              ; preds = %640
  %643 = invoke noundef ptr @_ZN13my_shared_ptrIN4urdf5JointEEptEv(ptr noundef nonnull align 8 dereferenceable(16) %38)
          to label %644 unwind label %671

644:                                              ; preds = %642
  %645 = getelementptr inbounds %"class.urdf::Joint", ptr %643, i32 0, i32 0
  %646 = invoke noundef nonnull align 8 dereferenceable(8) ptr @_ZStlsIcSt11char_traitsIcESaIcEERSt13basic_ostreamIT_T0_ES7_RKNSt7__cxx1112basic_stringIS4_S5_T1_EE(ptr noundef nonnull align 8 dereferenceable(8) %641, ptr noundef nonnull align 8 dereferenceable(32) %645)
          to label %647 unwind label %671

647:                                              ; preds = %644
  %648 = invoke noundef nonnull align 8 dereferenceable(8) ptr @_ZStlsISt11char_traitsIcEERSt13basic_ostreamIcT_ES5_PKc(ptr noundef nonnull align 8 dereferenceable(8) %646, ptr noundef @.str.15)
          to label %649 unwind label %671

649:                                              ; preds = %647
  %650 = invoke noundef ptr @_ZN13my_shared_ptrIN4urdf4LinkEEptEv(ptr noundef nonnull align 8 dereferenceable(16) %39)
          to label %651 unwind label %671

651:                                              ; preds = %649
  %652 = getelementptr inbounds %"class.urdf::Link", ptr %650, i32 0, i32 0
  %653 = invoke noundef nonnull align 8 dereferenceable(8) ptr @_ZStlsIcSt11char_traitsIcESaIcEERSt13basic_ostreamIT_T0_ES7_RKNSt7__cxx1112basic_stringIS4_S5_T1_EE(ptr noundef nonnull align 8 dereferenceable(8) %648, ptr noundef nonnull align 8 dereferenceable(32) %652)
          to label %654 unwind label %671

654:                                              ; preds = %651
  %655 = invoke noundef nonnull align 8 dereferenceable(8) ptr @_ZStlsISt11char_traitsIcEERSt13basic_ostreamIcT_ES5_PKc(ptr noundef nonnull align 8 dereferenceable(8) %653, ptr noundef @.str.16)
          to label %656 unwind label %671

656:                                              ; preds = %654
  %657 = invoke noundef nonnull align 8 dereferenceable(8) ptr @_ZNSolsEPFRSoS_E(ptr noundef nonnull align 8 dereferenceable(8) %655, ptr noundef @_ZSt4endlIcSt11char_traitsIcEERSt13basic_ostreamIT_T0_ES6_)
          to label %658 unwind label %671

658:                                              ; preds = %656
  br label %675

659:                                              ; preds = %616, %611
  %660 = landingpad { ptr, i32 }
          cleanup
  %661 = extractvalue { ptr, i32 } %660, 0
  store ptr %661, ptr %12, align 8
  %662 = extractvalue { ptr, i32 } %660, 1
  store i32 %662, ptr %13, align 4
  br label %1415

663:                                              ; preds = %622, %619, %617
  %664 = landingpad { ptr, i32 }
          cleanup
  %665 = extractvalue { ptr, i32 } %664, 0
  store ptr %665, ptr %12, align 8
  %666 = extractvalue { ptr, i32 } %664, 1
  store i32 %666, ptr %13, align 4
  br label %1414

667:                                              ; preds = %628, %625, %623
  %668 = landingpad { ptr, i32 }
          cleanup
  %669 = extractvalue { ptr, i32 } %668, 0
  store ptr %669, ptr %12, align 8
  %670 = extractvalue { ptr, i32 } %668, 1
  store i32 %670, ptr %13, align 4
  br label %1413

671:                                              ; preds = %656, %654, %651, %649, %647, %644, %642, %640, %632, %629
  %672 = landingpad { ptr, i32 }
          cleanup
  %673 = extractvalue { ptr, i32 } %672, 0
  store ptr %673, ptr %12, align 8
  %674 = extractvalue { ptr, i32 } %672, 1
  store i32 %674, ptr %13, align 4
  br label %1412

675:                                              ; preds = %658, %636
  call void @llvm.lifetime.start.p0(i64 24, ptr %42) #7
  invoke void @_ZN17RigidBodyDynamics5JointC2Ev(ptr noundef nonnull align 8 dereferenceable(24) %42)
          to label %676 unwind label %713

676:                                              ; preds = %675
  %677 = invoke noundef ptr @_ZN13my_shared_ptrIN4urdf5JointEEptEv(ptr noundef nonnull align 8 dereferenceable(16) %38)
          to label %678 unwind label %717

678:                                              ; preds = %676
  %679 = getelementptr inbounds %"class.urdf::Joint", ptr %677, i32 0, i32 1
  %680 = load i32, ptr %679, align 8, !tbaa !42
  %681 = icmp eq i32 %680, 1
  br i1 %681, label %688, label %682

682:                                              ; preds = %678
  %683 = invoke noundef ptr @_ZN13my_shared_ptrIN4urdf5JointEEptEv(ptr noundef nonnull align 8 dereferenceable(16) %38)
          to label %684 unwind label %717

684:                                              ; preds = %682
  %685 = getelementptr inbounds %"class.urdf::Joint", ptr %683, i32 0, i32 1
  %686 = load i32, ptr %685, align 8, !tbaa !42
  %687 = icmp eq i32 %686, 2
  br i1 %687, label %688, label %805

688:                                              ; preds = %684, %678
  call void @llvm.lifetime.start.p0(i64 24, ptr %43) #7
  %689 = invoke noundef ptr @_ZN13my_shared_ptrIN4urdf5JointEEptEv(ptr noundef nonnull align 8 dereferenceable(16) %38)
          to label %690 unwind label %721

690:                                              ; preds = %688
  %691 = getelementptr inbounds %"class.urdf::Joint", ptr %689, i32 0, i32 2
  %692 = getelementptr inbounds %"class.urdf::Vector3", ptr %691, i32 0, i32 0
  %693 = invoke noundef ptr @_ZN13my_shared_ptrIN4urdf5JointEEptEv(ptr noundef nonnull align 8 dereferenceable(16) %38)
          to label %694 unwind label %721

694:                                              ; preds = %690
  %695 = getelementptr inbounds %"class.urdf::Joint", ptr %693, i32 0, i32 2
  %696 = getelementptr inbounds %"class.urdf::Vector3", ptr %695, i32 0, i32 1
  %697 = invoke noundef ptr @_ZN13my_shared_ptrIN4urdf5JointEEptEv(ptr noundef nonnull align 8 dereferenceable(16) %38)
          to label %698 unwind label %721

698:                                              ; preds = %694
  %699 = getelementptr inbounds %"class.urdf::Joint", ptr %697, i32 0, i32 2
  %700 = getelementptr inbounds %"class.urdf::Vector3", ptr %699, i32 0, i32 2
  invoke void @_ZN9Vector3_tC2ERKdS1_S1_(ptr noundef nonnull align 8 dereferenceable(24) %43, ptr noundef nonnull align 8 dereferenceable(8) %692, ptr noundef nonnull align 8 dereferenceable(8) %696, ptr noundef nonnull align 8 dereferenceable(8) %700)
          to label %701 unwind label %721

701:                                              ; preds = %698
  call void @llvm.lifetime.start.p0(i64 24, ptr %44) #7
  call void @llvm.lifetime.start.p0(i64 8, ptr %45) #7
  store double 1.000000e+00, ptr %45, align 8, !tbaa !17
  call void @llvm.lifetime.start.p0(i64 8, ptr %46) #7
  store double 0.000000e+00, ptr %46, align 8, !tbaa !17
  call void @llvm.lifetime.start.p0(i64 8, ptr %47) #7
  store double 0.000000e+00, ptr %47, align 8, !tbaa !17
  invoke void @_ZN9Vector3_tC2ERKdS1_S1_(ptr noundef nonnull align 8 dereferenceable(24) %44, ptr noundef nonnull align 8 dereferenceable(8) %45, ptr noundef nonnull align 8 dereferenceable(8) %46, ptr noundef nonnull align 8 dereferenceable(8) %47)
          to label %702 unwind label %725

702:                                              ; preds = %701
  %703 = invoke noundef double @_ZNK5Eigen10MatrixBaseINS_6MatrixIdLi3ELi1ELi0ELi3ELi1EEEE3dotIS2_EENS_20ScalarBinaryOpTraitsIdNS_8internal6traitsIT_E6ScalarENS6_17scalar_product_opIdSA_EEE10ReturnTypeERKNS0_IS8_EE(ptr noundef nonnull align 1 dereferenceable(1) %43, ptr noundef nonnull align 1 dereferenceable(1) %44)
          to label %704 unwind label %725

704:                                              ; preds = %702
  %705 = fsub double %703, 1.000000e+00
  %706 = call double @llvm.fabs.f64(double %705)
  %707 = call noundef double @_ZNSt14numeric_limitsIdE7epsilonEv() #7
  %708 = fcmp olt double %706, %707
  call void @llvm.lifetime.end.p0(i64 8, ptr %47) #7
  call void @llvm.lifetime.end.p0(i64 8, ptr %46) #7
  call void @llvm.lifetime.end.p0(i64 8, ptr %45) #7
  call void @llvm.lifetime.end.p0(i64 24, ptr %44) #7
  br i1 %708, label %709, label %738

709:                                              ; preds = %704
  call void @llvm.lifetime.start.p0(i64 24, ptr %48) #7
  invoke void @_ZN17RigidBodyDynamics5JointC2ENS_9JointTypeE(ptr noundef nonnull align 8 dereferenceable(24) %48, i32 noundef 3)
          to label %710 unwind label %729

710:                                              ; preds = %709
  %711 = invoke noundef nonnull align 8 dereferenceable(24) ptr @_ZN17RigidBodyDynamics5JointaSERKS0_(ptr noundef nonnull align 8 dereferenceable(24) %42, ptr noundef nonnull align 8 dereferenceable(24) %48)
          to label %712 unwind label %733

712:                                              ; preds = %710
  call void @_ZN17RigidBodyDynamics5JointD2Ev(ptr noundef nonnull align 8 dereferenceable(24) %48) #7
  call void @llvm.lifetime.end.p0(i64 24, ptr %48) #7
  br label %803

713:                                              ; preds = %675
  %714 = landingpad { ptr, i32 }
          cleanup
  %715 = extractvalue { ptr, i32 } %714, 0
  store ptr %715, ptr %12, align 8
  %716 = extractvalue { ptr, i32 } %714, 1
  store i32 %716, ptr %13, align 4
  br label %1411

717:                                              ; preds = %926, %924, %921, %919, %917, %911, %861, %842, %805, %682, %676
  %718 = landingpad { ptr, i32 }
          cleanup
  %719 = extractvalue { ptr, i32 } %718, 0
  store ptr %719, ptr %12, align 8
  %720 = extractvalue { ptr, i32 } %718, 1
  store i32 %720, ptr %13, align 4
  br label %1410

721:                                              ; preds = %698, %694, %690, %688
  %722 = landingpad { ptr, i32 }
          cleanup
  %723 = extractvalue { ptr, i32 } %722, 0
  store ptr %723, ptr %12, align 8
  %724 = extractvalue { ptr, i32 } %722, 1
  store i32 %724, ptr %13, align 4
  br label %804

725:                                              ; preds = %702, %701
  %726 = landingpad { ptr, i32 }
          cleanup
  %727 = extractvalue { ptr, i32 } %726, 0
  store ptr %727, ptr %12, align 8
  %728 = extractvalue { ptr, i32 } %726, 1
  store i32 %728, ptr %13, align 4
  call void @llvm.lifetime.end.p0(i64 8, ptr %47) #7
  call void @llvm.lifetime.end.p0(i64 8, ptr %46) #7
  call void @llvm.lifetime.end.p0(i64 8, ptr %45) #7
  call void @llvm.lifetime.end.p0(i64 24, ptr %44) #7
  br label %804

729:                                              ; preds = %709
  %730 = landingpad { ptr, i32 }
          cleanup
  %731 = extractvalue { ptr, i32 } %730, 0
  store ptr %731, ptr %12, align 8
  %732 = extractvalue { ptr, i32 } %730, 1
  store i32 %732, ptr %13, align 4
  br label %737

733:                                              ; preds = %710
  %734 = landingpad { ptr, i32 }
          cleanup
  %735 = extractvalue { ptr, i32 } %734, 0
  store ptr %735, ptr %12, align 8
  %736 = extractvalue { ptr, i32 } %734, 1
  store i32 %736, ptr %13, align 4
  call void @_ZN17RigidBodyDynamics5JointD2Ev(ptr noundef nonnull align 8 dereferenceable(24) %48) #7
  br label %737

737:                                              ; preds = %733, %729
  call void @llvm.lifetime.end.p0(i64 24, ptr %48) #7
  br label %804

738:                                              ; preds = %704
  call void @llvm.lifetime.start.p0(i64 24, ptr %49) #7
  call void @llvm.lifetime.start.p0(i64 8, ptr %50) #7
  store double 0.000000e+00, ptr %50, align 8, !tbaa !17
  call void @llvm.lifetime.start.p0(i64 8, ptr %51) #7
  store double 1.000000e+00, ptr %51, align 8, !tbaa !17
  call void @llvm.lifetime.start.p0(i64 8, ptr %52) #7
  store double 0.000000e+00, ptr %52, align 8, !tbaa !17
  invoke void @_ZN9Vector3_tC2ERKdS1_S1_(ptr noundef nonnull align 8 dereferenceable(24) %49, ptr noundef nonnull align 8 dereferenceable(8) %50, ptr noundef nonnull align 8 dereferenceable(8) %51, ptr noundef nonnull align 8 dereferenceable(8) %52)
          to label %739 unwind label %750

739:                                              ; preds = %738
  %740 = invoke noundef double @_ZNK5Eigen10MatrixBaseINS_6MatrixIdLi3ELi1ELi0ELi3ELi1EEEE3dotIS2_EENS_20ScalarBinaryOpTraitsIdNS_8internal6traitsIT_E6ScalarENS6_17scalar_product_opIdSA_EEE10ReturnTypeERKNS0_IS8_EE(ptr noundef nonnull align 1 dereferenceable(1) %43, ptr noundef nonnull align 1 dereferenceable(1) %49)
          to label %741 unwind label %750

741:                                              ; preds = %739
  %742 = fsub double %740, 1.000000e+00
  %743 = call double @llvm.fabs.f64(double %742)
  %744 = call noundef double @_ZNSt14numeric_limitsIdE7epsilonEv() #7
  %745 = fcmp olt double %743, %744
  call void @llvm.lifetime.end.p0(i64 8, ptr %52) #7
  call void @llvm.lifetime.end.p0(i64 8, ptr %51) #7
  call void @llvm.lifetime.end.p0(i64 8, ptr %50) #7
  call void @llvm.lifetime.end.p0(i64 24, ptr %49) #7
  br i1 %745, label %746, label %763

746:                                              ; preds = %741
  call void @llvm.lifetime.start.p0(i64 24, ptr %53) #7
  invoke void @_ZN17RigidBodyDynamics5JointC2ENS_9JointTypeE(ptr noundef nonnull align 8 dereferenceable(24) %53, i32 noundef 4)
          to label %747 unwind label %754

747:                                              ; preds = %746
  %748 = invoke noundef nonnull align 8 dereferenceable(24) ptr @_ZN17RigidBodyDynamics5JointaSERKS0_(ptr noundef nonnull align 8 dereferenceable(24) %42, ptr noundef nonnull align 8 dereferenceable(24) %53)
          to label %749 unwind label %758

749:                                              ; preds = %747
  call void @_ZN17RigidBodyDynamics5JointD2Ev(ptr noundef nonnull align 8 dereferenceable(24) %53) #7
  call void @llvm.lifetime.end.p0(i64 24, ptr %53) #7
  br label %802

750:                                              ; preds = %739, %738
  %751 = landingpad { ptr, i32 }
          cleanup
  %752 = extractvalue { ptr, i32 } %751, 0
  store ptr %752, ptr %12, align 8
  %753 = extractvalue { ptr, i32 } %751, 1
  store i32 %753, ptr %13, align 4
  call void @llvm.lifetime.end.p0(i64 8, ptr %52) #7
  call void @llvm.lifetime.end.p0(i64 8, ptr %51) #7
  call void @llvm.lifetime.end.p0(i64 8, ptr %50) #7
  call void @llvm.lifetime.end.p0(i64 24, ptr %49) #7
  br label %804

754:                                              ; preds = %746
  %755 = landingpad { ptr, i32 }
          cleanup
  %756 = extractvalue { ptr, i32 } %755, 0
  store ptr %756, ptr %12, align 8
  %757 = extractvalue { ptr, i32 } %755, 1
  store i32 %757, ptr %13, align 4
  br label %762

758:                                              ; preds = %747
  %759 = landingpad { ptr, i32 }
          cleanup
  %760 = extractvalue { ptr, i32 } %759, 0
  store ptr %760, ptr %12, align 8
  %761 = extractvalue { ptr, i32 } %759, 1
  store i32 %761, ptr %13, align 4
  call void @_ZN17RigidBodyDynamics5JointD2Ev(ptr noundef nonnull align 8 dereferenceable(24) %53) #7
  br label %762

762:                                              ; preds = %758, %754
  call void @llvm.lifetime.end.p0(i64 24, ptr %53) #7
  br label %804

763:                                              ; preds = %741
  call void @llvm.lifetime.start.p0(i64 24, ptr %54) #7
  call void @llvm.lifetime.start.p0(i64 8, ptr %55) #7
  store double 0.000000e+00, ptr %55, align 8, !tbaa !17
  call void @llvm.lifetime.start.p0(i64 8, ptr %56) #7
  store double 0.000000e+00, ptr %56, align 8, !tbaa !17
  call void @llvm.lifetime.start.p0(i64 8, ptr %57) #7
  store double 1.000000e+00, ptr %57, align 8, !tbaa !17
  invoke void @_ZN9Vector3_tC2ERKdS1_S1_(ptr noundef nonnull align 8 dereferenceable(24) %54, ptr noundef nonnull align 8 dereferenceable(8) %55, ptr noundef nonnull align 8 dereferenceable(8) %56, ptr noundef nonnull align 8 dereferenceable(8) %57)
          to label %764 unwind label %775

764:                                              ; preds = %763
  %765 = invoke noundef double @_ZNK5Eigen10MatrixBaseINS_6MatrixIdLi3ELi1ELi0ELi3ELi1EEEE3dotIS2_EENS_20ScalarBinaryOpTraitsIdNS_8internal6traitsIT_E6ScalarENS6_17scalar_product_opIdSA_EEE10ReturnTypeERKNS0_IS8_EE(ptr noundef nonnull align 1 dereferenceable(1) %43, ptr noundef nonnull align 1 dereferenceable(1) %54)
          to label %766 unwind label %775

766:                                              ; preds = %764
  %767 = fsub double %765, 1.000000e+00
  %768 = call double @llvm.fabs.f64(double %767)
  %769 = call noundef double @_ZNSt14numeric_limitsIdE7epsilonEv() #7
  %770 = fcmp olt double %768, %769
  call void @llvm.lifetime.end.p0(i64 8, ptr %57) #7
  call void @llvm.lifetime.end.p0(i64 8, ptr %56) #7
  call void @llvm.lifetime.end.p0(i64 8, ptr %55) #7
  call void @llvm.lifetime.end.p0(i64 24, ptr %54) #7
  br i1 %770, label %771, label %788

771:                                              ; preds = %766
  call void @llvm.lifetime.start.p0(i64 24, ptr %58) #7
  invoke void @_ZN17RigidBodyDynamics5JointC2ENS_9JointTypeE(ptr noundef nonnull align 8 dereferenceable(24) %58, i32 noundef 5)
          to label %772 unwind label %779

772:                                              ; preds = %771
  %773 = invoke noundef nonnull align 8 dereferenceable(24) ptr @_ZN17RigidBodyDynamics5JointaSERKS0_(ptr noundef nonnull align 8 dereferenceable(24) %42, ptr noundef nonnull align 8 dereferenceable(24) %58)
          to label %774 unwind label %783

774:                                              ; preds = %772
  call void @_ZN17RigidBodyDynamics5JointD2Ev(ptr noundef nonnull align 8 dereferenceable(24) %58) #7
  call void @llvm.lifetime.end.p0(i64 24, ptr %58) #7
  br label %801

775:                                              ; preds = %764, %763
  %776 = landingpad { ptr, i32 }
          cleanup
  %777 = extractvalue { ptr, i32 } %776, 0
  store ptr %777, ptr %12, align 8
  %778 = extractvalue { ptr, i32 } %776, 1
  store i32 %778, ptr %13, align 4
  call void @llvm.lifetime.end.p0(i64 8, ptr %57) #7
  call void @llvm.lifetime.end.p0(i64 8, ptr %56) #7
  call void @llvm.lifetime.end.p0(i64 8, ptr %55) #7
  call void @llvm.lifetime.end.p0(i64 24, ptr %54) #7
  br label %804

779:                                              ; preds = %771
  %780 = landingpad { ptr, i32 }
          cleanup
  %781 = extractvalue { ptr, i32 } %780, 0
  store ptr %781, ptr %12, align 8
  %782 = extractvalue { ptr, i32 } %780, 1
  store i32 %782, ptr %13, align 4
  br label %787

783:                                              ; preds = %772
  %784 = landingpad { ptr, i32 }
          cleanup
  %785 = extractvalue { ptr, i32 } %784, 0
  store ptr %785, ptr %12, align 8
  %786 = extractvalue { ptr, i32 } %784, 1
  store i32 %786, ptr %13, align 4
  call void @_ZN17RigidBodyDynamics5JointD2Ev(ptr noundef nonnull align 8 dereferenceable(24) %58) #7
  br label %787

787:                                              ; preds = %783, %779
  call void @llvm.lifetime.end.p0(i64 24, ptr %58) #7
  br label %804

788:                                              ; preds = %766
  call void @llvm.lifetime.start.p0(i64 24, ptr %59) #7
  invoke void @_ZN17RigidBodyDynamics5JointC2ENS_9JointTypeERK9Vector3_t(ptr noundef nonnull align 8 dereferenceable(24) %59, i32 noundef 1, ptr noundef nonnull align 8 dereferenceable(24) %43)
          to label %789 unwind label %792

789:                                              ; preds = %788
  %790 = invoke noundef nonnull align 8 dereferenceable(24) ptr @_ZN17RigidBodyDynamics5JointaSERKS0_(ptr noundef nonnull align 8 dereferenceable(24) %42, ptr noundef nonnull align 8 dereferenceable(24) %59)
          to label %791 unwind label %796

791:                                              ; preds = %789
  call void @_ZN17RigidBodyDynamics5JointD2Ev(ptr noundef nonnull align 8 dereferenceable(24) %59) #7
  call void @llvm.lifetime.end.p0(i64 24, ptr %59) #7
  br label %801

792:                                              ; preds = %788
  %793 = landingpad { ptr, i32 }
          cleanup
  %794 = extractvalue { ptr, i32 } %793, 0
  store ptr %794, ptr %12, align 8
  %795 = extractvalue { ptr, i32 } %793, 1
  store i32 %795, ptr %13, align 4
  br label %800

796:                                              ; preds = %789
  %797 = landingpad { ptr, i32 }
          cleanup
  %798 = extractvalue { ptr, i32 } %797, 0
  store ptr %798, ptr %12, align 8
  %799 = extractvalue { ptr, i32 } %797, 1
  store i32 %799, ptr %13, align 4
  call void @_ZN17RigidBodyDynamics5JointD2Ev(ptr noundef nonnull align 8 dereferenceable(24) %59) #7
  br label %800

800:                                              ; preds = %796, %792
  call void @llvm.lifetime.end.p0(i64 24, ptr %59) #7
  br label %804

801:                                              ; preds = %791, %774
  br label %802

802:                                              ; preds = %801, %749
  br label %803

803:                                              ; preds = %802, %712
  call void @llvm.lifetime.end.p0(i64 24, ptr %43) #7
  br label %933

804:                                              ; preds = %800, %787, %775, %762, %750, %737, %725, %721
  call void @llvm.lifetime.end.p0(i64 24, ptr %43) #7
  br label %1410

805:                                              ; preds = %684
  %806 = invoke noundef ptr @_ZN13my_shared_ptrIN4urdf5JointEEptEv(ptr noundef nonnull align 8 dereferenceable(16) %38)
          to label %807 unwind label %717

807:                                              ; preds = %805
  %808 = getelementptr inbounds %"class.urdf::Joint", ptr %806, i32 0, i32 1
  %809 = load i32, ptr %808, align 8, !tbaa !42
  %810 = icmp eq i32 %809, 3
  br i1 %810, label %811, label %842

811:                                              ; preds = %807
  call void @llvm.lifetime.start.p0(i64 24, ptr %60) #7
  %812 = invoke noundef ptr @_ZN13my_shared_ptrIN4urdf5JointEEptEv(ptr noundef nonnull align 8 dereferenceable(16) %38)
          to label %813 unwind label %828

813:                                              ; preds = %811
  %814 = getelementptr inbounds %"class.urdf::Joint", ptr %812, i32 0, i32 2
  %815 = getelementptr inbounds %"class.urdf::Vector3", ptr %814, i32 0, i32 0
  %816 = invoke noundef ptr @_ZN13my_shared_ptrIN4urdf5JointEEptEv(ptr noundef nonnull align 8 dereferenceable(16) %38)
          to label %817 unwind label %828

817:                                              ; preds = %813
  %818 = getelementptr inbounds %"class.urdf::Joint", ptr %816, i32 0, i32 2
  %819 = getelementptr inbounds %"class.urdf::Vector3", ptr %818, i32 0, i32 1
  %820 = invoke noundef ptr @_ZN13my_shared_ptrIN4urdf5JointEEptEv(ptr noundef nonnull align 8 dereferenceable(16) %38)
          to label %821 unwind label %828

821:                                              ; preds = %817
  %822 = getelementptr inbounds %"class.urdf::Joint", ptr %820, i32 0, i32 2
  %823 = getelementptr inbounds %"class.urdf::Vector3", ptr %822, i32 0, i32 2
  invoke void @_ZN9Vector3_tC2ERKdS1_S1_(ptr noundef nonnull align 8 dereferenceable(24) %60, ptr noundef nonnull align 8 dereferenceable(8) %815, ptr noundef nonnull align 8 dereferenceable(8) %819, ptr noundef nonnull align 8 dereferenceable(8) %823)
          to label %824 unwind label %828

824:                                              ; preds = %821
  call void @llvm.lifetime.start.p0(i64 24, ptr %61) #7
  invoke void @_ZN17RigidBodyDynamics5JointC2ENS_9JointTypeERK9Vector3_t(ptr noundef nonnull align 8 dereferenceable(24) %61, i32 noundef 2, ptr noundef nonnull align 8 dereferenceable(24) %60)
          to label %825 unwind label %832

825:                                              ; preds = %824
  %826 = invoke noundef nonnull align 8 dereferenceable(24) ptr @_ZN17RigidBodyDynamics5JointaSERKS0_(ptr noundef nonnull align 8 dereferenceable(24) %42, ptr noundef nonnull align 8 dereferenceable(24) %61)
          to label %827 unwind label %836

827:                                              ; preds = %825
  call void @_ZN17RigidBodyDynamics5JointD2Ev(ptr noundef nonnull align 8 dereferenceable(24) %61) #7
  call void @llvm.lifetime.end.p0(i64 24, ptr %61) #7
  call void @llvm.lifetime.end.p0(i64 24, ptr %60) #7
  br label %932

828:                                              ; preds = %821, %817, %813, %811
  %829 = landingpad { ptr, i32 }
          cleanup
  %830 = extractvalue { ptr, i32 } %829, 0
  store ptr %830, ptr %12, align 8
  %831 = extractvalue { ptr, i32 } %829, 1
  store i32 %831, ptr %13, align 4
  br label %841

832:                                              ; preds = %824
  %833 = landingpad { ptr, i32 }
          cleanup
  %834 = extractvalue { ptr, i32 } %833, 0
  store ptr %834, ptr %12, align 8
  %835 = extractvalue { ptr, i32 } %833, 1
  store i32 %835, ptr %13, align 4
  br label %840

836:                                              ; preds = %825
  %837 = landingpad { ptr, i32 }
          cleanup
  %838 = extractvalue { ptr, i32 } %837, 0
  store ptr %838, ptr %12, align 8
  %839 = extractvalue { ptr, i32 } %837, 1
  store i32 %839, ptr %13, align 4
  call void @_ZN17RigidBodyDynamics5JointD2Ev(ptr noundef nonnull align 8 dereferenceable(24) %61) #7
  br label %840

840:                                              ; preds = %836, %832
  call void @llvm.lifetime.end.p0(i64 24, ptr %61) #7
  br label %841

841:                                              ; preds = %840, %828
  call void @llvm.lifetime.end.p0(i64 24, ptr %60) #7
  br label %1410

842:                                              ; preds = %807
  %843 = invoke noundef ptr @_ZN13my_shared_ptrIN4urdf5JointEEptEv(ptr noundef nonnull align 8 dereferenceable(16) %38)
          to label %844 unwind label %717

844:                                              ; preds = %842
  %845 = getelementptr inbounds %"class.urdf::Joint", ptr %843, i32 0, i32 1
  %846 = load i32, ptr %845, align 8, !tbaa !42
  %847 = icmp eq i32 %846, 6
  br i1 %847, label %848, label %861

848:                                              ; preds = %844
  call void @llvm.lifetime.start.p0(i64 24, ptr %62) #7
  invoke void @_ZN17RigidBodyDynamics5JointC2ENS_9JointTypeE(ptr noundef nonnull align 8 dereferenceable(24) %62, i32 noundef 13)
          to label %849 unwind label %852

849:                                              ; preds = %848
  %850 = invoke noundef nonnull align 8 dereferenceable(24) ptr @_ZN17RigidBodyDynamics5JointaSERKS0_(ptr noundef nonnull align 8 dereferenceable(24) %42, ptr noundef nonnull align 8 dereferenceable(24) %62)
          to label %851 unwind label %856

851:                                              ; preds = %849
  call void @_ZN17RigidBodyDynamics5JointD2Ev(ptr noundef nonnull align 8 dereferenceable(24) %62) #7
  call void @llvm.lifetime.end.p0(i64 24, ptr %62) #7
  br label %931

852:                                              ; preds = %848
  %853 = landingpad { ptr, i32 }
          cleanup
  %854 = extractvalue { ptr, i32 } %853, 0
  store ptr %854, ptr %12, align 8
  %855 = extractvalue { ptr, i32 } %853, 1
  store i32 %855, ptr %13, align 4
  br label %860

856:                                              ; preds = %849
  %857 = landingpad { ptr, i32 }
          cleanup
  %858 = extractvalue { ptr, i32 } %857, 0
  store ptr %858, ptr %12, align 8
  %859 = extractvalue { ptr, i32 } %857, 1
  store i32 %859, ptr %13, align 4
  call void @_ZN17RigidBodyDynamics5JointD2Ev(ptr noundef nonnull align 8 dereferenceable(24) %62) #7
  br label %860

860:                                              ; preds = %856, %852
  call void @llvm.lifetime.end.p0(i64 24, ptr %62) #7
  br label %1410

861:                                              ; preds = %844
  %862 = invoke noundef ptr @_ZN13my_shared_ptrIN4urdf5JointEEptEv(ptr noundef nonnull align 8 dereferenceable(16) %38)
          to label %863 unwind label %717

863:                                              ; preds = %861
  %864 = getelementptr inbounds %"class.urdf::Joint", ptr %862, i32 0, i32 1
  %865 = load i32, ptr %864, align 8, !tbaa !42
  %866 = icmp eq i32 %865, 4
  br i1 %866, label %867, label %911

867:                                              ; preds = %863
  call void @llvm.lifetime.start.p0(i64 24, ptr %63) #7
  call void @llvm.lifetime.start.p0(i64 48, ptr %64) #7
  call void @llvm.lifetime.start.p0(i64 8, ptr %65) #7
  store double 0.000000e+00, ptr %65, align 8, !tbaa !17
  call void @llvm.lifetime.start.p0(i64 8, ptr %66) #7
  store double 0.000000e+00, ptr %66, align 8, !tbaa !17
  call void @llvm.lifetime.start.p0(i64 8, ptr %67) #7
  store double 0.000000e+00, ptr %67, align 8, !tbaa !17
  call void @llvm.lifetime.start.p0(i64 8, ptr %68) #7
  store double 1.000000e+00, ptr %68, align 8, !tbaa !17
  call void @llvm.lifetime.start.p0(i64 8, ptr %69) #7
  store double 0.000000e+00, ptr %69, align 8, !tbaa !17
  call void @llvm.lifetime.start.p0(i64 8, ptr %70) #7
  store double 0.000000e+00, ptr %70, align 8, !tbaa !17
  invoke void @_ZN15SpatialVector_tC2ERKdS1_S1_S1_S1_S1_(ptr noundef nonnull align 16 dereferenceable(48) %64, ptr noundef nonnull align 8 dereferenceable(8) %65, ptr noundef nonnull align 8 dereferenceable(8) %66, ptr noundef nonnull align 8 dereferenceable(8) %67, ptr noundef nonnull align 8 dereferenceable(8) %68, ptr noundef nonnull align 8 dereferenceable(8) %69, ptr noundef nonnull align 8 dereferenceable(8) %70)
          to label %868 unwind label %877

868:                                              ; preds = %867
  call void @llvm.lifetime.start.p0(i64 48, ptr %71) #7
  call void @llvm.lifetime.start.p0(i64 8, ptr %72) #7
  store double 0.000000e+00, ptr %72, align 8, !tbaa !17
  call void @llvm.lifetime.start.p0(i64 8, ptr %73) #7
  store double 0.000000e+00, ptr %73, align 8, !tbaa !17
  call void @llvm.lifetime.start.p0(i64 8, ptr %74) #7
  store double 0.000000e+00, ptr %74, align 8, !tbaa !17
  call void @llvm.lifetime.start.p0(i64 8, ptr %75) #7
  store double 0.000000e+00, ptr %75, align 8, !tbaa !17
  call void @llvm.lifetime.start.p0(i64 8, ptr %76) #7
  store double 1.000000e+00, ptr %76, align 8, !tbaa !17
  call void @llvm.lifetime.start.p0(i64 8, ptr %77) #7
  store double 0.000000e+00, ptr %77, align 8, !tbaa !17
  invoke void @_ZN15SpatialVector_tC2ERKdS1_S1_S1_S1_S1_(ptr noundef nonnull align 16 dereferenceable(48) %71, ptr noundef nonnull align 8 dereferenceable(8) %72, ptr noundef nonnull align 8 dereferenceable(8) %73, ptr noundef nonnull align 8 dereferenceable(8) %74, ptr noundef nonnull align 8 dereferenceable(8) %75, ptr noundef nonnull align 8 dereferenceable(8) %76, ptr noundef nonnull align 8 dereferenceable(8) %77)
          to label %869 unwind label %881

869:                                              ; preds = %868
  call void @llvm.lifetime.start.p0(i64 48, ptr %78) #7
  call void @llvm.lifetime.start.p0(i64 8, ptr %79) #7
  store double 0.000000e+00, ptr %79, align 8, !tbaa !17
  call void @llvm.lifetime.start.p0(i64 8, ptr %80) #7
  store double 0.000000e+00, ptr %80, align 8, !tbaa !17
  call void @llvm.lifetime.start.p0(i64 8, ptr %81) #7
  store double 0.000000e+00, ptr %81, align 8, !tbaa !17
  call void @llvm.lifetime.start.p0(i64 8, ptr %82) #7
  store double 0.000000e+00, ptr %82, align 8, !tbaa !17
  call void @llvm.lifetime.start.p0(i64 8, ptr %83) #7
  store double 0.000000e+00, ptr %83, align 8, !tbaa !17
  call void @llvm.lifetime.start.p0(i64 8, ptr %84) #7
  store double 1.000000e+00, ptr %84, align 8, !tbaa !17
  invoke void @_ZN15SpatialVector_tC2ERKdS1_S1_S1_S1_S1_(ptr noundef nonnull align 16 dereferenceable(48) %78, ptr noundef nonnull align 8 dereferenceable(8) %79, ptr noundef nonnull align 8 dereferenceable(8) %80, ptr noundef nonnull align 8 dereferenceable(8) %81, ptr noundef nonnull align 8 dereferenceable(8) %82, ptr noundef nonnull align 8 dereferenceable(8) %83, ptr noundef nonnull align 8 dereferenceable(8) %84)
          to label %870 unwind label %885

870:                                              ; preds = %869
  call void @llvm.lifetime.start.p0(i64 48, ptr %85) #7
  call void @llvm.lifetime.start.p0(i64 8, ptr %86) #7
  store double 1.000000e+00, ptr %86, align 8, !tbaa !17
  call void @llvm.lifetime.start.p0(i64 8, ptr %87) #7
  store double 0.000000e+00, ptr %87, align 8, !tbaa !17
  call void @llvm.lifetime.start.p0(i64 8, ptr %88) #7
  store double 0.000000e+00, ptr %88, align 8, !tbaa !17
  call void @llvm.lifetime.start.p0(i64 8, ptr %89) #7
  store double 0.000000e+00, ptr %89, align 8, !tbaa !17
  call void @llvm.lifetime.start.p0(i64 8, ptr %90) #7
  store double 0.000000e+00, ptr %90, align 8, !tbaa !17
  call void @llvm.lifetime.start.p0(i64 8, ptr %91) #7
  store double 0.000000e+00, ptr %91, align 8, !tbaa !17
  invoke void @_ZN15SpatialVector_tC2ERKdS1_S1_S1_S1_S1_(ptr noundef nonnull align 16 dereferenceable(48) %85, ptr noundef nonnull align 8 dereferenceable(8) %86, ptr noundef nonnull align 8 dereferenceable(8) %87, ptr noundef nonnull align 8 dereferenceable(8) %88, ptr noundef nonnull align 8 dereferenceable(8) %89, ptr noundef nonnull align 8 dereferenceable(8) %90, ptr noundef nonnull align 8 dereferenceable(8) %91)
          to label %871 unwind label %889

871:                                              ; preds = %870
  call void @llvm.lifetime.start.p0(i64 48, ptr %92) #7
  call void @llvm.lifetime.start.p0(i64 8, ptr %93) #7
  store double 0.000000e+00, ptr %93, align 8, !tbaa !17
  call void @llvm.lifetime.start.p0(i64 8, ptr %94) #7
  store double 1.000000e+00, ptr %94, align 8, !tbaa !17
  call void @llvm.lifetime.start.p0(i64 8, ptr %95) #7
  store double 0.000000e+00, ptr %95, align 8, !tbaa !17
  call void @llvm.lifetime.start.p0(i64 8, ptr %96) #7
  store double 0.000000e+00, ptr %96, align 8, !tbaa !17
  call void @llvm.lifetime.start.p0(i64 8, ptr %97) #7
  store double 0.000000e+00, ptr %97, align 8, !tbaa !17
  call void @llvm.lifetime.start.p0(i64 8, ptr %98) #7
  store double 0.000000e+00, ptr %98, align 8, !tbaa !17
  invoke void @_ZN15SpatialVector_tC2ERKdS1_S1_S1_S1_S1_(ptr noundef nonnull align 16 dereferenceable(48) %92, ptr noundef nonnull align 8 dereferenceable(8) %93, ptr noundef nonnull align 8 dereferenceable(8) %94, ptr noundef nonnull align 8 dereferenceable(8) %95, ptr noundef nonnull align 8 dereferenceable(8) %96, ptr noundef nonnull align 8 dereferenceable(8) %97, ptr noundef nonnull align 8 dereferenceable(8) %98)
          to label %872 unwind label %893

872:                                              ; preds = %871
  call void @llvm.lifetime.start.p0(i64 48, ptr %99) #7
  call void @llvm.lifetime.start.p0(i64 8, ptr %100) #7
  store double 0.000000e+00, ptr %100, align 8, !tbaa !17
  call void @llvm.lifetime.start.p0(i64 8, ptr %101) #7
  store double 0.000000e+00, ptr %101, align 8, !tbaa !17
  call void @llvm.lifetime.start.p0(i64 8, ptr %102) #7
  store double 1.000000e+00, ptr %102, align 8, !tbaa !17
  call void @llvm.lifetime.start.p0(i64 8, ptr %103) #7
  store double 0.000000e+00, ptr %103, align 8, !tbaa !17
  call void @llvm.lifetime.start.p0(i64 8, ptr %104) #7
  store double 0.000000e+00, ptr %104, align 8, !tbaa !17
  call void @llvm.lifetime.start.p0(i64 8, ptr %105) #7
  store double 0.000000e+00, ptr %105, align 8, !tbaa !17
  invoke void @_ZN15SpatialVector_tC2ERKdS1_S1_S1_S1_S1_(ptr noundef nonnull align 16 dereferenceable(48) %99, ptr noundef nonnull align 8 dereferenceable(8) %100, ptr noundef nonnull align 8 dereferenceable(8) %101, ptr noundef nonnull align 8 dereferenceable(8) %102, ptr noundef nonnull align 8 dereferenceable(8) %103, ptr noundef nonnull align 8 dereferenceable(8) %104, ptr noundef nonnull align 8 dereferenceable(8) %105)
          to label %873 unwind label %897

873:                                              ; preds = %872
  invoke void @_ZN17RigidBodyDynamics5JointC2ERK15SpatialVector_tS3_S3_S3_S3_S3_(ptr noundef nonnull align 8 dereferenceable(24) %63, ptr noundef nonnull align 16 dereferenceable(48) %64, ptr noundef nonnull align 16 dereferenceable(48) %71, ptr noundef nonnull align 16 dereferenceable(48) %78, ptr noundef nonnull align 16 dereferenceable(48) %85, ptr noundef nonnull align 16 dereferenceable(48) %92, ptr noundef nonnull align 16 dereferenceable(48) %99)
          to label %874 unwind label %897

874:                                              ; preds = %873
  %875 = invoke noundef nonnull align 8 dereferenceable(24) ptr @_ZN17RigidBodyDynamics5JointaSERKS0_(ptr noundef nonnull align 8 dereferenceable(24) %42, ptr noundef nonnull align 8 dereferenceable(24) %63)
          to label %876 unwind label %901

876:                                              ; preds = %874
  call void @_ZN17RigidBodyDynamics5JointD2Ev(ptr noundef nonnull align 8 dereferenceable(24) %63) #7
  call void @llvm.lifetime.end.p0(i64 8, ptr %105) #7
  call void @llvm.lifetime.end.p0(i64 8, ptr %104) #7
  call void @llvm.lifetime.end.p0(i64 8, ptr %103) #7
  call void @llvm.lifetime.end.p0(i64 8, ptr %102) #7
  call void @llvm.lifetime.end.p0(i64 8, ptr %101) #7
  call void @llvm.lifetime.end.p0(i64 8, ptr %100) #7
  call void @llvm.lifetime.end.p0(i64 48, ptr %99) #7
  call void @llvm.lifetime.end.p0(i64 8, ptr %98) #7
  call void @llvm.lifetime.end.p0(i64 8, ptr %97) #7
  call void @llvm.lifetime.end.p0(i64 8, ptr %96) #7
  call void @llvm.lifetime.end.p0(i64 8, ptr %95) #7
  call void @llvm.lifetime.end.p0(i64 8, ptr %94) #7
  call void @llvm.lifetime.end.p0(i64 8, ptr %93) #7
  call void @llvm.lifetime.end.p0(i64 48, ptr %92) #7
  call void @llvm.lifetime.end.p0(i64 8, ptr %91) #7
  call void @llvm.lifetime.end.p0(i64 8, ptr %90) #7
  call void @llvm.lifetime.end.p0(i64 8, ptr %89) #7
  call void @llvm.lifetime.end.p0(i64 8, ptr %88) #7
  call void @llvm.lifetime.end.p0(i64 8, ptr %87) #7
  call void @llvm.lifetime.end.p0(i64 8, ptr %86) #7
  call void @llvm.lifetime.end.p0(i64 48, ptr %85) #7
  call void @llvm.lifetime.end.p0(i64 8, ptr %84) #7
  call void @llvm.lifetime.end.p0(i64 8, ptr %83) #7
  call void @llvm.lifetime.end.p0(i64 8, ptr %82) #7
  call void @llvm.lifetime.end.p0(i64 8, ptr %81) #7
  call void @llvm.lifetime.end.p0(i64 8, ptr %80) #7
  call void @llvm.lifetime.end.p0(i64 8, ptr %79) #7
  call void @llvm.lifetime.end.p0(i64 48, ptr %78) #7
  call void @llvm.lifetime.end.p0(i64 8, ptr %77) #7
  call void @llvm.lifetime.end.p0(i64 8, ptr %76) #7
  call void @llvm.lifetime.end.p0(i64 8, ptr %75) #7
  call void @llvm.lifetime.end.p0(i64 8, ptr %74) #7
  call void @llvm.lifetime.end.p0(i64 8, ptr %73) #7
  call void @llvm.lifetime.end.p0(i64 8, ptr %72) #7
  call void @llvm.lifetime.end.p0(i64 48, ptr %71) #7
  call void @llvm.lifetime.end.p0(i64 8, ptr %70) #7
  call void @llvm.lifetime.end.p0(i64 8, ptr %69) #7
  call void @llvm.lifetime.end.p0(i64 8, ptr %68) #7
  call void @llvm.lifetime.end.p0(i64 8, ptr %67) #7
  call void @llvm.lifetime.end.p0(i64 8, ptr %66) #7
  call void @llvm.lifetime.end.p0(i64 8, ptr %65) #7
  call void @llvm.lifetime.end.p0(i64 48, ptr %64) #7
  call void @llvm.lifetime.end.p0(i64 24, ptr %63) #7
  br label %930

877:                                              ; preds = %867
  %878 = landingpad { ptr, i32 }
          cleanup
  %879 = extractvalue { ptr, i32 } %878, 0
  store ptr %879, ptr %12, align 8
  %880 = extractvalue { ptr, i32 } %878, 1
  store i32 %880, ptr %13, align 4
  br label %910

881:                                              ; preds = %868
  %882 = landingpad { ptr, i32 }
          cleanup
  %883 = extractvalue { ptr, i32 } %882, 0
  store ptr %883, ptr %12, align 8
  %884 = extractvalue { ptr, i32 } %882, 1
  store i32 %884, ptr %13, align 4
  br label %909

885:                                              ; preds = %869
  %886 = landingpad { ptr, i32 }
          cleanup
  %887 = extractvalue { ptr, i32 } %886, 0
  store ptr %887, ptr %12, align 8
  %888 = extractvalue { ptr, i32 } %886, 1
  store i32 %888, ptr %13, align 4
  br label %908

889:                                              ; preds = %870
  %890 = landingpad { ptr, i32 }
          cleanup
  %891 = extractvalue { ptr, i32 } %890, 0
  store ptr %891, ptr %12, align 8
  %892 = extractvalue { ptr, i32 } %890, 1
  store i32 %892, ptr %13, align 4
  br label %907

893:                                              ; preds = %871
  %894 = landingpad { ptr, i32 }
          cleanup
  %895 = extractvalue { ptr, i32 } %894, 0
  store ptr %895, ptr %12, align 8
  %896 = extractvalue { ptr, i32 } %894, 1
  store i32 %896, ptr %13, align 4
  br label %906

897:                                              ; preds = %873, %872
  %898 = landingpad { ptr, i32 }
          cleanup
  %899 = extractvalue { ptr, i32 } %898, 0
  store ptr %899, ptr %12, align 8
  %900 = extractvalue { ptr, i32 } %898, 1
  store i32 %900, ptr %13, align 4
  br label %905

901:                                              ; preds = %874
  %902 = landingpad { ptr, i32 }
          cleanup
  %903 = extractvalue { ptr, i32 } %902, 0
  store ptr %903, ptr %12, align 8
  %904 = extractvalue { ptr, i32 } %902, 1
  store i32 %904, ptr %13, align 4
  call void @_ZN17RigidBodyDynamics5JointD2Ev(ptr noundef nonnull align 8 dereferenceable(24) %63) #7
  br label %905

905:                                              ; preds = %901, %897
  call void @llvm.lifetime.end.p0(i64 8, ptr %105) #7
  call void @llvm.lifetime.end.p0(i64 8, ptr %104) #7
  call void @llvm.lifetime.end.p0(i64 8, ptr %103) #7
  call void @llvm.lifetime.end.p0(i64 8, ptr %102) #7
  call void @llvm.lifetime.end.p0(i64 8, ptr %101) #7
  call void @llvm.lifetime.end.p0(i64 8, ptr %100) #7
  call void @llvm.lifetime.end.p0(i64 48, ptr %99) #7
  br label %906

906:                                              ; preds = %905, %893
  call void @llvm.lifetime.end.p0(i64 8, ptr %98) #7
  call void @llvm.lifetime.end.p0(i64 8, ptr %97) #7
  call void @llvm.lifetime.end.p0(i64 8, ptr %96) #7
  call void @llvm.lifetime.end.p0(i64 8, ptr %95) #7
  call void @llvm.lifetime.end.p0(i64 8, ptr %94) #7
  call void @llvm.lifetime.end.p0(i64 8, ptr %93) #7
  call void @llvm.lifetime.end.p0(i64 48, ptr %92) #7
  br label %907

907:                                              ; preds = %906, %889
  call void @llvm.lifetime.end.p0(i64 8, ptr %91) #7
  call void @llvm.lifetime.end.p0(i64 8, ptr %90) #7
  call void @llvm.lifetime.end.p0(i64 8, ptr %89) #7
  call void @llvm.lifetime.end.p0(i64 8, ptr %88) #7
  call void @llvm.lifetime.end.p0(i64 8, ptr %87) #7
  call void @llvm.lifetime.end.p0(i64 8, ptr %86) #7
  call void @llvm.lifetime.end.p0(i64 48, ptr %85) #7
  br label %908

908:                                              ; preds = %907, %885
  call void @llvm.lifetime.end.p0(i64 8, ptr %84) #7
  call void @llvm.lifetime.end.p0(i64 8, ptr %83) #7
  call void @llvm.lifetime.end.p0(i64 8, ptr %82) #7
  call void @llvm.lifetime.end.p0(i64 8, ptr %81) #7
  call void @llvm.lifetime.end.p0(i64 8, ptr %80) #7
  call void @llvm.lifetime.end.p0(i64 8, ptr %79) #7
  call void @llvm.lifetime.end.p0(i64 48, ptr %78) #7
  br label %909

909:                                              ; preds = %908, %881
  call void @llvm.lifetime.end.p0(i64 8, ptr %77) #7
  call void @llvm.lifetime.end.p0(i64 8, ptr %76) #7
  call void @llvm.lifetime.end.p0(i64 8, ptr %75) #7
  call void @llvm.lifetime.end.p0(i64 8, ptr %74) #7
  call void @llvm.lifetime.end.p0(i64 8, ptr %73) #7
  call void @llvm.lifetime.end.p0(i64 8, ptr %72) #7
  call void @llvm.lifetime.end.p0(i64 48, ptr %71) #7
  br label %910

910:                                              ; preds = %909, %877
  call void @llvm.lifetime.end.p0(i64 8, ptr %70) #7
  call void @llvm.lifetime.end.p0(i64 8, ptr %69) #7
  call void @llvm.lifetime.end.p0(i64 8, ptr %68) #7
  call void @llvm.lifetime.end.p0(i64 8, ptr %67) #7
  call void @llvm.lifetime.end.p0(i64 8, ptr %66) #7
  call void @llvm.lifetime.end.p0(i64 8, ptr %65) #7
  call void @llvm.lifetime.end.p0(i64 48, ptr %64) #7
  call void @llvm.lifetime.end.p0(i64 24, ptr %63) #7
  br label %1410

911:                                              ; preds = %863
  %912 = invoke noundef ptr @_ZN13my_shared_ptrIN4urdf5JointEEptEv(ptr noundef nonnull align 8 dereferenceable(16) %38)
          to label %913 unwind label %717

913:                                              ; preds = %911
  %914 = getelementptr inbounds %"class.urdf::Joint", ptr %912, i32 0, i32 1
  %915 = load i32, ptr %914, align 8, !tbaa !42
  %916 = icmp eq i32 %915, 5
  br i1 %916, label %917, label %929

917:                                              ; preds = %913
  %918 = invoke noundef nonnull align 8 dereferenceable(8) ptr @_ZStlsISt11char_traitsIcEERSt13basic_ostreamIcT_ES5_PKc(ptr noundef nonnull align 8 dereferenceable(8) @_ZSt4cerr, ptr noundef @.str.14)
          to label %919 unwind label %717

919:                                              ; preds = %917
  %920 = invoke noundef ptr @_ZN13my_shared_ptrIN4urdf5JointEEptEv(ptr noundef nonnull align 8 dereferenceable(16) %38)
          to label %921 unwind label %717

921:                                              ; preds = %919
  %922 = getelementptr inbounds %"class.urdf::Joint", ptr %920, i32 0, i32 0
  %923 = invoke noundef nonnull align 8 dereferenceable(8) ptr @_ZStlsIcSt11char_traitsIcESaIcEERSt13basic_ostreamIT_T0_ES7_RKNSt7__cxx1112basic_stringIS4_S5_T1_EE(ptr noundef nonnull align 8 dereferenceable(8) %918, ptr noundef nonnull align 8 dereferenceable(32) %922)
          to label %924 unwind label %717

924:                                              ; preds = %921
  %925 = invoke noundef nonnull align 8 dereferenceable(8) ptr @_ZStlsISt11char_traitsIcEERSt13basic_ostreamIcT_ES5_PKc(ptr noundef nonnull align 8 dereferenceable(8) %923, ptr noundef @.str.17)
          to label %926 unwind label %717

926:                                              ; preds = %924
  %927 = invoke noundef nonnull align 8 dereferenceable(8) ptr @_ZNSolsEPFRSoS_E(ptr noundef nonnull align 8 dereferenceable(8) %925, ptr noundef @_ZSt4endlIcSt11char_traitsIcEERSt13basic_ostreamIT_T0_ES6_)
          to label %928 unwind label %717

928:                                              ; preds = %926
  store i1 false, ptr %5, align 1
  store i32 1, ptr %106, align 4
  br label %1395

929:                                              ; preds = %913
  br label %930

930:                                              ; preds = %929, %876
  br label %931

931:                                              ; preds = %930, %851
  br label %932

932:                                              ; preds = %931, %827
  br label %933

933:                                              ; preds = %932, %803
  call void @llvm.lifetime.start.p0(i64 24, ptr %107) #7
  invoke void @_ZN9Vector3_tC2Ev(ptr noundef nonnull align 8 dereferenceable(24) %107)
          to label %934 unwind label %1135

934:                                              ; preds = %933
  call void @llvm.lifetime.start.p0(i64 24, ptr %108) #7
  invoke void @_ZN9Vector3_tC2Ev(ptr noundef nonnull align 8 dereferenceable(24) %108)
          to label %935 unwind label %1139

935:                                              ; preds = %934
  %936 = invoke noundef ptr @_ZN13my_shared_ptrIN4urdf5JointEEptEv(ptr noundef nonnull align 8 dereferenceable(16) %38)
          to label %937 unwind label %1139

937:                                              ; preds = %935
  %938 = getelementptr inbounds %"class.urdf::Joint", ptr %936, i32 0, i32 5
  %939 = getelementptr inbounds %"class.urdf::Pose", ptr %938, i32 0, i32 1
  %940 = invoke noundef nonnull align 8 dereferenceable(8) ptr @_ZN5Eigen15DenseCoeffsBaseINS_6MatrixIdLi3ELi1ELi0ELi3ELi1EEELi1EEixEl(ptr noundef nonnull align 1 dereferenceable(1) %107, i64 noundef 0)
          to label %941 unwind label %1139

941:                                              ; preds = %937
  %942 = invoke noundef nonnull align 8 dereferenceable(8) ptr @_ZN5Eigen15DenseCoeffsBaseINS_6MatrixIdLi3ELi1ELi0ELi3ELi1EEELi1EEixEl(ptr noundef nonnull align 1 dereferenceable(1) %107, i64 noundef 1)
          to label %943 unwind label %1139

943:                                              ; preds = %941
  %944 = invoke noundef nonnull align 8 dereferenceable(8) ptr @_ZN5Eigen15DenseCoeffsBaseINS_6MatrixIdLi3ELi1ELi0ELi3ELi1EEELi1EEixEl(ptr noundef nonnull align 1 dereferenceable(1) %107, i64 noundef 2)
          to label %945 unwind label %1139

945:                                              ; preds = %943
  invoke void @_ZNK4urdf8Rotation6getRPYERdS1_S1_(ptr noundef nonnull align 8 dereferenceable(32) %939, ptr noundef nonnull align 8 dereferenceable(8) %940, ptr noundef nonnull align 8 dereferenceable(8) %942, ptr noundef nonnull align 8 dereferenceable(8) %944)
          to label %946 unwind label %1139

946:                                              ; preds = %945
  %947 = invoke noundef ptr @_ZN13my_shared_ptrIN4urdf5JointEEptEv(ptr noundef nonnull align 8 dereferenceable(16) %38)
          to label %948 unwind label %1139

948:                                              ; preds = %946
  %949 = getelementptr inbounds %"class.urdf::Joint", ptr %947, i32 0, i32 5
  %950 = getelementptr inbounds %"class.urdf::Pose", ptr %949, i32 0, i32 0
  %951 = getelementptr inbounds %"class.urdf::Vector3", ptr %950, i32 0, i32 0
  %952 = invoke noundef ptr @_ZN13my_shared_ptrIN4urdf5JointEEptEv(ptr noundef nonnull align 8 dereferenceable(16) %38)
          to label %953 unwind label %1139

953:                                              ; preds = %948
  %954 = getelementptr inbounds %"class.urdf::Joint", ptr %952, i32 0, i32 5
  %955 = getelementptr inbounds %"class.urdf::Pose", ptr %954, i32 0, i32 0
  %956 = getelementptr inbounds %"class.urdf::Vector3", ptr %955, i32 0, i32 1
  %957 = invoke noundef ptr @_ZN13my_shared_ptrIN4urdf5JointEEptEv(ptr noundef nonnull align 8 dereferenceable(16) %38)
          to label %958 unwind label %1139

958:                                              ; preds = %953
  %959 = getelementptr inbounds %"class.urdf::Joint", ptr %957, i32 0, i32 5
  %960 = getelementptr inbounds %"class.urdf::Pose", ptr %959, i32 0, i32 0
  %961 = getelementptr inbounds %"class.urdf::Vector3", ptr %960, i32 0, i32 2
  invoke void @_ZN9Vector3_t3setERKdS1_S1_(ptr noundef nonnull align 8 dereferenceable(24) %108, ptr noundef nonnull align 8 dereferenceable(8) %951, ptr noundef nonnull align 8 dereferenceable(8) %956, ptr noundef nonnull align 8 dereferenceable(8) %961)
          to label %962 unwind label %1139

962:                                              ; preds = %958
  call void @llvm.lifetime.start.p0(i64 96, ptr %109) #7
  call void @llvm.lifetime.start.p0(i64 96, ptr %110) #7
  call void @llvm.lifetime.start.p0(i64 96, ptr %111) #7
  call void @llvm.lifetime.start.p0(i64 96, ptr %112) #7
  %963 = invoke noundef nonnull align 8 dereferenceable(8) ptr @_ZN5Eigen15DenseCoeffsBaseINS_6MatrixIdLi3ELi1ELi0ELi3ELi1EEELi1EEixEl(ptr noundef nonnull align 1 dereferenceable(1) %107, i64 noundef 0)
          to label %964 unwind label %1143

964:                                              ; preds = %962
  %965 = load double, ptr %963, align 8, !tbaa !17
  call void @llvm.lifetime.start.p0(i64 24, ptr %113) #7
  call void @llvm.lifetime.start.p0(i64 8, ptr %114) #7
  store double 1.000000e+00, ptr %114, align 8, !tbaa !17
  call void @llvm.lifetime.start.p0(i64 8, ptr %115) #7
  store double 0.000000e+00, ptr %115, align 8, !tbaa !17
  call void @llvm.lifetime.start.p0(i64 8, ptr %116) #7
  store double 0.000000e+00, ptr %116, align 8, !tbaa !17
  invoke void @_ZN9Vector3_tC2ERKdS1_S1_(ptr noundef nonnull align 8 dereferenceable(24) %113, ptr noundef nonnull align 8 dereferenceable(8) %114, ptr noundef nonnull align 8 dereferenceable(8) %115, ptr noundef nonnull align 8 dereferenceable(8) %116)
          to label %966 unwind label %1147

966:                                              ; preds = %964
  invoke void @_ZN17RigidBodyDynamics4Math4XrotEdRK9Vector3_t(ptr dead_on_unwind writable sret(%"struct.RigidBodyDynamics::Math::SpatialTransform") align 8 %112, double noundef %965, ptr noundef nonnull align 8 dereferenceable(24) %113)
          to label %967 unwind label %1147

967:                                              ; preds = %966
  call void @llvm.lifetime.start.p0(i64 96, ptr %117) #7
  %968 = invoke noundef nonnull align 8 dereferenceable(8) ptr @_ZN5Eigen15DenseCoeffsBaseINS_6MatrixIdLi3ELi1ELi0ELi3ELi1EEELi1EEixEl(ptr noundef nonnull align 1 dereferenceable(1) %107, i64 noundef 1)
          to label %969 unwind label %1151

969:                                              ; preds = %967
  %970 = load double, ptr %968, align 8, !tbaa !17
  call void @llvm.lifetime.start.p0(i64 24, ptr %118) #7
  call void @llvm.lifetime.start.p0(i64 8, ptr %119) #7
  store double 0.000000e+00, ptr %119, align 8, !tbaa !17
  call void @llvm.lifetime.start.p0(i64 8, ptr %120) #7
  store double 1.000000e+00, ptr %120, align 8, !tbaa !17
  call void @llvm.lifetime.start.p0(i64 8, ptr %121) #7
  store double 0.000000e+00, ptr %121, align 8, !tbaa !17
  invoke void @_ZN9Vector3_tC2ERKdS1_S1_(ptr noundef nonnull align 8 dereferenceable(24) %118, ptr noundef nonnull align 8 dereferenceable(8) %119, ptr noundef nonnull align 8 dereferenceable(8) %120, ptr noundef nonnull align 8 dereferenceable(8) %121)
          to label %971 unwind label %1155

971:                                              ; preds = %969
  invoke void @_ZN17RigidBodyDynamics4Math4XrotEdRK9Vector3_t(ptr dead_on_unwind writable sret(%"struct.RigidBodyDynamics::Math::SpatialTransform") align 8 %117, double noundef %970, ptr noundef nonnull align 8 dereferenceable(24) %118)
          to label %972 unwind label %1155

972:                                              ; preds = %971
  invoke void @_ZNK17RigidBodyDynamics4Math16SpatialTransformmlERKS1_(ptr dead_on_unwind writable sret(%"struct.RigidBodyDynamics::Math::SpatialTransform") align 8 %111, ptr noundef nonnull align 8 dereferenceable(96) %112, ptr noundef nonnull align 8 dereferenceable(96) %117)
          to label %973 unwind label %1155

973:                                              ; preds = %972
  call void @llvm.lifetime.start.p0(i64 96, ptr %122) #7
  %974 = invoke noundef nonnull align 8 dereferenceable(8) ptr @_ZN5Eigen15DenseCoeffsBaseINS_6MatrixIdLi3ELi1ELi0ELi3ELi1EEELi1EEixEl(ptr noundef nonnull align 1 dereferenceable(1) %107, i64 noundef 2)
          to label %975 unwind label %1159

975:                                              ; preds = %973
  %976 = load double, ptr %974, align 8, !tbaa !17
  call void @llvm.lifetime.start.p0(i64 24, ptr %123) #7
  call void @llvm.lifetime.start.p0(i64 8, ptr %124) #7
  store double 0.000000e+00, ptr %124, align 8, !tbaa !17
  call void @llvm.lifetime.start.p0(i64 8, ptr %125) #7
  store double 0.000000e+00, ptr %125, align 8, !tbaa !17
  call void @llvm.lifetime.start.p0(i64 8, ptr %126) #7
  store double 1.000000e+00, ptr %126, align 8, !tbaa !17
  invoke void @_ZN9Vector3_tC2ERKdS1_S1_(ptr noundef nonnull align 8 dereferenceable(24) %123, ptr noundef nonnull align 8 dereferenceable(8) %124, ptr noundef nonnull align 8 dereferenceable(8) %125, ptr noundef nonnull align 8 dereferenceable(8) %126)
          to label %977 unwind label %1163

977:                                              ; preds = %975
  invoke void @_ZN17RigidBodyDynamics4Math4XrotEdRK9Vector3_t(ptr dead_on_unwind writable sret(%"struct.RigidBodyDynamics::Math::SpatialTransform") align 8 %122, double noundef %976, ptr noundef nonnull align 8 dereferenceable(24) %123)
          to label %978 unwind label %1163

978:                                              ; preds = %977
  invoke void @_ZNK17RigidBodyDynamics4Math16SpatialTransformmlERKS1_(ptr dead_on_unwind writable sret(%"struct.RigidBodyDynamics::Math::SpatialTransform") align 8 %110, ptr noundef nonnull align 8 dereferenceable(96) %111, ptr noundef nonnull align 8 dereferenceable(96) %122)
          to label %979 unwind label %1163

979:                                              ; preds = %978
  call void @llvm.lifetime.start.p0(i64 96, ptr %127) #7
  call void @llvm.lifetime.start.p0(i64 24, ptr %128) #7
  invoke void @_ZN9Vector3_tC2ERKS_(ptr noundef nonnull align 8 dereferenceable(24) %128, ptr noundef nonnull align 8 dereferenceable(24) %108)
          to label %980 unwind label %1167

980:                                              ; preds = %979
  invoke void @_ZN17RigidBodyDynamics4Math6XtransERK9Vector3_t(ptr dead_on_unwind writable sret(%"struct.RigidBodyDynamics::Math::SpatialTransform") align 8 %127, ptr noundef nonnull align 8 dereferenceable(24) %128)
          to label %981 unwind label %1167

981:                                              ; preds = %980
  invoke void @_ZNK17RigidBodyDynamics4Math16SpatialTransformmlERKS1_(ptr dead_on_unwind writable sret(%"struct.RigidBodyDynamics::Math::SpatialTransform") align 8 %109, ptr noundef nonnull align 8 dereferenceable(96) %110, ptr noundef nonnull align 8 dereferenceable(96) %127)
          to label %982 unwind label %1167

982:                                              ; preds = %981
  call void @llvm.lifetime.end.p0(i64 24, ptr %128) #7
  call void @llvm.lifetime.end.p0(i64 96, ptr %127) #7
  call void @llvm.lifetime.end.p0(i64 8, ptr %126) #7
  call void @llvm.lifetime.end.p0(i64 8, ptr %125) #7
  call void @llvm.lifetime.end.p0(i64 8, ptr %124) #7
  call void @llvm.lifetime.end.p0(i64 24, ptr %123) #7
  call void @llvm.lifetime.end.p0(i64 96, ptr %122) #7
  call void @llvm.lifetime.end.p0(i64 8, ptr %121) #7
  call void @llvm.lifetime.end.p0(i64 8, ptr %120) #7
  call void @llvm.lifetime.end.p0(i64 8, ptr %119) #7
  call void @llvm.lifetime.end.p0(i64 24, ptr %118) #7
  call void @llvm.lifetime.end.p0(i64 96, ptr %117) #7
  call void @llvm.lifetime.end.p0(i64 8, ptr %116) #7
  call void @llvm.lifetime.end.p0(i64 8, ptr %115) #7
  call void @llvm.lifetime.end.p0(i64 8, ptr %114) #7
  call void @llvm.lifetime.end.p0(i64 24, ptr %113) #7
  call void @llvm.lifetime.end.p0(i64 96, ptr %112) #7
  call void @llvm.lifetime.end.p0(i64 96, ptr %111) #7
  call void @llvm.lifetime.end.p0(i64 96, ptr %110) #7
  call void @llvm.lifetime.start.p0(i64 24, ptr %129) #7
  invoke void @_ZN9Vector3_tC2Ev(ptr noundef nonnull align 8 dereferenceable(24) %129)
          to label %983 unwind label %1177

983:                                              ; preds = %982
  call void @llvm.lifetime.start.p0(i64 24, ptr %130) #7
  invoke void @_ZN9Vector3_tC2Ev(ptr noundef nonnull align 8 dereferenceable(24) %130)
          to label %984 unwind label %1181

984:                                              ; preds = %983
  call void @llvm.lifetime.start.p0(i64 72, ptr %131) #7
  call void @llvm.lifetime.start.p0(i64 16, ptr %132) #7
  invoke void @_ZN5Eigen9DenseBaseINS_6MatrixIdLi3ELi3ELi0ELi3ELi3EEEE4ZeroEv(ptr dead_on_unwind writable sret(%"class.Eigen::CwiseNullaryOp") align 8 %132)
          to label %985 unwind label %1185

985:                                              ; preds = %984
  invoke void @_ZN9Matrix3_tC2IN5Eigen14CwiseNullaryOpINS1_8internal18scalar_constant_opIdEENS1_6MatrixIdLi3ELi3ELi0ELi3ELi3EEEEEEERKNS1_10MatrixBaseIT_EE(ptr noundef nonnull align 8 dereferenceable(72) %131, ptr noundef nonnull align 1 dereferenceable(1) %132)
          to label %986 unwind label %1185

986:                                              ; preds = %985
  call void @llvm.lifetime.end.p0(i64 16, ptr %132) #7
  call void @llvm.lifetime.start.p0(i64 8, ptr %133) #7
  store double 0.000000e+00, ptr %133, align 8, !tbaa !17
  %987 = invoke noundef ptr @_ZN13my_shared_ptrIN4urdf4LinkEEptEv(ptr noundef nonnull align 8 dereferenceable(16) %40)
          to label %988 unwind label %1189

988:                                              ; preds = %986
  %989 = getelementptr inbounds %"class.urdf::Link", ptr %987, i32 0, i32 1
  %990 = invoke noundef zeroext i1 @_ZNK13my_shared_ptrIN4urdf8InertialEEcvbEv(ptr noundef nonnull align 8 dereferenceable(16) %989)
          to label %991 unwind label %1189

991:                                              ; preds = %988
  br i1 %990, label %992, label %1198

992:                                              ; preds = %991
  %993 = invoke noundef ptr @_ZN13my_shared_ptrIN4urdf4LinkEEptEv(ptr noundef nonnull align 8 dereferenceable(16) %40)
          to label %994 unwind label %1189

994:                                              ; preds = %992
  %995 = getelementptr inbounds %"class.urdf::Link", ptr %993, i32 0, i32 1
  %996 = invoke noundef ptr @_ZN13my_shared_ptrIN4urdf8InertialEEptEv(ptr noundef nonnull align 8 dereferenceable(16) %995)
          to label %997 unwind label %1189

997:                                              ; preds = %994
  %998 = getelementptr inbounds %"class.urdf::Inertial", ptr %996, i32 0, i32 1
  %999 = load double, ptr %998, align 8, !tbaa !11
  store double %999, ptr %133, align 8, !tbaa !17
  %1000 = invoke noundef ptr @_ZN13my_shared_ptrIN4urdf4LinkEEptEv(ptr noundef nonnull align 8 dereferenceable(16) %40)
          to label %1001 unwind label %1189

1001:                                             ; preds = %997
  %1002 = getelementptr inbounds %"class.urdf::Link", ptr %1000, i32 0, i32 1
  %1003 = invoke noundef ptr @_ZN13my_shared_ptrIN4urdf8InertialEEptEv(ptr noundef nonnull align 8 dereferenceable(16) %1002)
          to label %1004 unwind label %1189

1004:                                             ; preds = %1001
  %1005 = getelementptr inbounds %"class.urdf::Inertial", ptr %1003, i32 0, i32 0
  %1006 = getelementptr inbounds %"class.urdf::Pose", ptr %1005, i32 0, i32 0
  %1007 = getelementptr inbounds %"class.urdf::Vector3", ptr %1006, i32 0, i32 0
  %1008 = invoke noundef ptr @_ZN13my_shared_ptrIN4urdf4LinkEEptEv(ptr noundef nonnull align 8 dereferenceable(16) %40)
          to label %1009 unwind label %1189

1009:                                             ; preds = %1004
  %1010 = getelementptr inbounds %"class.urdf::Link", ptr %1008, i32 0, i32 1
  %1011 = invoke noundef ptr @_ZN13my_shared_ptrIN4urdf8InertialEEptEv(ptr noundef nonnull align 8 dereferenceable(16) %1010)
          to label %1012 unwind label %1189

1012:                                             ; preds = %1009
  %1013 = getelementptr inbounds %"class.urdf::Inertial", ptr %1011, i32 0, i32 0
  %1014 = getelementptr inbounds %"class.urdf::Pose", ptr %1013, i32 0, i32 0
  %1015 = getelementptr inbounds %"class.urdf::Vector3", ptr %1014, i32 0, i32 1
  %1016 = invoke noundef ptr @_ZN13my_shared_ptrIN4urdf4LinkEEptEv(ptr noundef nonnull align 8 dereferenceable(16) %40)
          to label %1017 unwind label %1189

1017:                                             ; preds = %1012
  %1018 = getelementptr inbounds %"class.urdf::Link", ptr %1016, i32 0, i32 1
  %1019 = invoke noundef ptr @_ZN13my_shared_ptrIN4urdf8InertialEEptEv(ptr noundef nonnull align 8 dereferenceable(16) %1018)
          to label %1020 unwind label %1189

1020:                                             ; preds = %1017
  %1021 = getelementptr inbounds %"class.urdf::Inertial", ptr %1019, i32 0, i32 0
  %1022 = getelementptr inbounds %"class.urdf::Pose", ptr %1021, i32 0, i32 0
  %1023 = getelementptr inbounds %"class.urdf::Vector3", ptr %1022, i32 0, i32 2
  invoke void @_ZN9Vector3_t3setERKdS1_S1_(ptr noundef nonnull align 8 dereferenceable(24) %129, ptr noundef nonnull align 8 dereferenceable(8) %1007, ptr noundef nonnull align 8 dereferenceable(8) %1015, ptr noundef nonnull align 8 dereferenceable(8) %1023)
          to label %1024 unwind label %1189

1024:                                             ; preds = %1020
  %1025 = invoke noundef ptr @_ZN13my_shared_ptrIN4urdf4LinkEEptEv(ptr noundef nonnull align 8 dereferenceable(16) %40)
          to label %1026 unwind label %1189

1026:                                             ; preds = %1024
  %1027 = getelementptr inbounds %"class.urdf::Link", ptr %1025, i32 0, i32 1
  %1028 = invoke noundef ptr @_ZN13my_shared_ptrIN4urdf8InertialEEptEv(ptr noundef nonnull align 8 dereferenceable(16) %1027)
          to label %1029 unwind label %1189

1029:                                             ; preds = %1026
  %1030 = getelementptr inbounds %"class.urdf::Inertial", ptr %1028, i32 0, i32 0
  %1031 = getelementptr inbounds %"class.urdf::Pose", ptr %1030, i32 0, i32 1
  %1032 = invoke noundef nonnull align 8 dereferenceable(8) ptr @_ZN5Eigen15DenseCoeffsBaseINS_6MatrixIdLi3ELi1ELi0ELi3ELi1EEELi1EEixEl(ptr noundef nonnull align 1 dereferenceable(1) %130, i64 noundef 0)
          to label %1033 unwind label %1189

1033:                                             ; preds = %1029
  %1034 = invoke noundef nonnull align 8 dereferenceable(8) ptr @_ZN5Eigen15DenseCoeffsBaseINS_6MatrixIdLi3ELi1ELi0ELi3ELi1EEELi1EEixEl(ptr noundef nonnull align 1 dereferenceable(1) %130, i64 noundef 1)
          to label %1035 unwind label %1189

1035:                                             ; preds = %1033
  %1036 = invoke noundef nonnull align 8 dereferenceable(8) ptr @_ZN5Eigen15DenseCoeffsBaseINS_6MatrixIdLi3ELi1ELi0ELi3ELi1EEELi1EEixEl(ptr noundef nonnull align 1 dereferenceable(1) %130, i64 noundef 2)
          to label %1037 unwind label %1189

1037:                                             ; preds = %1035
  invoke void @_ZNK4urdf8Rotation6getRPYERdS1_S1_(ptr noundef nonnull align 8 dereferenceable(32) %1031, ptr noundef nonnull align 8 dereferenceable(8) %1032, ptr noundef nonnull align 8 dereferenceable(8) %1034, ptr noundef nonnull align 8 dereferenceable(8) %1036)
          to label %1038 unwind label %1189

1038:                                             ; preds = %1037
  %1039 = invoke noundef ptr @_ZN13my_shared_ptrIN4urdf4LinkEEptEv(ptr noundef nonnull align 8 dereferenceable(16) %40)
          to label %1040 unwind label %1189

1040:                                             ; preds = %1038
  %1041 = getelementptr inbounds %"class.urdf::Link", ptr %1039, i32 0, i32 1
  %1042 = invoke noundef ptr @_ZN13my_shared_ptrIN4urdf8InertialEEptEv(ptr noundef nonnull align 8 dereferenceable(16) %1041)
          to label %1043 unwind label %1189

1043:                                             ; preds = %1040
  %1044 = getelementptr inbounds %"class.urdf::Inertial", ptr %1042, i32 0, i32 2
  %1045 = load double, ptr %1044, align 8, !tbaa !18
  %1046 = invoke noundef nonnull align 8 dereferenceable(8) ptr @_ZN5Eigen15DenseCoeffsBaseINS_6MatrixIdLi3ELi3ELi0ELi3ELi3EEELi1EEclEll(ptr noundef nonnull align 1 dereferenceable(1) %131, i64 noundef 0, i64 noundef 0)
          to label %1047 unwind label %1189

1047:                                             ; preds = %1043
  store double %1045, ptr %1046, align 8, !tbaa !17
  %1048 = invoke noundef ptr @_ZN13my_shared_ptrIN4urdf4LinkEEptEv(ptr noundef nonnull align 8 dereferenceable(16) %40)
          to label %1049 unwind label %1189

1049:                                             ; preds = %1047
  %1050 = getelementptr inbounds %"class.urdf::Link", ptr %1048, i32 0, i32 1
  %1051 = invoke noundef ptr @_ZN13my_shared_ptrIN4urdf8InertialEEptEv(ptr noundef nonnull align 8 dereferenceable(16) %1050)
          to label %1052 unwind label %1189

1052:                                             ; preds = %1049
  %1053 = getelementptr inbounds %"class.urdf::Inertial", ptr %1051, i32 0, i32 3
  %1054 = load double, ptr %1053, align 8, !tbaa !19
  %1055 = invoke noundef nonnull align 8 dereferenceable(8) ptr @_ZN5Eigen15DenseCoeffsBaseINS_6MatrixIdLi3ELi3ELi0ELi3ELi3EEELi1EEclEll(ptr noundef nonnull align 1 dereferenceable(1) %131, i64 noundef 0, i64 noundef 1)
          to label %1056 unwind label %1189

1056:                                             ; preds = %1052
  store double %1054, ptr %1055, align 8, !tbaa !17
  %1057 = invoke noundef ptr @_ZN13my_shared_ptrIN4urdf4LinkEEptEv(ptr noundef nonnull align 8 dereferenceable(16) %40)
          to label %1058 unwind label %1189

1058:                                             ; preds = %1056
  %1059 = getelementptr inbounds %"class.urdf::Link", ptr %1057, i32 0, i32 1
  %1060 = invoke noundef ptr @_ZN13my_shared_ptrIN4urdf8InertialEEptEv(ptr noundef nonnull align 8 dereferenceable(16) %1059)
          to label %1061 unwind label %1189

1061:                                             ; preds = %1058
  %1062 = getelementptr inbounds %"class.urdf::Inertial", ptr %1060, i32 0, i32 4
  %1063 = load double, ptr %1062, align 8, !tbaa !20
  %1064 = invoke noundef nonnull align 8 dereferenceable(8) ptr @_ZN5Eigen15DenseCoeffsBaseINS_6MatrixIdLi3ELi3ELi0ELi3ELi3EEELi1EEclEll(ptr noundef nonnull align 1 dereferenceable(1) %131, i64 noundef 0, i64 noundef 2)
          to label %1065 unwind label %1189

1065:                                             ; preds = %1061
  store double %1063, ptr %1064, align 8, !tbaa !17
  %1066 = invoke noundef ptr @_ZN13my_shared_ptrIN4urdf4LinkEEptEv(ptr noundef nonnull align 8 dereferenceable(16) %40)
          to label %1067 unwind label %1189

1067:                                             ; preds = %1065
  %1068 = getelementptr inbounds %"class.urdf::Link", ptr %1066, i32 0, i32 1
  %1069 = invoke noundef ptr @_ZN13my_shared_ptrIN4urdf8InertialEEptEv(ptr noundef nonnull align 8 dereferenceable(16) %1068)
          to label %1070 unwind label %1189

1070:                                             ; preds = %1067
  %1071 = getelementptr inbounds %"class.urdf::Inertial", ptr %1069, i32 0, i32 3
  %1072 = load double, ptr %1071, align 8, !tbaa !19
  %1073 = invoke noundef nonnull align 8 dereferenceable(8) ptr @_ZN5Eigen15DenseCoeffsBaseINS_6MatrixIdLi3ELi3ELi0ELi3ELi3EEELi1EEclEll(ptr noundef nonnull align 1 dereferenceable(1) %131, i64 noundef 1, i64 noundef 0)
          to label %1074 unwind label %1189

1074:                                             ; preds = %1070
  store double %1072, ptr %1073, align 8, !tbaa !17
  %1075 = invoke noundef ptr @_ZN13my_shared_ptrIN4urdf4LinkEEptEv(ptr noundef nonnull align 8 dereferenceable(16) %40)
          to label %1076 unwind label %1189

1076:                                             ; preds = %1074
  %1077 = getelementptr inbounds %"class.urdf::Link", ptr %1075, i32 0, i32 1
  %1078 = invoke noundef ptr @_ZN13my_shared_ptrIN4urdf8InertialEEptEv(ptr noundef nonnull align 8 dereferenceable(16) %1077)
          to label %1079 unwind label %1189

1079:                                             ; preds = %1076
  %1080 = getelementptr inbounds %"class.urdf::Inertial", ptr %1078, i32 0, i32 5
  %1081 = load double, ptr %1080, align 8, !tbaa !21
  %1082 = invoke noundef nonnull align 8 dereferenceable(8) ptr @_ZN5Eigen15DenseCoeffsBaseINS_6MatrixIdLi3ELi3ELi0ELi3ELi3EEELi1EEclEll(ptr noundef nonnull align 1 dereferenceable(1) %131, i64 noundef 1, i64 noundef 1)
          to label %1083 unwind label %1189

1083:                                             ; preds = %1079
  store double %1081, ptr %1082, align 8, !tbaa !17
  %1084 = invoke noundef ptr @_ZN13my_shared_ptrIN4urdf4LinkEEptEv(ptr noundef nonnull align 8 dereferenceable(16) %40)
          to label %1085 unwind label %1189

1085:                                             ; preds = %1083
  %1086 = getelementptr inbounds %"class.urdf::Link", ptr %1084, i32 0, i32 1
  %1087 = invoke noundef ptr @_ZN13my_shared_ptrIN4urdf8InertialEEptEv(ptr noundef nonnull align 8 dereferenceable(16) %1086)
          to label %1088 unwind label %1189

1088:                                             ; preds = %1085
  %1089 = getelementptr inbounds %"class.urdf::Inertial", ptr %1087, i32 0, i32 6
  %1090 = load double, ptr %1089, align 8, !tbaa !22
  %1091 = invoke noundef nonnull align 8 dereferenceable(8) ptr @_ZN5Eigen15DenseCoeffsBaseINS_6MatrixIdLi3ELi3ELi0ELi3ELi3EEELi1EEclEll(ptr noundef nonnull align 1 dereferenceable(1) %131, i64 noundef 1, i64 noundef 2)
          to label %1092 unwind label %1189

1092:                                             ; preds = %1088
  store double %1090, ptr %1091, align 8, !tbaa !17
  %1093 = invoke noundef ptr @_ZN13my_shared_ptrIN4urdf4LinkEEptEv(ptr noundef nonnull align 8 dereferenceable(16) %40)
          to label %1094 unwind label %1189

1094:                                             ; preds = %1092
  %1095 = getelementptr inbounds %"class.urdf::Link", ptr %1093, i32 0, i32 1
  %1096 = invoke noundef ptr @_ZN13my_shared_ptrIN4urdf8InertialEEptEv(ptr noundef nonnull align 8 dereferenceable(16) %1095)
          to label %1097 unwind label %1189

1097:                                             ; preds = %1094
  %1098 = getelementptr inbounds %"class.urdf::Inertial", ptr %1096, i32 0, i32 4
  %1099 = load double, ptr %1098, align 8, !tbaa !20
  %1100 = invoke noundef nonnull align 8 dereferenceable(8) ptr @_ZN5Eigen15DenseCoeffsBaseINS_6MatrixIdLi3ELi3ELi0ELi3ELi3EEELi1EEclEll(ptr noundef nonnull align 1 dereferenceable(1) %131, i64 noundef 2, i64 noundef 0)
          to label %1101 unwind label %1189

1101:                                             ; preds = %1097
  store double %1099, ptr %1100, align 8, !tbaa !17
  %1102 = invoke noundef ptr @_ZN13my_shared_ptrIN4urdf4LinkEEptEv(ptr noundef nonnull align 8 dereferenceable(16) %40)
          to label %1103 unwind label %1189

1103:                                             ; preds = %1101
  %1104 = getelementptr inbounds %"class.urdf::Link", ptr %1102, i32 0, i32 1
  %1105 = invoke noundef ptr @_ZN13my_shared_ptrIN4urdf8InertialEEptEv(ptr noundef nonnull align 8 dereferenceable(16) %1104)
          to label %1106 unwind label %1189

1106:                                             ; preds = %1103
  %1107 = getelementptr inbounds %"class.urdf::Inertial", ptr %1105, i32 0, i32 6
  %1108 = load double, ptr %1107, align 8, !tbaa !22
  %1109 = invoke noundef nonnull align 8 dereferenceable(8) ptr @_ZN5Eigen15DenseCoeffsBaseINS_6MatrixIdLi3ELi3ELi0ELi3ELi3EEELi1EEclEll(ptr noundef nonnull align 1 dereferenceable(1) %131, i64 noundef 2, i64 noundef 1)
          to label %1110 unwind label %1189

1110:                                             ; preds = %1106
  store double %1108, ptr %1109, align 8, !tbaa !17
  %1111 = invoke noundef ptr @_ZN13my_shared_ptrIN4urdf4LinkEEptEv(ptr noundef nonnull align 8 dereferenceable(16) %40)
          to label %1112 unwind label %1189

1112:                                             ; preds = %1110
  %1113 = getelementptr inbounds %"class.urdf::Link", ptr %1111, i32 0, i32 1
  %1114 = invoke noundef ptr @_ZN13my_shared_ptrIN4urdf8InertialEEptEv(ptr noundef nonnull align 8 dereferenceable(16) %1113)
          to label %1115 unwind label %1189

1115:                                             ; preds = %1112
  %1116 = getelementptr inbounds %"class.urdf::Inertial", ptr %1114, i32 0, i32 7
  %1117 = load double, ptr %1116, align 8, !tbaa !23
  %1118 = invoke noundef nonnull align 8 dereferenceable(8) ptr @_ZN5Eigen15DenseCoeffsBaseINS_6MatrixIdLi3ELi3ELi0ELi3ELi3EEELi1EEclEll(ptr noundef nonnull align 1 dereferenceable(1) %131, i64 noundef 2, i64 noundef 2)
          to label %1119 unwind label %1189

1119:                                             ; preds = %1115
  store double %1117, ptr %1118, align 8, !tbaa !17
  call void @llvm.lifetime.start.p0(i64 24, ptr %134) #7
  call void @llvm.lifetime.start.p0(i64 8, ptr %135) #7
  store double 0.000000e+00, ptr %135, align 8, !tbaa !17
  call void @llvm.lifetime.start.p0(i64 8, ptr %136) #7
  store double 0.000000e+00, ptr %136, align 8, !tbaa !17
  call void @llvm.lifetime.start.p0(i64 8, ptr %137) #7
  store double 0.000000e+00, ptr %137, align 8, !tbaa !17
  invoke void @_ZN9Vector3_tC2ERKdS1_S1_(ptr noundef nonnull align 8 dereferenceable(24) %134, ptr noundef nonnull align 8 dereferenceable(8) %135, ptr noundef nonnull align 8 dereferenceable(8) %136, ptr noundef nonnull align 8 dereferenceable(8) %137)
          to label %1120 unwind label %1193

1120:                                             ; preds = %1119
  %1121 = invoke noundef zeroext i1 @_ZNK5Eigen10MatrixBaseINS_6MatrixIdLi3ELi1ELi0ELi3ELi1EEEEneIS2_EEbRKNS0_IT_EE(ptr noundef nonnull align 1 dereferenceable(1) %130, ptr noundef nonnull align 1 dereferenceable(1) %134)
          to label %1122 unwind label %1193

1122:                                             ; preds = %1120
  call void @llvm.lifetime.end.p0(i64 8, ptr %137) #7
  call void @llvm.lifetime.end.p0(i64 8, ptr %136) #7
  call void @llvm.lifetime.end.p0(i64 8, ptr %135) #7
  call void @llvm.lifetime.end.p0(i64 24, ptr %134) #7
  br i1 %1121, label %1123, label %1197

1123:                                             ; preds = %1122
  %1124 = invoke noundef nonnull align 8 dereferenceable(8) ptr @_ZStlsISt11char_traitsIcEERSt13basic_ostreamIcT_ES5_PKc(ptr noundef nonnull align 8 dereferenceable(8) @_ZSt4cerr, ptr noundef @.str.18)
          to label %1125 unwind label %1189

1125:                                             ; preds = %1123
  %1126 = invoke noundef ptr @_ZN13my_shared_ptrIN4urdf4LinkEEptEv(ptr noundef nonnull align 8 dereferenceable(16) %40)
          to label %1127 unwind label %1189

1127:                                             ; preds = %1125
  %1128 = getelementptr inbounds %"class.urdf::Link", ptr %1126, i32 0, i32 0
  %1129 = invoke noundef nonnull align 8 dereferenceable(8) ptr @_ZStlsIcSt11char_traitsIcESaIcEERSt13basic_ostreamIT_T0_ES7_RKNSt7__cxx1112basic_stringIS4_S5_T1_EE(ptr noundef nonnull align 8 dereferenceable(8) %1124, ptr noundef nonnull align 8 dereferenceable(32) %1128)
          to label %1130 unwind label %1189

1130:                                             ; preds = %1127
  %1131 = invoke noundef nonnull align 8 dereferenceable(8) ptr @_ZStlsISt11char_traitsIcEERSt13basic_ostreamIcT_ES5_PKc(ptr noundef nonnull align 8 dereferenceable(8) %1129, ptr noundef @.str.19)
          to label %1132 unwind label %1189

1132:                                             ; preds = %1130
  %1133 = invoke noundef nonnull align 8 dereferenceable(8) ptr @_ZNSolsEPFRSoS_E(ptr noundef nonnull align 8 dereferenceable(8) %1131, ptr noundef @_ZSt4endlIcSt11char_traitsIcEERSt13basic_ostreamIT_T0_ES6_)
          to label %1134 unwind label %1189

1134:                                             ; preds = %1132
  store i1 false, ptr %5, align 1
  store i32 1, ptr %106, align 4
  br label %1394

1135:                                             ; preds = %933
  %1136 = landingpad { ptr, i32 }
          cleanup
  %1137 = extractvalue { ptr, i32 } %1136, 0
  store ptr %1137, ptr %12, align 8
  %1138 = extractvalue { ptr, i32 } %1136, 1
  store i32 %1138, ptr %13, align 4
  br label %1409

1139:                                             ; preds = %958, %953, %948, %946, %945, %943, %941, %937, %935, %934
  %1140 = landingpad { ptr, i32 }
          cleanup
  %1141 = extractvalue { ptr, i32 } %1140, 0
  store ptr %1141, ptr %12, align 8
  %1142 = extractvalue { ptr, i32 } %1140, 1
  store i32 %1142, ptr %13, align 4
  br label %1408

1143:                                             ; preds = %962
  %1144 = landingpad { ptr, i32 }
          cleanup
  %1145 = extractvalue { ptr, i32 } %1144, 0
  store ptr %1145, ptr %12, align 8
  %1146 = extractvalue { ptr, i32 } %1144, 1
  store i32 %1146, ptr %13, align 4
  br label %1176

1147:                                             ; preds = %966, %964
  %1148 = landingpad { ptr, i32 }
          cleanup
  %1149 = extractvalue { ptr, i32 } %1148, 0
  store ptr %1149, ptr %12, align 8
  %1150 = extractvalue { ptr, i32 } %1148, 1
  store i32 %1150, ptr %13, align 4
  br label %1175

1151:                                             ; preds = %967
  %1152 = landingpad { ptr, i32 }
          cleanup
  %1153 = extractvalue { ptr, i32 } %1152, 0
  store ptr %1153, ptr %12, align 8
  %1154 = extractvalue { ptr, i32 } %1152, 1
  store i32 %1154, ptr %13, align 4
  br label %1174

1155:                                             ; preds = %972, %971, %969
  %1156 = landingpad { ptr, i32 }
          cleanup
  %1157 = extractvalue { ptr, i32 } %1156, 0
  store ptr %1157, ptr %12, align 8
  %1158 = extractvalue { ptr, i32 } %1156, 1
  store i32 %1158, ptr %13, align 4
  br label %1173

1159:                                             ; preds = %973
  %1160 = landingpad { ptr, i32 }
          cleanup
  %1161 = extractvalue { ptr, i32 } %1160, 0
  store ptr %1161, ptr %12, align 8
  %1162 = extractvalue { ptr, i32 } %1160, 1
  store i32 %1162, ptr %13, align 4
  br label %1172

1163:                                             ; preds = %978, %977, %975
  %1164 = landingpad { ptr, i32 }
          cleanup
  %1165 = extractvalue { ptr, i32 } %1164, 0
  store ptr %1165, ptr %12, align 8
  %1166 = extractvalue { ptr, i32 } %1164, 1
  store i32 %1166, ptr %13, align 4
  br label %1171

1167:                                             ; preds = %981, %980, %979
  %1168 = landingpad { ptr, i32 }
          cleanup
  %1169 = extractvalue { ptr, i32 } %1168, 0
  store ptr %1169, ptr %12, align 8
  %1170 = extractvalue { ptr, i32 } %1168, 1
  store i32 %1170, ptr %13, align 4
  call void @llvm.lifetime.end.p0(i64 24, ptr %128) #7
  call void @llvm.lifetime.end.p0(i64 96, ptr %127) #7
  br label %1171

1171:                                             ; preds = %1167, %1163
  call void @llvm.lifetime.end.p0(i64 8, ptr %126) #7
  call void @llvm.lifetime.end.p0(i64 8, ptr %125) #7
  call void @llvm.lifetime.end.p0(i64 8, ptr %124) #7
  call void @llvm.lifetime.end.p0(i64 24, ptr %123) #7
  br label %1172

1172:                                             ; preds = %1171, %1159
  call void @llvm.lifetime.end.p0(i64 96, ptr %122) #7
  br label %1173

1173:                                             ; preds = %1172, %1155
  call void @llvm.lifetime.end.p0(i64 8, ptr %121) #7
  call void @llvm.lifetime.end.p0(i64 8, ptr %120) #7
  call void @llvm.lifetime.end.p0(i64 8, ptr %119) #7
  call void @llvm.lifetime.end.p0(i64 24, ptr %118) #7
  br label %1174

1174:                                             ; preds = %1173, %1151
  call void @llvm.lifetime.end.p0(i64 96, ptr %117) #7
  br label %1175

1175:                                             ; preds = %1174, %1147
  call void @llvm.lifetime.end.p0(i64 8, ptr %116) #7
  call void @llvm.lifetime.end.p0(i64 8, ptr %115) #7
  call void @llvm.lifetime.end.p0(i64 8, ptr %114) #7
  call void @llvm.lifetime.end.p0(i64 24, ptr %113) #7
  br label %1176

1176:                                             ; preds = %1175, %1143
  call void @llvm.lifetime.end.p0(i64 96, ptr %112) #7
  call void @llvm.lifetime.end.p0(i64 96, ptr %111) #7
  call void @llvm.lifetime.end.p0(i64 96, ptr %110) #7
  br label %1407

1177:                                             ; preds = %982
  %1178 = landingpad { ptr, i32 }
          cleanup
  %1179 = extractvalue { ptr, i32 } %1178, 0
  store ptr %1179, ptr %12, align 8
  %1180 = extractvalue { ptr, i32 } %1178, 1
  store i32 %1180, ptr %13, align 4
  br label %1406

1181:                                             ; preds = %983
  %1182 = landingpad { ptr, i32 }
          cleanup
  %1183 = extractvalue { ptr, i32 } %1182, 0
  store ptr %1183, ptr %12, align 8
  %1184 = extractvalue { ptr, i32 } %1182, 1
  store i32 %1184, ptr %13, align 4
  br label %1405

1185:                                             ; preds = %985, %984
  %1186 = landingpad { ptr, i32 }
          cleanup
  %1187 = extractvalue { ptr, i32 } %1186, 0
  store ptr %1187, ptr %12, align 8
  %1188 = extractvalue { ptr, i32 } %1186, 1
  store i32 %1188, ptr %13, align 4
  call void @llvm.lifetime.end.p0(i64 16, ptr %132) #7
  br label %1404

1189:                                             ; preds = %1132, %1130, %1127, %1125, %1123, %1115, %1112, %1110, %1106, %1103, %1101, %1097, %1094, %1092, %1088, %1085, %1083, %1079, %1076, %1074, %1070, %1067, %1065, %1061, %1058, %1056, %1052, %1049, %1047, %1043, %1040, %1038, %1037, %1035, %1033, %1029, %1026, %1024, %1020, %1017, %1012, %1009, %1004, %1001, %997, %994, %992, %988, %986
  %1190 = landingpad { ptr, i32 }
          cleanup
  %1191 = extractvalue { ptr, i32 } %1190, 0
  store ptr %1191, ptr %12, align 8
  %1192 = extractvalue { ptr, i32 } %1190, 1
  store i32 %1192, ptr %13, align 4
  br label %1403

1193:                                             ; preds = %1120, %1119
  %1194 = landingpad { ptr, i32 }
          cleanup
  %1195 = extractvalue { ptr, i32 } %1194, 0
  store ptr %1195, ptr %12, align 8
  %1196 = extractvalue { ptr, i32 } %1194, 1
  store i32 %1196, ptr %13, align 4
  call void @llvm.lifetime.end.p0(i64 8, ptr %137) #7
  call void @llvm.lifetime.end.p0(i64 8, ptr %136) #7
  call void @llvm.lifetime.end.p0(i64 8, ptr %135) #7
  call void @llvm.lifetime.end.p0(i64 24, ptr %134) #7
  br label %1403

1197:                                             ; preds = %1122
  br label %1198

1198:                                             ; preds = %1197, %991
  call void @llvm.lifetime.start.p0(i64 112, ptr %138) #7
  invoke void @_ZN17RigidBodyDynamics4BodyC2ERKdRK9Vector3_tRK9Matrix3_t(ptr noundef nonnull align 8 dereferenceable(105) %138, ptr noundef nonnull align 8 dereferenceable(8) %133, ptr noundef nonnull align 8 dereferenceable(24) %129, ptr noundef nonnull align 8 dereferenceable(72) %131)
          to label %1199 unwind label %1239

1199:                                             ; preds = %1198
  %1200 = load i8, ptr %9, align 1, !tbaa !9, !range !24, !noundef !25
  %1201 = trunc i8 %1200 to i1
  br i1 %1201, label %1202, label %1306

1202:                                             ; preds = %1199
  %1203 = invoke noundef nonnull align 8 dereferenceable(8) ptr @_ZStlsISt11char_traitsIcEERSt13basic_ostreamIcT_ES5_PKc(ptr noundef nonnull align 8 dereferenceable(8) @_ZSt4cout, ptr noundef @.str.20)
          to label %1204 unwind label %1243

1204:                                             ; preds = %1202
  %1205 = invoke noundef ptr @_ZN13my_shared_ptrIN4urdf4LinkEEptEv(ptr noundef nonnull align 8 dereferenceable(16) %40)
          to label %1206 unwind label %1243

1206:                                             ; preds = %1204
  %1207 = getelementptr inbounds %"class.urdf::Link", ptr %1205, i32 0, i32 0
  %1208 = invoke noundef nonnull align 8 dereferenceable(8) ptr @_ZStlsIcSt11char_traitsIcESaIcEERSt13basic_ostreamIT_T0_ES7_RKNSt7__cxx1112basic_stringIS4_S5_T1_EE(ptr noundef nonnull align 8 dereferenceable(8) %1203, ptr noundef nonnull align 8 dereferenceable(32) %1207)
          to label %1209 unwind label %1243

1209:                                             ; preds = %1206
  %1210 = invoke noundef nonnull align 8 dereferenceable(8) ptr @_ZNSolsEPFRSoS_E(ptr noundef nonnull align 8 dereferenceable(8) %1208, ptr noundef @_ZSt4endlIcSt11char_traitsIcEERSt13basic_ostreamIT_T0_ES6_)
          to label %1211 unwind label %1243

1211:                                             ; preds = %1209
  %1212 = invoke noundef nonnull align 8 dereferenceable(8) ptr @_ZStlsISt11char_traitsIcEERSt13basic_ostreamIcT_ES5_PKc(ptr noundef nonnull align 8 dereferenceable(8) @_ZSt4cout, ptr noundef @.str.21)
          to label %1213 unwind label %1243

1213:                                             ; preds = %1211
  %1214 = load i32, ptr %41, align 4, !tbaa !38
  %1215 = invoke noundef nonnull align 8 dereferenceable(8) ptr @_ZNSolsEj(ptr noundef nonnull align 8 dereferenceable(8) %1212, i32 noundef %1214)
          to label %1216 unwind label %1243

1216:                                             ; preds = %1213
  %1217 = invoke noundef nonnull align 8 dereferenceable(8) ptr @_ZNSolsEPFRSoS_E(ptr noundef nonnull align 8 dereferenceable(8) %1215, ptr noundef @_ZSt4endlIcSt11char_traitsIcEERSt13basic_ostreamIT_T0_ES6_)
          to label %1218 unwind label %1243

1218:                                             ; preds = %1216
  %1219 = invoke noundef nonnull align 8 dereferenceable(8) ptr @_ZStlsISt11char_traitsIcEERSt13basic_ostreamIcT_ES5_PKc(ptr noundef nonnull align 8 dereferenceable(8) @_ZSt4cout, ptr noundef @.str.4)
          to label %1220 unwind label %1243

1220:                                             ; preds = %1218
  %1221 = invoke noundef nonnull align 8 dereferenceable(8) ptr @_ZN17RigidBodyDynamics4MathlsERSoRKNS0_16SpatialTransformE(ptr noundef nonnull align 8 dereferenceable(8) %1219, ptr noundef nonnull align 8 dereferenceable(96) %109)
          to label %1222 unwind label %1243

1222:                                             ; preds = %1220
  %1223 = invoke noundef nonnull align 8 dereferenceable(8) ptr @_ZNSolsEPFRSoS_E(ptr noundef nonnull align 8 dereferenceable(8) %1221, ptr noundef @_ZSt4endlIcSt11char_traitsIcEERSt13basic_ostreamIT_T0_ES6_)
          to label %1224 unwind label %1243

1224:                                             ; preds = %1222
  %1225 = invoke noundef nonnull align 8 dereferenceable(8) ptr @_ZStlsISt11char_traitsIcEERSt13basic_ostreamIcT_ES5_PKc(ptr noundef nonnull align 8 dereferenceable(8) @_ZSt4cout, ptr noundef @.str.22)
          to label %1226 unwind label %1243

1226:                                             ; preds = %1224
  %1227 = getelementptr inbounds %"struct.RigidBodyDynamics::Joint", ptr %42, i32 0, i32 2
  %1228 = load i32, ptr %1227, align 4, !tbaa !54
  %1229 = invoke noundef nonnull align 8 dereferenceable(8) ptr @_ZNSolsEj(ptr noundef nonnull align 8 dereferenceable(8) %1225, i32 noundef %1228)
          to label %1230 unwind label %1243

1230:                                             ; preds = %1226
  %1231 = invoke noundef nonnull align 8 dereferenceable(8) ptr @_ZNSolsEPFRSoS_E(ptr noundef nonnull align 8 dereferenceable(8) %1229, ptr noundef @_ZSt4endlIcSt11char_traitsIcEERSt13basic_ostreamIT_T0_ES6_)
          to label %1232 unwind label %1243

1232:                                             ; preds = %1230
  call void @llvm.lifetime.start.p0(i64 4, ptr %139) #7
  store i32 0, ptr %139, align 4, !tbaa !38
  br label %1233

1233:                                             ; preds = %1267, %1232
  %1234 = load i32, ptr %139, align 4, !tbaa !38
  %1235 = getelementptr inbounds %"struct.RigidBodyDynamics::Joint", ptr %42, i32 0, i32 2
  %1236 = load i32, ptr %1235, align 4, !tbaa !54
  %1237 = icmp ult i32 %1234, %1236
  br i1 %1237, label %1247, label %1238

1238:                                             ; preds = %1233
  store i32 10, ptr %106, align 4
  call void @llvm.lifetime.end.p0(i64 4, ptr %139) #7
  br label %1279

1239:                                             ; preds = %1198
  %1240 = landingpad { ptr, i32 }
          cleanup
  %1241 = extractvalue { ptr, i32 } %1240, 0
  store ptr %1241, ptr %12, align 8
  %1242 = extractvalue { ptr, i32 } %1240, 1
  store i32 %1242, ptr %13, align 4
  br label %1402

1243:                                             ; preds = %1384, %1380, %1306, %1303, %1300, %1298, %1296, %1294, %1290, %1288, %1286, %1283, %1281, %1279, %1230, %1226, %1224, %1222, %1220, %1218, %1216, %1213, %1211, %1209, %1206, %1204, %1202
  %1244 = landingpad { ptr, i32 }
          cleanup
  %1245 = extractvalue { ptr, i32 } %1244, 0
  store ptr %1245, ptr %12, align 8
  %1246 = extractvalue { ptr, i32 } %1244, 1
  store i32 %1246, ptr %13, align 4
  br label %1401

1247:                                             ; preds = %1233
  %1248 = invoke noundef nonnull align 8 dereferenceable(8) ptr @_ZStlsISt11char_traitsIcEERSt13basic_ostreamIcT_ES5_PKc(ptr noundef nonnull align 8 dereferenceable(8) @_ZSt4cout, ptr noundef @.str.23)
          to label %1249 unwind label %1270

1249:                                             ; preds = %1247
  %1250 = load i32, ptr %139, align 4, !tbaa !38
  %1251 = invoke noundef nonnull align 8 dereferenceable(8) ptr @_ZNSolsEj(ptr noundef nonnull align 8 dereferenceable(8) %1248, i32 noundef %1250)
          to label %1252 unwind label %1270

1252:                                             ; preds = %1249
  %1253 = invoke noundef nonnull align 8 dereferenceable(8) ptr @_ZStlsISt11char_traitsIcEERSt13basic_ostreamIcT_ES5_PKc(ptr noundef nonnull align 8 dereferenceable(8) %1251, ptr noundef @.str.24)
          to label %1254 unwind label %1270

1254:                                             ; preds = %1252
  call void @llvm.lifetime.start.p0(i64 8, ptr %140) #7
  %1255 = getelementptr inbounds %"struct.RigidBodyDynamics::Joint", ptr %42, i32 0, i32 0
  %1256 = load ptr, ptr %1255, align 8, !tbaa !57
  %1257 = load i32, ptr %139, align 4, !tbaa !38
  %1258 = zext i32 %1257 to i64
  %1259 = getelementptr inbounds %class.SpatialVector_t, ptr %1256, i64 %1258
  %1260 = invoke ptr @_ZN5Eigen9DenseBaseINS_6MatrixIdLi6ELi1ELi0ELi6ELi1EEEE9transposeEv(ptr noundef nonnull align 1 dereferenceable(1) %1259)
          to label %1261 unwind label %1274

1261:                                             ; preds = %1254
  %1262 = getelementptr inbounds %"class.Eigen::Transpose", ptr %140, i32 0, i32 0
  store ptr %1260, ptr %1262, align 8
  %1263 = invoke noundef nonnull align 8 dereferenceable(8) ptr @_ZN5EigenlsINS_9TransposeINS_6MatrixIdLi6ELi1ELi0ELi6ELi1EEEEEEERSoS5_RKNS_9DenseBaseIT_EE(ptr noundef nonnull align 8 dereferenceable(8) %1253, ptr noundef nonnull align 1 dereferenceable(1) %140)
          to label %1264 unwind label %1274

1264:                                             ; preds = %1261
  %1265 = invoke noundef nonnull align 8 dereferenceable(8) ptr @_ZNSolsEPFRSoS_E(ptr noundef nonnull align 8 dereferenceable(8) %1263, ptr noundef @_ZSt4endlIcSt11char_traitsIcEERSt13basic_ostreamIT_T0_ES6_)
          to label %1266 unwind label %1274

1266:                                             ; preds = %1264
  call void @llvm.lifetime.end.p0(i64 8, ptr %140) #7
  br label %1267

1267:                                             ; preds = %1266
  %1268 = load i32, ptr %139, align 4, !tbaa !38
  %1269 = add i32 %1268, 1
  store i32 %1269, ptr %139, align 4, !tbaa !38
  br label %1233, !llvm.loop !58

1270:                                             ; preds = %1252, %1249, %1247
  %1271 = landingpad { ptr, i32 }
          cleanup
  %1272 = extractvalue { ptr, i32 } %1271, 0
  store ptr %1272, ptr %12, align 8
  %1273 = extractvalue { ptr, i32 } %1271, 1
  store i32 %1273, ptr %13, align 4
  br label %1278

1274:                                             ; preds = %1264, %1261, %1254
  %1275 = landingpad { ptr, i32 }
          cleanup
  %1276 = extractvalue { ptr, i32 } %1275, 0
  store ptr %1276, ptr %12, align 8
  %1277 = extractvalue { ptr, i32 } %1275, 1
  store i32 %1277, ptr %13, align 4
  call void @llvm.lifetime.end.p0(i64 8, ptr %140) #7
  br label %1278

1278:                                             ; preds = %1274, %1270
  call void @llvm.lifetime.end.p0(i64 4, ptr %139) #7
  br label %1401

1279:                                             ; preds = %1238
  %1280 = invoke noundef nonnull align 8 dereferenceable(8) ptr @_ZStlsISt11char_traitsIcEERSt13basic_ostreamIcT_ES5_PKc(ptr noundef nonnull align 8 dereferenceable(8) @_ZSt4cout, ptr noundef @.str.7)
          to label %1281 unwind label %1243

1281:                                             ; preds = %1279
  %1282 = invoke noundef nonnull align 8 dereferenceable(8) ptr @_ZNSolsEPFRSoS_E(ptr noundef nonnull align 8 dereferenceable(8) %1280, ptr noundef @_ZSt4endlIcSt11char_traitsIcEERSt13basic_ostreamIT_T0_ES6_)
          to label %1283 unwind label %1243

1283:                                             ; preds = %1281
  %1284 = getelementptr inbounds %"struct.RigidBodyDynamics::Body", ptr %138, i32 0, i32 2
  %1285 = invoke noundef nonnull align 8 dereferenceable(8) ptr @_ZN5EigenlsINS_6MatrixIdLi3ELi3ELi0ELi3ELi3EEEEERSoS3_RKNS_9DenseBaseIT_EE(ptr noundef nonnull align 8 dereferenceable(8) %1282, ptr noundef nonnull align 1 dereferenceable(1) %1284)
          to label %1286 unwind label %1243

1286:                                             ; preds = %1283
  %1287 = invoke noundef nonnull align 8 dereferenceable(8) ptr @_ZNSolsEPFRSoS_E(ptr noundef nonnull align 8 dereferenceable(8) %1285, ptr noundef @_ZSt4endlIcSt11char_traitsIcEERSt13basic_ostreamIT_T0_ES6_)
          to label %1288 unwind label %1243

1288:                                             ; preds = %1286
  %1289 = invoke noundef nonnull align 8 dereferenceable(8) ptr @_ZStlsISt11char_traitsIcEERSt13basic_ostreamIcT_ES5_PKc(ptr noundef nonnull align 8 dereferenceable(8) @_ZSt4cout, ptr noundef @.str.8)
          to label %1290 unwind label %1243

1290:                                             ; preds = %1288
  %1291 = getelementptr inbounds %"struct.RigidBodyDynamics::Body", ptr %138, i32 0, i32 0
  %1292 = load double, ptr %1291, align 8, !tbaa !26
  %1293 = invoke noundef nonnull align 8 dereferenceable(8) ptr @_ZNSolsEd(ptr noundef nonnull align 8 dereferenceable(8) %1289, double noundef %1292)
          to label %1294 unwind label %1243

1294:                                             ; preds = %1290
  %1295 = invoke noundef nonnull align 8 dereferenceable(8) ptr @_ZNSolsEPFRSoS_E(ptr noundef nonnull align 8 dereferenceable(8) %1293, ptr noundef @_ZSt4endlIcSt11char_traitsIcEERSt13basic_ostreamIT_T0_ES6_)
          to label %1296 unwind label %1243

1296:                                             ; preds = %1294
  %1297 = invoke noundef nonnull align 8 dereferenceable(8) ptr @_ZStlsISt11char_traitsIcEERSt13basic_ostreamIcT_ES5_PKc(ptr noundef nonnull align 8 dereferenceable(8) @_ZSt4cout, ptr noundef @.str.9)
          to label %1298 unwind label %1243

1298:                                             ; preds = %1296
  %1299 = invoke noundef ptr @_ZN13my_shared_ptrIN4urdf4LinkEEptEv(ptr noundef nonnull align 8 dereferenceable(16) %40)
          to label %1300 unwind label %1243

1300:                                             ; preds = %1298
  %1301 = getelementptr inbounds %"class.urdf::Link", ptr %1299, i32 0, i32 0
  %1302 = invoke noundef nonnull align 8 dereferenceable(8) ptr @_ZStlsIcSt11char_traitsIcESaIcEERSt13basic_ostreamIT_T0_ES7_RKNSt7__cxx1112basic_stringIS4_S5_T1_EE(ptr noundef nonnull align 8 dereferenceable(8) %1297, ptr noundef nonnull align 8 dereferenceable(32) %1301)
          to label %1303 unwind label %1243

1303:                                             ; preds = %1300
  %1304 = invoke noundef nonnull align 8 dereferenceable(8) ptr @_ZNSolsEPFRSoS_E(ptr noundef nonnull align 8 dereferenceable(8) %1302, ptr noundef @_ZSt4endlIcSt11char_traitsIcEERSt13basic_ostreamIT_T0_ES6_)
          to label %1305 unwind label %1243

1305:                                             ; preds = %1303
  br label %1306

1306:                                             ; preds = %1305, %1199
  %1307 = invoke noundef ptr @_ZN13my_shared_ptrIN4urdf5JointEEptEv(ptr noundef nonnull align 8 dereferenceable(16) %38)
          to label %1308 unwind label %1243

1308:                                             ; preds = %1306
  %1309 = getelementptr inbounds %"class.urdf::Joint", ptr %1307, i32 0, i32 1
  %1310 = load i32, ptr %1309, align 8, !tbaa !42
  %1311 = icmp eq i32 %1310, 4
  br i1 %1311, label %1312, label %1380

1312:                                             ; preds = %1308
  call void @llvm.lifetime.start.p0(i64 72, ptr %141) #7
  call void @llvm.lifetime.start.p0(i64 16, ptr %142) #7
  invoke void @_ZN5Eigen9DenseBaseINS_6MatrixIdLi3ELi3ELi0ELi3ELi3EEEE4ZeroEv(ptr dead_on_unwind writable sret(%"class.Eigen::CwiseNullaryOp") align 8 %142)
          to label %1313 unwind label %1337

1313:                                             ; preds = %1312
  invoke void @_ZN9Matrix3_tC2IN5Eigen14CwiseNullaryOpINS1_8internal18scalar_constant_opIdEENS1_6MatrixIdLi3ELi3ELi0ELi3ELi3EEEEEEERKNS1_10MatrixBaseIT_EE(ptr noundef nonnull align 8 dereferenceable(72) %141, ptr noundef nonnull align 1 dereferenceable(1) %142)
          to label %1314 unwind label %1337

1314:                                             ; preds = %1313
  call void @llvm.lifetime.end.p0(i64 16, ptr %142) #7
  call void @llvm.lifetime.start.p0(i64 112, ptr %143) #7
  call void @llvm.lifetime.start.p0(i64 8, ptr %144) #7
  store double 0.000000e+00, ptr %144, align 8, !tbaa !17
  call void @llvm.lifetime.start.p0(i64 24, ptr %145) #7
  call void @llvm.lifetime.start.p0(i64 16, ptr %146) #7
  invoke void @_ZN5Eigen9DenseBaseINS_6MatrixIdLi3ELi1ELi0ELi3ELi1EEEE4ZeroEl(ptr dead_on_unwind writable sret(%"class.Eigen::CwiseNullaryOp.181") align 8 %146, i64 noundef 3)
          to label %1315 unwind label %1341

1315:                                             ; preds = %1314
  invoke void @_ZN9Vector3_tC2IN5Eigen14CwiseNullaryOpINS1_8internal18scalar_constant_opIdEENS1_6MatrixIdLi3ELi1ELi0ELi3ELi1EEEEEEERKNS1_10MatrixBaseIT_EE(ptr noundef nonnull align 8 dereferenceable(24) %145, ptr noundef nonnull align 1 dereferenceable(1) %146)
          to label %1316 unwind label %1341

1316:                                             ; preds = %1315
  invoke void @_ZN17RigidBodyDynamics4BodyC2ERKdRK9Vector3_tRK9Matrix3_t(ptr noundef nonnull align 8 dereferenceable(105) %143, ptr noundef nonnull align 8 dereferenceable(8) %144, ptr noundef nonnull align 8 dereferenceable(24) %145, ptr noundef nonnull align 8 dereferenceable(72) %141)
          to label %1317 unwind label %1341

1317:                                             ; preds = %1316
  call void @llvm.lifetime.end.p0(i64 16, ptr %146) #7
  call void @llvm.lifetime.end.p0(i64 24, ptr %145) #7
  call void @llvm.lifetime.end.p0(i64 8, ptr %144) #7
  call void @llvm.lifetime.start.p0(i64 24, ptr %147) #7
  invoke void @_ZN17RigidBodyDynamics5JointC2ENS_9JointTypeE(ptr noundef nonnull align 8 dereferenceable(24) %147, i32 noundef 11)
          to label %1318 unwind label %1345

1318:                                             ; preds = %1317
  call void @llvm.lifetime.start.p0(i64 32, ptr %148) #7
  %1319 = invoke noundef ptr @_ZN13my_shared_ptrIN4urdf4LinkEEptEv(ptr noundef nonnull align 8 dereferenceable(16) %40)
          to label %1320 unwind label %1349

1320:                                             ; preds = %1318
  %1321 = getelementptr inbounds %"class.urdf::Link", ptr %1319, i32 0, i32 0
  invoke void @_ZStplIcSt11char_traitsIcESaIcEENSt7__cxx1112basic_stringIT_T0_T1_EERKS8_PKS5_(ptr dead_on_unwind writable sret(%"class.std::__cxx11::basic_string") align 8 %148, ptr noundef nonnull align 8 dereferenceable(32) %1321, ptr noundef @.str.25)
          to label %1322 unwind label %1349

1322:                                             ; preds = %1320
  %1323 = load ptr, ptr %6, align 8, !tbaa !5
  %1324 = load i32, ptr %41, align 4, !tbaa !38
  invoke void @_ZNSt7__cxx1112basic_stringIcSt11char_traitsIcESaIcEEC2ERKS4_(ptr noundef nonnull align 8 dereferenceable(32) %149, ptr noundef nonnull align 8 dereferenceable(32) %148)
          to label %1325 unwind label %1353

1325:                                             ; preds = %1322
  %1326 = invoke noundef i32 @_ZN17RigidBodyDynamics5Model7AddBodyEjRKNS_4Math16SpatialTransformERKNS_5JointERKNS_4BodyENSt7__cxx1112basic_stringIcSt11char_traitsIcESaIcEEE(ptr noundef nonnull align 8 dereferenceable(872) %1323, i32 noundef %1324, ptr noundef nonnull align 8 dereferenceable(96) %109, ptr noundef nonnull align 8 dereferenceable(24) %147, ptr noundef nonnull align 8 dereferenceable(105) %143, ptr noundef %149)
          to label %1327 unwind label %1357

1327:                                             ; preds = %1325
  call void @_ZNSt7__cxx1112basic_stringIcSt11char_traitsIcESaIcEED2Ev(ptr noundef nonnull align 8 dereferenceable(32) %149) #7
  call void @llvm.lifetime.start.p0(i64 24, ptr %150) #7
  invoke void @_ZN17RigidBodyDynamics5JointC2ENS_9JointTypeE(ptr noundef nonnull align 8 dereferenceable(24) %150, i32 noundef 8)
          to label %1328 unwind label %1361

1328:                                             ; preds = %1327
  %1329 = load ptr, ptr %6, align 8, !tbaa !5
  call void @llvm.lifetime.start.p0(i64 96, ptr %151) #7
  invoke void @_ZN17RigidBodyDynamics4Math16SpatialTransformC2Ev(ptr noundef nonnull align 8 dereferenceable(96) %151)
          to label %1330 unwind label %1365

1330:                                             ; preds = %1328
  %1331 = invoke noundef ptr @_ZN13my_shared_ptrIN4urdf4LinkEEptEv(ptr noundef nonnull align 8 dereferenceable(16) %40)
          to label %1332 unwind label %1365

1332:                                             ; preds = %1330
  %1333 = getelementptr inbounds %"class.urdf::Link", ptr %1331, i32 0, i32 0
  invoke void @_ZNSt7__cxx1112basic_stringIcSt11char_traitsIcESaIcEEC2ERKS4_(ptr noundef nonnull align 8 dereferenceable(32) %152, ptr noundef nonnull align 8 dereferenceable(32) %1333)
          to label %1334 unwind label %1365

1334:                                             ; preds = %1332
  %1335 = invoke noundef i32 @_ZN17RigidBodyDynamics5Model10AppendBodyERKNS_4Math16SpatialTransformERKNS_5JointERKNS_4BodyENSt7__cxx1112basic_stringIcSt11char_traitsIcESaIcEEE(ptr noundef nonnull align 8 dereferenceable(872) %1329, ptr noundef nonnull align 8 dereferenceable(96) %151, ptr noundef nonnull align 8 dereferenceable(24) %150, ptr noundef nonnull align 8 dereferenceable(105) %138, ptr noundef %152)
          to label %1336 unwind label %1369

1336:                                             ; preds = %1334
  call void @_ZNSt7__cxx1112basic_stringIcSt11char_traitsIcESaIcEED2Ev(ptr noundef nonnull align 8 dereferenceable(32) %152) #7
  call void @llvm.lifetime.end.p0(i64 96, ptr %151) #7
  call void @_ZN17RigidBodyDynamics5JointD2Ev(ptr noundef nonnull align 8 dereferenceable(24) %150) #7
  call void @llvm.lifetime.end.p0(i64 24, ptr %150) #7
  call void @_ZNSt7__cxx1112basic_stringIcSt11char_traitsIcESaIcEED2Ev(ptr noundef nonnull align 8 dereferenceable(32) %148) #7
  call void @llvm.lifetime.end.p0(i64 32, ptr %148) #7
  call void @_ZN17RigidBodyDynamics5JointD2Ev(ptr noundef nonnull align 8 dereferenceable(24) %147) #7
  call void @llvm.lifetime.end.p0(i64 24, ptr %147) #7
  call void @_ZN17RigidBodyDynamics4BodyD2Ev(ptr noundef nonnull align 8 dereferenceable(105) %143) #7
  call void @llvm.lifetime.end.p0(i64 112, ptr %143) #7
  call void @llvm.lifetime.end.p0(i64 72, ptr %141) #7
  br label %1393

1337:                                             ; preds = %1313, %1312
  %1338 = landingpad { ptr, i32 }
          cleanup
  %1339 = extractvalue { ptr, i32 } %1338, 0
  store ptr %1339, ptr %12, align 8
  %1340 = extractvalue { ptr, i32 } %1338, 1
  store i32 %1340, ptr %13, align 4
  call void @llvm.lifetime.end.p0(i64 16, ptr %142) #7
  br label %1379

1341:                                             ; preds = %1316, %1315, %1314
  %1342 = landingpad { ptr, i32 }
          cleanup
  %1343 = extractvalue { ptr, i32 } %1342, 0
  store ptr %1343, ptr %12, align 8
  %1344 = extractvalue { ptr, i32 } %1342, 1
  store i32 %1344, ptr %13, align 4
  call void @llvm.lifetime.end.p0(i64 16, ptr %146) #7
  call void @llvm.lifetime.end.p0(i64 24, ptr %145) #7
  call void @llvm.lifetime.end.p0(i64 8, ptr %144) #7
  br label %1378

1345:                                             ; preds = %1317
  %1346 = landingpad { ptr, i32 }
          cleanup
  %1347 = extractvalue { ptr, i32 } %1346, 0
  store ptr %1347, ptr %12, align 8
  %1348 = extractvalue { ptr, i32 } %1346, 1
  store i32 %1348, ptr %13, align 4
  br label %1377

1349:                                             ; preds = %1320, %1318
  %1350 = landingpad { ptr, i32 }
          cleanup
  %1351 = extractvalue { ptr, i32 } %1350, 0
  store ptr %1351, ptr %12, align 8
  %1352 = extractvalue { ptr, i32 } %1350, 1
  store i32 %1352, ptr %13, align 4
  br label %1376

1353:                                             ; preds = %1322
  %1354 = landingpad { ptr, i32 }
          cleanup
  %1355 = extractvalue { ptr, i32 } %1354, 0
  store ptr %1355, ptr %12, align 8
  %1356 = extractvalue { ptr, i32 } %1354, 1
  store i32 %1356, ptr %13, align 4
  br label %1375

1357:                                             ; preds = %1325
  %1358 = landingpad { ptr, i32 }
          cleanup
  %1359 = extractvalue { ptr, i32 } %1358, 0
  store ptr %1359, ptr %12, align 8
  %1360 = extractvalue { ptr, i32 } %1358, 1
  store i32 %1360, ptr %13, align 4
  call void @_ZNSt7__cxx1112basic_stringIcSt11char_traitsIcESaIcEED2Ev(ptr noundef nonnull align 8 dereferenceable(32) %149) #7
  br label %1375

1361:                                             ; preds = %1327
  %1362 = landingpad { ptr, i32 }
          cleanup
  %1363 = extractvalue { ptr, i32 } %1362, 0
  store ptr %1363, ptr %12, align 8
  %1364 = extractvalue { ptr, i32 } %1362, 1
  store i32 %1364, ptr %13, align 4
  br label %1374

1365:                                             ; preds = %1332, %1330, %1328
  %1366 = landingpad { ptr, i32 }
          cleanup
  %1367 = extractvalue { ptr, i32 } %1366, 0
  store ptr %1367, ptr %12, align 8
  %1368 = extractvalue { ptr, i32 } %1366, 1
  store i32 %1368, ptr %13, align 4
  br label %1373

1369:                                             ; preds = %1334
  %1370 = landingpad { ptr, i32 }
          cleanup
  %1371 = extractvalue { ptr, i32 } %1370, 0
  store ptr %1371, ptr %12, align 8
  %1372 = extractvalue { ptr, i32 } %1370, 1
  store i32 %1372, ptr %13, align 4
  call void @_ZNSt7__cxx1112basic_stringIcSt11char_traitsIcESaIcEED2Ev(ptr noundef nonnull align 8 dereferenceable(32) %152) #7
  br label %1373

1373:                                             ; preds = %1369, %1365
  call void @llvm.lifetime.end.p0(i64 96, ptr %151) #7
  call void @_ZN17RigidBodyDynamics5JointD2Ev(ptr noundef nonnull align 8 dereferenceable(24) %150) #7
  br label %1374

1374:                                             ; preds = %1373, %1361
  call void @llvm.lifetime.end.p0(i64 24, ptr %150) #7
  br label %1375

1375:                                             ; preds = %1374, %1357, %1353
  call void @_ZNSt7__cxx1112basic_stringIcSt11char_traitsIcESaIcEED2Ev(ptr noundef nonnull align 8 dereferenceable(32) %148) #7
  br label %1376

1376:                                             ; preds = %1375, %1349
  call void @llvm.lifetime.end.p0(i64 32, ptr %148) #7
  call void @_ZN17RigidBodyDynamics5JointD2Ev(ptr noundef nonnull align 8 dereferenceable(24) %147) #7
  br label %1377

1377:                                             ; preds = %1376, %1345
  call void @llvm.lifetime.end.p0(i64 24, ptr %147) #7
  call void @_ZN17RigidBodyDynamics4BodyD2Ev(ptr noundef nonnull align 8 dereferenceable(105) %143) #7
  br label %1378

1378:                                             ; preds = %1377, %1341
  call void @llvm.lifetime.end.p0(i64 112, ptr %143) #7
  br label %1379

1379:                                             ; preds = %1378, %1337
  call void @llvm.lifetime.end.p0(i64 72, ptr %141) #7
  br label %1401

1380:                                             ; preds = %1308
  %1381 = load ptr, ptr %6, align 8, !tbaa !5
  %1382 = load i32, ptr %41, align 4, !tbaa !38
  %1383 = invoke noundef ptr @_ZN13my_shared_ptrIN4urdf4LinkEEptEv(ptr noundef nonnull align 8 dereferenceable(16) %40)
          to label %1384 unwind label %1243

1384:                                             ; preds = %1380
  %1385 = getelementptr inbounds %"class.urdf::Link", ptr %1383, i32 0, i32 0
  invoke void @_ZNSt7__cxx1112basic_stringIcSt11char_traitsIcESaIcEEC2ERKS4_(ptr noundef nonnull align 8 dereferenceable(32) %153, ptr noundef nonnull align 8 dereferenceable(32) %1385)
          to label %1386 unwind label %1243

1386:                                             ; preds = %1384
  %1387 = invoke noundef i32 @_ZN17RigidBodyDynamics5Model7AddBodyEjRKNS_4Math16SpatialTransformERKNS_5JointERKNS_4BodyENSt7__cxx1112basic_stringIcSt11char_traitsIcESaIcEEE(ptr noundef nonnull align 8 dereferenceable(872) %1381, i32 noundef %1382, ptr noundef nonnull align 8 dereferenceable(96) %109, ptr noundef nonnull align 8 dereferenceable(24) %42, ptr noundef nonnull align 8 dereferenceable(105) %138, ptr noundef %153)
          to label %1388 unwind label %1389

1388:                                             ; preds = %1386
  call void @_ZNSt7__cxx1112basic_stringIcSt11char_traitsIcESaIcEED2Ev(ptr noundef nonnull align 8 dereferenceable(32) %153) #7
  br label %1393

1389:                                             ; preds = %1386
  %1390 = landingpad { ptr, i32 }
          cleanup
  %1391 = extractvalue { ptr, i32 } %1390, 0
  store ptr %1391, ptr %12, align 8
  %1392 = extractvalue { ptr, i32 } %1390, 1
  store i32 %1392, ptr %13, align 4
  call void @_ZNSt7__cxx1112basic_stringIcSt11char_traitsIcESaIcEED2Ev(ptr noundef nonnull align 8 dereferenceable(32) %153) #7
  br label %1401

1393:                                             ; preds = %1388, %1336
  call void @_ZN17RigidBodyDynamics4BodyD2Ev(ptr noundef nonnull align 8 dereferenceable(105) %138) #7
  call void @llvm.lifetime.end.p0(i64 112, ptr %138) #7
  store i32 0, ptr %106, align 4
  br label %1394

1394:                                             ; preds = %1393, %1134
  call void @llvm.lifetime.end.p0(i64 8, ptr %133) #7
  call void @llvm.lifetime.end.p0(i64 72, ptr %131) #7
  call void @llvm.lifetime.end.p0(i64 24, ptr %130) #7
  call void @llvm.lifetime.end.p0(i64 24, ptr %129) #7
  call void @llvm.lifetime.end.p0(i64 96, ptr %109) #7
  call void @llvm.lifetime.end.p0(i64 24, ptr %108) #7
  call void @llvm.lifetime.end.p0(i64 24, ptr %107) #7
  br label %1395

1395:                                             ; preds = %1394, %928
  call void @_ZN17RigidBodyDynamics5JointD2Ev(ptr noundef nonnull align 8 dereferenceable(24) %42) #7
  call void @llvm.lifetime.end.p0(i64 24, ptr %42) #7
  call void @llvm.lifetime.end.p0(i64 4, ptr %41) #7
  call void @_ZN13my_shared_ptrIN4urdf4LinkEED2Ev(ptr noundef nonnull align 8 dereferenceable(16) %40) #7
  call void @llvm.lifetime.end.p0(i64 16, ptr %40) #7
  call void @_ZN13my_shared_ptrIN4urdf4LinkEED2Ev(ptr noundef nonnull align 8 dereferenceable(16) %39) #7
  call void @llvm.lifetime.end.p0(i64 16, ptr %39) #7
  call void @_ZN13my_shared_ptrIN4urdf5JointEED2Ev(ptr noundef nonnull align 8 dereferenceable(16) %38) #7
  call void @llvm.lifetime.end.p0(i64 16, ptr %38) #7
  %1396 = load i32, ptr %106, align 4
  switch i32 %1396, label %1417 [
    i32 0, label %1397
  ]

1397:                                             ; preds = %1395
  br label %1398

1398:                                             ; preds = %1397
  %1399 = load i32, ptr %37, align 4, !tbaa !38
  %1400 = add i32 %1399, 1
  store i32 %1400, ptr %37, align 4, !tbaa !38
  br label %606, !llvm.loop !59

1401:                                             ; preds = %1389, %1379, %1278, %1243
  call void @_ZN17RigidBodyDynamics4BodyD2Ev(ptr noundef nonnull align 8 dereferenceable(105) %138) #7
  br label %1402

1402:                                             ; preds = %1401, %1239
  call void @llvm.lifetime.end.p0(i64 112, ptr %138) #7
  br label %1403

1403:                                             ; preds = %1402, %1193, %1189
  call void @llvm.lifetime.end.p0(i64 8, ptr %133) #7
  br label %1404

1404:                                             ; preds = %1403, %1185
  call void @llvm.lifetime.end.p0(i64 72, ptr %131) #7
  br label %1405

1405:                                             ; preds = %1404, %1181
  call void @llvm.lifetime.end.p0(i64 24, ptr %130) #7
  br label %1406

1406:                                             ; preds = %1405, %1177
  call void @llvm.lifetime.end.p0(i64 24, ptr %129) #7
  br label %1407

1407:                                             ; preds = %1406, %1176
  call void @llvm.lifetime.end.p0(i64 96, ptr %109) #7
  br label %1408

1408:                                             ; preds = %1407, %1139
  call void @llvm.lifetime.end.p0(i64 24, ptr %108) #7
  br label %1409

1409:                                             ; preds = %1408, %1135
  call void @llvm.lifetime.end.p0(i64 24, ptr %107) #7
  br label %1410

1410:                                             ; preds = %1409, %910, %860, %841, %804, %717
  call void @_ZN17RigidBodyDynamics5JointD2Ev(ptr noundef nonnull align 8 dereferenceable(24) %42) #7
  br label %1411

1411:                                             ; preds = %1410, %713
  call void @llvm.lifetime.end.p0(i64 24, ptr %42) #7
  br label %1412

1412:                                             ; preds = %1411, %671
  call void @llvm.lifetime.end.p0(i64 4, ptr %41) #7
  call void @_ZN13my_shared_ptrIN4urdf4LinkEED2Ev(ptr noundef nonnull align 8 dereferenceable(16) %40) #7
  br label %1413

1413:                                             ; preds = %1412, %667
  call void @llvm.lifetime.end.p0(i64 16, ptr %40) #7
  call void @_ZN13my_shared_ptrIN4urdf4LinkEED2Ev(ptr noundef nonnull align 8 dereferenceable(16) %39) #7
  br label %1414

1414:                                             ; preds = %1413, %663
  call void @llvm.lifetime.end.p0(i64 16, ptr %39) #7
  call void @_ZN13my_shared_ptrIN4urdf5JointEED2Ev(ptr noundef nonnull align 8 dereferenceable(16) %38) #7
  br label %1415

1415:                                             ; preds = %1414, %659
  call void @llvm.lifetime.end.p0(i64 16, ptr %38) #7
  call void @llvm.lifetime.end.p0(i64 4, ptr %37) #7
  br label %1419

1416:                                             ; preds = %606
  store i1 true, ptr %5, align 1
  store i32 1, ptr %106, align 4
  br label %1417

1417:                                             ; preds = %1416, %1395
  call void @llvm.lifetime.end.p0(i64 4, ptr %37) #7
  call void @llvm.lifetime.end.p0(i64 8, ptr %24) #7
  call void @llvm.lifetime.end.p0(i64 72, ptr %23) #7
  call void @llvm.lifetime.end.p0(i64 24, ptr %22) #7
  call void @llvm.lifetime.end.p0(i64 24, ptr %21) #7
  call void @_ZN13my_shared_ptrIKN4urdf4LinkEED2Ev(ptr noundef nonnull align 8 dereferenceable(16) %20) #7
  call void @llvm.lifetime.end.p0(i64 16, ptr %20) #7
  call void @llvm.lifetime.end.p0(i64 8, ptr %19) #7
  call void @_ZNSt5stackIiSt5dequeIiSaIiEEED2Ev(ptr noundef nonnull align 8 dereferenceable(80) %17) #7
  call void @llvm.lifetime.end.p0(i64 80, ptr %17) #7
  call void @_ZNSt5stackI13my_shared_ptrIN4urdf4LinkEESt5dequeIS3_SaIS3_EEED2Ev(ptr noundef nonnull align 8 dereferenceable(80) %16) #7
  call void @llvm.lifetime.end.p0(i64 80, ptr %16) #7
  call void @_ZNSt6vectorINSt7__cxx1112basic_stringIcSt11char_traitsIcESaIcEEESaIS5_EED2Ev(ptr noundef nonnull align 8 dereferenceable(24) %15) #7
  call void @llvm.lifetime.end.p0(i64 24, ptr %15) #7
  call void @_ZNSt3mapINSt7__cxx1112basic_stringIcSt11char_traitsIcESaIcEEE13my_shared_ptrIN4urdf5JointEESt4lessIS5_ESaISt4pairIKS5_S9_EEED2Ev(ptr noundef nonnull align 8 dereferenceable(48) %14) #7
  call void @llvm.lifetime.end.p0(i64 48, ptr %14) #7
  call void @_ZNSt3mapINSt7__cxx1112basic_stringIcSt11char_traitsIcESaIcEEE13my_shared_ptrIN4urdf4LinkEESt4lessIS5_ESaISt4pairIKS5_S9_EEED2Ev(ptr noundef nonnull align 8 dereferenceable(48) %11) #7
  call void @llvm.lifetime.end.p0(i64 48, ptr %11) #7
  call void @_ZN13my_shared_ptrIN4urdf4LinkEED2Ev(ptr noundef nonnull align 8 dereferenceable(16) %10) #7
  call void @llvm.lifetime.end.p0(i64 16, ptr %10) #7
  %1418 = load i1, ptr %5, align 1
  ret i1 %1418

1419:                                             ; preds = %1415, %604, %527, %476, %380
  call void @llvm.lifetime.end.p0(i64 8, ptr %24) #7
  br label %1420

1420:                                             ; preds = %1419, %376
  call void @llvm.lifetime.end.p0(i64 72, ptr %23) #7
  br label %1421

1421:                                             ; preds = %1420, %372
  call void @llvm.lifetime.end.p0(i64 24, ptr %22) #7
  br label %1422

1422:                                             ; preds = %1421, %368
  call void @llvm.lifetime.end.p0(i64 24, ptr %21) #7
  call void @_ZN13my_shared_ptrIKN4urdf4LinkEED2Ev(ptr noundef nonnull align 8 dereferenceable(16) %20) #7
  call void @llvm.lifetime.end.p0(i64 16, ptr %20) #7
  br label %1423

1423:                                             ; preds = %1422, %364
  call void @llvm.lifetime.end.p0(i64 8, ptr %19) #7
  br label %1424

1424:                                             ; preds = %1423, %363
  call void @_ZNSt5stackIiSt5dequeIiSaIiEEED2Ev(ptr noundef nonnull align 8 dereferenceable(80) %17) #7
  br label %1425

1425:                                             ; preds = %1424, %351
  call void @llvm.lifetime.end.p0(i64 80, ptr %17) #7
  call void @_ZNSt5stackI13my_shared_ptrIN4urdf4LinkEESt5dequeIS3_SaIS3_EEED2Ev(ptr noundef nonnull align 8 dereferenceable(80) %16) #7
  br label %1426

1426:                                             ; preds = %1425, %347
  call void @llvm.lifetime.end.p0(i64 80, ptr %16) #7
  call void @_ZNSt6vectorINSt7__cxx1112basic_stringIcSt11char_traitsIcESaIcEEESaIS5_EED2Ev(ptr noundef nonnull align 8 dereferenceable(24) %15) #7
  call void @llvm.lifetime.end.p0(i64 24, ptr %15) #7
  br label %1427

1427:                                             ; preds = %1426, %343
  call void @_ZNSt3mapINSt7__cxx1112basic_stringIcSt11char_traitsIcESaIcEEE13my_shared_ptrIN4urdf5JointEESt4lessIS5_ESaISt4pairIKS5_S9_EEED2Ev(ptr noundef nonnull align 8 dereferenceable(48) %14) #7
  call void @llvm.lifetime.end.p0(i64 48, ptr %14) #7
  br label %1428

1428:                                             ; preds = %1427, %339
  call void @_ZNSt3mapINSt7__cxx1112basic_stringIcSt11char_traitsIcESaIcEEE13my_shared_ptrIN4urdf4LinkEESt4lessIS5_ESaISt4pairIKS5_S9_EEED2Ev(ptr noundef nonnull align 8 dereferenceable(48) %11) #7
  call void @llvm.lifetime.end.p0(i64 48, ptr %11) #7
  call void @_ZN13my_shared_ptrIN4urdf4LinkEED2Ev(ptr noundef nonnull align 8 dereferenceable(16) %10) #7
  call void @llvm.lifetime.end.p0(i64 16, ptr %10) #7
  br label %1429

1429:                                             ; preds = %1428
  %1430 = load ptr, ptr %12, align 8
  %1431 = load i32, ptr %13, align 4
  %1432 = insertvalue { ptr, i32 } poison, ptr %1430, 0
  %1433 = insertvalue { ptr, i32 } %1432, i32 %1431, 1
  resume { ptr, i32 } %1433
}

; Function Attrs: nocallback nofree nosync nounwind willreturn memory(argmem: readwrite)
declare void @llvm.lifetime.start.p0(i64 immarg, ptr nocapture) #1

; Function Attrs: mustprogress nounwind uwtable
declare dso_local void @_ZN13my_shared_ptrIN4urdf4LinkEEC2Ev(ptr noundef nonnull align 8 dereferenceable(16)) unnamed_addr #2 align 2

; Function Attrs: mustprogress nounwind uwtable
declare dso_local void @_ZNSt3mapINSt7__cxx1112basic_stringIcSt11char_traitsIcESaIcEEE13my_shared_ptrIN4urdf4LinkEESt4lessIS5_ESaISt4pairIKS5_S9_EEEC2Ev(ptr noundef nonnull align 8 dereferenceable(48)) unnamed_addr #2 align 2

; Function Attrs: mustprogress nounwind uwtable
declare dso_local noundef ptr @_ZN13my_shared_ptrIN4urdf14ModelInterfaceEEptEv(ptr noundef nonnull align 8 dereferenceable(16)) #2 align 2

declare i32 @__gxx_personality_v0(...)

; Function Attrs: mustprogress uwtable
declare dso_local noundef nonnull align 8 dereferenceable(48) ptr @_ZNSt3mapINSt7__cxx1112basic_stringIcSt11char_traitsIcESaIcEEE13my_shared_ptrIN4urdf4LinkEESt4lessIS5_ESaISt4pairIKS5_S9_EEEaSERKSG_(ptr noundef nonnull align 8 dereferenceable(48), ptr noundef nonnull align 8 dereferenceable(48)) #0 align 2

; Function Attrs: mustprogress nounwind uwtable
declare dso_local void @_ZNSt3mapINSt7__cxx1112basic_stringIcSt11char_traitsIcESaIcEEE13my_shared_ptrIN4urdf5JointEESt4lessIS5_ESaISt4pairIKS5_S9_EEEC2Ev(ptr noundef nonnull align 8 dereferenceable(48)) unnamed_addr #2 align 2

; Function Attrs: mustprogress uwtable
declare dso_local noundef nonnull align 8 dereferenceable(48) ptr @_ZNSt3mapINSt7__cxx1112basic_stringIcSt11char_traitsIcESaIcEEE13my_shared_ptrIN4urdf5JointEESt4lessIS5_ESaISt4pairIKS5_S9_EEEaSERKSG_(ptr noundef nonnull align 8 dereferenceable(48), ptr noundef nonnull align 8 dereferenceable(48)) #0 align 2

; Function Attrs: mustprogress nounwind uwtable
declare dso_local void @_ZNSt6vectorINSt7__cxx1112basic_stringIcSt11char_traitsIcESaIcEEESaIS5_EEC2Ev(ptr noundef nonnull align 8 dereferenceable(24)) unnamed_addr #2 align 2

; Function Attrs: mustprogress uwtable
declare dso_local void @_ZNSt5stackI13my_shared_ptrIN4urdf4LinkEESt5dequeIS3_SaIS3_EEEC2IS6_vEEv(ptr noundef nonnull align 8 dereferenceable(80)) unnamed_addr #0 align 2

; Function Attrs: mustprogress uwtable
declare dso_local void @_ZNSt5stackIiSt5dequeIiSaIiEEEC2IS2_vEEv(ptr noundef nonnull align 8 dereferenceable(80)) unnamed_addr #0 align 2

; Function Attrs: mustprogress uwtable
declare dso_local void @_ZNSt5stackI13my_shared_ptrIN4urdf4LinkEESt5dequeIS3_SaIS3_EEE4pushERKS3_(ptr noundef nonnull align 8 dereferenceable(80), ptr noundef nonnull align 8 dereferenceable(16)) #0 align 2

; Function Attrs: mustprogress uwtable
declare dso_local noundef nonnull align 8 dereferenceable(16) ptr @_ZNSt3mapINSt7__cxx1112basic_stringIcSt11char_traitsIcESaIcEEE13my_shared_ptrIN4urdf4LinkEESt4lessIS5_ESaISt4pairIKS5_S9_EEEixERSD_(ptr noundef nonnull align 8 dereferenceable(48), ptr noundef nonnull align 8 dereferenceable(32)) #0 align 2

; Function Attrs: mustprogress uwtable
declare dso_local void @_ZNK4urdf14ModelInterface7getRootEv(ptr dead_on_unwind noalias writable sret(%class.my_shared_ptr.28) align 8, ptr noundef nonnull align 8 dereferenceable(200)) #0 align 2

; Function Attrs: mustprogress nounwind uwtable
declare dso_local noundef ptr @_ZN13my_shared_ptrIKN4urdf4LinkEEptEv(ptr noundef nonnull align 8 dereferenceable(16)) #2 align 2

; Function Attrs: mustprogress nounwind uwtable
declare dso_local void @_ZN13my_shared_ptrIKN4urdf4LinkEED2Ev(ptr noundef nonnull align 8 dereferenceable(16)) unnamed_addr #2 align 2

; Function Attrs: nocallback nofree nosync nounwind willreturn memory(argmem: readwrite)
declare void @llvm.lifetime.end.p0(i64 immarg, ptr nocapture) #1

; Function Attrs: inlinehint mustprogress uwtable
declare dso_local void @_ZN9Vector3_tC2Ev(ptr noundef nonnull align 8 dereferenceable(24)) unnamed_addr #3 align 2

; Function Attrs: inlinehint mustprogress uwtable
declare dso_local void @_ZN9Matrix3_tC2Ev(ptr noundef nonnull align 8 dereferenceable(72)) unnamed_addr #3 align 2

; Function Attrs: mustprogress nounwind uwtable
declare dso_local noundef ptr @_ZNK13my_shared_ptrIKN4urdf4LinkEEptEv(ptr noundef nonnull align 8 dereferenceable(16)) #2 align 2

; Function Attrs: mustprogress nounwind uwtable
declare dso_local noundef zeroext i1 @_ZNK13my_shared_ptrIN4urdf8InertialEEcvbEv(ptr noundef nonnull align 8 dereferenceable(16)) #2 align 2

; Function Attrs: mustprogress nounwind uwtable
declare dso_local noundef ptr @_ZNK13my_shared_ptrIN4urdf8InertialEEptEv(ptr noundef nonnull align 8 dereferenceable(16)) #2 align 2

; Function Attrs: mustprogress uwtable
declare dso_local void @_ZN9Vector3_t3setERKdS1_S1_(ptr noundef nonnull align 8 dereferenceable(24), ptr noundef nonnull align 8 dereferenceable(8), ptr noundef nonnull align 8 dereferenceable(8), ptr noundef nonnull align 8 dereferenceable(8)) #0 align 2

; Function Attrs: inlinehint mustprogress uwtable
declare dso_local noundef nonnull align 8 dereferenceable(8) ptr @_ZN5Eigen15DenseCoeffsBaseINS_6MatrixIdLi3ELi3ELi0ELi3ELi3EEELi1EEclEll(ptr noundef nonnull align 1 dereferenceable(1), i64 noundef, i64 noundef) #3 align 2

; Function Attrs: mustprogress nounwind uwtable
declare dso_local void @_ZNK4urdf8Rotation6getRPYERdS1_S1_(ptr noundef nonnull align 8 dereferenceable(32), ptr noundef nonnull align 8 dereferenceable(8), ptr noundef nonnull align 8 dereferenceable(8), ptr noundef nonnull align 8 dereferenceable(8)) #2 align 2

; Function Attrs: inlinehint mustprogress uwtable
declare dso_local noundef nonnull align 8 dereferenceable(8) ptr @_ZN5Eigen15DenseCoeffsBaseINS_6MatrixIdLi3ELi1ELi0ELi3ELi1EEELi1EEixEl(ptr noundef nonnull align 1 dereferenceable(1), i64 noundef) #3 align 2

; Function Attrs: mustprogress uwtable
declare dso_local void @_ZN17RigidBodyDynamics4BodyC2ERKdRK9Vector3_tRK9Matrix3_t(ptr noundef nonnull align 8 dereferenceable(105), ptr noundef nonnull align 8 dereferenceable(8), ptr noundef nonnull align 8 dereferenceable(24), ptr noundef nonnull align 8 dereferenceable(72)) unnamed_addr #0 align 2

; Function Attrs: mustprogress uwtable
declare dso_local void @_ZN17RigidBodyDynamics5JointC2ENS_9JointTypeE(ptr noundef nonnull align 8 dereferenceable(24), i32 noundef) unnamed_addr #0 align 2

; Function Attrs: mustprogress uwtable
declare dso_local noundef nonnull align 8 dereferenceable(24) ptr @_ZN17RigidBodyDynamics5JointaSERKS0_(ptr noundef nonnull align 8 dereferenceable(24), ptr noundef nonnull align 8 dereferenceable(24)) #0 align 2

; Function Attrs: mustprogress nounwind uwtable
declare dso_local void @_ZN17RigidBodyDynamics5JointD2Ev(ptr noundef nonnull align 8 dereferenceable(24)) unnamed_addr #2 align 2

; Function Attrs: mustprogress uwtable
declare dso_local void @_ZN17RigidBodyDynamics4Math16SpatialTransformC2Ev(ptr noundef nonnull align 8 dereferenceable(96)) unnamed_addr #0 align 2

; Function Attrs: inlinehint mustprogress uwtable
declare noundef nonnull align 8 dereferenceable(8) ptr @_ZStlsISt11char_traitsIcEERSt13basic_ostreamIcT_ES5_PKc(ptr noundef nonnull align 8 dereferenceable(8), ptr noundef) #3

; Function Attrs: mustprogress uwtable
declare noundef nonnull align 8 dereferenceable(8) ptr @_ZNSolsEPFRSoS_E(ptr noundef nonnull align 8 dereferenceable(8), ptr noundef) #0 align 2

; Function Attrs: inlinehint mustprogress uwtable
declare noundef nonnull align 8 dereferenceable(8) ptr @_ZSt4endlIcSt11char_traitsIcEERSt13basic_ostreamIT_T0_ES6_(ptr noundef nonnull align 8 dereferenceable(8)) #3

; Function Attrs: inlinehint mustprogress uwtable
declare dso_local noundef nonnull align 8 dereferenceable(8) ptr @_ZN17RigidBodyDynamics4MathlsERSoRKNS0_16SpatialTransformE(ptr noundef nonnull align 8 dereferenceable(8), ptr noundef nonnull align 8 dereferenceable(96)) #3

; Function Attrs: mustprogress uwtable
declare dso_local noundef nonnull align 8 dereferenceable(8) ptr @_ZN5EigenlsINS_6MatrixIdLi3ELi3ELi0ELi3ELi3EEEEERSoS3_RKNS_9DenseBaseIT_EE(ptr noundef nonnull align 8 dereferenceable(8), ptr noundef nonnull align 1 dereferenceable(1)) #0

; Function Attrs: mustprogress uwtable
declare noundef nonnull align 8 dereferenceable(8) ptr @_ZNSolsEd(ptr noundef nonnull align 8 dereferenceable(8), double noundef) #0 align 2

; Function Attrs: inlinehint mustprogress uwtable
declare noundef nonnull align 8 dereferenceable(8) ptr @_ZStlsIcSt11char_traitsIcESaIcEERSt13basic_ostreamIT_T0_ES7_RKNSt7__cxx1112basic_stringIS4_S5_T1_EE(ptr noundef nonnull align 8 dereferenceable(8), ptr noundef nonnull align 8 dereferenceable(32)) #3

declare noundef i32 @_ZN17RigidBodyDynamics5Model10AppendBodyERKNS_4Math16SpatialTransformERKNS_5JointERKNS_4BodyENSt7__cxx1112basic_stringIcSt11char_traitsIcESaIcEEE(ptr noundef nonnull align 8 dereferenceable(872), ptr noundef nonnull align 8 dereferenceable(96), ptr noundef nonnull align 8 dereferenceable(24), ptr noundef nonnull align 8 dereferenceable(105), ptr noundef) #4

; Function Attrs: mustprogress uwtable
declare void @_ZNSt7__cxx1112basic_stringIcSt11char_traitsIcESaIcEEC2ERKS4_(ptr noundef nonnull align 8 dereferenceable(32), ptr noundef nonnull align 8 dereferenceable(32)) unnamed_addr #0 align 2

; Function Attrs: mustprogress nounwind uwtable
declare void @_ZNSt7__cxx1112basic_stringIcSt11char_traitsIcESaIcEED2Ev(ptr noundef nonnull align 8 dereferenceable(32)) unnamed_addr #2 align 2

; Function Attrs: mustprogress nounwind uwtable
declare dso_local void @_ZN17RigidBodyDynamics4BodyD2Ev(ptr noundef nonnull align 8 dereferenceable(105)) unnamed_addr #2 align 2

; Function Attrs: mustprogress uwtable
declare dso_local void @_ZNSt5stackIiSt5dequeIiSaIiEEE4pushEOi(ptr noundef nonnull align 8 dereferenceable(80), ptr noundef nonnull align 4 dereferenceable(4)) #0 align 2

; Function Attrs: mustprogress nounwind uwtable
declare dso_local noundef i64 @_ZNKSt5stackI13my_shared_ptrIN4urdf4LinkEESt5dequeIS3_SaIS3_EEE4sizeEv(ptr noundef nonnull align 8 dereferenceable(80)) #2 align 2

; Function Attrs: mustprogress nounwind uwtable
declare dso_local noundef nonnull align 8 dereferenceable(16) ptr @_ZNSt5stackI13my_shared_ptrIN4urdf4LinkEESt5dequeIS3_SaIS3_EEE3topEv(ptr noundef nonnull align 8 dereferenceable(80)) #2 align 2

; Function Attrs: mustprogress nounwind uwtable
declare dso_local void @_ZN13my_shared_ptrIN4urdf4LinkEEC2ERKS2_(ptr noundef nonnull align 8 dereferenceable(16), ptr noundef nonnull align 8 dereferenceable(16)) unnamed_addr #2 align 2

; Function Attrs: mustprogress nounwind uwtable
declare dso_local noundef nonnull align 4 dereferenceable(4) ptr @_ZNSt5stackIiSt5dequeIiSaIiEEE3topEv(ptr noundef nonnull align 8 dereferenceable(80)) #2 align 2

; Function Attrs: mustprogress nounwind uwtable
declare dso_local noundef ptr @_ZN13my_shared_ptrIN4urdf4LinkEEptEv(ptr noundef nonnull align 8 dereferenceable(16)) #2 align 2

; Function Attrs: mustprogress nounwind uwtable
declare dso_local noundef i64 @_ZNKSt6vectorI13my_shared_ptrIN4urdf5JointEESaIS3_EE4sizeEv(ptr noundef nonnull align 8 dereferenceable(24)) #2 align 2

; Function Attrs: mustprogress nounwind uwtable
declare dso_local noundef nonnull align 8 dereferenceable(16) ptr @_ZNSt6vectorI13my_shared_ptrIN4urdf5JointEESaIS3_EEixEm(ptr noundef nonnull align 8 dereferenceable(24), i64 noundef) #2 align 2

; Function Attrs: mustprogress nounwind uwtable
declare dso_local void @_ZN13my_shared_ptrIN4urdf5JointEEC2ERKS2_(ptr noundef nonnull align 8 dereferenceable(16), ptr noundef nonnull align 8 dereferenceable(16)) unnamed_addr #2 align 2

; Function Attrs: mustprogress nounwind uwtable
declare dso_local void @_ZNSt5stackIiSt5dequeIiSaIiEEE3popEv(ptr noundef nonnull align 8 dereferenceable(80)) #2 align 2

; Function Attrs: mustprogress nounwind uwtable
declare dso_local noundef ptr @_ZN13my_shared_ptrIN4urdf5JointEEptEv(ptr noundef nonnull align 8 dereferenceable(16)) #2 align 2

; Function Attrs: mustprogress nounwind uwtable
declare dso_local noundef i64 @_ZNKSt5stackIiSt5dequeIiSaIiEEE4sizeEv(ptr noundef nonnull align 8 dereferenceable(80)) #2 align 2

declare noundef nonnull align 8 dereferenceable(8) ptr @_ZNSolsEi(ptr noundef nonnull align 8 dereferenceable(8), i32 noundef) #4

; Function Attrs: mustprogress uwtable
declare dso_local void @_ZNSt6vectorINSt7__cxx1112basic_stringIcSt11char_traitsIcESaIcEEESaIS5_EE9push_backERKS5_(ptr noundef nonnull align 8 dereferenceable(24), ptr noundef nonnull align 8 dereferenceable(32)) #0 align 2

; Function Attrs: mustprogress nounwind uwtable
declare dso_local void @_ZN13my_shared_ptrIN4urdf5JointEED2Ev(ptr noundef nonnull align 8 dereferenceable(16)) unnamed_addr #2 align 2

; Function Attrs: mustprogress nounwind uwtable
declare dso_local void @_ZNSt5stackI13my_shared_ptrIN4urdf4LinkEESt5dequeIS3_SaIS3_EEE3popEv(ptr noundef nonnull align 8 dereferenceable(80)) #2 align 2

; Function Attrs: mustprogress nounwind uwtable
declare dso_local void @_ZN13my_shared_ptrIN4urdf4LinkEED2Ev(ptr noundef nonnull align 8 dereferenceable(16)) unnamed_addr #2 align 2

; Function Attrs: mustprogress nounwind uwtable
declare dso_local noundef i64 @_ZNKSt6vectorINSt7__cxx1112basic_stringIcSt11char_traitsIcESaIcEEESaIS5_EE4sizeEv(ptr noundef nonnull align 8 dereferenceable(24)) #2 align 2

; Function Attrs: mustprogress uwtable
declare dso_local noundef nonnull align 8 dereferenceable(16) ptr @_ZNSt3mapINSt7__cxx1112basic_stringIcSt11char_traitsIcESaIcEEE13my_shared_ptrIN4urdf5JointEESt4lessIS5_ESaISt4pairIKS5_S9_EEEixERSD_(ptr noundef nonnull align 8 dereferenceable(48), ptr noundef nonnull align 8 dereferenceable(32)) #0 align 2

; Function Attrs: mustprogress nounwind uwtable
declare dso_local noundef nonnull align 8 dereferenceable(32) ptr @_ZNSt6vectorINSt7__cxx1112basic_stringIcSt11char_traitsIcESaIcEEESaIS5_EEixEm(ptr noundef nonnull align 8 dereferenceable(24), i64 noundef) #2 align 2

; Function Attrs: mustprogress uwtable
declare dso_local noundef i32 @_ZNK17RigidBodyDynamics5Model9GetBodyIdEPKc(ptr noundef nonnull align 8 dereferenceable(872), ptr noundef) #0 align 2

; Function Attrs: mustprogress nounwind uwtable
declare noundef ptr @_ZNKSt7__cxx1112basic_stringIcSt11char_traitsIcESaIcEE5c_strEv(ptr noundef nonnull align 8 dereferenceable(32)) #2 align 2

; Function Attrs: mustprogress nounwind uwtable
declare dso_local noundef i32 @_ZNSt14numeric_limitsIjE3maxEv() #2 align 2

; Function Attrs: mustprogress nounwind uwtable
declare dso_local void @_ZN17RigidBodyDynamics5JointC2Ev(ptr noundef nonnull align 8 dereferenceable(24)) unnamed_addr #2 align 2

; Function Attrs: inlinehint mustprogress uwtable
declare dso_local void @_ZN9Vector3_tC2ERKdS1_S1_(ptr noundef nonnull align 8 dereferenceable(24), ptr noundef nonnull align 8 dereferenceable(8), ptr noundef nonnull align 8 dereferenceable(8), ptr noundef nonnull align 8 dereferenceable(8)) unnamed_addr #3 align 2

; Function Attrs: inlinehint mustprogress uwtable
declare dso_local noundef double @_ZNK5Eigen10MatrixBaseINS_6MatrixIdLi3ELi1ELi0ELi3ELi1EEEE3dotIS2_EENS_20ScalarBinaryOpTraitsIdNS_8internal6traitsIT_E6ScalarENS6_17scalar_product_opIdSA_EEE10ReturnTypeERKNS0_IS8_EE(ptr noundef nonnull align 1 dereferenceable(1), ptr noundef nonnull align 1 dereferenceable(1)) #3 align 2

; Function Attrs: nocallback nofree nosync nounwind speculatable willreturn memory(none)
declare double @llvm.fabs.f64(double) #5

; Function Attrs: mustprogress nounwind uwtable
declare dso_local noundef double @_ZNSt14numeric_limitsIdE7epsilonEv() #2 align 2

; Function Attrs: mustprogress uwtable
declare dso_local void @_ZN17RigidBodyDynamics5JointC2ENS_9JointTypeERK9Vector3_t(ptr noundef nonnull align 8 dereferenceable(24), i32 noundef, ptr noundef nonnull align 8 dereferenceable(24)) unnamed_addr #0 align 2

; Function Attrs: inlinehint mustprogress uwtable
declare dso_local void @_ZN15SpatialVector_tC2ERKdS1_S1_S1_S1_S1_(ptr noundef nonnull align 16 dereferenceable(48), ptr noundef nonnull align 8 dereferenceable(8), ptr noundef nonnull align 8 dereferenceable(8), ptr noundef nonnull align 8 dereferenceable(8), ptr noundef nonnull align 8 dereferenceable(8), ptr noundef nonnull align 8 dereferenceable(8), ptr noundef nonnull align 8 dereferenceable(8)) unnamed_addr #3 align 2

; Function Attrs: mustprogress uwtable
declare dso_local void @_ZN17RigidBodyDynamics5JointC2ERK15SpatialVector_tS3_S3_S3_S3_S3_(ptr noundef nonnull align 8 dereferenceable(24), ptr noundef nonnull align 16 dereferenceable(48), ptr noundef nonnull align 16 dereferenceable(48), ptr noundef nonnull align 16 dereferenceable(48), ptr noundef nonnull align 16 dereferenceable(48), ptr noundef nonnull align 16 dereferenceable(48), ptr noundef nonnull align 16 dereferenceable(48)) unnamed_addr #0 align 2

; Function Attrs: inlinehint mustprogress uwtable
declare dso_local void @_ZN17RigidBodyDynamics4Math4XrotEdRK9Vector3_t(ptr dead_on_unwind noalias writable sret(%"struct.RigidBodyDynamics::Math::SpatialTransform") align 8, double noundef, ptr noundef nonnull align 8 dereferenceable(24)) #3

; Function Attrs: mustprogress uwtable
declare dso_local void @_ZNK17RigidBodyDynamics4Math16SpatialTransformmlERKS1_(ptr dead_on_unwind noalias writable sret(%"struct.RigidBodyDynamics::Math::SpatialTransform") align 8, ptr noundef nonnull align 8 dereferenceable(96), ptr noundef nonnull align 8 dereferenceable(96)) #0 align 2

; Function Attrs: inlinehint mustprogress uwtable
declare dso_local void @_ZN17RigidBodyDynamics4Math6XtransERK9Vector3_t(ptr dead_on_unwind noalias writable sret(%"struct.RigidBodyDynamics::Math::SpatialTransform") align 8, ptr noundef nonnull align 8 dereferenceable(24)) #3

; Function Attrs: inlinehint mustprogress uwtable
declare dso_local void @_ZN9Vector3_tC2ERKS_(ptr noundef nonnull align 8 dereferenceable(24), ptr noundef nonnull align 8 dereferenceable(24)) unnamed_addr #3 align 2

; Function Attrs: inlinehint mustprogress uwtable
declare dso_local void @_ZN5Eigen9DenseBaseINS_6MatrixIdLi3ELi3ELi0ELi3ELi3EEEE4ZeroEv(ptr dead_on_unwind noalias writable sret(%"class.Eigen::CwiseNullaryOp") align 8) #3 align 2

; Function Attrs: mustprogress uwtable
declare dso_local void @_ZN9Matrix3_tC2IN5Eigen14CwiseNullaryOpINS1_8internal18scalar_constant_opIdEENS1_6MatrixIdLi3ELi3ELi0ELi3ELi3EEEEEEERKNS1_10MatrixBaseIT_EE(ptr noundef nonnull align 8 dereferenceable(72), ptr noundef nonnull align 1 dereferenceable(1)) unnamed_addr #0 align 2

; Function Attrs: mustprogress nounwind uwtable
declare dso_local noundef ptr @_ZN13my_shared_ptrIN4urdf8InertialEEptEv(ptr noundef nonnull align 8 dereferenceable(16)) #2 align 2

; Function Attrs: inlinehint mustprogress uwtable
declare dso_local noundef zeroext i1 @_ZNK5Eigen10MatrixBaseINS_6MatrixIdLi3ELi1ELi0ELi3ELi1EEEEneIS2_EEbRKNS0_IT_EE(ptr noundef nonnull align 1 dereferenceable(1), ptr noundef nonnull align 1 dereferenceable(1)) #3 align 2

; Function Attrs: mustprogress uwtable
declare noundef nonnull align 8 dereferenceable(8) ptr @_ZNSolsEj(ptr noundef nonnull align 8 dereferenceable(8), i32 noundef) #0 align 2

; Function Attrs: mustprogress uwtable
declare dso_local noundef nonnull align 8 dereferenceable(8) ptr @_ZN5EigenlsINS_9TransposeINS_6MatrixIdLi6ELi1ELi0ELi6ELi1EEEEEEERSoS5_RKNS_9DenseBaseIT_EE(ptr noundef nonnull align 8 dereferenceable(8), ptr noundef nonnull align 1 dereferenceable(1)) #0

; Function Attrs: inlinehint mustprogress uwtable
declare dso_local ptr @_ZN5Eigen9DenseBaseINS_6MatrixIdLi6ELi1ELi0ELi6ELi1EEEE9transposeEv(ptr noundef nonnull align 1 dereferenceable(1)) #3 align 2

; Function Attrs: inlinehint mustprogress uwtable
declare dso_local void @_ZN5Eigen9DenseBaseINS_6MatrixIdLi3ELi1ELi0ELi3ELi1EEEE4ZeroEl(ptr dead_on_unwind noalias writable sret(%"class.Eigen::CwiseNullaryOp.181") align 8, i64 noundef) #3 align 2

; Function Attrs: mustprogress uwtable
declare dso_local void @_ZN9Vector3_tC2IN5Eigen14CwiseNullaryOpINS1_8internal18scalar_constant_opIdEENS1_6MatrixIdLi3ELi1ELi0ELi3ELi1EEEEEEERKNS1_10MatrixBaseIT_EE(ptr noundef nonnull align 8 dereferenceable(24), ptr noundef nonnull align 1 dereferenceable(1)) unnamed_addr #0 align 2

; Function Attrs: inlinehint mustprogress uwtable
declare dso_local void @_ZStplIcSt11char_traitsIcESaIcEENSt7__cxx1112basic_stringIT_T0_T1_EERKS8_PKS5_(ptr dead_on_unwind noalias writable sret(%"class.std::__cxx11::basic_string") align 8, ptr noundef nonnull align 8 dereferenceable(32), ptr noundef) #3

declare noundef i32 @_ZN17RigidBodyDynamics5Model7AddBodyEjRKNS_4Math16SpatialTransformERKNS_5JointERKNS_4BodyENSt7__cxx1112basic_stringIcSt11char_traitsIcESaIcEEE(ptr noundef nonnull align 8 dereferenceable(872), i32 noundef, ptr noundef nonnull align 8 dereferenceable(96), ptr noundef nonnull align 8 dereferenceable(24), ptr noundef nonnull align 8 dereferenceable(105), ptr noundef) #4

; Function Attrs: inlinehint mustprogress nounwind uwtable
declare dso_local void @_ZNSt5stackIiSt5dequeIiSaIiEEED2Ev(ptr noundef nonnull align 8 dereferenceable(80)) unnamed_addr #6 align 2

; Function Attrs: inlinehint mustprogress nounwind uwtable
declare dso_local void @_ZNSt5stackI13my_shared_ptrIN4urdf4LinkEESt5dequeIS3_SaIS3_EEED2Ev(ptr noundef nonnull align 8 dereferenceable(80)) unnamed_addr #6 align 2

; Function Attrs: mustprogress nounwind uwtable
declare dso_local void @_ZNSt6vectorINSt7__cxx1112basic_stringIcSt11char_traitsIcESaIcEEESaIS5_EED2Ev(ptr noundef nonnull align 8 dereferenceable(24)) unnamed_addr #2 align 2

; Function Attrs: mustprogress nounwind uwtable
declare dso_local void @_ZNSt3mapINSt7__cxx1112basic_stringIcSt11char_traitsIcESaIcEEE13my_shared_ptrIN4urdf5JointEESt4lessIS5_ESaISt4pairIKS5_S9_EEED2Ev(ptr noundef nonnull align 8 dereferenceable(48)) unnamed_addr #2 align 2

; Function Attrs: mustprogress nounwind uwtable
declare dso_local void @_ZNSt3mapINSt7__cxx1112basic_stringIcSt11char_traitsIcESaIcEEE13my_shared_ptrIN4urdf4LinkEESt4lessIS5_ESaISt4pairIKS5_S9_EEED2Ev(ptr noundef nonnull align 8 dereferenceable(48)) unnamed_addr #2 align 2

attributes #0 = { mustprogress uwtable "min-legal-vector-width"="0" "no-trapping-math"="true" "stack-protector-buffer-size"="8" "target-cpu"="x86-64" "target-features"="+cmov,+cx8,+fxsr,+mmx,+sse,+sse2,+x87" "tune-cpu"="generic" }
attributes #1 = { nocallback nofree nosync nounwind willreturn memory(argmem: readwrite) }
attributes #2 = { mustprogress nounwind uwtable "min-legal-vector-width"="0" "no-trapping-math"="true" "stack-protector-buffer-size"="8" "target-cpu"="x86-64" "target-features"="+cmov,+cx8,+fxsr,+mmx,+sse,+sse2,+x87" "tune-cpu"="generic" }
attributes #3 = { inlinehint mustprogress uwtable "min-legal-vector-width"="0" "no-trapping-math"="true" "stack-protector-buffer-size"="8" "target-cpu"="x86-64" "target-features"="+cmov,+cx8,+fxsr,+mmx,+sse,+sse2,+x87" "tune-cpu"="generic" }
attributes #4 = { "no-trapping-math"="true" "stack-protector-buffer-size"="8" "target-cpu"="x86-64" "target-features"="+cmov,+cx8,+fxsr,+mmx,+sse,+sse2,+x87" "tune-cpu"="generic" }
attributes #5 = { nocallback nofree nosync nounwind speculatable willreturn memory(none) }
attributes #6 = { inlinehint mustprogress nounwind uwtable "min-legal-vector-width"="0" "no-trapping-math"="true" "stack-protector-buffer-size"="8" "target-cpu"="x86-64" "target-features"="+cmov,+cx8,+fxsr,+mmx,+sse,+sse2,+x87" "tune-cpu"="generic" }
attributes #7 = { nounwind }

!llvm.module.flags = !{!0, !1, !2, !3}
!llvm.ident = !{!4}

!0 = !{i32 1, !"wchar_size", i32 4}
!1 = !{i32 8, !"PIC Level", i32 2}
!2 = !{i32 7, !"PIE Level", i32 2}
!3 = !{i32 7, !"uwtable", i32 2}
!4 = !{!"clang version 19.1.0 (git@github.com:llvm/llvm-project.git a4bf6cd7cfb1a1421ba92bca9d017b49936c55e4)"}
!5 = !{!6, !6, i64 0}
!6 = !{!"any pointer", !7, i64 0}
!7 = !{!"omnipotent char", !8, i64 0}
!8 = !{!"Simple C++ TBAA"}
!9 = !{!10, !10, i64 0}
!10 = !{!"bool", !7, i64 0}
!11 = !{!12, !15, i64 56}
!12 = !{!"_ZTSN4urdf8InertialE", !13, i64 0, !15, i64 56, !15, i64 64, !15, i64 72, !15, i64 80, !15, i64 88, !15, i64 96, !15, i64 104}
!13 = !{!"_ZTSN4urdf4PoseE", !14, i64 0, !16, i64 24}
!14 = !{!"_ZTSN4urdf7Vector3E", !15, i64 0, !15, i64 8, !15, i64 16}
!15 = !{!"double", !7, i64 0}
!16 = !{!"_ZTSN4urdf8RotationE", !15, i64 0, !15, i64 8, !15, i64 16, !15, i64 24}
!17 = !{!15, !15, i64 0}
!18 = !{!12, !15, i64 64}
!19 = !{!12, !15, i64 72}
!20 = !{!12, !15, i64 80}
!21 = !{!12, !15, i64 88}
!22 = !{!12, !15, i64 96}
!23 = !{!12, !15, i64 104}
!24 = !{i8 0, i8 2}
!25 = !{}
!26 = !{!27, !15, i64 0}
!27 = !{!"_ZTSN17RigidBodyDynamics4BodyE", !15, i64 0, !28, i64 8, !33, i64 32, !10, i64 104}
!28 = !{!"_ZTS9Vector3_t", !29, i64 0}
!29 = !{!"_ZTSN5Eigen6MatrixIdLi3ELi1ELi0ELi3ELi1EEE", !30, i64 0}
!30 = !{!"_ZTSN5Eigen15PlainObjectBaseINS_6MatrixIdLi3ELi1ELi0ELi3ELi1EEEEE", !31, i64 0}
!31 = !{!"_ZTSN5Eigen12DenseStorageIdLi3ELi3ELi1ELi0EEE", !32, i64 0}
!32 = !{!"_ZTSN5Eigen8internal11plain_arrayIdLi3ELi0ELi0EEE", !7, i64 0}
!33 = !{!"_ZTS9Matrix3_t", !34, i64 0}
!34 = !{!"_ZTSN5Eigen6MatrixIdLi3ELi3ELi0ELi3ELi3EEE", !35, i64 0}
!35 = !{!"_ZTSN5Eigen15PlainObjectBaseINS_6MatrixIdLi3ELi3ELi0ELi3ELi3EEEEE", !36, i64 0}
!36 = !{!"_ZTSN5Eigen12DenseStorageIdLi9ELi3ELi3ELi0EEE", !37, i64 0}
!37 = !{!"_ZTSN5Eigen8internal11plain_arrayIdLi9ELi0ELi0EEE", !7, i64 0}
!38 = !{!39, !39, i64 0}
!39 = !{!"int", !7, i64 0}
!40 = distinct !{!40, !41}
!41 = !{!"llvm.loop.mustprogress"}
!42 = !{!43, !47, i64 32}
!43 = !{!"_ZTSN4urdf5JointE", !44, i64 0, !47, i64 32, !14, i64 40, !44, i64 64, !44, i64 96, !13, i64 128, !48, i64 184, !49, i64 200, !50, i64 216, !51, i64 232, !52, i64 248}
!44 = !{!"_ZTSNSt7__cxx1112basic_stringIcSt11char_traitsIcESaIcEEE", !45, i64 0, !46, i64 8, !7, i64 16}
!45 = !{!"_ZTSNSt7__cxx1112basic_stringIcSt11char_traitsIcESaIcEE12_Alloc_hiderE", !6, i64 0}
!46 = !{!"long", !7, i64 0}
!47 = !{!"_ZTSN4urdf5JointUt_E", !7, i64 0}
!48 = !{!"_ZTS13my_shared_ptrIN4urdf13JointDynamicsEE", !6, i64 0, !6, i64 8}
!49 = !{!"_ZTS13my_shared_ptrIN4urdf11JointLimitsEE", !6, i64 0, !6, i64 8}
!50 = !{!"_ZTS13my_shared_ptrIN4urdf11JointSafetyEE", !6, i64 0, !6, i64 8}
!51 = !{!"_ZTS13my_shared_ptrIN4urdf16JointCalibrationEE", !6, i64 0, !6, i64 8}
!52 = !{!"_ZTS13my_shared_ptrIN4urdf10JointMimicEE", !6, i64 0, !6, i64 8}
!53 = distinct !{!53, !41}
!54 = !{!55, !39, i64 12}
!55 = !{!"_ZTSN17RigidBodyDynamics5JointE", !6, i64 0, !56, i64 8, !39, i64 12, !39, i64 16, !39, i64 20}
!56 = !{!"_ZTSN17RigidBodyDynamics9JointTypeE", !7, i64 0}
!57 = !{!55, !6, i64 0}
!58 = distinct !{!58, !41}
!59 = distinct !{!59, !41}


; Optimized IR (opt -passes='default<O3>'):
target datalayout = "e-m:e-p270:32:32-p271:32:32-p272:64:64-i64:64-i128:128-f80:128-n8:16:32:64-S128"
target triple = "x86_64-unknown-linux-gnu"

%"class.std::basic_ostream" = type { ptr, %"class.std::basic_ios" }
%"class.std::basic_ios" = type { %"class.std::ios_base", ptr, i8, i8, ptr, ptr, ptr, ptr }
%"class.std::ios_base" = type { ptr, i64, i64, i32, i32, i32, ptr, %"struct.std::ios_base::_Words", [8 x %"struct.std::ios_base::_Words"], i32, ptr, %"class.std::locale" }
%"struct.std::ios_base::_Words" = type { ptr, i64 }
%"class.std::locale" = type { ptr }
%class.my_shared_ptr.1 = type { ptr, ptr }
%"class.std::map" = type { %"class.std::_Rb_tree" }
%"class.std::_Rb_tree" = type { %"struct.std::_Rb_tree<std::__cxx11::basic_string<char>, std::pair<const std::__cxx11::basic_string<char>, my_shared_ptr<urdf::Link>>, std::_Select1st<std::pair<const std::__cxx11::basic_string<char>, my_shared_ptr<urdf::Link>>>, std::less<std::__cxx11::basic_string<char>>>::_Rb_tree_impl" }
%"struct.std::_Rb_tree<std::__cxx11::basic_string<char>, std::pair<const std::__cxx11::basic_string<char>, my_shared_ptr<urdf::Link>>, std::_Select1st<std::pair<const std::__cxx11::basic_string<char>, my_shared_ptr<urdf::Link>>>, std::less<std::__cxx11::basic_string<char>>>::_Rb_tree_impl" = type { [8 x i8], %"struct.std::_Rb_tree_header" }
%"struct.std::_Rb_tree_header" = type { %"struct.std::_Rb_tree_node_base", i64 }
%"struct.std::_Rb_tree_node_base" = type { i32, ptr, ptr, ptr }
%"class.std::map.2" = type { %"class.std::_Rb_tree.3" }
%"class.std::_Rb_tree.3" = type { %"struct.std::_Rb_tree<std::__cxx11::basic_string<char>, std::pair<const std::__cxx11::basic_string<char>, my_shared_ptr<urdf::Joint>>, std::_Select1st<std::pair<const std::__cxx11::basic_string<char>, my_shared_ptr<urdf::Joint>>>, std::less<std::__cxx11::basic_string<char>>>::_Rb_tree_impl" }
%"struct.std::_Rb_tree<std::__cxx11::basic_string<char>, std::pair<const std::__cxx11::basic_string<char>, my_shared_ptr<urdf::Joint>>, std::_Select1st<std::pair<const std::__cxx11::basic_string<char>, my_shared_ptr<urdf::Joint>>>, std::less<std::__cxx11::basic_string<char>>>::_Rb_tree_impl" = type { [8 x i8], %"struct.std::_Rb_tree_header" }
%"class.std::vector" = type { %"struct.std::_Vector_base" }
%"struct.std::_Vector_base" = type { %"struct.std::_Vector_base<std::__cxx11::basic_string<char>, std::allocator<std::__cxx11::basic_string<char>>>::_Vector_impl" }
%"struct.std::_Vector_base<std::__cxx11::basic_string<char>, std::allocator<std::__cxx11::basic_string<char>>>::_Vector_impl" = type { %"struct.std::_Vector_base<std::__cxx11::basic_string<char>, std::allocator<std::__cxx11::basic_string<char>>>::_Vector_impl_data" }
%"struct.std::_Vector_base<std::__cxx11::basic_string<char>, std::allocator<std::__cxx11::basic_string<char>>>::_Vector_impl_data" = type { ptr, ptr, ptr }
%"class.std::stack" = type { %"class.std::deque" }
%"class.std::deque" = type { %"class.std::_Deque_base" }
%"class.std::_Deque_base" = type { %"struct.std::_Deque_base<my_shared_ptr<urdf::Link>, std::allocator<my_shared_ptr<urdf::Link>>>::_Deque_impl" }
%"struct.std::_Deque_base<my_shared_ptr<urdf::Link>, std::allocator<my_shared_ptr<urdf::Link>>>::_Deque_impl" = type { %"struct.std::_Deque_base<my_shared_ptr<urdf::Link>, std::allocator<my_shared_ptr<urdf::Link>>>::_Deque_impl_data" }
%"struct.std::_Deque_base<my_shared_ptr<urdf::Link>, std::allocator<my_shared_ptr<urdf::Link>>>::_Deque_impl_data" = type { ptr, i64, %"struct.std::_Deque_iterator", %"struct.std::_Deque_iterator" }
%"struct.std::_Deque_iterator" = type { ptr, ptr, ptr, ptr }
%"class.std::stack.21" = type { %"class.std::deque.22" }
%"class.std::deque.22" = type { %"class.std::_Deque_base.23" }
%"class.std::_Deque_base.23" = type { %"struct.std::_Deque_base<int, std::allocator<int>>::_Deque_impl" }
%"struct.std::_Deque_base<int, std::allocator<int>>::_Deque_impl" = type { %"struct.std::_Deque_base<int, std::allocator<int>>::_Deque_impl_data" }
%"struct.std::_Deque_base<int, std::allocator<int>>::_Deque_impl_data" = type { ptr, i64, %"struct.std::_Deque_iterator.27", %"struct.std::_Deque_iterator.27" }
%"struct.std::_Deque_iterator.27" = type { ptr, ptr, ptr, ptr }
%class.my_shared_ptr.28 = type { ptr, ptr }
%class.Vector3_t = type { %"class.Eigen::Matrix" }
%"class.Eigen::Matrix" = type { %"class.Eigen::PlainObjectBase" }
%"class.Eigen::PlainObjectBase" = type { %"class.Eigen::DenseStorage" }
%"class.Eigen::DenseStorage" = type { %"struct.Eigen::internal::plain_array" }
%"struct.Eigen::internal::plain_array" = type { [3 x double] }
%class.Matrix3_t = type { %"class.Eigen::Matrix.52" }
%"class.Eigen::Matrix.52" = type { %"class.Eigen::PlainObjectBase.53" }
%"class.Eigen::PlainObjectBase.53" = type { %"class.Eigen::DenseStorage.60" }
%"class.Eigen::DenseStorage.60" = type { %"struct.Eigen::internal::plain_array.61" }
%"struct.Eigen::internal::plain_array.61" = type { [9 x double] }
%"struct.RigidBodyDynamics::Body" = type <{ double, %class.Vector3_t, %class.Matrix3_t, i8, [7 x i8] }>
%"struct.RigidBodyDynamics::Joint" = type { ptr, i32, i32, i32, i32 }
%"struct.RigidBodyDynamics::Math::SpatialTransform" = type { %class.Matrix3_t, %class.Vector3_t }
%"class.std::__cxx11::basic_string" = type { %"struct.std::__cxx11::basic_string<char>::_Alloc_hider", i64, %union.anon }
%"struct.std::__cxx11::basic_string<char>::_Alloc_hider" = type { ptr }
%union.anon = type { i64, [8 x i8] }
%class.my_shared_ptr.42 = type { ptr, ptr }
%class.SpatialVector_t = type { %"class.Eigen::Matrix.161" }
%"class.Eigen::Matrix.161" = type { %"class.Eigen::PlainObjectBase.162" }
%"class.Eigen::PlainObjectBase.162" = type { %"class.Eigen::DenseStorage.169" }
%"class.Eigen::DenseStorage.169" = type { %"struct.Eigen::internal::plain_array.170" }
%"struct.Eigen::internal::plain_array.170" = type { [6 x double] }
%"class.Eigen::CwiseNullaryOp" = type { [8 x i8], %"struct.Eigen::internal::scalar_constant_op" }
%"struct.Eigen::internal::scalar_constant_op" = type { double }
%"class.Eigen::Transpose" = type { ptr }
%"class.Eigen::CwiseNullaryOp.181" = type { [8 x i8], %"struct.Eigen::internal::scalar_constant_op" }

@_ZSt4cout = external global %"class.std::basic_ostream", align 8
@.str = external hidden unnamed_addr constant [20 x i8], align 1
@.str.4 = external hidden unnamed_addr constant [16 x i8], align 1
@.str.5 = external hidden unnamed_addr constant [24 x i8], align 1
@.str.6 = external hidden unnamed_addr constant [21 x i8], align 1
@.str.7 = external hidden unnamed_addr constant [17 x i8], align 1
@.str.8 = external hidden unnamed_addr constant [17 x i8], align 1
@.str.9 = external hidden unnamed_addr constant [17 x i8], align 1
@.str.10 = external hidden unnamed_addr constant [3 x i8], align 1
@.str.11 = external hidden unnamed_addr constant [8 x i8], align 1
@.str.12 = external hidden unnamed_addr constant [15 x i8], align 1
@.str.13 = external hidden unnamed_addr constant [10 x i8], align 1
@_ZSt4cerr = external global %"class.std::basic_ostream", align 8
@.str.14 = external hidden unnamed_addr constant [31 x i8], align 1
@.str.15 = external hidden unnamed_addr constant [17 x i8], align 1
@.str.16 = external hidden unnamed_addr constant [22 x i8], align 1
@.str.17 = external hidden unnamed_addr constant [36 x i8], align 1
@.str.18 = external hidden unnamed_addr constant [30 x i8], align 1
@.str.19 = external hidden unnamed_addr constant [85 x i8], align 1
@.str.20 = external hidden unnamed_addr constant [16 x i8], align 1
@.str.21 = external hidden unnamed_addr constant [16 x i8], align 1
@.str.22 = external hidden unnamed_addr constant [16 x i8], align 1
@.str.23 = external hidden unnamed_addr constant [5 x i8], align 1
@.str.24 = external hidden unnamed_addr constant [3 x i8], align 1
@.str.25 = external hidden unnamed_addr constant [11 x i8], align 1

; Function Attrs: mustprogress uwtable
define dso_local noundef zeroext i1 @_ZN17RigidBodyDynamics6Addons15construct_modelEPNS_5ModelE13my_shared_ptrIN4urdf14ModelInterfaceEEbb(ptr noundef %0, ptr noundef %1, i1 noundef zeroext %2, i1 noundef zeroext %3) local_unnamed_addr #0 personality ptr @__gxx_personality_v0 {
  %5 = alloca %class.my_shared_ptr.1, align 8
  %6 = alloca %"class.std::map", align 8
  %7 = alloca %"class.std::map.2", align 8
  %8 = alloca %"class.std::vector", align 8
  %9 = alloca %"class.std::stack", align 8
  %10 = alloca %"class.std::stack.21", align 8
  %11 = alloca %class.my_shared_ptr.28, align 8
  %12 = alloca %class.my_shared_ptr.28, align 8
  %13 = alloca %class.Vector3_t, align 8
  %14 = alloca %class.Vector3_t, align 8
  %15 = alloca %class.Matrix3_t, align 8
  %16 = alloca double, align 8
  %17 = alloca %"struct.RigidBodyDynamics::Body", align 8
  %18 = alloca %"struct.RigidBodyDynamics::Joint", align 8
  %19 = alloca %"struct.RigidBodyDynamics::Joint", align 8
  %20 = alloca %"struct.RigidBodyDynamics::Math::SpatialTransform", align 8
  %21 = alloca %"class.std::__cxx11::basic_string", align 8
  %22 = alloca i32, align 4
  %23 = alloca %class.my_shared_ptr.1, align 8
  %24 = alloca %class.my_shared_ptr.42, align 8
  %25 = alloca i32, align 4
  %26 = alloca i32, align 4
  %27 = alloca %class.my_shared_ptr.42, align 8
  %28 = alloca %class.my_shared_ptr.1, align 8
  %29 = alloca %class.my_shared_ptr.1, align 8
  %30 = alloca %"struct.RigidBodyDynamics::Joint", align 8
  %31 = alloca %class.Vector3_t, align 8
  %32 = alloca %class.Vector3_t, align 8
  %33 = alloca double, align 8
  %34 = alloca double, align 8
  %35 = alloca double, align 8
  %36 = alloca %"struct.RigidBodyDynamics::Joint", align 8
  %37 = alloca %class.Vector3_t, align 8
  %38 = alloca double, align 8
  %39 = alloca double, align 8
  %40 = alloca double, align 8
  %41 = alloca %"struct.RigidBodyDynamics::Joint", align 8
  %42 = alloca %class.Vector3_t, align 8
  %43 = alloca double, align 8
  %44 = alloca double, align 8
  %45 = alloca double, align 8
  %46 = alloca %"struct.RigidBodyDynamics::Joint", align 8
  %47 = alloca %"struct.RigidBodyDynamics::Joint", align 8
  %48 = alloca %class.Vector3_t, align 8
  %49 = alloca %"struct.RigidBodyDynamics::Joint", align 8
  %50 = alloca %"struct.RigidBodyDynamics::Joint", align 8
  %51 = alloca %"struct.RigidBodyDynamics::Joint", align 8
  %52 = alloca %class.SpatialVector_t, align 16
  %53 = alloca double, align 8
  %54 = alloca double, align 8
  %55 = alloca double, align 8
  %56 = alloca double, align 8
  %57 = alloca double, align 8
  %58 = alloca double, align 8
  %59 = alloca %class.SpatialVector_t, align 16
  %60 = alloca double, align 8
  %61 = alloca double, align 8
  %62 = alloca double, align 8
  %63 = alloca double, align 8
  %64 = alloca double, align 8
  %65 = alloca double, align 8
  %66 = alloca %class.SpatialVector_t, align 16
  %67 = alloca double, align 8
  %68 = alloca double, align 8
  %69 = alloca double, align 8
  %70 = alloca double, align 8
  %71 = alloca double, align 8
  %72 = alloca double, align 8
  %73 = alloca %class.SpatialVector_t, align 16
  %74 = alloca double, align 8
  %75 = alloca double, align 8
  %76 = alloca double, align 8
  %77 = alloca double, align 8
  %78 = alloca double, align 8
  %79 = alloca double, align 8
  %80 = alloca %class.SpatialVector_t, align 16
  %81 = alloca double, align 8
  %82 = alloca double, align 8
  %83 = alloca double, align 8
  %84 = alloca double, align 8
  %85 = alloca double, align 8
  %86 = alloca double, align 8
  %87 = alloca %class.SpatialVector_t, align 16
  %88 = alloca double, align 8
  %89 = alloca double, align 8
  %90 = alloca double, align 8
  %91 = alloca double, align 8
  %92 = alloca double, align 8
  %93 = alloca double, align 8
  %94 = alloca %class.Vector3_t, align 8
  %95 = alloca %class.Vector3_t, align 8
  %96 = alloca %"struct.RigidBodyDynamics::Math::SpatialTransform", align 8
  %97 = alloca %"struct.RigidBodyDynamics::Math::SpatialTransform", align 8
  %98 = alloca %"struct.RigidBodyDynamics::Math::SpatialTransform", align 8
  %99 = alloca %"struct.RigidBodyDynamics::Math::SpatialTransform", align 8
  %100 = alloca %class.Vector3_t, align 8
  %101 = alloca double, align 8
  %102 = alloca double, align 8
  %103 = alloca double, align 8
  %104 = alloca %"struct.RigidBodyDynamics::Math::SpatialTransform", align 8
  %105 = alloca %class.Vector3_t, align 8
  %106 = alloca double, align 8
  %107 = alloca double, align 8
  %108 = alloca double, align 8
  %109 = alloca %"struct.RigidBodyDynamics::Math::SpatialTransform", align 8
  %110 = alloca %class.Vector3_t, align 8
  %111 = alloca double, align 8
  %112 = alloca double, align 8
  %113 = alloca double, align 8
  %114 = alloca %"struct.RigidBodyDynamics::Math::SpatialTransform", align 8
  %115 = alloca %class.Vector3_t, align 8
  %116 = alloca %class.Vector3_t, align 8
  %117 = alloca %class.Vector3_t, align 8
  %118 = alloca %class.Matrix3_t, align 8
  %119 = alloca %"class.Eigen::CwiseNullaryOp", align 8
  %120 = alloca double, align 8
  %121 = alloca %class.Vector3_t, align 8
  %122 = alloca double, align 8
  %123 = alloca double, align 8
  %124 = alloca double, align 8
  %125 = alloca %"struct.RigidBodyDynamics::Body", align 8
  %126 = alloca %"class.Eigen::Transpose", align 8
  %127 = alloca %class.Matrix3_t, align 8
  %128 = alloca %"class.Eigen::CwiseNullaryOp", align 8
  %129 = alloca %"struct.RigidBodyDynamics::Body", align 8
  %130 = alloca double, align 8
  %131 = alloca %class.Vector3_t, align 8
  %132 = alloca %"class.Eigen::CwiseNullaryOp.181", align 8
  %133 = alloca %"struct.RigidBodyDynamics::Joint", align 8
  %134 = alloca %"class.std::__cxx11::basic_string", align 8
  %135 = alloca %"class.std::__cxx11::basic_string", align 8
  %136 = alloca %"struct.RigidBodyDynamics::Joint", align 8
  %137 = alloca %"struct.RigidBodyDynamics::Math::SpatialTransform", align 8
  %138 = alloca %"class.std::__cxx11::basic_string", align 8
  %139 = alloca %"class.std::__cxx11::basic_string", align 8
  call void @llvm.lifetime.start.p0(i64 16, ptr nonnull %5) #7
  call void @_ZN13my_shared_ptrIN4urdf4LinkEEC2Ev(ptr noundef nonnull align 8 dereferenceable(16) %5)
  call void @llvm.lifetime.start.p0(i64 48, ptr nonnull %6) #7
  call void @_ZNSt3mapINSt7__cxx1112basic_stringIcSt11char_traitsIcESaIcEEE13my_shared_ptrIN4urdf4LinkEESt4lessIS5_ESaISt4pairIKS5_S9_EEEC2Ev(ptr noundef nonnull align 8 dereferenceable(48) %6) #7
  %140 = call noundef ptr @_ZN13my_shared_ptrIN4urdf14ModelInterfaceEEptEv(ptr noundef nonnull align 8 dereferenceable(16) %1)
  %141 = invoke noundef nonnull align 8 dereferenceable(48) ptr @_ZNSt3mapINSt7__cxx1112basic_stringIcSt11char_traitsIcESaIcEEE13my_shared_ptrIN4urdf4LinkEESt4lessIS5_ESaISt4pairIKS5_S9_EEEaSERKSG_(ptr noundef nonnull align 8 dereferenceable(48) %6, ptr noundef nonnull align 8 dereferenceable(48) %140)
          to label %142 unwind label %260

142:                                              ; preds = %4
  call void @llvm.lifetime.start.p0(i64 48, ptr nonnull %7) #7
  call void @_ZNSt3mapINSt7__cxx1112basic_stringIcSt11char_traitsIcESaIcEEE13my_shared_ptrIN4urdf5JointEESt4lessIS5_ESaISt4pairIKS5_S9_EEEC2Ev(ptr noundef nonnull align 8 dereferenceable(48) %7) #7
  %143 = call noundef ptr @_ZN13my_shared_ptrIN4urdf14ModelInterfaceEEptEv(ptr noundef nonnull align 8 dereferenceable(16) %1)
  %144 = getelementptr inbounds i8, ptr %143, i64 48
  %145 = invoke noundef nonnull align 8 dereferenceable(48) ptr @_ZNSt3mapINSt7__cxx1112basic_stringIcSt11char_traitsIcESaIcEEE13my_shared_ptrIN4urdf5JointEESt4lessIS5_ESaISt4pairIKS5_S9_EEEaSERKSG_(ptr noundef nonnull align 8 dereferenceable(48) %7, ptr noundef nonnull align 8 dereferenceable(48) %144)
          to label %146 unwind label %262

146:                                              ; preds = %142
  call void @llvm.lifetime.start.p0(i64 24, ptr nonnull %8) #7
  call void @_ZNSt6vectorINSt7__cxx1112basic_stringIcSt11char_traitsIcESaIcEEESaIS5_EEC2Ev(ptr noundef nonnull align 8 dereferenceable(24) %8) #7
  call void @llvm.lifetime.start.p0(i64 80, ptr nonnull %9) #7
  invoke void @_ZNSt5stackI13my_shared_ptrIN4urdf4LinkEESt5dequeIS3_SaIS3_EEEC2IS6_vEEv(ptr noundef nonnull align 8 dereferenceable(80) %9)
          to label %147 unwind label %264

147:                                              ; preds = %146
  call void @llvm.lifetime.start.p0(i64 80, ptr nonnull %10) #7
  invoke void @_ZNSt5stackIiSt5dequeIiSaIiEEEC2IS2_vEEv(ptr noundef nonnull align 8 dereferenceable(80) %10)
          to label %148 unwind label %266

148:                                              ; preds = %147
  call void @llvm.lifetime.start.p0(i64 16, ptr nonnull %11) #7
  %149 = call noundef ptr @_ZN13my_shared_ptrIN4urdf14ModelInterfaceEEptEv(ptr noundef nonnull align 8 dereferenceable(16) %1)
  invoke void @_ZNK4urdf14ModelInterface7getRootEv(ptr dead_on_unwind nonnull writable sret(%class.my_shared_ptr.28) align 8 %11, ptr noundef nonnull align 8 dereferenceable(200) %149)
          to label %150 unwind label %268

150:                                              ; preds = %148
  %151 = call noundef ptr @_ZN13my_shared_ptrIKN4urdf4LinkEEptEv(ptr noundef nonnull align 8 dereferenceable(16) %11)
  %152 = invoke noundef nonnull align 8 dereferenceable(16) ptr @_ZNSt3mapINSt7__cxx1112basic_stringIcSt11char_traitsIcESaIcEEE13my_shared_ptrIN4urdf4LinkEESt4lessIS5_ESaISt4pairIKS5_S9_EEEixERSD_(ptr noundef nonnull align 8 dereferenceable(48) %6, ptr noundef nonnull align 8 dereferenceable(32) %151)
          to label %153 unwind label %270

153:                                              ; preds = %150
  invoke void @_ZNSt5stackI13my_shared_ptrIN4urdf4LinkEESt5dequeIS3_SaIS3_EEE4pushERKS3_(ptr noundef nonnull align 8 dereferenceable(80) %9, ptr noundef nonnull align 8 dereferenceable(16) %152)
          to label %154 unwind label %270

154:                                              ; preds = %153
  call void @_ZN13my_shared_ptrIKN4urdf4LinkEED2Ev(ptr noundef nonnull align 8 dereferenceable(16) %11) #7
  call void @llvm.lifetime.end.p0(i64 16, ptr nonnull %11) #7
  call void @llvm.lifetime.start.p0(i64 16, ptr nonnull %12) #7
  %155 = call noundef ptr @_ZN13my_shared_ptrIN4urdf14ModelInterfaceEEptEv(ptr noundef nonnull align 8 dereferenceable(16) %1)
  invoke void @_ZNK4urdf14ModelInterface7getRootEv(ptr dead_on_unwind nonnull writable sret(%class.my_shared_ptr.28) align 8 %12, ptr noundef nonnull align 8 dereferenceable(200) %155)
          to label %156 unwind label %273

156:                                              ; preds = %154
  call void @llvm.lifetime.start.p0(i64 24, ptr nonnull %13) #7
  invoke void @_ZN9Vector3_tC2Ev(ptr noundef nonnull align 8 dereferenceable(24) %13)
          to label %157 unwind label %275

157:                                              ; preds = %156
  call void @llvm.lifetime.start.p0(i64 24, ptr nonnull %14) #7
  invoke void @_ZN9Vector3_tC2Ev(ptr noundef nonnull align 8 dereferenceable(24) %14)
          to label %158 unwind label %277

158:                                              ; preds = %157
  call void @llvm.lifetime.start.p0(i64 72, ptr nonnull %15) #7
  invoke void @_ZN9Matrix3_tC2Ev(ptr noundef nonnull align 8 dereferenceable(72) %15)
          to label %159 unwind label %279

159:                                              ; preds = %158
  call void @llvm.lifetime.start.p0(i64 8, ptr nonnull %16) #7
  %160 = call noundef ptr @_ZNK13my_shared_ptrIKN4urdf4LinkEEptEv(ptr noundef nonnull align 8 dereferenceable(16) %12)
  %161 = getelementptr inbounds i8, ptr %160, i64 32
  %162 = call noundef zeroext i1 @_ZNK13my_shared_ptrIN4urdf8InertialEEcvbEv(ptr noundef nonnull align 8 dereferenceable(16) %161)
  br i1 %162, label %163, label %343

163:                                              ; preds = %159
  %164 = call noundef ptr @_ZNK13my_shared_ptrIKN4urdf4LinkEEptEv(ptr noundef nonnull align 8 dereferenceable(16) %12)
  %165 = getelementptr inbounds i8, ptr %164, i64 32
  %166 = call noundef ptr @_ZNK13my_shared_ptrIN4urdf8InertialEEptEv(ptr noundef nonnull align 8 dereferenceable(16) %165)
  %167 = getelementptr inbounds i8, ptr %166, i64 56
  %168 = load double, ptr %167, align 8, !tbaa !5
  store double %168, ptr %16, align 8, !tbaa !13
  %169 = call noundef ptr @_ZNK13my_shared_ptrIKN4urdf4LinkEEptEv(ptr noundef nonnull align 8 dereferenceable(16) %12)
  %170 = getelementptr inbounds i8, ptr %169, i64 32
  %171 = call noundef ptr @_ZNK13my_shared_ptrIN4urdf8InertialEEptEv(ptr noundef nonnull align 8 dereferenceable(16) %170)
  %172 = call noundef ptr @_ZNK13my_shared_ptrIKN4urdf4LinkEEptEv(ptr noundef nonnull align 8 dereferenceable(16) %12)
  %173 = getelementptr inbounds i8, ptr %172, i64 32
  %174 = call noundef ptr @_ZNK13my_shared_ptrIN4urdf8InertialEEptEv(ptr noundef nonnull align 8 dereferenceable(16) %173)
  %175 = getelementptr inbounds i8, ptr %174, i64 8
  %176 = call noundef ptr @_ZNK13my_shared_ptrIKN4urdf4LinkEEptEv(ptr noundef nonnull align 8 dereferenceable(16) %12)
  %177 = getelementptr inbounds i8, ptr %176, i64 32
  %178 = call noundef ptr @_ZNK13my_shared_ptrIN4urdf8InertialEEptEv(ptr noundef nonnull align 8 dereferenceable(16) %177)
  %179 = getelementptr inbounds i8, ptr %178, i64 16
  invoke void @_ZN9Vector3_t3setERKdS1_S1_(ptr noundef nonnull align 8 dereferenceable(24) %14, ptr noundef nonnull align 8 dereferenceable(8) %171, ptr noundef nonnull align 8 dereferenceable(8) %175, ptr noundef nonnull align 8 dereferenceable(8) %179)
          to label %180 unwind label %281

180:                                              ; preds = %163
  %181 = call noundef ptr @_ZNK13my_shared_ptrIKN4urdf4LinkEEptEv(ptr noundef nonnull align 8 dereferenceable(16) %12)
  %182 = getelementptr inbounds i8, ptr %181, i64 32
  %183 = call noundef ptr @_ZNK13my_shared_ptrIN4urdf8InertialEEptEv(ptr noundef nonnull align 8 dereferenceable(16) %182)
  %184 = getelementptr inbounds i8, ptr %183, i64 64
  %185 = load double, ptr %184, align 8, !tbaa !14
  %186 = invoke noundef nonnull align 8 dereferenceable(8) ptr @_ZN5Eigen15DenseCoeffsBaseINS_6MatrixIdLi3ELi3ELi0ELi3ELi3EEELi1EEclEll(ptr noundef nonnull align 1 dereferenceable(1) %15, i64 noundef 0, i64 noundef 0)
          to label %187 unwind label %281

187:                                              ; preds = %180
  store double %185, ptr %186, align 8, !tbaa !13
  %188 = call noundef ptr @_ZNK13my_shared_ptrIKN4urdf4LinkEEptEv(ptr noundef nonnull align 8 dereferenceable(16) %12)
  %189 = getelementptr inbounds i8, ptr %188, i64 32
  %190 = call noundef ptr @_ZNK13my_shared_ptrIN4urdf8InertialEEptEv(ptr noundef nonnull align 8 dereferenceable(16) %189)
  %191 = getelementptr inbounds i8, ptr %190, i64 72
  %192 = load double, ptr %191, align 8, !tbaa !15
  %193 = invoke noundef nonnull align 8 dereferenceable(8) ptr @_ZN5Eigen15DenseCoeffsBaseINS_6MatrixIdLi3ELi3ELi0ELi3ELi3EEELi1EEclEll(ptr noundef nonnull align 1 dereferenceable(1) %15, i64 noundef 0, i64 noundef 1)
          to label %194 unwind label %281

194:                                              ; preds = %187
  store double %192, ptr %193, align 8, !tbaa !13
  %195 = call noundef ptr @_ZNK13my_shared_ptrIKN4urdf4LinkEEptEv(ptr noundef nonnull align 8 dereferenceable(16) %12)
  %196 = getelementptr inbounds i8, ptr %195, i64 32
  %197 = call noundef ptr @_ZNK13my_shared_ptrIN4urdf8InertialEEptEv(ptr noundef nonnull align 8 dereferenceable(16) %196)
  %198 = getelementptr inbounds i8, ptr %197, i64 80
  %199 = load double, ptr %198, align 8, !tbaa !16
  %200 = invoke noundef nonnull align 8 dereferenceable(8) ptr @_ZN5Eigen15DenseCoeffsBaseINS_6MatrixIdLi3ELi3ELi0ELi3ELi3EEELi1EEclEll(ptr noundef nonnull align 1 dereferenceable(1) %15, i64 noundef 0, i64 noundef 2)
          to label %201 unwind label %281

201:                                              ; preds = %194
  store double %199, ptr %200, align 8, !tbaa !13
  %202 = call noundef ptr @_ZNK13my_shared_ptrIKN4urdf4LinkEEptEv(ptr noundef nonnull align 8 dereferenceable(16) %12)
  %203 = getelementptr inbounds i8, ptr %202, i64 32
  %204 = call noundef ptr @_ZNK13my_shared_ptrIN4urdf8InertialEEptEv(ptr noundef nonnull align 8 dereferenceable(16) %203)
  %205 = getelementptr inbounds i8, ptr %204, i64 72
  %206 = load double, ptr %205, align 8, !tbaa !15
  %207 = invoke noundef nonnull align 8 dereferenceable(8) ptr @_ZN5Eigen15DenseCoeffsBaseINS_6MatrixIdLi3ELi3ELi0ELi3ELi3EEELi1EEclEll(ptr noundef nonnull align 1 dereferenceable(1) %15, i64 noundef 1, i64 noundef 0)
          to label %208 unwind label %281

208:                                              ; preds = %201
  store double %206, ptr %207, align 8, !tbaa !13
  %209 = call noundef ptr @_ZNK13my_shared_ptrIKN4urdf4LinkEEptEv(ptr noundef nonnull align 8 dereferenceable(16) %12)
  %210 = getelementptr inbounds i8, ptr %209, i64 32
  %211 = call noundef ptr @_ZNK13my_shared_ptrIN4urdf8InertialEEptEv(ptr noundef nonnull align 8 dereferenceable(16) %210)
  %212 = getelementptr inbounds i8, ptr %211, i64 88
  %213 = load double, ptr %212, align 8, !tbaa !17
  %214 = invoke noundef nonnull align 8 dereferenceable(8) ptr @_ZN5Eigen15DenseCoeffsBaseINS_6MatrixIdLi3ELi3ELi0ELi3ELi3EEELi1EEclEll(ptr noundef nonnull align 1 dereferenceable(1) %15, i64 noundef 1, i64 noundef 1)
          to label %215 unwind label %281

215:                                              ; preds = %208
  store double %213, ptr %214, align 8, !tbaa !13
  %216 = call noundef ptr @_ZNK13my_shared_ptrIKN4urdf4LinkEEptEv(ptr noundef nonnull align 8 dereferenceable(16) %12)
  %217 = getelementptr inbounds i8, ptr %216, i64 32
  %218 = call noundef ptr @_ZNK13my_shared_ptrIN4urdf8InertialEEptEv(ptr noundef nonnull align 8 dereferenceable(16) %217)
  %219 = getelementptr inbounds i8, ptr %218, i64 96
  %220 = load double, ptr %219, align 8, !tbaa !18
  %221 = invoke noundef nonnull align 8 dereferenceable(8) ptr @_ZN5Eigen15DenseCoeffsBaseINS_6MatrixIdLi3ELi3ELi0ELi3ELi3EEELi1EEclEll(ptr noundef nonnull align 1 dereferenceable(1) %15, i64 noundef 1, i64 noundef 2)
          to label %222 unwind label %281

222:                                              ; preds = %215
  store double %220, ptr %221, align 8, !tbaa !13
  %223 = call noundef ptr @_ZNK13my_shared_ptrIKN4urdf4LinkEEptEv(ptr noundef nonnull align 8 dereferenceable(16) %12)
  %224 = getelementptr inbounds i8, ptr %223, i64 32
  %225 = call noundef ptr @_ZNK13my_shared_ptrIN4urdf8InertialEEptEv(ptr noundef nonnull align 8 dereferenceable(16) %224)
  %226 = getelementptr inbounds i8, ptr %225, i64 80
  %227 = load double, ptr %226, align 8, !tbaa !16
  %228 = invoke noundef nonnull align 8 dereferenceable(8) ptr @_ZN5Eigen15DenseCoeffsBaseINS_6MatrixIdLi3ELi3ELi0ELi3ELi3EEELi1EEclEll(ptr noundef nonnull align 1 dereferenceable(1) %15, i64 noundef 2, i64 noundef 0)
          to label %229 unwind label %281

229:                                              ; preds = %222
  store double %227, ptr %228, align 8, !tbaa !13
  %230 = call noundef ptr @_ZNK13my_shared_ptrIKN4urdf4LinkEEptEv(ptr noundef nonnull align 8 dereferenceable(16) %12)
  %231 = getelementptr inbounds i8, ptr %230, i64 32
  %232 = call noundef ptr @_ZNK13my_shared_ptrIN4urdf8InertialEEptEv(ptr noundef nonnull align 8 dereferenceable(16) %231)
  %233 = getelementptr inbounds i8, ptr %232, i64 96
  %234 = load double, ptr %233, align 8, !tbaa !18
  %235 = invoke noundef nonnull align 8 dereferenceable(8) ptr @_ZN5Eigen15DenseCoeffsBaseINS_6MatrixIdLi3ELi3ELi0ELi3ELi3EEELi1EEclEll(ptr noundef nonnull align 1 dereferenceable(1) %15, i64 noundef 2, i64 noundef 1)
          to label %236 unwind label %281

236:                                              ; preds = %229
  store double %234, ptr %235, align 8, !tbaa !13
  %237 = call noundef ptr @_ZNK13my_shared_ptrIKN4urdf4LinkEEptEv(ptr noundef nonnull align 8 dereferenceable(16) %12)
  %238 = getelementptr inbounds i8, ptr %237, i64 32
  %239 = call noundef ptr @_ZNK13my_shared_ptrIN4urdf8InertialEEptEv(ptr noundef nonnull align 8 dereferenceable(16) %238)
  %240 = getelementptr inbounds i8, ptr %239, i64 104
  %241 = load double, ptr %240, align 8, !tbaa !19
  %242 = invoke noundef nonnull align 8 dereferenceable(8) ptr @_ZN5Eigen15DenseCoeffsBaseINS_6MatrixIdLi3ELi3ELi0ELi3ELi3EEELi1EEclEll(ptr noundef nonnull align 1 dereferenceable(1) %15, i64 noundef 2, i64 noundef 2)
          to label %243 unwind label %281

243:                                              ; preds = %236
  store double %241, ptr %242, align 8, !tbaa !13
  %244 = call noundef ptr @_ZNK13my_shared_ptrIKN4urdf4LinkEEptEv(ptr noundef nonnull align 8 dereferenceable(16) %12)
  %245 = getelementptr inbounds i8, ptr %244, i64 32
  %246 = call noundef ptr @_ZNK13my_shared_ptrIN4urdf8InertialEEptEv(ptr noundef nonnull align 8 dereferenceable(16) %245)
  %247 = getelementptr inbounds i8, ptr %246, i64 24
  %248 = invoke noundef nonnull align 8 dereferenceable(8) ptr @_ZN5Eigen15DenseCoeffsBaseINS_6MatrixIdLi3ELi1ELi0ELi3ELi1EEELi1EEixEl(ptr noundef nonnull align 1 dereferenceable(1) %13, i64 noundef 0)
          to label %249 unwind label %281

249:                                              ; preds = %243
  %250 = invoke noundef nonnull align 8 dereferenceable(8) ptr @_ZN5Eigen15DenseCoeffsBaseINS_6MatrixIdLi3ELi1ELi0ELi3ELi1EEELi1EEixEl(ptr noundef nonnull align 1 dereferenceable(1) %13, i64 noundef 1)
          to label %251 unwind label %281

251:                                              ; preds = %249
  %252 = invoke noundef nonnull align 8 dereferenceable(8) ptr @_ZN5Eigen15DenseCoeffsBaseINS_6MatrixIdLi3ELi1ELi0ELi3ELi1EEELi1EEixEl(ptr noundef nonnull align 1 dereferenceable(1) %13, i64 noundef 2)
          to label %253 unwind label %281

253:                                              ; preds = %251
  call void @_ZNK4urdf8Rotation6getRPYERdS1_S1_(ptr noundef nonnull align 8 dereferenceable(32) %247, ptr noundef nonnull align 8 dereferenceable(8) %248, ptr noundef nonnull align 8 dereferenceable(8) %250, ptr noundef nonnull align 8 dereferenceable(8) %252)
  call void @llvm.lifetime.start.p0(i64 112, ptr nonnull %17) #7
  invoke void @_ZN17RigidBodyDynamics4BodyC2ERKdRK9Vector3_tRK9Matrix3_t(ptr noundef nonnull align 8 dereferenceable(105) %17, ptr noundef nonnull align 8 dereferenceable(8) %16, ptr noundef nonnull align 8 dereferenceable(24) %14, ptr noundef nonnull align 8 dereferenceable(72) %15)
          to label %254 unwind label %283

254:                                              ; preds = %253
  call void @llvm.lifetime.start.p0(i64 24, ptr nonnull %18) #7
  invoke void @_ZN17RigidBodyDynamics5JointC2ENS_9JointTypeE(ptr noundef nonnull align 8 dereferenceable(24) %18, i32 noundef 13)
          to label %255 unwind label %285

255:                                              ; preds = %254
  br i1 %2, label %256, label %292

256:                                              ; preds = %255
  call void @llvm.lifetime.start.p0(i64 24, ptr nonnull %19) #7
  invoke void @_ZN17RigidBodyDynamics5JointC2ENS_9JointTypeE(ptr noundef nonnull align 8 dereferenceable(24) %19, i32 noundef 12)
          to label %257 unwind label %287

257:                                              ; preds = %256
  %258 = invoke noundef nonnull align 8 dereferenceable(24) ptr @_ZN17RigidBodyDynamics5JointaSERKS0_(ptr noundef nonnull align 8 dereferenceable(24) %18, ptr noundef nonnull align 8 dereferenceable(24) %19)
          to label %259 unwind label %289

259:                                              ; preds = %257
  call void @_ZN17RigidBodyDynamics5JointD2Ev(ptr noundef nonnull align 8 dereferenceable(24) %19) #7
  call void @llvm.lifetime.end.p0(i64 24, ptr nonnull %19) #7
  br label %292

260:                                              ; preds = %4
  %261 = landingpad { ptr, i32 }
          cleanup
  br label %975

262:                                              ; preds = %142
  %263 = landingpad { ptr, i32 }
          cleanup
  br label %974

264:                                              ; preds = %146
  %265 = landingpad { ptr, i32 }
          cleanup
  br label %973

266:                                              ; preds = %147
  %267 = landingpad { ptr, i32 }
          cleanup
  br label %972

268:                                              ; preds = %148
  %269 = landingpad { ptr, i32 }
          cleanup
  br label %272

270:                                              ; preds = %153, %150
  %271 = landingpad { ptr, i32 }
          cleanup
  call void @_ZN13my_shared_ptrIKN4urdf4LinkEED2Ev(ptr noundef nonnull align 8 dereferenceable(16) %11) #7
  br label %272

272:                                              ; preds = %270, %268
  %.pn = phi { ptr, i32 } [ %271, %270 ], [ %269, %268 ]
  call void @llvm.lifetime.end.p0(i64 16, ptr nonnull %11) #7
  br label %971

273:                                              ; preds = %154
  %274 = landingpad { ptr, i32 }
          cleanup
  br label %971

275:                                              ; preds = %156
  %276 = landingpad { ptr, i32 }
          cleanup
  br label %970

277:                                              ; preds = %157
  %278 = landingpad { ptr, i32 }
          cleanup
  br label %969

279:                                              ; preds = %158
  %280 = landingpad { ptr, i32 }
          cleanup
  br label %968

281:                                              ; preds = %251, %249, %243, %236, %229, %222, %215, %208, %201, %194, %187, %180, %163
  %282 = landingpad { ptr, i32 }
          cleanup
  br label %967

283:                                              ; preds = %253
  %284 = landingpad { ptr, i32 }
          cleanup
  br label %342

285:                                              ; preds = %254
  %286 = landingpad { ptr, i32 }
          cleanup
  br label %341

287:                                              ; preds = %256
  %288 = landingpad { ptr, i32 }
          cleanup
  br label %291

289:                                              ; preds = %257
  %290 = landingpad { ptr, i32 }
          cleanup
  call void @_ZN17RigidBodyDynamics5JointD2Ev(ptr noundef nonnull align 8 dereferenceable(24) %19) #7
  br label %291

291:                                              ; preds = %289, %287
  %.pn117 = phi { ptr, i32 } [ %290, %289 ], [ %288, %287 ]
  call void @llvm.lifetime.end.p0(i64 24, ptr nonnull %19) #7
  br label %340

292:                                              ; preds = %259, %255
  call void @llvm.lifetime.start.p0(i64 96, ptr nonnull %20) #7
  invoke void @_ZN17RigidBodyDynamics4Math16SpatialTransformC2Ev(ptr noundef nonnull align 8 dereferenceable(96) %20)
          to label %293 unwind label %306

293:                                              ; preds = %292
  br i1 %3, label %294, label %332

294:                                              ; preds = %293
  %295 = invoke noundef nonnull align 8 dereferenceable(8) ptr @_ZStlsISt11char_traitsIcEERSt13basic_ostreamIcT_ES5_PKc(ptr noundef nonnull align 8 dereferenceable(8) @_ZSt4cout, ptr noundef nonnull @.str)
          to label %296 unwind label %306

296:                                              ; preds = %294
  %297 = invoke noundef nonnull align 8 dereferenceable(8) ptr @_ZNSolsEPFRSoS_E(ptr noundef nonnull align 8 dereferenceable(8) %295, ptr noundef nonnull @_ZSt4endlIcSt11char_traitsIcEERSt13basic_ostreamIT_T0_ES6_)
          to label %298 unwind label %306

298:                                              ; preds = %296
  %299 = invoke noundef nonnull align 8 dereferenceable(8) ptr @_ZStlsISt11char_traitsIcEERSt13basic_ostreamIcT_ES5_PKc(ptr noundef nonnull align 8 dereferenceable(8) @_ZSt4cout, ptr noundef nonnull @.str.4)
          to label %300 unwind label %306

300:                                              ; preds = %298
  %301 = invoke noundef nonnull align 8 dereferenceable(8) ptr @_ZN17RigidBodyDynamics4MathlsERSoRKNS0_16SpatialTransformE(ptr noundef nonnull align 8 dereferenceable(8) %299, ptr noundef nonnull align 8 dereferenceable(96) %20)
          to label %302 unwind label %306

302:                                              ; preds = %300
  %303 = invoke noundef nonnull align 8 dereferenceable(8) ptr @_ZNSolsEPFRSoS_E(ptr noundef nonnull align 8 dereferenceable(8) %301, ptr noundef nonnull @_ZSt4endlIcSt11char_traitsIcEERSt13basic_ostreamIT_T0_ES6_)
          to label %.invoke263 unwind label %306

.invoke263:                                       ; preds = %302
  %304 = select i1 %2, ptr @.str.5, ptr @.str.6
  %305 = invoke noundef nonnull align 8 dereferenceable(8) ptr @_ZStlsISt11char_traitsIcEERSt13basic_ostreamIcT_ES5_PKc(ptr noundef nonnull align 8 dereferenceable(8) @_ZSt4cout, ptr noundef nonnull %304)
          to label %.invoke unwind label %306

306:                                              ; preds = %.invoke263, %.invoke, %332, %330, %327, %325, %323, %320, %318, %316, %313, %311, %309, %302, %300, %298, %296, %294, %292
  %307 = landingpad { ptr, i32 }
          cleanup
  br label %339

.invoke:                                          ; preds = %.invoke263
  %308 = invoke noundef nonnull align 8 dereferenceable(8) ptr @_ZNSolsEPFRSoS_E(ptr noundef nonnull align 8 dereferenceable(8) %305, ptr noundef nonnull @_ZSt4endlIcSt11char_traitsIcEERSt13basic_ostreamIT_T0_ES6_)
          to label %309 unwind label %306

309:                                              ; preds = %.invoke
  %310 = invoke noundef nonnull align 8 dereferenceable(8) ptr @_ZStlsISt11char_traitsIcEERSt13basic_ostreamIcT_ES5_PKc(ptr noundef nonnull align 8 dereferenceable(8) @_ZSt4cout, ptr noundef nonnull @.str.7)
          to label %311 unwind label %306

311:                                              ; preds = %309
  %312 = invoke noundef nonnull align 8 dereferenceable(8) ptr @_ZNSolsEPFRSoS_E(ptr noundef nonnull align 8 dereferenceable(8) %310, ptr noundef nonnull @_ZSt4endlIcSt11char_traitsIcEERSt13basic_ostreamIT_T0_ES6_)
          to label %313 unwind label %306

313:                                              ; preds = %311
  %314 = getelementptr inbounds i8, ptr %17, i64 32
  %315 = invoke noundef nonnull align 8 dereferenceable(8) ptr @_ZN5EigenlsINS_6MatrixIdLi3ELi3ELi0ELi3ELi3EEEEERSoS3_RKNS_9DenseBaseIT_EE(ptr noundef nonnull align 8 dereferenceable(8) %312, ptr noundef nonnull align 1 dereferenceable(1) %314)
          to label %316 unwind label %306

316:                                              ; preds = %313
  %317 = invoke noundef nonnull align 8 dereferenceable(8) ptr @_ZNSolsEPFRSoS_E(ptr noundef nonnull align 8 dereferenceable(8) %315, ptr noundef nonnull @_ZSt4endlIcSt11char_traitsIcEERSt13basic_ostreamIT_T0_ES6_)
          to label %318 unwind label %306

318:                                              ; preds = %316
  %319 = invoke noundef nonnull align 8 dereferenceable(8) ptr @_ZStlsISt11char_traitsIcEERSt13basic_ostreamIcT_ES5_PKc(ptr noundef nonnull align 8 dereferenceable(8) @_ZSt4cout, ptr noundef nonnull @.str.8)
          to label %320 unwind label %306

320:                                              ; preds = %318
  %321 = load double, ptr %17, align 8, !tbaa !20
  %322 = invoke noundef nonnull align 8 dereferenceable(8) ptr @_ZNSolsEd(ptr noundef nonnull align 8 dereferenceable(8) %319, double noundef %321)
          to label %323 unwind label %306

323:                                              ; preds = %320
  %324 = invoke noundef nonnull align 8 dereferenceable(8) ptr @_ZNSolsEPFRSoS_E(ptr noundef nonnull align 8 dereferenceable(8) %322, ptr noundef nonnull @_ZSt4endlIcSt11char_traitsIcEERSt13basic_ostreamIT_T0_ES6_)
          to label %325 unwind label %306

325:                                              ; preds = %323
  %326 = invoke noundef nonnull align 8 dereferenceable(8) ptr @_ZStlsISt11char_traitsIcEERSt13basic_ostreamIcT_ES5_PKc(ptr noundef nonnull align 8 dereferenceable(8) @_ZSt4cout, ptr noundef nonnull @.str.9)
          to label %327 unwind label %306

327:                                              ; preds = %325
  %328 = call noundef ptr @_ZNK13my_shared_ptrIKN4urdf4LinkEEptEv(ptr noundef nonnull align 8 dereferenceable(16) %12)
  %329 = invoke noundef nonnull align 8 dereferenceable(8) ptr @_ZStlsIcSt11char_traitsIcESaIcEERSt13basic_ostreamIT_T0_ES7_RKNSt7__cxx1112basic_stringIS4_S5_T1_EE(ptr noundef nonnull align 8 dereferenceable(8) %326, ptr noundef nonnull align 8 dereferenceable(32) %328)
          to label %330 unwind label %306

330:                                              ; preds = %327
  %331 = invoke noundef nonnull align 8 dereferenceable(8) ptr @_ZNSolsEPFRSoS_E(ptr noundef nonnull align 8 dereferenceable(8) %329, ptr noundef nonnull @_ZSt4endlIcSt11char_traitsIcEERSt13basic_ostreamIT_T0_ES6_)
          to label %332 unwind label %306

332:                                              ; preds = %330, %293
  %333 = call noundef ptr @_ZNK13my_shared_ptrIKN4urdf4LinkEEptEv(ptr noundef nonnull align 8 dereferenceable(16) %12)
  invoke void @_ZNSt7__cxx1112basic_stringIcSt11char_traitsIcESaIcEEC2ERKS4_(ptr noundef nonnull align 8 dereferenceable(32) %21, ptr noundef nonnull align 8 dereferenceable(32) %333)
          to label %334 unwind label %306

334:                                              ; preds = %332
  %335 = invoke noundef i32 @_ZN17RigidBodyDynamics5Model10AppendBodyERKNS_4Math16SpatialTransformERKNS_5JointERKNS_4BodyENSt7__cxx1112basic_stringIcSt11char_traitsIcESaIcEEE(ptr noundef nonnull align 8 dereferenceable(872) %0, ptr noundef nonnull align 8 dereferenceable(96) %20, ptr noundef nonnull align 8 dereferenceable(24) %18, ptr noundef nonnull align 8 dereferenceable(105) %17, ptr noundef nonnull %21)
          to label %336 unwind label %337

336:                                              ; preds = %334
  call void @_ZNSt7__cxx1112basic_stringIcSt11char_traitsIcESaIcEED2Ev(ptr noundef nonnull align 8 dereferenceable(32) %21) #7
  call void @llvm.lifetime.end.p0(i64 96, ptr nonnull %20) #7
  call void @_ZN17RigidBodyDynamics5JointD2Ev(ptr noundef nonnull align 8 dereferenceable(24) %18) #7
  call void @llvm.lifetime.end.p0(i64 24, ptr nonnull %18) #7
  call void @_ZN17RigidBodyDynamics4BodyD2Ev(ptr noundef nonnull align 8 dereferenceable(105) %17) #7
  call void @llvm.lifetime.end.p0(i64 112, ptr nonnull %17) #7
  br label %343

337:                                              ; preds = %334
  %338 = landingpad { ptr, i32 }
          cleanup
  call void @_ZNSt7__cxx1112basic_stringIcSt11char_traitsIcESaIcEED2Ev(ptr noundef nonnull align 8 dereferenceable(32) %21) #7
  br label %339

339:                                              ; preds = %337, %306
  %.pn119 = phi { ptr, i32 } [ %338, %337 ], [ %307, %306 ]
  call void @llvm.lifetime.end.p0(i64 96, ptr nonnull %20) #7
  br label %340

340:                                              ; preds = %339, %291
  %.pn119.pn = phi { ptr, i32 } [ %.pn119, %339 ], [ %.pn117, %291 ]
  call void @_ZN17RigidBodyDynamics5JointD2Ev(ptr noundef nonnull align 8 dereferenceable(24) %18) #7
  br label %341

341:                                              ; preds = %340, %285
  %.pn119.pn.pn = phi { ptr, i32 } [ %.pn119.pn, %340 ], [ %286, %285 ]
  call void @llvm.lifetime.end.p0(i64 24, ptr nonnull %18) #7
  call void @_ZN17RigidBodyDynamics4BodyD2Ev(ptr noundef nonnull align 8 dereferenceable(105) %17) #7
  br label %342

342:                                              ; preds = %341, %283
  %.pn119.pn.pn.pn = phi { ptr, i32 } [ %.pn119.pn.pn, %341 ], [ %284, %283 ]
  call void @llvm.lifetime.end.p0(i64 112, ptr nonnull %17) #7
  br label %967

343:                                              ; preds = %336, %159
  call void @llvm.lifetime.start.p0(i64 4, ptr nonnull %22) #7
  store i32 0, ptr %22, align 4, !tbaa !33
  invoke void @_ZNSt5stackIiSt5dequeIiSaIiEEE4pushEOi(ptr noundef nonnull align 8 dereferenceable(80) %10, ptr noundef nonnull align 4 dereferenceable(4) %22)
          to label %344 unwind label %374

344:                                              ; preds = %343
  call void @llvm.lifetime.end.p0(i64 4, ptr nonnull %22) #7
  %345 = call noundef i64 @_ZNKSt5stackI13my_shared_ptrIN4urdf4LinkEESt5dequeIS3_SaIS3_EEE4sizeEv(ptr noundef nonnull align 8 dereferenceable(80) %9)
  %.not249 = icmp eq i64 %345, 0
  br i1 %.not249, label %.preheader189, label %.lr.ph251

.preheader189:                                    ; preds = %414, %344
  %346 = call noundef i64 @_ZNKSt6vectorINSt7__cxx1112basic_stringIcSt11char_traitsIcESaIcEEESaIS5_EE4sizeEv(ptr noundef nonnull align 8 dereferenceable(24) %8) #7
  %347 = icmp eq i64 %346, 0
  br i1 %347, label %._crit_edge257, label %.lr.ph256

.lr.ph256:                                        ; preds = %.preheader189
  %348 = getelementptr inbounds i8, ptr %30, i64 12
  %349 = getelementptr inbounds i8, ptr %125, i64 32
  br label %420

.lr.ph251:                                        ; preds = %344, %414
  call void @llvm.lifetime.start.p0(i64 16, ptr nonnull %23) #7
  %350 = call noundef nonnull align 8 dereferenceable(16) ptr @_ZNSt5stackI13my_shared_ptrIN4urdf4LinkEESt5dequeIS3_SaIS3_EEE3topEv(ptr noundef nonnull align 8 dereferenceable(80) %9)
  call void @_ZN13my_shared_ptrIN4urdf4LinkEEC2ERKS2_(ptr noundef nonnull align 8 dereferenceable(16) %23, ptr noundef nonnull align 8 dereferenceable(16) %350)
  %351 = call noundef nonnull align 4 dereferenceable(4) ptr @_ZNSt5stackIiSt5dequeIiSaIiEEE3topEv(ptr noundef nonnull align 8 dereferenceable(80) %10)
  %352 = load i32, ptr %351, align 4, !tbaa !33
  %353 = zext i32 %352 to i64
  %354 = call noundef ptr @_ZN13my_shared_ptrIN4urdf4LinkEEptEv(ptr noundef nonnull align 8 dereferenceable(16) %23)
  %355 = getelementptr inbounds i8, ptr %354, i64 144
  %356 = call noundef i64 @_ZNKSt6vectorI13my_shared_ptrIN4urdf5JointEESaIS3_EE4sizeEv(ptr noundef nonnull align 8 dereferenceable(24) %355) #7
  %357 = icmp ugt i64 %356, %353
  br i1 %357, label %358, label %413

358:                                              ; preds = %.lr.ph251
  call void @llvm.lifetime.start.p0(i64 16, ptr nonnull %24) #7
  %359 = call noundef ptr @_ZN13my_shared_ptrIN4urdf4LinkEEptEv(ptr noundef nonnull align 8 dereferenceable(16) %23)
  %360 = getelementptr inbounds i8, ptr %359, i64 144
  %361 = call noundef nonnull align 8 dereferenceable(16) ptr @_ZNSt6vectorI13my_shared_ptrIN4urdf5JointEESaIS3_EEixEm(ptr noundef nonnull align 8 dereferenceable(24) %360, i64 noundef %353) #7
  call void @_ZN13my_shared_ptrIN4urdf5JointEEC2ERKS2_(ptr noundef nonnull align 8 dereferenceable(16) %24, ptr noundef nonnull align 8 dereferenceable(16) %361)
  call void @_ZNSt5stackIiSt5dequeIiSaIiEEE3popEv(ptr noundef nonnull align 8 dereferenceable(80) %10)
  call void @llvm.lifetime.start.p0(i64 4, ptr nonnull %25) #7
  %362 = add i32 %352, 1
  store i32 %362, ptr %25, align 4, !tbaa !33
  invoke void @_ZNSt5stackIiSt5dequeIiSaIiEEE4pushEOi(ptr noundef nonnull align 8 dereferenceable(80) %10, ptr noundef nonnull align 4 dereferenceable(4) %25)
          to label %363 unwind label %378

363:                                              ; preds = %358
  call void @llvm.lifetime.end.p0(i64 4, ptr nonnull %25) #7
  %364 = call noundef ptr @_ZN13my_shared_ptrIN4urdf5JointEEptEv(ptr noundef nonnull align 8 dereferenceable(16) %24)
  %365 = getelementptr inbounds i8, ptr %364, i64 64
  %366 = invoke noundef nonnull align 8 dereferenceable(16) ptr @_ZNSt3mapINSt7__cxx1112basic_stringIcSt11char_traitsIcESaIcEEE13my_shared_ptrIN4urdf4LinkEESt4lessIS5_ESaISt4pairIKS5_S9_EEEixERSD_(ptr noundef nonnull align 8 dereferenceable(48) %6, ptr noundef nonnull align 8 dereferenceable(32) %365)
          to label %367 unwind label %376

367:                                              ; preds = %363
  invoke void @_ZNSt5stackI13my_shared_ptrIN4urdf4LinkEESt5dequeIS3_SaIS3_EEE4pushERKS3_(ptr noundef nonnull align 8 dereferenceable(80) %9, ptr noundef nonnull align 8 dereferenceable(16) %366)
          to label %368 unwind label %376

368:                                              ; preds = %367
  call void @llvm.lifetime.start.p0(i64 4, ptr nonnull %26) #7
  store i32 0, ptr %26, align 4, !tbaa !33
  invoke void @_ZNSt5stackIiSt5dequeIiSaIiEEE4pushEOi(ptr noundef nonnull align 8 dereferenceable(80) %10, ptr noundef nonnull align 4 dereferenceable(4) %26)
          to label %369 unwind label %380

369:                                              ; preds = %368
  call void @llvm.lifetime.end.p0(i64 4, ptr nonnull %26) #7
  br i1 %3, label %.preheader190, label %409

.preheader190:                                    ; preds = %369
  %370 = call noundef i64 @_ZNKSt5stackIiSt5dequeIiSaIiEEE4sizeEv(ptr noundef nonnull align 8 dereferenceable(80) %10)
  %371 = add i64 %370, -3
  %372 = icmp ult i64 %371, -2
  br i1 %372, label %.lr.ph, label %._crit_edge

._crit_edge:                                      ; preds = %385, %.preheader190
  %373 = invoke noundef nonnull align 8 dereferenceable(8) ptr @_ZStlsISt11char_traitsIcEERSt13basic_ostreamIcT_ES5_PKc(ptr noundef nonnull align 8 dereferenceable(8) @_ZSt4cout, ptr noundef nonnull @.str.11)
          to label %391 unwind label %376

374:                                              ; preds = %343
  %375 = landingpad { ptr, i32 }
          cleanup
  call void @llvm.lifetime.end.p0(i64 4, ptr nonnull %22) #7
  br label %967

376:                                              ; preds = %409, %407, %402, %400, %396, %394, %391, %._crit_edge, %367, %363
  %377 = landingpad { ptr, i32 }
          cleanup
  br label %412

378:                                              ; preds = %358
  %379 = landingpad { ptr, i32 }
          cleanup
  call void @llvm.lifetime.end.p0(i64 4, ptr nonnull %25) #7
  br label %412

380:                                              ; preds = %368
  %381 = landingpad { ptr, i32 }
          cleanup
  call void @llvm.lifetime.end.p0(i64 4, ptr nonnull %26) #7
  br label %412

382:                                              ; preds = %.lr.ph
  %383 = landingpad { ptr, i32 }
          cleanup
  br label %412

.lr.ph:                                           ; preds = %.preheader190, %385
  %.048248 = phi i32 [ %386, %385 ], [ 1, %.preheader190 ]
  %384 = invoke noundef nonnull align 8 dereferenceable(8) ptr @_ZStlsISt11char_traitsIcEERSt13basic_ostreamIcT_ES5_PKc(ptr noundef nonnull align 8 dereferenceable(8) @_ZSt4cout, ptr noundef nonnull @.str.10)
          to label %385 unwind label %382

385:                                              ; preds = %.lr.ph
  %386 = add i32 %.048248, 1
  %387 = zext i32 %386 to i64
  %388 = call noundef i64 @_ZNKSt5stackIiSt5dequeIiSaIiEEE4sizeEv(ptr noundef nonnull align 8 dereferenceable(80) %10)
  %389 = add i64 %388, -1
  %390 = icmp ugt i64 %389, %387
  br i1 %390, label %.lr.ph, label %._crit_edge, !llvm.loop !35

391:                                              ; preds = %._crit_edge
  %392 = call noundef ptr @_ZN13my_shared_ptrIN4urdf5JointEEptEv(ptr noundef nonnull align 8 dereferenceable(16) %24)
  %393 = invoke noundef nonnull align 8 dereferenceable(8) ptr @_ZStlsIcSt11char_traitsIcESaIcEERSt13basic_ostreamIT_T0_ES7_RKNSt7__cxx1112basic_stringIS4_S5_T1_EE(ptr noundef nonnull align 8 dereferenceable(8) %373, ptr noundef nonnull align 8 dereferenceable(32) %392)
          to label %394 unwind label %376

394:                                              ; preds = %391
  %395 = invoke noundef nonnull align 8 dereferenceable(8) ptr @_ZStlsISt11char_traitsIcEERSt13basic_ostreamIcT_ES5_PKc(ptr noundef nonnull align 8 dereferenceable(8) %393, ptr noundef nonnull @.str.12)
          to label %396 unwind label %376

396:                                              ; preds = %394
  %397 = call noundef nonnull align 8 dereferenceable(16) ptr @_ZNSt5stackI13my_shared_ptrIN4urdf4LinkEESt5dequeIS3_SaIS3_EEE3topEv(ptr noundef nonnull align 8 dereferenceable(80) %9)
  %398 = call noundef ptr @_ZN13my_shared_ptrIN4urdf4LinkEEptEv(ptr noundef nonnull align 8 dereferenceable(16) %397)
  %399 = invoke noundef nonnull align 8 dereferenceable(8) ptr @_ZStlsIcSt11char_traitsIcESaIcEERSt13basic_ostreamIT_T0_ES7_RKNSt7__cxx1112basic_stringIS4_S5_T1_EE(ptr noundef nonnull align 8 dereferenceable(8) %395, ptr noundef nonnull align 8 dereferenceable(32) %398)
          to label %400 unwind label %376

400:                                              ; preds = %396
  %401 = invoke noundef nonnull align 8 dereferenceable(8) ptr @_ZStlsISt11char_traitsIcEERSt13basic_ostreamIcT_ES5_PKc(ptr noundef nonnull align 8 dereferenceable(8) %399, ptr noundef nonnull @.str.13)
          to label %402 unwind label %376

402:                                              ; preds = %400
  %403 = call noundef ptr @_ZN13my_shared_ptrIN4urdf5JointEEptEv(ptr noundef nonnull align 8 dereferenceable(16) %24)
  %404 = getelementptr inbounds i8, ptr %403, i64 32
  %405 = load i32, ptr %404, align 8, !tbaa !37
  %406 = invoke noundef nonnull align 8 dereferenceable(8) ptr @_ZNSolsEi(ptr noundef nonnull align 8 dereferenceable(8) %401, i32 noundef %405)
          to label %407 unwind label %376

407:                                              ; preds = %402
  %408 = invoke noundef nonnull align 8 dereferenceable(8) ptr @_ZNSolsEPFRSoS_E(ptr noundef nonnull align 8 dereferenceable(8) %406, ptr noundef nonnull @_ZSt4endlIcSt11char_traitsIcEERSt13basic_ostreamIT_T0_ES6_)
          to label %409 unwind label %376

409:                                              ; preds = %407, %369
  %410 = call noundef ptr @_ZN13my_shared_ptrIN4urdf5JointEEptEv(ptr noundef nonnull align 8 dereferenceable(16) %24)
  invoke void @_ZNSt6vectorINSt7__cxx1112basic_stringIcSt11char_traitsIcESaIcEEESaIS5_EE9push_backERKS5_(ptr noundef nonnull align 8 dereferenceable(24) %8, ptr noundef nonnull align 8 dereferenceable(32) %410)
          to label %411 unwind label %376

411:                                              ; preds = %409
  call void @_ZN13my_shared_ptrIN4urdf5JointEED2Ev(ptr noundef nonnull align 8 dereferenceable(16) %24) #7
  call void @llvm.lifetime.end.p0(i64 16, ptr nonnull %24) #7
  br label %414

412:                                              ; preds = %382, %380, %378, %376
  %.pn177 = phi { ptr, i32 } [ %383, %382 ], [ %377, %376 ], [ %381, %380 ], [ %379, %378 ]
  call void @_ZN13my_shared_ptrIN4urdf5JointEED2Ev(ptr noundef nonnull align 8 dereferenceable(16) %24) #7
  call void @llvm.lifetime.end.p0(i64 16, ptr nonnull %24) #7
  call void @_ZN13my_shared_ptrIN4urdf4LinkEED2Ev(ptr noundef nonnull align 8 dereferenceable(16) %23) #7
  call void @llvm.lifetime.end.p0(i64 16, ptr nonnull %23) #7
  br label %967

413:                                              ; preds = %.lr.ph251
  call void @_ZNSt5stackI13my_shared_ptrIN4urdf4LinkEESt5dequeIS3_SaIS3_EEE3popEv(ptr noundef nonnull align 8 dereferenceable(80) %9)
  call void @_ZNSt5stackIiSt5dequeIiSaIiEEE3popEv(ptr noundef nonnull align 8 dereferenceable(80) %10)
  br label %414

414:                                              ; preds = %413, %411
  call void @_ZN13my_shared_ptrIN4urdf4LinkEED2Ev(ptr noundef nonnull align 8 dereferenceable(16) %23) #7
  call void @llvm.lifetime.end.p0(i64 16, ptr nonnull %23) #7
  %415 = call noundef i64 @_ZNKSt5stackI13my_shared_ptrIN4urdf4LinkEESt5dequeIS3_SaIS3_EEE4sizeEv(ptr noundef nonnull align 8 dereferenceable(80) %9)
  %.not = icmp eq i64 %415, 0
  br i1 %.not, label %.preheader189, label %.lr.ph251, !llvm.loop !49

416:                                              ; preds = %952
  %417 = add i32 %.047255, 1
  %418 = zext i32 %417 to i64
  %419 = call noundef i64 @_ZNKSt6vectorINSt7__cxx1112basic_stringIcSt11char_traitsIcESaIcEEESaIS5_EE4sizeEv(ptr noundef nonnull align 8 dereferenceable(24) %8) #7
  %.not262 = icmp ugt i64 %419, %418
  br i1 %.not262, label %420, label %._crit_edge257, !llvm.loop !50

420:                                              ; preds = %.lr.ph256, %416
  %421 = phi i64 [ 0, %.lr.ph256 ], [ %418, %416 ]
  %.047255 = phi i32 [ 0, %.lr.ph256 ], [ %417, %416 ]
  call void @llvm.lifetime.start.p0(i64 16, ptr nonnull %27) #7
  %422 = call noundef nonnull align 8 dereferenceable(32) ptr @_ZNSt6vectorINSt7__cxx1112basic_stringIcSt11char_traitsIcESaIcEEESaIS5_EEixEm(ptr noundef nonnull align 8 dereferenceable(24) %8, i64 noundef %421) #7
  %423 = invoke noundef nonnull align 8 dereferenceable(16) ptr @_ZNSt3mapINSt7__cxx1112basic_stringIcSt11char_traitsIcESaIcEEE13my_shared_ptrIN4urdf5JointEESt4lessIS5_ESaISt4pairIKS5_S9_EEEixERSD_(ptr noundef nonnull align 8 dereferenceable(48) %7, ptr noundef nonnull align 8 dereferenceable(32) %422)
          to label %424 unwind label %453

424:                                              ; preds = %420
  call void @_ZN13my_shared_ptrIN4urdf5JointEEC2ERKS2_(ptr noundef nonnull align 8 dereferenceable(16) %27, ptr noundef nonnull align 8 dereferenceable(16) %423)
  call void @llvm.lifetime.start.p0(i64 16, ptr nonnull %28) #7
  %425 = call noundef ptr @_ZN13my_shared_ptrIN4urdf5JointEEptEv(ptr noundef nonnull align 8 dereferenceable(16) %27)
  %426 = getelementptr inbounds i8, ptr %425, i64 96
  %427 = invoke noundef nonnull align 8 dereferenceable(16) ptr @_ZNSt3mapINSt7__cxx1112basic_stringIcSt11char_traitsIcESaIcEEE13my_shared_ptrIN4urdf4LinkEESt4lessIS5_ESaISt4pairIKS5_S9_EEEixERSD_(ptr noundef nonnull align 8 dereferenceable(48) %6, ptr noundef nonnull align 8 dereferenceable(32) %426)
          to label %428 unwind label %455

428:                                              ; preds = %424
  call void @_ZN13my_shared_ptrIN4urdf4LinkEEC2ERKS2_(ptr noundef nonnull align 8 dereferenceable(16) %28, ptr noundef nonnull align 8 dereferenceable(16) %427)
  call void @llvm.lifetime.start.p0(i64 16, ptr nonnull %29) #7
  %429 = call noundef ptr @_ZN13my_shared_ptrIN4urdf5JointEEptEv(ptr noundef nonnull align 8 dereferenceable(16) %27)
  %430 = getelementptr inbounds i8, ptr %429, i64 64
  %431 = invoke noundef nonnull align 8 dereferenceable(16) ptr @_ZNSt3mapINSt7__cxx1112basic_stringIcSt11char_traitsIcESaIcEEE13my_shared_ptrIN4urdf4LinkEESt4lessIS5_ESaISt4pairIKS5_S9_EEEixERSD_(ptr noundef nonnull align 8 dereferenceable(48) %6, ptr noundef nonnull align 8 dereferenceable(32) %430)
          to label %432 unwind label %457

432:                                              ; preds = %428
  call void @_ZN13my_shared_ptrIN4urdf4LinkEEC2ERKS2_(ptr noundef nonnull align 8 dereferenceable(16) %29, ptr noundef nonnull align 8 dereferenceable(16) %431)
  %433 = call noundef ptr @_ZN13my_shared_ptrIN4urdf4LinkEEptEv(ptr noundef nonnull align 8 dereferenceable(16) %28)
  %434 = call noundef ptr @_ZNKSt7__cxx1112basic_stringIcSt11char_traitsIcESaIcEE5c_strEv(ptr noundef nonnull align 8 dereferenceable(32) %433) #7
  %435 = invoke noundef i32 @_ZNK17RigidBodyDynamics5Model9GetBodyIdEPKc(ptr noundef nonnull align 8 dereferenceable(872) %0, ptr noundef %434)
          to label %436 unwind label %459

436:                                              ; preds = %432
  %437 = call noundef i32 @_ZNSt14numeric_limitsIjE3maxEv() #7
  %438 = icmp eq i32 %435, %437
  br i1 %438, label %439, label %461

439:                                              ; preds = %436
  %440 = invoke noundef nonnull align 8 dereferenceable(8) ptr @_ZStlsISt11char_traitsIcEERSt13basic_ostreamIcT_ES5_PKc(ptr noundef nonnull align 8 dereferenceable(8) @_ZSt4cerr, ptr noundef nonnull @.str.14)
          to label %441 unwind label %459

441:                                              ; preds = %439
  %442 = call noundef ptr @_ZN13my_shared_ptrIN4urdf5JointEEptEv(ptr noundef nonnull align 8 dereferenceable(16) %27)
  %443 = invoke noundef nonnull align 8 dereferenceable(8) ptr @_ZStlsIcSt11char_traitsIcESaIcEERSt13basic_ostreamIT_T0_ES7_RKNSt7__cxx1112basic_stringIS4_S5_T1_EE(ptr noundef nonnull align 8 dereferenceable(8) %440, ptr noundef nonnull align 8 dereferenceable(32) %442)
          to label %444 unwind label %459

444:                                              ; preds = %441
  %445 = invoke noundef nonnull align 8 dereferenceable(8) ptr @_ZStlsISt11char_traitsIcEERSt13basic_ostreamIcT_ES5_PKc(ptr noundef nonnull align 8 dereferenceable(8) %443, ptr noundef nonnull @.str.15)
          to label %446 unwind label %459

446:                                              ; preds = %444
  %447 = call noundef ptr @_ZN13my_shared_ptrIN4urdf4LinkEEptEv(ptr noundef nonnull align 8 dereferenceable(16) %28)
  %448 = invoke noundef nonnull align 8 dereferenceable(8) ptr @_ZStlsIcSt11char_traitsIcESaIcEERSt13basic_ostreamIT_T0_ES7_RKNSt7__cxx1112basic_stringIS4_S5_T1_EE(ptr noundef nonnull align 8 dereferenceable(8) %445, ptr noundef nonnull align 8 dereferenceable(32) %447)
          to label %449 unwind label %459

449:                                              ; preds = %446
  %450 = invoke noundef nonnull align 8 dereferenceable(8) ptr @_ZStlsISt11char_traitsIcEERSt13basic_ostreamIcT_ES5_PKc(ptr noundef nonnull align 8 dereferenceable(8) %448, ptr noundef nonnull @.str.16)
          to label %451 unwind label %459

451:                                              ; preds = %449
  %452 = invoke noundef nonnull align 8 dereferenceable(8) ptr @_ZNSolsEPFRSoS_E(ptr noundef nonnull align 8 dereferenceable(8) %450, ptr noundef nonnull @_ZSt4endlIcSt11char_traitsIcEERSt13basic_ostreamIT_T0_ES6_)
          to label %461 unwind label %459

453:                                              ; preds = %420
  %454 = landingpad { ptr, i32 }
          cleanup
  br label %966

455:                                              ; preds = %424
  %456 = landingpad { ptr, i32 }
          cleanup
  br label %965

457:                                              ; preds = %428
  %458 = landingpad { ptr, i32 }
          cleanup
  br label %964

459:                                              ; preds = %451, %449, %446, %444, %441, %439, %432
  %460 = landingpad { ptr, i32 }
          cleanup
  br label %963

461:                                              ; preds = %451, %436
  call void @llvm.lifetime.start.p0(i64 24, ptr nonnull %30) #7
  call void @_ZN17RigidBodyDynamics5JointC2Ev(ptr noundef nonnull align 8 dereferenceable(24) %30)
  %462 = call noundef ptr @_ZN13my_shared_ptrIN4urdf5JointEEptEv(ptr noundef nonnull align 8 dereferenceable(16) %27)
  %463 = getelementptr inbounds i8, ptr %462, i64 32
  %464 = load i32, ptr %463, align 8, !tbaa !37
  %465 = icmp eq i32 %464, 1
  br i1 %465, label %471, label %466

466:                                              ; preds = %461
  %467 = call noundef ptr @_ZN13my_shared_ptrIN4urdf5JointEEptEv(ptr noundef nonnull align 8 dereferenceable(16) %27)
  %468 = getelementptr inbounds i8, ptr %467, i64 32
  %469 = load i32, ptr %468, align 8, !tbaa !37
  %470 = icmp eq i32 %469, 2
  br i1 %470, label %471, label %550

471:                                              ; preds = %466, %461
  call void @llvm.lifetime.start.p0(i64 24, ptr nonnull %31) #7
  %472 = call noundef ptr @_ZN13my_shared_ptrIN4urdf5JointEEptEv(ptr noundef nonnull align 8 dereferenceable(16) %27)
  %473 = getelementptr inbounds i8, ptr %472, i64 40
  %474 = call noundef ptr @_ZN13my_shared_ptrIN4urdf5JointEEptEv(ptr noundef nonnull align 8 dereferenceable(16) %27)
  %475 = getelementptr inbounds i8, ptr %474, i64 48
  %476 = call noundef ptr @_ZN13my_shared_ptrIN4urdf5JointEEptEv(ptr noundef nonnull align 8 dereferenceable(16) %27)
  %477 = getelementptr inbounds i8, ptr %476, i64 56
  invoke void @_ZN9Vector3_tC2ERKdS1_S1_(ptr noundef nonnull align 8 dereferenceable(24) %31, ptr noundef nonnull align 8 dereferenceable(8) %473, ptr noundef nonnull align 8 dereferenceable(8) %475, ptr noundef nonnull align 8 dereferenceable(8) %477)
          to label %478 unwind label %492

478:                                              ; preds = %471
  call void @llvm.lifetime.start.p0(i64 24, ptr nonnull %32) #7
  call void @llvm.lifetime.start.p0(i64 8, ptr nonnull %33) #7
  store double 1.000000e+00, ptr %33, align 8, !tbaa !13
  call void @llvm.lifetime.start.p0(i64 8, ptr nonnull %34) #7
  store double 0.000000e+00, ptr %34, align 8, !tbaa !13
  call void @llvm.lifetime.start.p0(i64 8, ptr nonnull %35) #7
  store double 0.000000e+00, ptr %35, align 8, !tbaa !13
  invoke void @_ZN9Vector3_tC2ERKdS1_S1_(ptr noundef nonnull align 8 dereferenceable(24) %32, ptr noundef nonnull align 8 dereferenceable(8) %33, ptr noundef nonnull align 8 dereferenceable(8) %34, ptr noundef nonnull align 8 dereferenceable(8) %35)
          to label %479 unwind label %494

479:                                              ; preds = %478
  %480 = invoke noundef double @_ZNK5Eigen10MatrixBaseINS_6MatrixIdLi3ELi1ELi0ELi3ELi1EEEE3dotIS2_EENS_20ScalarBinaryOpTraitsIdNS_8internal6traitsIT_E6ScalarENS6_17scalar_product_opIdSA_EEE10ReturnTypeERKNS0_IS8_EE(ptr noundef nonnull align 1 dereferenceable(1) %31, ptr noundef nonnull align 1 dereferenceable(1) %32)
          to label %481 unwind label %494

481:                                              ; preds = %479
  %482 = fadd double %480, -1.000000e+00
  %483 = call double @llvm.fabs.f64(double %482)
  %484 = call noundef double @_ZNSt14numeric_limitsIdE7epsilonEv() #7
  %485 = fcmp olt double %483, %484
  call void @llvm.lifetime.end.p0(i64 8, ptr nonnull %35) #7
  call void @llvm.lifetime.end.p0(i64 8, ptr nonnull %34) #7
  call void @llvm.lifetime.end.p0(i64 8, ptr nonnull %33) #7
  call void @llvm.lifetime.end.p0(i64 24, ptr nonnull %32) #7
  br i1 %485, label %486, label %501

486:                                              ; preds = %481
  call void @llvm.lifetime.start.p0(i64 24, ptr nonnull %36) #7
  invoke void @_ZN17RigidBodyDynamics5JointC2ENS_9JointTypeE(ptr noundef nonnull align 8 dereferenceable(24) %36, i32 noundef 3)
          to label %487 unwind label %496

487:                                              ; preds = %486
  %488 = invoke noundef nonnull align 8 dereferenceable(24) ptr @_ZN17RigidBodyDynamics5JointaSERKS0_(ptr noundef nonnull align 8 dereferenceable(24) %30, ptr noundef nonnull align 8 dereferenceable(24) %36)
          to label %489 unwind label %498

489:                                              ; preds = %487
  call void @_ZN17RigidBodyDynamics5JointD2Ev(ptr noundef nonnull align 8 dereferenceable(24) %36) #7
  call void @llvm.lifetime.end.p0(i64 24, ptr nonnull %36) #7
  br label %548

490:                                              ; preds = %635, %633, %630, %628
  %491 = landingpad { ptr, i32 }
          cleanup
  br label %962

492:                                              ; preds = %471
  %493 = landingpad { ptr, i32 }
          cleanup
  br label %549

494:                                              ; preds = %479, %478
  %495 = landingpad { ptr, i32 }
          cleanup
  call void @llvm.lifetime.end.p0(i64 8, ptr nonnull %35) #7
  call void @llvm.lifetime.end.p0(i64 8, ptr nonnull %34) #7
  call void @llvm.lifetime.end.p0(i64 8, ptr nonnull %33) #7
  call void @llvm.lifetime.end.p0(i64 24, ptr nonnull %32) #7
  br label %549

496:                                              ; preds = %486
  %497 = landingpad { ptr, i32 }
          cleanup
  br label %500

498:                                              ; preds = %487
  %499 = landingpad { ptr, i32 }
          cleanup
  call void @_ZN17RigidBodyDynamics5JointD2Ev(ptr noundef nonnull align 8 dereferenceable(24) %36) #7
  br label %500

500:                                              ; preds = %498, %496
  %.pn142 = phi { ptr, i32 } [ %499, %498 ], [ %497, %496 ]
  call void @llvm.lifetime.end.p0(i64 24, ptr nonnull %36) #7
  br label %549

501:                                              ; preds = %481
  call void @llvm.lifetime.start.p0(i64 24, ptr nonnull %37) #7
  call void @llvm.lifetime.start.p0(i64 8, ptr nonnull %38) #7
  store double 0.000000e+00, ptr %38, align 8, !tbaa !13
  call void @llvm.lifetime.start.p0(i64 8, ptr nonnull %39) #7
  store double 1.000000e+00, ptr %39, align 8, !tbaa !13
  call void @llvm.lifetime.start.p0(i64 8, ptr nonnull %40) #7
  store double 0.000000e+00, ptr %40, align 8, !tbaa !13
  invoke void @_ZN9Vector3_tC2ERKdS1_S1_(ptr noundef nonnull align 8 dereferenceable(24) %37, ptr noundef nonnull align 8 dereferenceable(8) %38, ptr noundef nonnull align 8 dereferenceable(8) %39, ptr noundef nonnull align 8 dereferenceable(8) %40)
          to label %502 unwind label %513

502:                                              ; preds = %501
  %503 = invoke noundef double @_ZNK5Eigen10MatrixBaseINS_6MatrixIdLi3ELi1ELi0ELi3ELi1EEEE3dotIS2_EENS_20ScalarBinaryOpTraitsIdNS_8internal6traitsIT_E6ScalarENS6_17scalar_product_opIdSA_EEE10ReturnTypeERKNS0_IS8_EE(ptr noundef nonnull align 1 dereferenceable(1) %31, ptr noundef nonnull align 1 dereferenceable(1) %37)
          to label %504 unwind label %513

504:                                              ; preds = %502
  %505 = fadd double %503, -1.000000e+00
  %506 = call double @llvm.fabs.f64(double %505)
  %507 = call noundef double @_ZNSt14numeric_limitsIdE7epsilonEv() #7
  %508 = fcmp olt double %506, %507
  call void @llvm.lifetime.end.p0(i64 8, ptr nonnull %40) #7
  call void @llvm.lifetime.end.p0(i64 8, ptr nonnull %39) #7
  call void @llvm.lifetime.end.p0(i64 8, ptr nonnull %38) #7
  call void @llvm.lifetime.end.p0(i64 24, ptr nonnull %37) #7
  br i1 %508, label %509, label %520

509:                                              ; preds = %504
  call void @llvm.lifetime.start.p0(i64 24, ptr nonnull %41) #7
  invoke void @_ZN17RigidBodyDynamics5JointC2ENS_9JointTypeE(ptr noundef nonnull align 8 dereferenceable(24) %41, i32 noundef 4)
          to label %510 unwind label %515

510:                                              ; preds = %509
  %511 = invoke noundef nonnull align 8 dereferenceable(24) ptr @_ZN17RigidBodyDynamics5JointaSERKS0_(ptr noundef nonnull align 8 dereferenceable(24) %30, ptr noundef nonnull align 8 dereferenceable(24) %41)
          to label %512 unwind label %517

512:                                              ; preds = %510
  call void @_ZN17RigidBodyDynamics5JointD2Ev(ptr noundef nonnull align 8 dereferenceable(24) %41) #7
  call void @llvm.lifetime.end.p0(i64 24, ptr nonnull %41) #7
  br label %548

513:                                              ; preds = %502, %501
  %514 = landingpad { ptr, i32 }
          cleanup
  call void @llvm.lifetime.end.p0(i64 8, ptr nonnull %40) #7
  call void @llvm.lifetime.end.p0(i64 8, ptr nonnull %39) #7
  call void @llvm.lifetime.end.p0(i64 8, ptr nonnull %38) #7
  call void @llvm.lifetime.end.p0(i64 24, ptr nonnull %37) #7
  br label %549

515:                                              ; preds = %509
  %516 = landingpad { ptr, i32 }
          cleanup
  br label %519

517:                                              ; preds = %510
  %518 = landingpad { ptr, i32 }
          cleanup
  call void @_ZN17RigidBodyDynamics5JointD2Ev(ptr noundef nonnull align 8 dereferenceable(24) %41) #7
  br label %519

519:                                              ; preds = %517, %515
  %.pn140 = phi { ptr, i32 } [ %518, %517 ], [ %516, %515 ]
  call void @llvm.lifetime.end.p0(i64 24, ptr nonnull %41) #7
  br label %549

520:                                              ; preds = %504
  call void @llvm.lifetime.start.p0(i64 24, ptr nonnull %42) #7
  call void @llvm.lifetime.start.p0(i64 8, ptr nonnull %43) #7
  store double 0.000000e+00, ptr %43, align 8, !tbaa !13
  call void @llvm.lifetime.start.p0(i64 8, ptr nonnull %44) #7
  store double 0.000000e+00, ptr %44, align 8, !tbaa !13
  call void @llvm.lifetime.start.p0(i64 8, ptr nonnull %45) #7
  store double 1.000000e+00, ptr %45, align 8, !tbaa !13
  invoke void @_ZN9Vector3_tC2ERKdS1_S1_(ptr noundef nonnull align 8 dereferenceable(24) %42, ptr noundef nonnull align 8 dereferenceable(8) %43, ptr noundef nonnull align 8 dereferenceable(8) %44, ptr noundef nonnull align 8 dereferenceable(8) %45)
          to label %521 unwind label %532

521:                                              ; preds = %520
  %522 = invoke noundef double @_ZNK5Eigen10MatrixBaseINS_6MatrixIdLi3ELi1ELi0ELi3ELi1EEEE3dotIS2_EENS_20ScalarBinaryOpTraitsIdNS_8internal6traitsIT_E6ScalarENS6_17scalar_product_opIdSA_EEE10ReturnTypeERKNS0_IS8_EE(ptr noundef nonnull align 1 dereferenceable(1) %31, ptr noundef nonnull align 1 dereferenceable(1) %42)
          to label %523 unwind label %532

523:                                              ; preds = %521
  %524 = fadd double %522, -1.000000e+00
  %525 = call double @llvm.fabs.f64(double %524)
  %526 = call noundef double @_ZNSt14numeric_limitsIdE7epsilonEv() #7
  %527 = fcmp olt double %525, %526
  call void @llvm.lifetime.end.p0(i64 8, ptr nonnull %45) #7
  call void @llvm.lifetime.end.p0(i64 8, ptr nonnull %44) #7
  call void @llvm.lifetime.end.p0(i64 8, ptr nonnull %43) #7
  call void @llvm.lifetime.end.p0(i64 24, ptr nonnull %42) #7
  br i1 %527, label %528, label %539

528:                                              ; preds = %523
  call void @llvm.lifetime.start.p0(i64 24, ptr nonnull %46) #7
  invoke void @_ZN17RigidBodyDynamics5JointC2ENS_9JointTypeE(ptr noundef nonnull align 8 dereferenceable(24) %46, i32 noundef 5)
          to label %529 unwind label %534

529:                                              ; preds = %528
  %530 = invoke noundef nonnull align 8 dereferenceable(24) ptr @_ZN17RigidBodyDynamics5JointaSERKS0_(ptr noundef nonnull align 8 dereferenceable(24) %30, ptr noundef nonnull align 8 dereferenceable(24) %46)
          to label %531 unwind label %536

531:                                              ; preds = %529
  call void @_ZN17RigidBodyDynamics5JointD2Ev(ptr noundef nonnull align 8 dereferenceable(24) %46) #7
  call void @llvm.lifetime.end.p0(i64 24, ptr nonnull %46) #7
  br label %548

532:                                              ; preds = %521, %520
  %533 = landingpad { ptr, i32 }
          cleanup
  call void @llvm.lifetime.end.p0(i64 8, ptr nonnull %45) #7
  call void @llvm.lifetime.end.p0(i64 8, ptr nonnull %44) #7
  call void @llvm.lifetime.end.p0(i64 8, ptr nonnull %43) #7
  call void @llvm.lifetime.end.p0(i64 24, ptr nonnull %42) #7
  br label %549

534:                                              ; preds = %528
  %535 = landingpad { ptr, i32 }
          cleanup
  br label %538

536:                                              ; preds = %529
  %537 = landingpad { ptr, i32 }
          cleanup
  call void @_ZN17RigidBodyDynamics5JointD2Ev(ptr noundef nonnull align 8 dereferenceable(24) %46) #7
  br label %538

538:                                              ; preds = %536, %534
  %.pn138 = phi { ptr, i32 } [ %537, %536 ], [ %535, %534 ]
  call void @llvm.lifetime.end.p0(i64 24, ptr nonnull %46) #7
  br label %549

539:                                              ; preds = %523
  call void @llvm.lifetime.start.p0(i64 24, ptr nonnull %47) #7
  invoke void @_ZN17RigidBodyDynamics5JointC2ENS_9JointTypeERK9Vector3_t(ptr noundef nonnull align 8 dereferenceable(24) %47, i32 noundef 1, ptr noundef nonnull align 8 dereferenceable(24) %31)
          to label %540 unwind label %543

540:                                              ; preds = %539
  %541 = invoke noundef nonnull align 8 dereferenceable(24) ptr @_ZN17RigidBodyDynamics5JointaSERKS0_(ptr noundef nonnull align 8 dereferenceable(24) %30, ptr noundef nonnull align 8 dereferenceable(24) %47)
          to label %542 unwind label %545

542:                                              ; preds = %540
  call void @_ZN17RigidBodyDynamics5JointD2Ev(ptr noundef nonnull align 8 dereferenceable(24) %47) #7
  call void @llvm.lifetime.end.p0(i64 24, ptr nonnull %47) #7
  br label %548

543:                                              ; preds = %539
  %544 = landingpad { ptr, i32 }
          cleanup
  br label %547

545:                                              ; preds = %540
  %546 = landingpad { ptr, i32 }
          cleanup
  call void @_ZN17RigidBodyDynamics5JointD2Ev(ptr noundef nonnull align 8 dereferenceable(24) %47) #7
  br label %547

547:                                              ; preds = %545, %543
  %.pn136 = phi { ptr, i32 } [ %546, %545 ], [ %544, %543 ]
  call void @llvm.lifetime.end.p0(i64 24, ptr nonnull %47) #7
  br label %549

548:                                              ; preds = %512, %542, %531, %489
  call void @llvm.lifetime.end.p0(i64 24, ptr nonnull %31) #7
  br label %637

549:                                              ; preds = %547, %538, %532, %519, %513, %500, %494, %492
  %.pn142.pn = phi { ptr, i32 } [ %.pn142, %500 ], [ %.pn140, %519 ], [ %.pn138, %538 ], [ %.pn136, %547 ], [ %533, %532 ], [ %514, %513 ], [ %495, %494 ], [ %493, %492 ]
  call void @llvm.lifetime.end.p0(i64 24, ptr nonnull %31) #7
  br label %962

550:                                              ; preds = %466
  %551 = call noundef ptr @_ZN13my_shared_ptrIN4urdf5JointEEptEv(ptr noundef nonnull align 8 dereferenceable(16) %27)
  %552 = getelementptr inbounds i8, ptr %551, i64 32
  %553 = load i32, ptr %552, align 8, !tbaa !37
  %554 = icmp eq i32 %553, 3
  br i1 %554, label %555, label %574

555:                                              ; preds = %550
  call void @llvm.lifetime.start.p0(i64 24, ptr nonnull %48) #7
  %556 = call noundef ptr @_ZN13my_shared_ptrIN4urdf5JointEEptEv(ptr noundef nonnull align 8 dereferenceable(16) %27)
  %557 = getelementptr inbounds i8, ptr %556, i64 40
  %558 = call noundef ptr @_ZN13my_shared_ptrIN4urdf5JointEEptEv(ptr noundef nonnull align 8 dereferenceable(16) %27)
  %559 = getelementptr inbounds i8, ptr %558, i64 48
  %560 = call noundef ptr @_ZN13my_shared_ptrIN4urdf5JointEEptEv(ptr noundef nonnull align 8 dereferenceable(16) %27)
  %561 = getelementptr inbounds i8, ptr %560, i64 56
  invoke void @_ZN9Vector3_tC2ERKdS1_S1_(ptr noundef nonnull align 8 dereferenceable(24) %48, ptr noundef nonnull align 8 dereferenceable(8) %557, ptr noundef nonnull align 8 dereferenceable(8) %559, ptr noundef nonnull align 8 dereferenceable(8) %561)
          to label %562 unwind label %566

562:                                              ; preds = %555
  call void @llvm.lifetime.start.p0(i64 24, ptr nonnull %49) #7
  invoke void @_ZN17RigidBodyDynamics5JointC2ENS_9JointTypeERK9Vector3_t(ptr noundef nonnull align 8 dereferenceable(24) %49, i32 noundef 2, ptr noundef nonnull align 8 dereferenceable(24) %48)
          to label %563 unwind label %568

563:                                              ; preds = %562
  %564 = invoke noundef nonnull align 8 dereferenceable(24) ptr @_ZN17RigidBodyDynamics5JointaSERKS0_(ptr noundef nonnull align 8 dereferenceable(24) %30, ptr noundef nonnull align 8 dereferenceable(24) %49)
          to label %565 unwind label %570

565:                                              ; preds = %563
  call void @_ZN17RigidBodyDynamics5JointD2Ev(ptr noundef nonnull align 8 dereferenceable(24) %49) #7
  call void @llvm.lifetime.end.p0(i64 24, ptr nonnull %49) #7
  call void @llvm.lifetime.end.p0(i64 24, ptr nonnull %48) #7
  br label %637

566:                                              ; preds = %555
  %567 = landingpad { ptr, i32 }
          cleanup
  br label %573

568:                                              ; preds = %562
  %569 = landingpad { ptr, i32 }
          cleanup
  br label %572

570:                                              ; preds = %563
  %571 = landingpad { ptr, i32 }
          cleanup
  call void @_ZN17RigidBodyDynamics5JointD2Ev(ptr noundef nonnull align 8 dereferenceable(24) %49) #7
  br label %572

572:                                              ; preds = %570, %568
  %.pn133 = phi { ptr, i32 } [ %571, %570 ], [ %569, %568 ]
  call void @llvm.lifetime.end.p0(i64 24, ptr nonnull %49) #7
  br label %573

573:                                              ; preds = %572, %566
  %.pn133.pn = phi { ptr, i32 } [ %.pn133, %572 ], [ %567, %566 ]
  call void @llvm.lifetime.end.p0(i64 24, ptr nonnull %48) #7
  br label %962

574:                                              ; preds = %550
  %575 = call noundef ptr @_ZN13my_shared_ptrIN4urdf5JointEEptEv(ptr noundef nonnull align 8 dereferenceable(16) %27)
  %576 = getelementptr inbounds i8, ptr %575, i64 32
  %577 = load i32, ptr %576, align 8, !tbaa !37
  %578 = icmp eq i32 %577, 6
  br i1 %578, label %579, label %588

579:                                              ; preds = %574
  call void @llvm.lifetime.start.p0(i64 24, ptr nonnull %50) #7
  invoke void @_ZN17RigidBodyDynamics5JointC2ENS_9JointTypeE(ptr noundef nonnull align 8 dereferenceable(24) %50, i32 noundef 13)
          to label %580 unwind label %583

580:                                              ; preds = %579
  %581 = invoke noundef nonnull align 8 dereferenceable(24) ptr @_ZN17RigidBodyDynamics5JointaSERKS0_(ptr noundef nonnull align 8 dereferenceable(24) %30, ptr noundef nonnull align 8 dereferenceable(24) %50)
          to label %582 unwind label %585

582:                                              ; preds = %580
  call void @_ZN17RigidBodyDynamics5JointD2Ev(ptr noundef nonnull align 8 dereferenceable(24) %50) #7
  call void @llvm.lifetime.end.p0(i64 24, ptr nonnull %50) #7
  br label %637

583:                                              ; preds = %579
  %584 = landingpad { ptr, i32 }
          cleanup
  br label %587

585:                                              ; preds = %580
  %586 = landingpad { ptr, i32 }
          cleanup
  call void @_ZN17RigidBodyDynamics5JointD2Ev(ptr noundef nonnull align 8 dereferenceable(24) %50) #7
  br label %587

587:                                              ; preds = %585, %583
  %.pn131 = phi { ptr, i32 } [ %586, %585 ], [ %584, %583 ]
  call void @llvm.lifetime.end.p0(i64 24, ptr nonnull %50) #7
  br label %962

588:                                              ; preds = %574
  %589 = call noundef ptr @_ZN13my_shared_ptrIN4urdf5JointEEptEv(ptr noundef nonnull align 8 dereferenceable(16) %27)
  %590 = getelementptr inbounds i8, ptr %589, i64 32
  %591 = load i32, ptr %590, align 8, !tbaa !37
  %592 = icmp eq i32 %591, 4
  br i1 %592, label %593, label %623

593:                                              ; preds = %588
  call void @llvm.lifetime.start.p0(i64 24, ptr nonnull %51) #7
  call void @llvm.lifetime.start.p0(i64 48, ptr nonnull %52) #7
  call void @llvm.lifetime.start.p0(i64 8, ptr nonnull %53) #7
  store double 0.000000e+00, ptr %53, align 8, !tbaa !13
  call void @llvm.lifetime.start.p0(i64 8, ptr nonnull %54) #7
  store double 0.000000e+00, ptr %54, align 8, !tbaa !13
  call void @llvm.lifetime.start.p0(i64 8, ptr nonnull %55) #7
  store double 0.000000e+00, ptr %55, align 8, !tbaa !13
  call void @llvm.lifetime.start.p0(i64 8, ptr nonnull %56) #7
  store double 1.000000e+00, ptr %56, align 8, !tbaa !13
  call void @llvm.lifetime.start.p0(i64 8, ptr nonnull %57) #7
  store double 0.000000e+00, ptr %57, align 8, !tbaa !13
  call void @llvm.lifetime.start.p0(i64 8, ptr nonnull %58) #7
  store double 0.000000e+00, ptr %58, align 8, !tbaa !13
  invoke void @_ZN15SpatialVector_tC2ERKdS1_S1_S1_S1_S1_(ptr noundef nonnull align 16 dereferenceable(48) %52, ptr noundef nonnull align 8 dereferenceable(8) %53, ptr noundef nonnull align 8 dereferenceable(8) %54, ptr noundef nonnull align 8 dereferenceable(8) %55, ptr noundef nonnull align 8 dereferenceable(8) %56, ptr noundef nonnull align 8 dereferenceable(8) %57, ptr noundef nonnull align 8 dereferenceable(8) %58)
          to label %594 unwind label %603

594:                                              ; preds = %593
  call void @llvm.lifetime.start.p0(i64 48, ptr nonnull %59) #7
  call void @llvm.lifetime.start.p0(i64 8, ptr nonnull %60) #7
  store double 0.000000e+00, ptr %60, align 8, !tbaa !13
  call void @llvm.lifetime.start.p0(i64 8, ptr nonnull %61) #7
  store double 0.000000e+00, ptr %61, align 8, !tbaa !13
  call void @llvm.lifetime.start.p0(i64 8, ptr nonnull %62) #7
  store double 0.000000e+00, ptr %62, align 8, !tbaa !13
  call void @llvm.lifetime.start.p0(i64 8, ptr nonnull %63) #7
  store double 0.000000e+00, ptr %63, align 8, !tbaa !13
  call void @llvm.lifetime.start.p0(i64 8, ptr nonnull %64) #7
  store double 1.000000e+00, ptr %64, align 8, !tbaa !13
  call void @llvm.lifetime.start.p0(i64 8, ptr nonnull %65) #7
  store double 0.000000e+00, ptr %65, align 8, !tbaa !13
  invoke void @_ZN15SpatialVector_tC2ERKdS1_S1_S1_S1_S1_(ptr noundef nonnull align 16 dereferenceable(48) %59, ptr noundef nonnull align 8 dereferenceable(8) %60, ptr noundef nonnull align 8 dereferenceable(8) %61, ptr noundef nonnull align 8 dereferenceable(8) %62, ptr noundef nonnull align 8 dereferenceable(8) %63, ptr noundef nonnull align 8 dereferenceable(8) %64, ptr noundef nonnull align 8 dereferenceable(8) %65)
          to label %595 unwind label %605

595:                                              ; preds = %594
  call void @llvm.lifetime.start.p0(i64 48, ptr nonnull %66) #7
  call void @llvm.lifetime.start.p0(i64 8, ptr nonnull %67) #7
  store double 0.000000e+00, ptr %67, align 8, !tbaa !13
  call void @llvm.lifetime.start.p0(i64 8, ptr nonnull %68) #7
  store double 0.000000e+00, ptr %68, align 8, !tbaa !13
  call void @llvm.lifetime.start.p0(i64 8, ptr nonnull %69) #7
  store double 0.000000e+00, ptr %69, align 8, !tbaa !13
  call void @llvm.lifetime.start.p0(i64 8, ptr nonnull %70) #7
  store double 0.000000e+00, ptr %70, align 8, !tbaa !13
  call void @llvm.lifetime.start.p0(i64 8, ptr nonnull %71) #7
  store double 0.000000e+00, ptr %71, align 8, !tbaa !13
  call void @llvm.lifetime.start.p0(i64 8, ptr nonnull %72) #7
  store double 1.000000e+00, ptr %72, align 8, !tbaa !13
  invoke void @_ZN15SpatialVector_tC2ERKdS1_S1_S1_S1_S1_(ptr noundef nonnull align 16 dereferenceable(48) %66, ptr noundef nonnull align 8 dereferenceable(8) %67, ptr noundef nonnull align 8 dereferenceable(8) %68, ptr noundef nonnull align 8 dereferenceable(8) %69, ptr noundef nonnull align 8 dereferenceable(8) %70, ptr noundef nonnull align 8 dereferenceable(8) %71, ptr noundef nonnull align 8 dereferenceable(8) %72)
          to label %596 unwind label %607

596:                                              ; preds = %595
  call void @llvm.lifetime.start.p0(i64 48, ptr nonnull %73) #7
  call void @llvm.lifetime.start.p0(i64 8, ptr nonnull %74) #7
  store double 1.000000e+00, ptr %74, align 8, !tbaa !13
  call void @llvm.lifetime.start.p0(i64 8, ptr nonnull %75) #7
  store double 0.000000e+00, ptr %75, align 8, !tbaa !13
  call void @llvm.lifetime.start.p0(i64 8, ptr nonnull %76) #7
  store double 0.000000e+00, ptr %76, align 8, !tbaa !13
  call void @llvm.lifetime.start.p0(i64 8, ptr nonnull %77) #7
  store double 0.000000e+00, ptr %77, align 8, !tbaa !13
  call void @llvm.lifetime.start.p0(i64 8, ptr nonnull %78) #7
  store double 0.000000e+00, ptr %78, align 8, !tbaa !13
  call void @llvm.lifetime.start.p0(i64 8, ptr nonnull %79) #7
  store double 0.000000e+00, ptr %79, align 8, !tbaa !13
  invoke void @_ZN15SpatialVector_tC2ERKdS1_S1_S1_S1_S1_(ptr noundef nonnull align 16 dereferenceable(48) %73, ptr noundef nonnull align 8 dereferenceable(8) %74, ptr noundef nonnull align 8 dereferenceable(8) %75, ptr noundef nonnull align 8 dereferenceable(8) %76, ptr noundef nonnull align 8 dereferenceable(8) %77, ptr noundef nonnull align 8 dereferenceable(8) %78, ptr noundef nonnull align 8 dereferenceable(8) %79)
          to label %597 unwind label %609

597:                                              ; preds = %596
  call void @llvm.lifetime.start.p0(i64 48, ptr nonnull %80) #7
  call void @llvm.lifetime.start.p0(i64 8, ptr nonnull %81) #7
  store double 0.000000e+00, ptr %81, align 8, !tbaa !13
  call void @llvm.lifetime.start.p0(i64 8, ptr nonnull %82) #7
  store double 1.000000e+00, ptr %82, align 8, !tbaa !13
  call void @llvm.lifetime.start.p0(i64 8, ptr nonnull %83) #7
  store double 0.000000e+00, ptr %83, align 8, !tbaa !13
  call void @llvm.lifetime.start.p0(i64 8, ptr nonnull %84) #7
  store double 0.000000e+00, ptr %84, align 8, !tbaa !13
  call void @llvm.lifetime.start.p0(i64 8, ptr nonnull %85) #7
  store double 0.000000e+00, ptr %85, align 8, !tbaa !13
  call void @llvm.lifetime.start.p0(i64 8, ptr nonnull %86) #7
  store double 0.000000e+00, ptr %86, align 8, !tbaa !13
  invoke void @_ZN15SpatialVector_tC2ERKdS1_S1_S1_S1_S1_(ptr noundef nonnull align 16 dereferenceable(48) %80, ptr noundef nonnull align 8 dereferenceable(8) %81, ptr noundef nonnull align 8 dereferenceable(8) %82, ptr noundef nonnull align 8 dereferenceable(8) %83, ptr noundef nonnull align 8 dereferenceable(8) %84, ptr noundef nonnull align 8 dereferenceable(8) %85, ptr noundef nonnull align 8 dereferenceable(8) %86)
          to label %598 unwind label %611

598:                                              ; preds = %597
  call void @llvm.lifetime.start.p0(i64 48, ptr nonnull %87) #7
  call void @llvm.lifetime.start.p0(i64 8, ptr nonnull %88) #7
  store double 0.000000e+00, ptr %88, align 8, !tbaa !13
  call void @llvm.lifetime.start.p0(i64 8, ptr nonnull %89) #7
  store double 0.000000e+00, ptr %89, align 8, !tbaa !13
  call void @llvm.lifetime.start.p0(i64 8, ptr nonnull %90) #7
  store double 1.000000e+00, ptr %90, align 8, !tbaa !13
  call void @llvm.lifetime.start.p0(i64 8, ptr nonnull %91) #7
  store double 0.000000e+00, ptr %91, align 8, !tbaa !13
  call void @llvm.lifetime.start.p0(i64 8, ptr nonnull %92) #7
  store double 0.000000e+00, ptr %92, align 8, !tbaa !13
  call void @llvm.lifetime.start.p0(i64 8, ptr nonnull %93) #7
  store double 0.000000e+00, ptr %93, align 8, !tbaa !13
  invoke void @_ZN15SpatialVector_tC2ERKdS1_S1_S1_S1_S1_(ptr noundef nonnull align 16 dereferenceable(48) %87, ptr noundef nonnull align 8 dereferenceable(8) %88, ptr noundef nonnull align 8 dereferenceable(8) %89, ptr noundef nonnull align 8 dereferenceable(8) %90, ptr noundef nonnull align 8 dereferenceable(8) %91, ptr noundef nonnull align 8 dereferenceable(8) %92, ptr noundef nonnull align 8 dereferenceable(8) %93)
          to label %599 unwind label %613

599:                                              ; preds = %598
  invoke void @_ZN17RigidBodyDynamics5JointC2ERK15SpatialVector_tS3_S3_S3_S3_S3_(ptr noundef nonnull align 8 dereferenceable(24) %51, ptr noundef nonnull align 16 dereferenceable(48) %52, ptr noundef nonnull align 16 dereferenceable(48) %59, ptr noundef nonnull align 16 dereferenceable(48) %66, ptr noundef nonnull align 16 dereferenceable(48) %73, ptr noundef nonnull align 16 dereferenceable(48) %80, ptr noundef nonnull align 16 dereferenceable(48) %87)
          to label %600 unwind label %613

600:                                              ; preds = %599
  %601 = invoke noundef nonnull align 8 dereferenceable(24) ptr @_ZN17RigidBodyDynamics5JointaSERKS0_(ptr noundef nonnull align 8 dereferenceable(24) %30, ptr noundef nonnull align 8 dereferenceable(24) %51)
          to label %602 unwind label %615

602:                                              ; preds = %600
  call void @_ZN17RigidBodyDynamics5JointD2Ev(ptr noundef nonnull align 8 dereferenceable(24) %51) #7
  call void @llvm.lifetime.end.p0(i64 8, ptr nonnull %93) #7
  call void @llvm.lifetime.end.p0(i64 8, ptr nonnull %92) #7
  call void @llvm.lifetime.end.p0(i64 8, ptr nonnull %91) #7
  call void @llvm.lifetime.end.p0(i64 8, ptr nonnull %90) #7
  call void @llvm.lifetime.end.p0(i64 8, ptr nonnull %89) #7
  call void @llvm.lifetime.end.p0(i64 8, ptr nonnull %88) #7
  call void @llvm.lifetime.end.p0(i64 48, ptr nonnull %87) #7
  call void @llvm.lifetime.end.p0(i64 8, ptr nonnull %86) #7
  call void @llvm.lifetime.end.p0(i64 8, ptr nonnull %85) #7
  call void @llvm.lifetime.end.p0(i64 8, ptr nonnull %84) #7
  call void @llvm.lifetime.end.p0(i64 8, ptr nonnull %83) #7
  call void @llvm.lifetime.end.p0(i64 8, ptr nonnull %82) #7
  call void @llvm.lifetime.end.p0(i64 8, ptr nonnull %81) #7
  call void @llvm.lifetime.end.p0(i64 48, ptr nonnull %80) #7
  call void @llvm.lifetime.end.p0(i64 8, ptr nonnull %79) #7
  call void @llvm.lifetime.end.p0(i64 8, ptr nonnull %78) #7
  call void @llvm.lifetime.end.p0(i64 8, ptr nonnull %77) #7
  call void @llvm.lifetime.end.p0(i64 8, ptr nonnull %76) #7
  call void @llvm.lifetime.end.p0(i64 8, ptr nonnull %75) #7
  call void @llvm.lifetime.end.p0(i64 8, ptr nonnull %74) #7
  call void @llvm.lifetime.end.p0(i64 48, ptr nonnull %73) #7
  call void @llvm.lifetime.end.p0(i64 8, ptr nonnull %72) #7
  call void @llvm.lifetime.end.p0(i64 8, ptr nonnull %71) #7
  call void @llvm.lifetime.end.p0(i64 8, ptr nonnull %70) #7
  call void @llvm.lifetime.end.p0(i64 8, ptr nonnull %69) #7
  call void @llvm.lifetime.end.p0(i64 8, ptr nonnull %68) #7
  call void @llvm.lifetime.end.p0(i64 8, ptr nonnull %67) #7
  call void @llvm.lifetime.end.p0(i64 48, ptr nonnull %66) #7
  call void @llvm.lifetime.end.p0(i64 8, ptr nonnull %65) #7
  call void @llvm.lifetime.end.p0(i64 8, ptr nonnull %64) #7
  call void @llvm.lifetime.end.p0(i64 8, ptr nonnull %63) #7
  call void @llvm.lifetime.end.p0(i64 8, ptr nonnull %62) #7
  call void @llvm.lifetime.end.p0(i64 8, ptr nonnull %61) #7
  call void @llvm.lifetime.end.p0(i64 8, ptr nonnull %60) #7
  call void @llvm.lifetime.end.p0(i64 48, ptr nonnull %59) #7
  call void @llvm.lifetime.end.p0(i64 8, ptr nonnull %58) #7
  call void @llvm.lifetime.end.p0(i64 8, ptr nonnull %57) #7
  call void @llvm.lifetime.end.p0(i64 8, ptr nonnull %56) #7
  call void @llvm.lifetime.end.p0(i64 8, ptr nonnull %55) #7
  call void @llvm.lifetime.end.p0(i64 8, ptr nonnull %54) #7
  call void @llvm.lifetime.end.p0(i64 8, ptr nonnull %53) #7
  call void @llvm.lifetime.end.p0(i64 48, ptr nonnull %52) #7
  call void @llvm.lifetime.end.p0(i64 24, ptr nonnull %51) #7
  br label %637

603:                                              ; preds = %593
  %604 = landingpad { ptr, i32 }
          cleanup
  br label %622

605:                                              ; preds = %594
  %606 = landingpad { ptr, i32 }
          cleanup
  br label %621

607:                                              ; preds = %595
  %608 = landingpad { ptr, i32 }
          cleanup
  br label %620

609:                                              ; preds = %596
  %610 = landingpad { ptr, i32 }
          cleanup
  br label %619

611:                                              ; preds = %597
  %612 = landingpad { ptr, i32 }
          cleanup
  br label %618

613:                                              ; preds = %599, %598
  %614 = landingpad { ptr, i32 }
          cleanup
  br label %617

615:                                              ; preds = %600
  %616 = landingpad { ptr, i32 }
          cleanup
  call void @_ZN17RigidBodyDynamics5JointD2Ev(ptr noundef nonnull align 8 dereferenceable(24) %51) #7
  br label %617

617:                                              ; preds = %615, %613
  %.pn124 = phi { ptr, i32 } [ %616, %615 ], [ %614, %613 ]
  call void @llvm.lifetime.end.p0(i64 8, ptr nonnull %93) #7
  call void @llvm.lifetime.end.p0(i64 8, ptr nonnull %92) #7
  call void @llvm.lifetime.end.p0(i64 8, ptr nonnull %91) #7
  call void @llvm.lifetime.end.p0(i64 8, ptr nonnull %90) #7
  call void @llvm.lifetime.end.p0(i64 8, ptr nonnull %89) #7
  call void @llvm.lifetime.end.p0(i64 8, ptr nonnull %88) #7
  call void @llvm.lifetime.end.p0(i64 48, ptr nonnull %87) #7
  br label %618

618:                                              ; preds = %617, %611
  %.pn124.pn = phi { ptr, i32 } [ %.pn124, %617 ], [ %612, %611 ]
  call void @llvm.lifetime.end.p0(i64 8, ptr nonnull %86) #7
  call void @llvm.lifetime.end.p0(i64 8, ptr nonnull %85) #7
  call void @llvm.lifetime.end.p0(i64 8, ptr nonnull %84) #7
  call void @llvm.lifetime.end.p0(i64 8, ptr nonnull %83) #7
  call void @llvm.lifetime.end.p0(i64 8, ptr nonnull %82) #7
  call void @llvm.lifetime.end.p0(i64 8, ptr nonnull %81) #7
  call void @llvm.lifetime.end.p0(i64 48, ptr nonnull %80) #7
  br label %619

619:                                              ; preds = %618, %609
  %.pn124.pn.pn = phi { ptr, i32 } [ %.pn124.pn, %618 ], [ %610, %609 ]
  call void @llvm.lifetime.end.p0(i64 8, ptr nonnull %79) #7
  call void @llvm.lifetime.end.p0(i64 8, ptr nonnull %78) #7
  call void @llvm.lifetime.end.p0(i64 8, ptr nonnull %77) #7
  call void @llvm.lifetime.end.p0(i64 8, ptr nonnull %76) #7
  call void @llvm.lifetime.end.p0(i64 8, ptr nonnull %75) #7
  call void @llvm.lifetime.end.p0(i64 8, ptr nonnull %74) #7
  call void @llvm.lifetime.end.p0(i64 48, ptr nonnull %73) #7
  br label %620

620:                                              ; preds = %619, %607
  %.pn124.pn.pn.pn = phi { ptr, i32 } [ %.pn124.pn.pn, %619 ], [ %608, %607 ]
  call void @llvm.lifetime.end.p0(i64 8, ptr nonnull %72) #7
  call void @llvm.lifetime.end.p0(i64 8, ptr nonnull %71) #7
  call void @llvm.lifetime.end.p0(i64 8, ptr nonnull %70) #7
  call void @llvm.lifetime.end.p0(i64 8, ptr nonnull %69) #7
  call void @llvm.lifetime.end.p0(i64 8, ptr nonnull %68) #7
  call void @llvm.lifetime.end.p0(i64 8, ptr nonnull %67) #7
  call void @llvm.lifetime.end.p0(i64 48, ptr nonnull %66) #7
  br label %621

621:                                              ; preds = %620, %605
  %.pn124.pn.pn.pn.pn = phi { ptr, i32 } [ %.pn124.pn.pn.pn, %620 ], [ %606, %605 ]
  call void @llvm.lifetime.end.p0(i64 8, ptr nonnull %65) #7
  call void @llvm.lifetime.end.p0(i64 8, ptr nonnull %64) #7
  call void @llvm.lifetime.end.p0(i64 8, ptr nonnull %63) #7
  call void @llvm.lifetime.end.p0(i64 8, ptr nonnull %62) #7
  call void @llvm.lifetime.end.p0(i64 8, ptr nonnull %61) #7
  call void @llvm.lifetime.end.p0(i64 8, ptr nonnull %60) #7
  call void @llvm.lifetime.end.p0(i64 48, ptr nonnull %59) #7
  br label %622

622:                                              ; preds = %621, %603
  %.pn124.pn.pn.pn.pn.pn = phi { ptr, i32 } [ %.pn124.pn.pn.pn.pn, %621 ], [ %604, %603 ]
  call void @llvm.lifetime.end.p0(i64 8, ptr nonnull %58) #7
  call void @llvm.lifetime.end.p0(i64 8, ptr nonnull %57) #7
  call void @llvm.lifetime.end.p0(i64 8, ptr nonnull %56) #7
  call void @llvm.lifetime.end.p0(i64 8, ptr nonnull %55) #7
  call void @llvm.lifetime.end.p0(i64 8, ptr nonnull %54) #7
  call void @llvm.lifetime.end.p0(i64 8, ptr nonnull %53) #7
  call void @llvm.lifetime.end.p0(i64 48, ptr nonnull %52) #7
  call void @llvm.lifetime.end.p0(i64 24, ptr nonnull %51) #7
  br label %962

623:                                              ; preds = %588
  %624 = call noundef ptr @_ZN13my_shared_ptrIN4urdf5JointEEptEv(ptr noundef nonnull align 8 dereferenceable(16) %27)
  %625 = getelementptr inbounds i8, ptr %624, i64 32
  %626 = load i32, ptr %625, align 8, !tbaa !37
  %627 = icmp eq i32 %626, 5
  br i1 %627, label %628, label %637

628:                                              ; preds = %623
  %629 = invoke noundef nonnull align 8 dereferenceable(8) ptr @_ZStlsISt11char_traitsIcEERSt13basic_ostreamIcT_ES5_PKc(ptr noundef nonnull align 8 dereferenceable(8) @_ZSt4cerr, ptr noundef nonnull @.str.14)
          to label %630 unwind label %490

630:                                              ; preds = %628
  %631 = call noundef ptr @_ZN13my_shared_ptrIN4urdf5JointEEptEv(ptr noundef nonnull align 8 dereferenceable(16) %27)
  %632 = invoke noundef nonnull align 8 dereferenceable(8) ptr @_ZStlsIcSt11char_traitsIcESaIcEERSt13basic_ostreamIT_T0_ES7_RKNSt7__cxx1112basic_stringIS4_S5_T1_EE(ptr noundef nonnull align 8 dereferenceable(8) %629, ptr noundef nonnull align 8 dereferenceable(32) %631)
          to label %633 unwind label %490

633:                                              ; preds = %630
  %634 = invoke noundef nonnull align 8 dereferenceable(8) ptr @_ZStlsISt11char_traitsIcEERSt13basic_ostreamIcT_ES5_PKc(ptr noundef nonnull align 8 dereferenceable(8) %632, ptr noundef nonnull @.str.17)
          to label %635 unwind label %490

635:                                              ; preds = %633
  %636 = invoke noundef nonnull align 8 dereferenceable(8) ptr @_ZNSolsEPFRSoS_E(ptr noundef nonnull align 8 dereferenceable(8) %634, ptr noundef nonnull @_ZSt4endlIcSt11char_traitsIcEERSt13basic_ostreamIT_T0_ES6_)
          to label %952 unwind label %490

637:                                              ; preds = %565, %602, %623, %582, %548
  call void @llvm.lifetime.start.p0(i64 24, ptr nonnull %94) #7
  invoke void @_ZN9Vector3_tC2Ev(ptr noundef nonnull align 8 dereferenceable(24) %94)
          to label %638 unwind label %785

638:                                              ; preds = %637
  call void @llvm.lifetime.start.p0(i64 24, ptr nonnull %95) #7
  invoke void @_ZN9Vector3_tC2Ev(ptr noundef nonnull align 8 dereferenceable(24) %95)
          to label %639 unwind label %787

639:                                              ; preds = %638
  %640 = call noundef ptr @_ZN13my_shared_ptrIN4urdf5JointEEptEv(ptr noundef nonnull align 8 dereferenceable(16) %27)
  %641 = getelementptr inbounds i8, ptr %640, i64 152
  %642 = invoke noundef nonnull align 8 dereferenceable(8) ptr @_ZN5Eigen15DenseCoeffsBaseINS_6MatrixIdLi3ELi1ELi0ELi3ELi1EEELi1EEixEl(ptr noundef nonnull align 1 dereferenceable(1) %94, i64 noundef 0)
          to label %643 unwind label %787

643:                                              ; preds = %639
  %644 = invoke noundef nonnull align 8 dereferenceable(8) ptr @_ZN5Eigen15DenseCoeffsBaseINS_6MatrixIdLi3ELi1ELi0ELi3ELi1EEELi1EEixEl(ptr noundef nonnull align 1 dereferenceable(1) %94, i64 noundef 1)
          to label %645 unwind label %787

645:                                              ; preds = %643
  %646 = invoke noundef nonnull align 8 dereferenceable(8) ptr @_ZN5Eigen15DenseCoeffsBaseINS_6MatrixIdLi3ELi1ELi0ELi3ELi1EEELi1EEixEl(ptr noundef nonnull align 1 dereferenceable(1) %94, i64 noundef 2)
          to label %647 unwind label %787

647:                                              ; preds = %645
  call void @_ZNK4urdf8Rotation6getRPYERdS1_S1_(ptr noundef nonnull align 8 dereferenceable(32) %641, ptr noundef nonnull align 8 dereferenceable(8) %642, ptr noundef nonnull align 8 dereferenceable(8) %644, ptr noundef nonnull align 8 dereferenceable(8) %646)
  %648 = call noundef ptr @_ZN13my_shared_ptrIN4urdf5JointEEptEv(ptr noundef nonnull align 8 dereferenceable(16) %27)
  %649 = getelementptr inbounds i8, ptr %648, i64 128
  %650 = call noundef ptr @_ZN13my_shared_ptrIN4urdf5JointEEptEv(ptr noundef nonnull align 8 dereferenceable(16) %27)
  %651 = getelementptr inbounds i8, ptr %650, i64 136
  %652 = call noundef ptr @_ZN13my_shared_ptrIN4urdf5JointEEptEv(ptr noundef nonnull align 8 dereferenceable(16) %27)
  %653 = getelementptr inbounds i8, ptr %652, i64 144
  invoke void @_ZN9Vector3_t3setERKdS1_S1_(ptr noundef nonnull align 8 dereferenceable(24) %95, ptr noundef nonnull align 8 dereferenceable(8) %649, ptr noundef nonnull align 8 dereferenceable(8) %651, ptr noundef nonnull align 8 dereferenceable(8) %653)
          to label %654 unwind label %787

654:                                              ; preds = %647
  call void @llvm.lifetime.start.p0(i64 96, ptr nonnull %96) #7
  call void @llvm.lifetime.start.p0(i64 96, ptr nonnull %97) #7
  call void @llvm.lifetime.start.p0(i64 96, ptr nonnull %98) #7
  call void @llvm.lifetime.start.p0(i64 96, ptr nonnull %99) #7
  %655 = invoke noundef nonnull align 8 dereferenceable(8) ptr @_ZN5Eigen15DenseCoeffsBaseINS_6MatrixIdLi3ELi1ELi0ELi3ELi1EEELi1EEixEl(ptr noundef nonnull align 1 dereferenceable(1) %94, i64 noundef 0)
          to label %656 unwind label %789

656:                                              ; preds = %654
  %657 = load double, ptr %655, align 8, !tbaa !13
  call void @llvm.lifetime.start.p0(i64 24, ptr nonnull %100) #7
  call void @llvm.lifetime.start.p0(i64 8, ptr nonnull %101) #7
  store double 1.000000e+00, ptr %101, align 8, !tbaa !13
  call void @llvm.lifetime.start.p0(i64 8, ptr nonnull %102) #7
  store double 0.000000e+00, ptr %102, align 8, !tbaa !13
  call void @llvm.lifetime.start.p0(i64 8, ptr nonnull %103) #7
  store double 0.000000e+00, ptr %103, align 8, !tbaa !13
  invoke void @_ZN9Vector3_tC2ERKdS1_S1_(ptr noundef nonnull align 8 dereferenceable(24) %100, ptr noundef nonnull align 8 dereferenceable(8) %101, ptr noundef nonnull align 8 dereferenceable(8) %102, ptr noundef nonnull align 8 dereferenceable(8) %103)
          to label %658 unwind label %791

658:                                              ; preds = %656
  invoke void @_ZN17RigidBodyDynamics4Math4XrotEdRK9Vector3_t(ptr dead_on_unwind nonnull writable sret(%"struct.RigidBodyDynamics::Math::SpatialTransform") align 8 %99, double noundef %657, ptr noundef nonnull align 8 dereferenceable(24) %100)
          to label %659 unwind label %791

659:                                              ; preds = %658
  call void @llvm.lifetime.start.p0(i64 96, ptr nonnull %104) #7
  %660 = invoke noundef nonnull align 8 dereferenceable(8) ptr @_ZN5Eigen15DenseCoeffsBaseINS_6MatrixIdLi3ELi1ELi0ELi3ELi1EEELi1EEixEl(ptr noundef nonnull align 1 dereferenceable(1) %94, i64 noundef 1)
          to label %661 unwind label %793

661:                                              ; preds = %659
  %662 = load double, ptr %660, align 8, !tbaa !13
  call void @llvm.lifetime.start.p0(i64 24, ptr nonnull %105) #7
  call void @llvm.lifetime.start.p0(i64 8, ptr nonnull %106) #7
  store double 0.000000e+00, ptr %106, align 8, !tbaa !13
  call void @llvm.lifetime.start.p0(i64 8, ptr nonnull %107) #7
  store double 1.000000e+00, ptr %107, align 8, !tbaa !13
  call void @llvm.lifetime.start.p0(i64 8, ptr nonnull %108) #7
  store double 0.000000e+00, ptr %108, align 8, !tbaa !13
  invoke void @_ZN9Vector3_tC2ERKdS1_S1_(ptr noundef nonnull align 8 dereferenceable(24) %105, ptr noundef nonnull align 8 dereferenceable(8) %106, ptr noundef nonnull align 8 dereferenceable(8) %107, ptr noundef nonnull align 8 dereferenceable(8) %108)
          to label %663 unwind label %795

663:                                              ; preds = %661
  invoke void @_ZN17RigidBodyDynamics4Math4XrotEdRK9Vector3_t(ptr dead_on_unwind nonnull writable sret(%"struct.RigidBodyDynamics::Math::SpatialTransform") align 8 %104, double noundef %662, ptr noundef nonnull align 8 dereferenceable(24) %105)
          to label %664 unwind label %795

664:                                              ; preds = %663
  invoke void @_ZNK17RigidBodyDynamics4Math16SpatialTransformmlERKS1_(ptr dead_on_unwind nonnull writable sret(%"struct.RigidBodyDynamics::Math::SpatialTransform") align 8 %98, ptr noundef nonnull align 8 dereferenceable(96) %99, ptr noundef nonnull align 8 dereferenceable(96) %104)
          to label %665 unwind label %795

665:                                              ; preds = %664
  call void @llvm.lifetime.start.p0(i64 96, ptr nonnull %109) #7
  %666 = invoke noundef nonnull align 8 dereferenceable(8) ptr @_ZN5Eigen15DenseCoeffsBaseINS_6MatrixIdLi3ELi1ELi0ELi3ELi1EEELi1EEixEl(ptr noundef nonnull align 1 dereferenceable(1) %94, i64 noundef 2)
          to label %667 unwind label %797

667:                                              ; preds = %665
  %668 = load double, ptr %666, align 8, !tbaa !13
  call void @llvm.lifetime.start.p0(i64 24, ptr nonnull %110) #7
  call void @llvm.lifetime.start.p0(i64 8, ptr nonnull %111) #7
  store double 0.000000e+00, ptr %111, align 8, !tbaa !13
  call void @llvm.lifetime.start.p0(i64 8, ptr nonnull %112) #7
  store double 0.000000e+00, ptr %112, align 8, !tbaa !13
  call void @llvm.lifetime.start.p0(i64 8, ptr nonnull %113) #7
  store double 1.000000e+00, ptr %113, align 8, !tbaa !13
  invoke void @_ZN9Vector3_tC2ERKdS1_S1_(ptr noundef nonnull align 8 dereferenceable(24) %110, ptr noundef nonnull align 8 dereferenceable(8) %111, ptr noundef nonnull align 8 dereferenceable(8) %112, ptr noundef nonnull align 8 dereferenceable(8) %113)
          to label %669 unwind label %799

669:                                              ; preds = %667
  invoke void @_ZN17RigidBodyDynamics4Math4XrotEdRK9Vector3_t(ptr dead_on_unwind nonnull writable sret(%"struct.RigidBodyDynamics::Math::SpatialTransform") align 8 %109, double noundef %668, ptr noundef nonnull align 8 dereferenceable(24) %110)
          to label %670 unwind label %799

670:                                              ; preds = %669
  invoke void @_ZNK17RigidBodyDynamics4Math16SpatialTransformmlERKS1_(ptr dead_on_unwind nonnull writable sret(%"struct.RigidBodyDynamics::Math::SpatialTransform") align 8 %97, ptr noundef nonnull align 8 dereferenceable(96) %98, ptr noundef nonnull align 8 dereferenceable(96) %109)
          to label %671 unwind label %799

671:                                              ; preds = %670
  call void @llvm.lifetime.start.p0(i64 96, ptr nonnull %114) #7
  call void @llvm.lifetime.start.p0(i64 24, ptr nonnull %115) #7
  invoke void @_ZN9Vector3_tC2ERKS_(ptr noundef nonnull align 8 dereferenceable(24) %115, ptr noundef nonnull align 8 dereferenceable(24) %95)
          to label %672 unwind label %801

672:                                              ; preds = %671
  invoke void @_ZN17RigidBodyDynamics4Math6XtransERK9Vector3_t(ptr dead_on_unwind nonnull writable sret(%"struct.RigidBodyDynamics::Math::SpatialTransform") align 8 %114, ptr noundef nonnull align 8 dereferenceable(24) %115)
          to label %673 unwind label %801

673:                                              ; preds = %672
  invoke void @_ZNK17RigidBodyDynamics4Math16SpatialTransformmlERKS1_(ptr dead_on_unwind nonnull writable sret(%"struct.RigidBodyDynamics::Math::SpatialTransform") align 8 %96, ptr noundef nonnull align 8 dereferenceable(96) %97, ptr noundef nonnull align 8 dereferenceable(96) %114)
          to label %674 unwind label %801

674:                                              ; preds = %673
  call void @llvm.lifetime.end.p0(i64 24, ptr nonnull %115) #7
  call void @llvm.lifetime.end.p0(i64 96, ptr nonnull %114) #7
  call void @llvm.lifetime.end.p0(i64 8, ptr nonnull %113) #7
  call void @llvm.lifetime.end.p0(i64 8, ptr nonnull %112) #7
  call void @llvm.lifetime.end.p0(i64 8, ptr nonnull %111) #7
  call void @llvm.lifetime.end.p0(i64 24, ptr nonnull %110) #7
  call void @llvm.lifetime.end.p0(i64 96, ptr nonnull %109) #7
  call void @llvm.lifetime.end.p0(i64 8, ptr nonnull %108) #7
  call void @llvm.lifetime.end.p0(i64 8, ptr nonnull %107) #7
  call void @llvm.lifetime.end.p0(i64 8, ptr nonnull %106) #7
  call void @llvm.lifetime.end.p0(i64 24, ptr nonnull %105) #7
  call void @llvm.lifetime.end.p0(i64 96, ptr nonnull %104) #7
  call void @llvm.lifetime.end.p0(i64 8, ptr nonnull %103) #7
  call void @llvm.lifetime.end.p0(i64 8, ptr nonnull %102) #7
  call void @llvm.lifetime.end.p0(i64 8, ptr nonnull %101) #7
  call void @llvm.lifetime.end.p0(i64 24, ptr nonnull %100) #7
  call void @llvm.lifetime.end.p0(i64 96, ptr nonnull %99) #7
  call void @llvm.lifetime.end.p0(i64 96, ptr nonnull %98) #7
  call void @llvm.lifetime.end.p0(i64 96, ptr nonnull %97) #7
  call void @llvm.lifetime.start.p0(i64 24, ptr nonnull %116) #7
  invoke void @_ZN9Vector3_tC2Ev(ptr noundef nonnull align 8 dereferenceable(24) %116)
          to label %675 unwind label %809

675:                                              ; preds = %674
  call void @llvm.lifetime.start.p0(i64 24, ptr nonnull %117) #7
  invoke void @_ZN9Vector3_tC2Ev(ptr noundef nonnull align 8 dereferenceable(24) %117)
          to label %676 unwind label %811

676:                                              ; preds = %675
  call void @llvm.lifetime.start.p0(i64 72, ptr nonnull %118) #7
  call void @llvm.lifetime.start.p0(i64 16, ptr nonnull %119) #7
  invoke void @_ZN5Eigen9DenseBaseINS_6MatrixIdLi3ELi3ELi0ELi3ELi3EEEE4ZeroEv(ptr dead_on_unwind nonnull writable sret(%"class.Eigen::CwiseNullaryOp") align 8 %119)
          to label %677 unwind label %813

677:                                              ; preds = %676
  invoke void @_ZN9Matrix3_tC2IN5Eigen14CwiseNullaryOpINS1_8internal18scalar_constant_opIdEENS1_6MatrixIdLi3ELi3ELi0ELi3ELi3EEEEEEERKNS1_10MatrixBaseIT_EE(ptr noundef nonnull align 8 dereferenceable(72) %118, ptr noundef nonnull align 1 dereferenceable(1) %119)
          to label %678 unwind label %813

678:                                              ; preds = %677
  call void @llvm.lifetime.end.p0(i64 16, ptr nonnull %119) #7
  call void @llvm.lifetime.start.p0(i64 8, ptr nonnull %120) #7
  store double 0.000000e+00, ptr %120, align 8, !tbaa !13
  %679 = call noundef ptr @_ZN13my_shared_ptrIN4urdf4LinkEEptEv(ptr noundef nonnull align 8 dereferenceable(16) %29)
  %680 = getelementptr inbounds i8, ptr %679, i64 32
  %681 = call noundef zeroext i1 @_ZNK13my_shared_ptrIN4urdf8InertialEEcvbEv(ptr noundef nonnull align 8 dereferenceable(16) %680)
  br i1 %681, label %682, label %819

682:                                              ; preds = %678
  %683 = call noundef ptr @_ZN13my_shared_ptrIN4urdf4LinkEEptEv(ptr noundef nonnull align 8 dereferenceable(16) %29)
  %684 = getelementptr inbounds i8, ptr %683, i64 32
  %685 = call noundef ptr @_ZN13my_shared_ptrIN4urdf8InertialEEptEv(ptr noundef nonnull align 8 dereferenceable(16) %684)
  %686 = getelementptr inbounds i8, ptr %685, i64 56
  %687 = load double, ptr %686, align 8, !tbaa !5
  store double %687, ptr %120, align 8, !tbaa !13
  %688 = call noundef ptr @_ZN13my_shared_ptrIN4urdf4LinkEEptEv(ptr noundef nonnull align 8 dereferenceable(16) %29)
  %689 = getelementptr inbounds i8, ptr %688, i64 32
  %690 = call noundef ptr @_ZN13my_shared_ptrIN4urdf8InertialEEptEv(ptr noundef nonnull align 8 dereferenceable(16) %689)
  %691 = call noundef ptr @_ZN13my_shared_ptrIN4urdf4LinkEEptEv(ptr noundef nonnull align 8 dereferenceable(16) %29)
  %692 = getelementptr inbounds i8, ptr %691, i64 32
  %693 = call noundef ptr @_ZN13my_shared_ptrIN4urdf8InertialEEptEv(ptr noundef nonnull align 8 dereferenceable(16) %692)
  %694 = getelementptr inbounds i8, ptr %693, i64 8
  %695 = call noundef ptr @_ZN13my_shared_ptrIN4urdf4LinkEEptEv(ptr noundef nonnull align 8 dereferenceable(16) %29)
  %696 = getelementptr inbounds i8, ptr %695, i64 32
  %697 = call noundef ptr @_ZN13my_shared_ptrIN4urdf8InertialEEptEv(ptr noundef nonnull align 8 dereferenceable(16) %696)
  %698 = getelementptr inbounds i8, ptr %697, i64 16
  invoke void @_ZN9Vector3_t3setERKdS1_S1_(ptr noundef nonnull align 8 dereferenceable(24) %116, ptr noundef nonnull align 8 dereferenceable(8) %690, ptr noundef nonnull align 8 dereferenceable(8) %694, ptr noundef nonnull align 8 dereferenceable(8) %698)
          to label %699 unwind label %815

699:                                              ; preds = %682
  %700 = call noundef ptr @_ZN13my_shared_ptrIN4urdf4LinkEEptEv(ptr noundef nonnull align 8 dereferenceable(16) %29)
  %701 = getelementptr inbounds i8, ptr %700, i64 32
  %702 = call noundef ptr @_ZN13my_shared_ptrIN4urdf8InertialEEptEv(ptr noundef nonnull align 8 dereferenceable(16) %701)
  %703 = getelementptr inbounds i8, ptr %702, i64 24
  %704 = invoke noundef nonnull align 8 dereferenceable(8) ptr @_ZN5Eigen15DenseCoeffsBaseINS_6MatrixIdLi3ELi1ELi0ELi3ELi1EEELi1EEixEl(ptr noundef nonnull align 1 dereferenceable(1) %117, i64 noundef 0)
          to label %705 unwind label %815

705:                                              ; preds = %699
  %706 = invoke noundef nonnull align 8 dereferenceable(8) ptr @_ZN5Eigen15DenseCoeffsBaseINS_6MatrixIdLi3ELi1ELi0ELi3ELi1EEELi1EEixEl(ptr noundef nonnull align 1 dereferenceable(1) %117, i64 noundef 1)
          to label %707 unwind label %815

707:                                              ; preds = %705
  %708 = invoke noundef nonnull align 8 dereferenceable(8) ptr @_ZN5Eigen15DenseCoeffsBaseINS_6MatrixIdLi3ELi1ELi0ELi3ELi1EEELi1EEixEl(ptr noundef nonnull align 1 dereferenceable(1) %117, i64 noundef 2)
          to label %709 unwind label %815

709:                                              ; preds = %707
  call void @_ZNK4urdf8Rotation6getRPYERdS1_S1_(ptr noundef nonnull align 8 dereferenceable(32) %703, ptr noundef nonnull align 8 dereferenceable(8) %704, ptr noundef nonnull align 8 dereferenceable(8) %706, ptr noundef nonnull align 8 dereferenceable(8) %708)
  %710 = call noundef ptr @_ZN13my_shared_ptrIN4urdf4LinkEEptEv(ptr noundef nonnull align 8 dereferenceable(16) %29)
  %711 = getelementptr inbounds i8, ptr %710, i64 32
  %712 = call noundef ptr @_ZN13my_shared_ptrIN4urdf8InertialEEptEv(ptr noundef nonnull align 8 dereferenceable(16) %711)
  %713 = getelementptr inbounds i8, ptr %712, i64 64
  %714 = load double, ptr %713, align 8, !tbaa !14
  %715 = invoke noundef nonnull align 8 dereferenceable(8) ptr @_ZN5Eigen15DenseCoeffsBaseINS_6MatrixIdLi3ELi3ELi0ELi3ELi3EEELi1EEclEll(ptr noundef nonnull align 1 dereferenceable(1) %118, i64 noundef 0, i64 noundef 0)
          to label %716 unwind label %815

716:                                              ; preds = %709
  store double %714, ptr %715, align 8, !tbaa !13
  %717 = call noundef ptr @_ZN13my_shared_ptrIN4urdf4LinkEEptEv(ptr noundef nonnull align 8 dereferenceable(16) %29)
  %718 = getelementptr inbounds i8, ptr %717, i64 32
  %719 = call noundef ptr @_ZN13my_shared_ptrIN4urdf8InertialEEptEv(ptr noundef nonnull align 8 dereferenceable(16) %718)
  %720 = getelementptr inbounds i8, ptr %719, i64 72
  %721 = load double, ptr %720, align 8, !tbaa !15
  %722 = invoke noundef nonnull align 8 dereferenceable(8) ptr @_ZN5Eigen15DenseCoeffsBaseINS_6MatrixIdLi3ELi3ELi0ELi3ELi3EEELi1EEclEll(ptr noundef nonnull align 1 dereferenceable(1) %118, i64 noundef 0, i64 noundef 1)
          to label %723 unwind label %815

723:                                              ; preds = %716
  store double %721, ptr %722, align 8, !tbaa !13
  %724 = call noundef ptr @_ZN13my_shared_ptrIN4urdf4LinkEEptEv(ptr noundef nonnull align 8 dereferenceable(16) %29)
  %725 = getelementptr inbounds i8, ptr %724, i64 32
  %726 = call noundef ptr @_ZN13my_shared_ptrIN4urdf8InertialEEptEv(ptr noundef nonnull align 8 dereferenceable(16) %725)
  %727 = getelementptr inbounds i8, ptr %726, i64 80
  %728 = load double, ptr %727, align 8, !tbaa !16
  %729 = invoke noundef nonnull align 8 dereferenceable(8) ptr @_ZN5Eigen15DenseCoeffsBaseINS_6MatrixIdLi3ELi3ELi0ELi3ELi3EEELi1EEclEll(ptr noundef nonnull align 1 dereferenceable(1) %118, i64 noundef 0, i64 noundef 2)
          to label %730 unwind label %815

730:                                              ; preds = %723
  store double %728, ptr %729, align 8, !tbaa !13
  %731 = call noundef ptr @_ZN13my_shared_ptrIN4urdf4LinkEEptEv(ptr noundef nonnull align 8 dereferenceable(16) %29)
  %732 = getelementptr inbounds i8, ptr %731, i64 32
  %733 = call noundef ptr @_ZN13my_shared_ptrIN4urdf8InertialEEptEv(ptr noundef nonnull align 8 dereferenceable(16) %732)
  %734 = getelementptr inbounds i8, ptr %733, i64 72
  %735 = load double, ptr %734, align 8, !tbaa !15
  %736 = invoke noundef nonnull align 8 dereferenceable(8) ptr @_ZN5Eigen15DenseCoeffsBaseINS_6MatrixIdLi3ELi3ELi0ELi3ELi3EEELi1EEclEll(ptr noundef nonnull align 1 dereferenceable(1) %118, i64 noundef 1, i64 noundef 0)
          to label %737 unwind label %815

737:                                              ; preds = %730
  store double %735, ptr %736, align 8, !tbaa !13
  %738 = call noundef ptr @_ZN13my_shared_ptrIN4urdf4LinkEEptEv(ptr noundef nonnull align 8 dereferenceable(16) %29)
  %739 = getelementptr inbounds i8, ptr %738, i64 32
  %740 = call noundef ptr @_ZN13my_shared_ptrIN4urdf8InertialEEptEv(ptr noundef nonnull align 8 dereferenceable(16) %739)
  %741 = getelementptr inbounds i8, ptr %740, i64 88
  %742 = load double, ptr %741, align 8, !tbaa !17
  %743 = invoke noundef nonnull align 8 dereferenceable(8) ptr @_ZN5Eigen15DenseCoeffsBaseINS_6MatrixIdLi3ELi3ELi0ELi3ELi3EEELi1EEclEll(ptr noundef nonnull align 1 dereferenceable(1) %118, i64 noundef 1, i64 noundef 1)
          to label %744 unwind label %815

744:                                              ; preds = %737
  store double %742, ptr %743, align 8, !tbaa !13
  %745 = call noundef ptr @_ZN13my_shared_ptrIN4urdf4LinkEEptEv(ptr noundef nonnull align 8 dereferenceable(16) %29)
  %746 = getelementptr inbounds i8, ptr %745, i64 32
  %747 = call noundef ptr @_ZN13my_shared_ptrIN4urdf8InertialEEptEv(ptr noundef nonnull align 8 dereferenceable(16) %746)
  %748 = getelementptr inbounds i8, ptr %747, i64 96
  %749 = load double, ptr %748, align 8, !tbaa !18
  %750 = invoke noundef nonnull align 8 dereferenceable(8) ptr @_ZN5Eigen15DenseCoeffsBaseINS_6MatrixIdLi3ELi3ELi0ELi3ELi3EEELi1EEclEll(ptr noundef nonnull align 1 dereferenceable(1) %118, i64 noundef 1, i64 noundef 2)
          to label %751 unwind label %815

751:                                              ; preds = %744
  store double %749, ptr %750, align 8, !tbaa !13
  %752 = call noundef ptr @_ZN13my_shared_ptrIN4urdf4LinkEEptEv(ptr noundef nonnull align 8 dereferenceable(16) %29)
  %753 = getelementptr inbounds i8, ptr %752, i64 32
  %754 = call noundef ptr @_ZN13my_shared_ptrIN4urdf8InertialEEptEv(ptr noundef nonnull align 8 dereferenceable(16) %753)
  %755 = getelementptr inbounds i8, ptr %754, i64 80
  %756 = load double, ptr %755, align 8, !tbaa !16
  %757 = invoke noundef nonnull align 8 dereferenceable(8) ptr @_ZN5Eigen15DenseCoeffsBaseINS_6MatrixIdLi3ELi3ELi0ELi3ELi3EEELi1EEclEll(ptr noundef nonnull align 1 dereferenceable(1) %118, i64 noundef 2, i64 noundef 0)
          to label %758 unwind label %815

758:                                              ; preds = %751
  store double %756, ptr %757, align 8, !tbaa !13
  %759 = call noundef ptr @_ZN13my_shared_ptrIN4urdf4LinkEEptEv(ptr noundef nonnull align 8 dereferenceable(16) %29)
  %760 = getelementptr inbounds i8, ptr %759, i64 32
  %761 = call noundef ptr @_ZN13my_shared_ptrIN4urdf8InertialEEptEv(ptr noundef nonnull align 8 dereferenceable(16) %760)
  %762 = getelementptr inbounds i8, ptr %761, i64 96
  %763 = load double, ptr %762, align 8, !tbaa !18
  %764 = invoke noundef nonnull align 8 dereferenceable(8) ptr @_ZN5Eigen15DenseCoeffsBaseINS_6MatrixIdLi3ELi3ELi0ELi3ELi3EEELi1EEclEll(ptr noundef nonnull align 1 dereferenceable(1) %118, i64 noundef 2, i64 noundef 1)
          to label %765 unwind label %815

765:                                              ; preds = %758
  store double %763, ptr %764, align 8, !tbaa !13
  %766 = call noundef ptr @_ZN13my_shared_ptrIN4urdf4LinkEEptEv(ptr noundef nonnull align 8 dereferenceable(16) %29)
  %767 = getelementptr inbounds i8, ptr %766, i64 32
  %768 = call noundef ptr @_ZN13my_shared_ptrIN4urdf8InertialEEptEv(ptr noundef nonnull align 8 dereferenceable(16) %767)
  %769 = getelementptr inbounds i8, ptr %768, i64 104
  %770 = load double, ptr %769, align 8, !tbaa !19
  %771 = invoke noundef nonnull align 8 dereferenceable(8) ptr @_ZN5Eigen15DenseCoeffsBaseINS_6MatrixIdLi3ELi3ELi0ELi3ELi3EEELi1EEclEll(ptr noundef nonnull align 1 dereferenceable(1) %118, i64 noundef 2, i64 noundef 2)
          to label %772 unwind label %815

772:                                              ; preds = %765
  store double %770, ptr %771, align 8, !tbaa !13
  call void @llvm.lifetime.start.p0(i64 24, ptr nonnull %121) #7
  call void @llvm.lifetime.start.p0(i64 8, ptr nonnull %122) #7
  store double 0.000000e+00, ptr %122, align 8, !tbaa !13
  call void @llvm.lifetime.start.p0(i64 8, ptr nonnull %123) #7
  store double 0.000000e+00, ptr %123, align 8, !tbaa !13
  call void @llvm.lifetime.start.p0(i64 8, ptr nonnull %124) #7
  store double 0.000000e+00, ptr %124, align 8, !tbaa !13
  invoke void @_ZN9Vector3_tC2ERKdS1_S1_(ptr noundef nonnull align 8 dereferenceable(24) %121, ptr noundef nonnull align 8 dereferenceable(8) %122, ptr noundef nonnull align 8 dereferenceable(8) %123, ptr noundef nonnull align 8 dereferenceable(8) %124)
          to label %773 unwind label %817

773:                                              ; preds = %772
  %774 = invoke noundef zeroext i1 @_ZNK5Eigen10MatrixBaseINS_6MatrixIdLi3ELi1ELi0ELi3ELi1EEEEneIS2_EEbRKNS0_IT_EE(ptr noundef nonnull align 1 dereferenceable(1) %117, ptr noundef nonnull align 1 dereferenceable(1) %121)
          to label %775 unwind label %817

775:                                              ; preds = %773
  call void @llvm.lifetime.end.p0(i64 8, ptr nonnull %124) #7
  call void @llvm.lifetime.end.p0(i64 8, ptr nonnull %123) #7
  call void @llvm.lifetime.end.p0(i64 8, ptr nonnull %122) #7
  call void @llvm.lifetime.end.p0(i64 24, ptr nonnull %121) #7
  br i1 %774, label %776, label %819

776:                                              ; preds = %775
  %777 = invoke noundef nonnull align 8 dereferenceable(8) ptr @_ZStlsISt11char_traitsIcEERSt13basic_ostreamIcT_ES5_PKc(ptr noundef nonnull align 8 dereferenceable(8) @_ZSt4cerr, ptr noundef nonnull @.str.18)
          to label %778 unwind label %815

778:                                              ; preds = %776
  %779 = call noundef ptr @_ZN13my_shared_ptrIN4urdf4LinkEEptEv(ptr noundef nonnull align 8 dereferenceable(16) %29)
  %780 = invoke noundef nonnull align 8 dereferenceable(8) ptr @_ZStlsIcSt11char_traitsIcESaIcEERSt13basic_ostreamIT_T0_ES7_RKNSt7__cxx1112basic_stringIS4_S5_T1_EE(ptr noundef nonnull align 8 dereferenceable(8) %777, ptr noundef nonnull align 8 dereferenceable(32) %779)
          to label %781 unwind label %815

781:                                              ; preds = %778
  %782 = invoke noundef nonnull align 8 dereferenceable(8) ptr @_ZStlsISt11char_traitsIcEERSt13basic_ostreamIcT_ES5_PKc(ptr noundef nonnull align 8 dereferenceable(8) %780, ptr noundef nonnull @.str.19)
          to label %783 unwind label %815

783:                                              ; preds = %781
  %784 = invoke noundef nonnull align 8 dereferenceable(8) ptr @_ZNSolsEPFRSoS_E(ptr noundef nonnull align 8 dereferenceable(8) %782, ptr noundef nonnull @_ZSt4endlIcSt11char_traitsIcEERSt13basic_ostreamIT_T0_ES6_)
          to label %951 unwind label %815

785:                                              ; preds = %637
  %786 = landingpad { ptr, i32 }
          cleanup
  br label %961

787:                                              ; preds = %647, %645, %643, %639, %638
  %788 = landingpad { ptr, i32 }
          cleanup
  br label %960

789:                                              ; preds = %654
  %790 = landingpad { ptr, i32 }
          cleanup
  br label %808

791:                                              ; preds = %658, %656
  %792 = landingpad { ptr, i32 }
          cleanup
  br label %807

793:                                              ; preds = %659
  %794 = landingpad { ptr, i32 }
          cleanup
  br label %806

795:                                              ; preds = %664, %663, %661
  %796 = landingpad { ptr, i32 }
          cleanup
  br label %805

797:                                              ; preds = %665
  %798 = landingpad { ptr, i32 }
          cleanup
  br label %804

799:                                              ; preds = %670, %669, %667
  %800 = landingpad { ptr, i32 }
          cleanup
  br label %803

801:                                              ; preds = %673, %672, %671
  %802 = landingpad { ptr, i32 }
          cleanup
  call void @llvm.lifetime.end.p0(i64 24, ptr nonnull %115) #7
  call void @llvm.lifetime.end.p0(i64 96, ptr nonnull %114) #7
  br label %803

803:                                              ; preds = %801, %799
  %.pn145 = phi { ptr, i32 } [ %802, %801 ], [ %800, %799 ]
  call void @llvm.lifetime.end.p0(i64 8, ptr nonnull %113) #7
  call void @llvm.lifetime.end.p0(i64 8, ptr nonnull %112) #7
  call void @llvm.lifetime.end.p0(i64 8, ptr nonnull %111) #7
  call void @llvm.lifetime.end.p0(i64 24, ptr nonnull %110) #7
  br label %804

804:                                              ; preds = %803, %797
  %.pn145.pn = phi { ptr, i32 } [ %.pn145, %803 ], [ %798, %797 ]
  call void @llvm.lifetime.end.p0(i64 96, ptr nonnull %109) #7
  br label %805

805:                                              ; preds = %804, %795
  %.pn145.pn.pn = phi { ptr, i32 } [ %.pn145.pn, %804 ], [ %796, %795 ]
  call void @llvm.lifetime.end.p0(i64 8, ptr nonnull %108) #7
  call void @llvm.lifetime.end.p0(i64 8, ptr nonnull %107) #7
  call void @llvm.lifetime.end.p0(i64 8, ptr nonnull %106) #7
  call void @llvm.lifetime.end.p0(i64 24, ptr nonnull %105) #7
  br label %806

806:                                              ; preds = %805, %793
  %.pn145.pn.pn.pn = phi { ptr, i32 } [ %.pn145.pn.pn, %805 ], [ %794, %793 ]
  call void @llvm.lifetime.end.p0(i64 96, ptr nonnull %104) #7
  br label %807

807:                                              ; preds = %806, %791
  %.pn145.pn.pn.pn.pn = phi { ptr, i32 } [ %.pn145.pn.pn.pn, %806 ], [ %792, %791 ]
  call void @llvm.lifetime.end.p0(i64 8, ptr nonnull %103) #7
  call void @llvm.lifetime.end.p0(i64 8, ptr nonnull %102) #7
  call void @llvm.lifetime.end.p0(i64 8, ptr nonnull %101) #7
  call void @llvm.lifetime.end.p0(i64 24, ptr nonnull %100) #7
  br label %808

808:                                              ; preds = %807, %789
  %.pn145.pn.pn.pn.pn.pn = phi { ptr, i32 } [ %.pn145.pn.pn.pn.pn, %807 ], [ %790, %789 ]
  call void @llvm.lifetime.end.p0(i64 96, ptr nonnull %99) #7
  call void @llvm.lifetime.end.p0(i64 96, ptr nonnull %98) #7
  call void @llvm.lifetime.end.p0(i64 96, ptr nonnull %97) #7
  br label %959

809:                                              ; preds = %674
  %810 = landingpad { ptr, i32 }
          cleanup
  br label %958

811:                                              ; preds = %675
  %812 = landingpad { ptr, i32 }
          cleanup
  br label %957

813:                                              ; preds = %677, %676
  %814 = landingpad { ptr, i32 }
          cleanup
  call void @llvm.lifetime.end.p0(i64 16, ptr nonnull %119) #7
  br label %956

815:                                              ; preds = %783, %781, %778, %776, %765, %758, %751, %744, %737, %730, %723, %716, %709, %707, %705, %699, %682
  %816 = landingpad { ptr, i32 }
          cleanup
  br label %955

817:                                              ; preds = %773, %772
  %818 = landingpad { ptr, i32 }
          cleanup
  call void @llvm.lifetime.end.p0(i64 8, ptr nonnull %124) #7
  call void @llvm.lifetime.end.p0(i64 8, ptr nonnull %123) #7
  call void @llvm.lifetime.end.p0(i64 8, ptr nonnull %122) #7
  call void @llvm.lifetime.end.p0(i64 24, ptr nonnull %121) #7
  br label %955

819:                                              ; preds = %775, %678
  call void @llvm.lifetime.start.p0(i64 112, ptr nonnull %125) #7
  invoke void @_ZN17RigidBodyDynamics4BodyC2ERKdRK9Vector3_tRK9Matrix3_t(ptr noundef nonnull align 8 dereferenceable(105) %125, ptr noundef nonnull align 8 dereferenceable(8) %120, ptr noundef nonnull align 8 dereferenceable(24) %116, ptr noundef nonnull align 8 dereferenceable(72) %118)
          to label %820 unwind label %849

820:                                              ; preds = %819
  br i1 %3, label %821, label %895

821:                                              ; preds = %820
  %822 = invoke noundef nonnull align 8 dereferenceable(8) ptr @_ZStlsISt11char_traitsIcEERSt13basic_ostreamIcT_ES5_PKc(ptr noundef nonnull align 8 dereferenceable(8) @_ZSt4cout, ptr noundef nonnull @.str.20)
          to label %823 unwind label %851

823:                                              ; preds = %821
  %824 = call noundef ptr @_ZN13my_shared_ptrIN4urdf4LinkEEptEv(ptr noundef nonnull align 8 dereferenceable(16) %29)
  %825 = invoke noundef nonnull align 8 dereferenceable(8) ptr @_ZStlsIcSt11char_traitsIcESaIcEERSt13basic_ostreamIT_T0_ES7_RKNSt7__cxx1112basic_stringIS4_S5_T1_EE(ptr noundef nonnull align 8 dereferenceable(8) %822, ptr noundef nonnull align 8 dereferenceable(32) %824)
          to label %826 unwind label %851

826:                                              ; preds = %823
  %827 = invoke noundef nonnull align 8 dereferenceable(8) ptr @_ZNSolsEPFRSoS_E(ptr noundef nonnull align 8 dereferenceable(8) %825, ptr noundef nonnull @_ZSt4endlIcSt11char_traitsIcEERSt13basic_ostreamIT_T0_ES6_)
          to label %828 unwind label %851

828:                                              ; preds = %826
  %829 = invoke noundef nonnull align 8 dereferenceable(8) ptr @_ZStlsISt11char_traitsIcEERSt13basic_ostreamIcT_ES5_PKc(ptr noundef nonnull align 8 dereferenceable(8) @_ZSt4cout, ptr noundef nonnull @.str.21)
          to label %830 unwind label %851

830:                                              ; preds = %828
  %831 = invoke noundef nonnull align 8 dereferenceable(8) ptr @_ZNSolsEj(ptr noundef nonnull align 8 dereferenceable(8) %829, i32 noundef %435)
          to label %832 unwind label %851

832:                                              ; preds = %830
  %833 = invoke noundef nonnull align 8 dereferenceable(8) ptr @_ZNSolsEPFRSoS_E(ptr noundef nonnull align 8 dereferenceable(8) %831, ptr noundef nonnull @_ZSt4endlIcSt11char_traitsIcEERSt13basic_ostreamIT_T0_ES6_)
          to label %834 unwind label %851

834:                                              ; preds = %832
  %835 = invoke noundef nonnull align 8 dereferenceable(8) ptr @_ZStlsISt11char_traitsIcEERSt13basic_ostreamIcT_ES5_PKc(ptr noundef nonnull align 8 dereferenceable(8) @_ZSt4cout, ptr noundef nonnull @.str.4)
          to label %836 unwind label %851

836:                                              ; preds = %834
  %837 = invoke noundef nonnull align 8 dereferenceable(8) ptr @_ZN17RigidBodyDynamics4MathlsERSoRKNS0_16SpatialTransformE(ptr noundef nonnull align 8 dereferenceable(8) %835, ptr noundef nonnull align 8 dereferenceable(96) %96)
          to label %838 unwind label %851

838:                                              ; preds = %836
  %839 = invoke noundef nonnull align 8 dereferenceable(8) ptr @_ZNSolsEPFRSoS_E(ptr noundef nonnull align 8 dereferenceable(8) %837, ptr noundef nonnull @_ZSt4endlIcSt11char_traitsIcEERSt13basic_ostreamIT_T0_ES6_)
          to label %840 unwind label %851

840:                                              ; preds = %838
  %841 = invoke noundef nonnull align 8 dereferenceable(8) ptr @_ZStlsISt11char_traitsIcEERSt13basic_ostreamIcT_ES5_PKc(ptr noundef nonnull align 8 dereferenceable(8) @_ZSt4cout, ptr noundef nonnull @.str.22)
          to label %842 unwind label %851

842:                                              ; preds = %840
  %843 = load i32, ptr %348, align 4, !tbaa !51
  %844 = invoke noundef nonnull align 8 dereferenceable(8) ptr @_ZNSolsEj(ptr noundef nonnull align 8 dereferenceable(8) %841, i32 noundef %843)
          to label %845 unwind label %851

845:                                              ; preds = %842
  %846 = invoke noundef nonnull align 8 dereferenceable(8) ptr @_ZNSolsEPFRSoS_E(ptr noundef nonnull align 8 dereferenceable(8) %844, ptr noundef nonnull @_ZSt4endlIcSt11char_traitsIcEERSt13basic_ostreamIT_T0_ES6_)
          to label %.preheader unwind label %851

.preheader:                                       ; preds = %845
  %847 = load i32, ptr %348, align 4, !tbaa !51
  %.not260 = icmp eq i32 %847, 0
  br i1 %.not260, label %._crit_edge254, label %.lr.ph253

._crit_edge254:                                   ; preds = %867, %.preheader
  %848 = invoke noundef nonnull align 8 dereferenceable(8) ptr @_ZStlsISt11char_traitsIcEERSt13basic_ostreamIcT_ES5_PKc(ptr noundef nonnull align 8 dereferenceable(8) @_ZSt4cout, ptr noundef nonnull @.str.7)
          to label %875 unwind label %851

849:                                              ; preds = %819
  %850 = landingpad { ptr, i32 }
          cleanup
  br label %954

851:                                              ; preds = %943, %893, %890, %888, %886, %883, %881, %879, %877, %875, %._crit_edge254, %845, %842, %840, %838, %836, %834, %832, %830, %828, %826, %823, %821
  %852 = landingpad { ptr, i32 }
          cleanup
  br label %953

.lr.ph253:                                        ; preds = %.preheader, %867
  %indvars.iv = phi i64 [ %indvars.iv.next, %867 ], [ 0, %.preheader ]
  %853 = invoke noundef nonnull align 8 dereferenceable(8) ptr @_ZStlsISt11char_traitsIcEERSt13basic_ostreamIcT_ES5_PKc(ptr noundef nonnull align 8 dereferenceable(8) @_ZSt4cout, ptr noundef nonnull @.str.23)
          to label %854 unwind label %871

854:                                              ; preds = %.lr.ph253
  %855 = trunc nuw i64 %indvars.iv to i32
  %856 = invoke noundef nonnull align 8 dereferenceable(8) ptr @_ZNSolsEj(ptr noundef nonnull align 8 dereferenceable(8) %853, i32 noundef %855)
          to label %857 unwind label %871

857:                                              ; preds = %854
  %858 = invoke noundef nonnull align 8 dereferenceable(8) ptr @_ZStlsISt11char_traitsIcEERSt13basic_ostreamIcT_ES5_PKc(ptr noundef nonnull align 8 dereferenceable(8) %856, ptr noundef nonnull @.str.24)
          to label %859 unwind label %871

859:                                              ; preds = %857
  call void @llvm.lifetime.start.p0(i64 8, ptr nonnull %126) #7
  %860 = load ptr, ptr %30, align 8, !tbaa !54
  %861 = getelementptr inbounds %class.SpatialVector_t, ptr %860, i64 %indvars.iv
  %862 = invoke ptr @_ZN5Eigen9DenseBaseINS_6MatrixIdLi6ELi1ELi0ELi6ELi1EEEE9transposeEv(ptr noundef nonnull align 1 dereferenceable(1) %861)
          to label %863 unwind label %873

863:                                              ; preds = %859
  store ptr %862, ptr %126, align 8
  %864 = invoke noundef nonnull align 8 dereferenceable(8) ptr @_ZN5EigenlsINS_9TransposeINS_6MatrixIdLi6ELi1ELi0ELi6ELi1EEEEEEERSoS5_RKNS_9DenseBaseIT_EE(ptr noundef nonnull align 8 dereferenceable(8) %858, ptr noundef nonnull align 1 dereferenceable(1) %126)
          to label %865 unwind label %873

865:                                              ; preds = %863
  %866 = invoke noundef nonnull align 8 dereferenceable(8) ptr @_ZNSolsEPFRSoS_E(ptr noundef nonnull align 8 dereferenceable(8) %864, ptr noundef nonnull @_ZSt4endlIcSt11char_traitsIcEERSt13basic_ostreamIT_T0_ES6_)
          to label %867 unwind label %873

867:                                              ; preds = %865
  call void @llvm.lifetime.end.p0(i64 8, ptr nonnull %126) #7
  %indvars.iv.next = add nuw nsw i64 %indvars.iv, 1
  %868 = load i32, ptr %348, align 4, !tbaa !51
  %869 = zext i32 %868 to i64
  %870 = icmp ult i64 %indvars.iv.next, %869
  br i1 %870, label %.lr.ph253, label %._crit_edge254, !llvm.loop !55

871:                                              ; preds = %857, %854, %.lr.ph253
  %872 = landingpad { ptr, i32 }
          cleanup
  br label %953

873:                                              ; preds = %865, %863, %859
  %874 = landingpad { ptr, i32 }
          cleanup
  call void @llvm.lifetime.end.p0(i64 8, ptr nonnull %126) #7
  br label %953

875:                                              ; preds = %._crit_edge254
  %876 = invoke noundef nonnull align 8 dereferenceable(8) ptr @_ZNSolsEPFRSoS_E(ptr noundef nonnull align 8 dereferenceable(8) %848, ptr noundef nonnull @_ZSt4endlIcSt11char_traitsIcEERSt13basic_ostreamIT_T0_ES6_)
          to label %877 unwind label %851

877:                                              ; preds = %875
  %878 = invoke noundef nonnull align 8 dereferenceable(8) ptr @_ZN5EigenlsINS_6MatrixIdLi3ELi3ELi0ELi3ELi3EEEEERSoS3_RKNS_9DenseBaseIT_EE(ptr noundef nonnull align 8 dereferenceable(8) %876, ptr noundef nonnull align 1 dereferenceable(1) %349)
          to label %879 unwind label %851

879:                                              ; preds = %877
  %880 = invoke noundef nonnull align 8 dereferenceable(8) ptr @_ZNSolsEPFRSoS_E(ptr noundef nonnull align 8 dereferenceable(8) %878, ptr noundef nonnull @_ZSt4endlIcSt11char_traitsIcEERSt13basic_ostreamIT_T0_ES6_)
          to label %881 unwind label %851

881:                                              ; preds = %879
  %882 = invoke noundef nonnull align 8 dereferenceable(8) ptr @_ZStlsISt11char_traitsIcEERSt13basic_ostreamIcT_ES5_PKc(ptr noundef nonnull align 8 dereferenceable(8) @_ZSt4cout, ptr noundef nonnull @.str.8)
          to label %883 unwind label %851

883:                                              ; preds = %881
  %884 = load double, ptr %125, align 8, !tbaa !20
  %885 = invoke noundef nonnull align 8 dereferenceable(8) ptr @_ZNSolsEd(ptr noundef nonnull align 8 dereferenceable(8) %882, double noundef %884)
          to label %886 unwind label %851

886:                                              ; preds = %883
  %887 = invoke noundef nonnull align 8 dereferenceable(8) ptr @_ZNSolsEPFRSoS_E(ptr noundef nonnull align 8 dereferenceable(8) %885, ptr noundef nonnull @_ZSt4endlIcSt11char_traitsIcEERSt13basic_ostreamIT_T0_ES6_)
          to label %888 unwind label %851

888:                                              ; preds = %886
  %889 = invoke noundef nonnull align 8 dereferenceable(8) ptr @_ZStlsISt11char_traitsIcEERSt13basic_ostreamIcT_ES5_PKc(ptr noundef nonnull align 8 dereferenceable(8) @_ZSt4cout, ptr noundef nonnull @.str.9)
          to label %890 unwind label %851

890:                                              ; preds = %888
  %891 = call noundef ptr @_ZN13my_shared_ptrIN4urdf4LinkEEptEv(ptr noundef nonnull align 8 dereferenceable(16) %29)
  %892 = invoke noundef nonnull align 8 dereferenceable(8) ptr @_ZStlsIcSt11char_traitsIcESaIcEERSt13basic_ostreamIT_T0_ES7_RKNSt7__cxx1112basic_stringIS4_S5_T1_EE(ptr noundef nonnull align 8 dereferenceable(8) %889, ptr noundef nonnull align 8 dereferenceable(32) %891)
          to label %893 unwind label %851

893:                                              ; preds = %890
  %894 = invoke noundef nonnull align 8 dereferenceable(8) ptr @_ZNSolsEPFRSoS_E(ptr noundef nonnull align 8 dereferenceable(8) %892, ptr noundef nonnull @_ZSt4endlIcSt11char_traitsIcEERSt13basic_ostreamIT_T0_ES6_)
          to label %895 unwind label %851

895:                                              ; preds = %893, %820
  %896 = call noundef ptr @_ZN13my_shared_ptrIN4urdf5JointEEptEv(ptr noundef nonnull align 8 dereferenceable(16) %27)
  %897 = getelementptr inbounds i8, ptr %896, i64 32
  %898 = load i32, ptr %897, align 8, !tbaa !37
  %899 = icmp eq i32 %898, 4
  br i1 %899, label %900, label %943

900:                                              ; preds = %895
  call void @llvm.lifetime.start.p0(i64 72, ptr nonnull %127) #7
  call void @llvm.lifetime.start.p0(i64 16, ptr nonnull %128) #7
  invoke void @_ZN5Eigen9DenseBaseINS_6MatrixIdLi3ELi3ELi0ELi3ELi3EEEE4ZeroEv(ptr dead_on_unwind nonnull writable sret(%"class.Eigen::CwiseNullaryOp") align 8 %128)
          to label %901 unwind label %918

901:                                              ; preds = %900
  invoke void @_ZN9Matrix3_tC2IN5Eigen14CwiseNullaryOpINS1_8internal18scalar_constant_opIdEENS1_6MatrixIdLi3ELi3ELi0ELi3ELi3EEEEEEERKNS1_10MatrixBaseIT_EE(ptr noundef nonnull align 8 dereferenceable(72) %127, ptr noundef nonnull align 1 dereferenceable(1) %128)
          to label %902 unwind label %918

902:                                              ; preds = %901
  call void @llvm.lifetime.end.p0(i64 16, ptr nonnull %128) #7
  call void @llvm.lifetime.start.p0(i64 112, ptr nonnull %129) #7
  call void @llvm.lifetime.start.p0(i64 8, ptr nonnull %130) #7
  store double 0.000000e+00, ptr %130, align 8, !tbaa !13
  call void @llvm.lifetime.start.p0(i64 24, ptr nonnull %131) #7
  call void @llvm.lifetime.start.p0(i64 16, ptr nonnull %132) #7
  invoke void @_ZN5Eigen9DenseBaseINS_6MatrixIdLi3ELi1ELi0ELi3ELi1EEEE4ZeroEl(ptr dead_on_unwind nonnull writable sret(%"class.Eigen::CwiseNullaryOp.181") align 8 %132, i64 noundef 3)
          to label %903 unwind label %920

903:                                              ; preds = %902
  invoke void @_ZN9Vector3_tC2IN5Eigen14CwiseNullaryOpINS1_8internal18scalar_constant_opIdEENS1_6MatrixIdLi3ELi1ELi0ELi3ELi1EEEEEEERKNS1_10MatrixBaseIT_EE(ptr noundef nonnull align 8 dereferenceable(24) %131, ptr noundef nonnull align 1 dereferenceable(1) %132)
          to label %904 unwind label %920

904:                                              ; preds = %903
  invoke void @_ZN17RigidBodyDynamics4BodyC2ERKdRK9Vector3_tRK9Matrix3_t(ptr noundef nonnull align 8 dereferenceable(105) %129, ptr noundef nonnull align 8 dereferenceable(8) %130, ptr noundef nonnull align 8 dereferenceable(24) %131, ptr noundef nonnull align 8 dereferenceable(72) %127)
          to label %905 unwind label %920

905:                                              ; preds = %904
  call void @llvm.lifetime.end.p0(i64 16, ptr nonnull %132) #7
  call void @llvm.lifetime.end.p0(i64 24, ptr nonnull %131) #7
  call void @llvm.lifetime.end.p0(i64 8, ptr nonnull %130) #7
  call void @llvm.lifetime.start.p0(i64 24, ptr nonnull %133) #7
  invoke void @_ZN17RigidBodyDynamics5JointC2ENS_9JointTypeE(ptr noundef nonnull align 8 dereferenceable(24) %133, i32 noundef 11)
          to label %906 unwind label %922

906:                                              ; preds = %905
  call void @llvm.lifetime.start.p0(i64 32, ptr nonnull %134) #7
  %907 = call noundef ptr @_ZN13my_shared_ptrIN4urdf4LinkEEptEv(ptr noundef nonnull align 8 dereferenceable(16) %29)
  invoke void @_ZStplIcSt11char_traitsIcESaIcEENSt7__cxx1112basic_stringIT_T0_T1_EERKS8_PKS5_(ptr dead_on_unwind nonnull writable sret(%"class.std::__cxx11::basic_string") align 8 %134, ptr noundef nonnull align 8 dereferenceable(32) %907, ptr noundef nonnull @.str.25)
          to label %908 unwind label %924

908:                                              ; preds = %906
  invoke void @_ZNSt7__cxx1112basic_stringIcSt11char_traitsIcESaIcEEC2ERKS4_(ptr noundef nonnull align 8 dereferenceable(32) %135, ptr noundef nonnull align 8 dereferenceable(32) %134)
          to label %909 unwind label %926

909:                                              ; preds = %908
  %910 = invoke noundef i32 @_ZN17RigidBodyDynamics5Model7AddBodyEjRKNS_4Math16SpatialTransformERKNS_5JointERKNS_4BodyENSt7__cxx1112basic_stringIcSt11char_traitsIcESaIcEEE(ptr noundef nonnull align 8 dereferenceable(872) %0, i32 noundef %435, ptr noundef nonnull align 8 dereferenceable(96) %96, ptr noundef nonnull align 8 dereferenceable(24) %133, ptr noundef nonnull align 8 dereferenceable(105) %129, ptr noundef nonnull %135)
          to label %911 unwind label %928

911:                                              ; preds = %909
  call void @_ZNSt7__cxx1112basic_stringIcSt11char_traitsIcESaIcEED2Ev(ptr noundef nonnull align 8 dereferenceable(32) %135) #7
  call void @llvm.lifetime.start.p0(i64 24, ptr nonnull %136) #7
  invoke void @_ZN17RigidBodyDynamics5JointC2ENS_9JointTypeE(ptr noundef nonnull align 8 dereferenceable(24) %136, i32 noundef 8)
          to label %912 unwind label %930

912:                                              ; preds = %911
  call void @llvm.lifetime.start.p0(i64 96, ptr nonnull %137) #7
  invoke void @_ZN17RigidBodyDynamics4Math16SpatialTransformC2Ev(ptr noundef nonnull align 8 dereferenceable(96) %137)
          to label %913 unwind label %932

913:                                              ; preds = %912
  %914 = call noundef ptr @_ZN13my_shared_ptrIN4urdf4LinkEEptEv(ptr noundef nonnull align 8 dereferenceable(16) %29)
  invoke void @_ZNSt7__cxx1112basic_stringIcSt11char_traitsIcESaIcEEC2ERKS4_(ptr noundef nonnull align 8 dereferenceable(32) %138, ptr noundef nonnull align 8 dereferenceable(32) %914)
          to label %915 unwind label %932

915:                                              ; preds = %913
  %916 = invoke noundef i32 @_ZN17RigidBodyDynamics5Model10AppendBodyERKNS_4Math16SpatialTransformERKNS_5JointERKNS_4BodyENSt7__cxx1112basic_stringIcSt11char_traitsIcESaIcEEE(ptr noundef nonnull align 8 dereferenceable(872) %0, ptr noundef nonnull align 8 dereferenceable(96) %137, ptr noundef nonnull align 8 dereferenceable(24) %136, ptr noundef nonnull align 8 dereferenceable(105) %125, ptr noundef nonnull %138)
          to label %917 unwind label %934

917:                                              ; preds = %915
  call void @_ZNSt7__cxx1112basic_stringIcSt11char_traitsIcESaIcEED2Ev(ptr noundef nonnull align 8 dereferenceable(32) %138) #7
  call void @llvm.lifetime.end.p0(i64 96, ptr nonnull %137) #7
  call void @_ZN17RigidBodyDynamics5JointD2Ev(ptr noundef nonnull align 8 dereferenceable(24) %136) #7
  call void @llvm.lifetime.end.p0(i64 24, ptr nonnull %136) #7
  call void @_ZNSt7__cxx1112basic_stringIcSt11char_traitsIcESaIcEED2Ev(ptr noundef nonnull align 8 dereferenceable(32) %134) #7
  call void @llvm.lifetime.end.p0(i64 32, ptr nonnull %134) #7
  call void @_ZN17RigidBodyDynamics5JointD2Ev(ptr noundef nonnull align 8 dereferenceable(24) %133) #7
  call void @llvm.lifetime.end.p0(i64 24, ptr nonnull %133) #7
  call void @_ZN17RigidBodyDynamics4BodyD2Ev(ptr noundef nonnull align 8 dereferenceable(105) %129) #7
  call void @llvm.lifetime.end.p0(i64 112, ptr nonnull %129) #7
  call void @llvm.lifetime.end.p0(i64 72, ptr nonnull %127) #7
  br label %950

918:                                              ; preds = %901, %900
  %919 = landingpad { ptr, i32 }
          cleanup
  call void @llvm.lifetime.end.p0(i64 16, ptr nonnull %128) #7
  br label %942

920:                                              ; preds = %904, %903, %902
  %921 = landingpad { ptr, i32 }
          cleanup
  call void @llvm.lifetime.end.p0(i64 16, ptr nonnull %132) #7
  call void @llvm.lifetime.end.p0(i64 24, ptr nonnull %131) #7
  call void @llvm.lifetime.end.p0(i64 8, ptr nonnull %130) #7
  br label %941

922:                                              ; preds = %905
  %923 = landingpad { ptr, i32 }
          cleanup
  br label %940

924:                                              ; preds = %906
  %925 = landingpad { ptr, i32 }
          cleanup
  br label %939

926:                                              ; preds = %908
  %927 = landingpad { ptr, i32 }
          cleanup
  br label %938

928:                                              ; preds = %909
  %929 = landingpad { ptr, i32 }
          cleanup
  call void @_ZNSt7__cxx1112basic_stringIcSt11char_traitsIcESaIcEED2Ev(ptr noundef nonnull align 8 dereferenceable(32) %135) #7
  br label %938

930:                                              ; preds = %911
  %931 = landingpad { ptr, i32 }
          cleanup
  br label %937

932:                                              ; preds = %913, %912
  %933 = landingpad { ptr, i32 }
          cleanup
  br label %936

934:                                              ; preds = %915
  %935 = landingpad { ptr, i32 }
          cleanup
  call void @_ZNSt7__cxx1112basic_stringIcSt11char_traitsIcESaIcEED2Ev(ptr noundef nonnull align 8 dereferenceable(32) %138) #7
  br label %936

936:                                              ; preds = %934, %932
  %.pn152 = phi { ptr, i32 } [ %935, %934 ], [ %933, %932 ]
  call void @llvm.lifetime.end.p0(i64 96, ptr nonnull %137) #7
  call void @_ZN17RigidBodyDynamics5JointD2Ev(ptr noundef nonnull align 8 dereferenceable(24) %136) #7
  br label %937

937:                                              ; preds = %936, %930
  %.pn152.pn = phi { ptr, i32 } [ %.pn152, %936 ], [ %931, %930 ]
  call void @llvm.lifetime.end.p0(i64 24, ptr nonnull %136) #7
  br label %938

938:                                              ; preds = %937, %928, %926
  %.pn152.pn.pn = phi { ptr, i32 } [ %.pn152.pn, %937 ], [ %929, %928 ], [ %927, %926 ]
  call void @_ZNSt7__cxx1112basic_stringIcSt11char_traitsIcESaIcEED2Ev(ptr noundef nonnull align 8 dereferenceable(32) %134) #7
  br label %939

939:                                              ; preds = %938, %924
  %.pn152.pn.pn.pn = phi { ptr, i32 } [ %.pn152.pn.pn, %938 ], [ %925, %924 ]
  call void @llvm.lifetime.end.p0(i64 32, ptr nonnull %134) #7
  call void @_ZN17RigidBodyDynamics5JointD2Ev(ptr noundef nonnull align 8 dereferenceable(24) %133) #7
  br label %940

940:                                              ; preds = %939, %922
  %.pn152.pn.pn.pn.pn = phi { ptr, i32 } [ %.pn152.pn.pn.pn, %939 ], [ %923, %922 ]
  call void @llvm.lifetime.end.p0(i64 24, ptr nonnull %133) #7
  call void @_ZN17RigidBodyDynamics4BodyD2Ev(ptr noundef nonnull align 8 dereferenceable(105) %129) #7
  br label %941

941:                                              ; preds = %940, %920
  %.pn152.pn.pn.pn.pn.pn = phi { ptr, i32 } [ %.pn152.pn.pn.pn.pn, %940 ], [ %921, %920 ]
  call void @llvm.lifetime.end.p0(i64 112, ptr nonnull %129) #7
  br label %942

942:                                              ; preds = %941, %918
  %.pn152.pn.pn.pn.pn.pn.pn = phi { ptr, i32 } [ %.pn152.pn.pn.pn.pn.pn, %941 ], [ %919, %918 ]
  call void @llvm.lifetime.end.p0(i64 72, ptr nonnull %127) #7
  br label %953

943:                                              ; preds = %895
  %944 = call noundef ptr @_ZN13my_shared_ptrIN4urdf4LinkEEptEv(ptr noundef nonnull align 8 dereferenceable(16) %29)
  invoke void @_ZNSt7__cxx1112basic_stringIcSt11char_traitsIcESaIcEEC2ERKS4_(ptr noundef nonnull align 8 dereferenceable(32) %139, ptr noundef nonnull align 8 dereferenceable(32) %944)
          to label %945 unwind label %851

945:                                              ; preds = %943
  %946 = invoke noundef i32 @_ZN17RigidBodyDynamics5Model7AddBodyEjRKNS_4Math16SpatialTransformERKNS_5JointERKNS_4BodyENSt7__cxx1112basic_stringIcSt11char_traitsIcESaIcEEE(ptr noundef nonnull align 8 dereferenceable(872) %0, i32 noundef %435, ptr noundef nonnull align 8 dereferenceable(96) %96, ptr noundef nonnull align 8 dereferenceable(24) %30, ptr noundef nonnull align 8 dereferenceable(105) %125, ptr noundef nonnull %139)
          to label %947 unwind label %948

947:                                              ; preds = %945
  call void @_ZNSt7__cxx1112basic_stringIcSt11char_traitsIcESaIcEED2Ev(ptr noundef nonnull align 8 dereferenceable(32) %139) #7
  br label %950

948:                                              ; preds = %945
  %949 = landingpad { ptr, i32 }
          cleanup
  call void @_ZNSt7__cxx1112basic_stringIcSt11char_traitsIcESaIcEED2Ev(ptr noundef nonnull align 8 dereferenceable(32) %139) #7
  br label %953

950:                                              ; preds = %947, %917
  call void @_ZN17RigidBodyDynamics4BodyD2Ev(ptr noundef nonnull align 8 dereferenceable(105) %125) #7
  call void @llvm.lifetime.end.p0(i64 112, ptr nonnull %125) #7
  br label %951

951:                                              ; preds = %783, %950
  %.046 = phi i32 [ 0, %950 ], [ 1, %783 ]
  call void @llvm.lifetime.end.p0(i64 8, ptr nonnull %120) #7
  call void @llvm.lifetime.end.p0(i64 72, ptr nonnull %118) #7
  call void @llvm.lifetime.end.p0(i64 24, ptr nonnull %117) #7
  call void @llvm.lifetime.end.p0(i64 24, ptr nonnull %116) #7
  call void @llvm.lifetime.end.p0(i64 96, ptr nonnull %96) #7
  call void @llvm.lifetime.end.p0(i64 24, ptr nonnull %95) #7
  call void @llvm.lifetime.end.p0(i64 24, ptr nonnull %94) #7
  br label %952

952:                                              ; preds = %635, %951
  %.1 = phi i32 [ %.046, %951 ], [ 1, %635 ]
  call void @_ZN17RigidBodyDynamics5JointD2Ev(ptr noundef nonnull align 8 dereferenceable(24) %30) #7
  call void @llvm.lifetime.end.p0(i64 24, ptr nonnull %30) #7
  call void @_ZN13my_shared_ptrIN4urdf4LinkEED2Ev(ptr noundef nonnull align 8 dereferenceable(16) %29) #7
  call void @llvm.lifetime.end.p0(i64 16, ptr nonnull %29) #7
  call void @_ZN13my_shared_ptrIN4urdf4LinkEED2Ev(ptr noundef nonnull align 8 dereferenceable(16) %28) #7
  call void @llvm.lifetime.end.p0(i64 16, ptr nonnull %28) #7
  call void @_ZN13my_shared_ptrIN4urdf5JointEED2Ev(ptr noundef nonnull align 8 dereferenceable(16) %27) #7
  call void @llvm.lifetime.end.p0(i64 16, ptr nonnull %27) #7
  %cond = icmp eq i32 %.1, 0
  br i1 %cond, label %416, label %._crit_edge257

953:                                              ; preds = %871, %873, %948, %942, %851
  %.pn160.pn = phi { ptr, i32 } [ %.pn152.pn.pn.pn.pn.pn.pn, %942 ], [ %949, %948 ], [ %852, %851 ], [ %874, %873 ], [ %872, %871 ]
  call void @_ZN17RigidBodyDynamics4BodyD2Ev(ptr noundef nonnull align 8 dereferenceable(105) %125) #7
  br label %954

954:                                              ; preds = %953, %849
  %.pn160.pn.pn = phi { ptr, i32 } [ %.pn160.pn, %953 ], [ %850, %849 ]
  call void @llvm.lifetime.end.p0(i64 112, ptr nonnull %125) #7
  br label %955

955:                                              ; preds = %954, %817, %815
  %.pn164 = phi { ptr, i32 } [ %816, %815 ], [ %.pn160.pn.pn, %954 ], [ %818, %817 ]
  call void @llvm.lifetime.end.p0(i64 8, ptr nonnull %120) #7
  br label %956

956:                                              ; preds = %955, %813
  %.pn164.pn = phi { ptr, i32 } [ %.pn164, %955 ], [ %814, %813 ]
  call void @llvm.lifetime.end.p0(i64 72, ptr nonnull %118) #7
  br label %957

957:                                              ; preds = %956, %811
  %.pn164.pn.pn = phi { ptr, i32 } [ %.pn164.pn, %956 ], [ %812, %811 ]
  call void @llvm.lifetime.end.p0(i64 24, ptr nonnull %117) #7
  br label %958

958:                                              ; preds = %957, %809
  %.pn164.pn.pn.pn = phi { ptr, i32 } [ %.pn164.pn.pn, %957 ], [ %810, %809 ]
  call void @llvm.lifetime.end.p0(i64 24, ptr nonnull %116) #7
  br label %959

959:                                              ; preds = %958, %808
  %.pn164.pn.pn.pn.pn = phi { ptr, i32 } [ %.pn164.pn.pn.pn, %958 ], [ %.pn145.pn.pn.pn.pn.pn, %808 ]
  call void @llvm.lifetime.end.p0(i64 96, ptr nonnull %96) #7
  br label %960

960:                                              ; preds = %959, %787
  %.pn164.pn.pn.pn.pn.pn = phi { ptr, i32 } [ %.pn164.pn.pn.pn.pn, %959 ], [ %788, %787 ]
  call void @llvm.lifetime.end.p0(i64 24, ptr nonnull %95) #7
  br label %961

961:                                              ; preds = %960, %785
  %.pn164.pn.pn.pn.pn.pn.pn = phi { ptr, i32 } [ %.pn164.pn.pn.pn.pn.pn, %960 ], [ %786, %785 ]
  call void @llvm.lifetime.end.p0(i64 24, ptr nonnull %94) #7
  br label %962

962:                                              ; preds = %961, %622, %587, %573, %549, %490
  %.pn164.pn.pn.pn.pn.pn.pn.pn = phi { ptr, i32 } [ %.pn164.pn.pn.pn.pn.pn.pn, %961 ], [ %.pn142.pn, %549 ], [ %.pn133.pn, %573 ], [ %.pn131, %587 ], [ %.pn124.pn.pn.pn.pn.pn, %622 ], [ %491, %490 ]
  call void @_ZN17RigidBodyDynamics5JointD2Ev(ptr noundef nonnull align 8 dereferenceable(24) %30) #7
  call void @llvm.lifetime.end.p0(i64 24, ptr nonnull %30) #7
  br label %963

963:                                              ; preds = %962, %459
  %.pn164.pn.pn.pn.pn.pn.pn.pn.pn = phi { ptr, i32 } [ %.pn164.pn.pn.pn.pn.pn.pn.pn, %962 ], [ %460, %459 ]
  call void @_ZN13my_shared_ptrIN4urdf4LinkEED2Ev(ptr noundef nonnull align 8 dereferenceable(16) %29) #7
  br label %964

964:                                              ; preds = %963, %457
  %.pn164.pn.pn.pn.pn.pn.pn.pn.pn.pn = phi { ptr, i32 } [ %.pn164.pn.pn.pn.pn.pn.pn.pn.pn, %963 ], [ %458, %457 ]
  call void @llvm.lifetime.end.p0(i64 16, ptr nonnull %29) #7
  call void @_ZN13my_shared_ptrIN4urdf4LinkEED2Ev(ptr noundef nonnull align 8 dereferenceable(16) %28) #7
  br label %965

965:                                              ; preds = %964, %455
  %.pn164.pn.pn.pn.pn.pn.pn.pn.pn.pn.pn = phi { ptr, i32 } [ %.pn164.pn.pn.pn.pn.pn.pn.pn.pn.pn, %964 ], [ %456, %455 ]
  call void @llvm.lifetime.end.p0(i64 16, ptr nonnull %28) #7
  call void @_ZN13my_shared_ptrIN4urdf5JointEED2Ev(ptr noundef nonnull align 8 dereferenceable(16) %27) #7
  br label %966

966:                                              ; preds = %965, %453
  %.pn164.pn.pn.pn.pn.pn.pn.pn.pn.pn.pn.pn = phi { ptr, i32 } [ %.pn164.pn.pn.pn.pn.pn.pn.pn.pn.pn.pn, %965 ], [ %454, %453 ]
  call void @llvm.lifetime.end.p0(i64 16, ptr nonnull %27) #7
  br label %967

._crit_edge257:                                   ; preds = %952, %416, %.preheader189
  %.lcssa = phi i1 [ true, %.preheader189 ], [ %cond, %416 ], [ %cond, %952 ]
  call void @llvm.lifetime.end.p0(i64 8, ptr nonnull %16) #7
  call void @llvm.lifetime.end.p0(i64 72, ptr nonnull %15) #7
  call void @llvm.lifetime.end.p0(i64 24, ptr nonnull %14) #7
  call void @llvm.lifetime.end.p0(i64 24, ptr nonnull %13) #7
  call void @_ZN13my_shared_ptrIKN4urdf4LinkEED2Ev(ptr noundef nonnull align 8 dereferenceable(16) %12) #7
  call void @llvm.lifetime.end.p0(i64 16, ptr nonnull %12) #7
  call void @_ZNSt5stackIiSt5dequeIiSaIiEEED2Ev(ptr noundef nonnull align 8 dereferenceable(80) %10) #7
  call void @llvm.lifetime.end.p0(i64 80, ptr nonnull %10) #7
  call void @_ZNSt5stackI13my_shared_ptrIN4urdf4LinkEESt5dequeIS3_SaIS3_EEED2Ev(ptr noundef nonnull align 8 dereferenceable(80) %9) #7
  call void @llvm.lifetime.end.p0(i64 80, ptr nonnull %9) #7
  call void @_ZNSt6vectorINSt7__cxx1112basic_stringIcSt11char_traitsIcESaIcEEESaIS5_EED2Ev(ptr noundef nonnull align 8 dereferenceable(24) %8) #7
  call void @llvm.lifetime.end.p0(i64 24, ptr nonnull %8) #7
  call void @_ZNSt3mapINSt7__cxx1112basic_stringIcSt11char_traitsIcESaIcEEE13my_shared_ptrIN4urdf5JointEESt4lessIS5_ESaISt4pairIKS5_S9_EEED2Ev(ptr noundef nonnull align 8 dereferenceable(48) %7) #7
  call void @llvm.lifetime.end.p0(i64 48, ptr nonnull %7) #7
  call void @_ZNSt3mapINSt7__cxx1112basic_stringIcSt11char_traitsIcESaIcEEE13my_shared_ptrIN4urdf4LinkEESt4lessIS5_ESaISt4pairIKS5_S9_EEED2Ev(ptr noundef nonnull align 8 dereferenceable(48) %6) #7
  call void @llvm.lifetime.end.p0(i64 48, ptr nonnull %6) #7
  call void @_ZN13my_shared_ptrIN4urdf4LinkEED2Ev(ptr noundef nonnull align 8 dereferenceable(16) %5) #7
  call void @llvm.lifetime.end.p0(i64 16, ptr nonnull %5) #7
  ret i1 %.lcssa

967:                                              ; preds = %966, %412, %374, %342, %281
  %.pn177.pn = phi { ptr, i32 } [ %.pn177, %412 ], [ %.pn164.pn.pn.pn.pn.pn.pn.pn.pn.pn.pn.pn, %966 ], [ %375, %374 ], [ %.pn119.pn.pn.pn, %342 ], [ %282, %281 ]
  call void @llvm.lifetime.end.p0(i64 8, ptr nonnull %16) #7
  br label %968

968:                                              ; preds = %967, %279
  %.pn177.pn.pn = phi { ptr, i32 } [ %.pn177.pn, %967 ], [ %280, %279 ]
  call void @llvm.lifetime.end.p0(i64 72, ptr nonnull %15) #7
  br label %969

969:                                              ; preds = %968, %277
  %.pn177.pn.pn.pn = phi { ptr, i32 } [ %.pn177.pn.pn, %968 ], [ %278, %277 ]
  call void @llvm.lifetime.end.p0(i64 24, ptr nonnull %14) #7
  br label %970

970:                                              ; preds = %969, %275
  %.pn177.pn.pn.pn.pn = phi { ptr, i32 } [ %.pn177.pn.pn.pn, %969 ], [ %276, %275 ]
  call void @llvm.lifetime.end.p0(i64 24, ptr nonnull %13) #7
  call void @_ZN13my_shared_ptrIKN4urdf4LinkEED2Ev(ptr noundef nonnull align 8 dereferenceable(16) %12) #7
  call void @llvm.lifetime.end.p0(i64 16, ptr nonnull %12) #7
  br label %971

971:                                              ; preds = %273, %970, %272
  %.pn177.pn.pn.pn.pn.pn.pn = phi { ptr, i32 } [ %.pn, %272 ], [ %.pn177.pn.pn.pn.pn, %970 ], [ %274, %273 ]
  call void @_ZNSt5stackIiSt5dequeIiSaIiEEED2Ev(ptr noundef nonnull align 8 dereferenceable(80) %10) #7
  br label %972

972:                                              ; preds = %971, %266
  %.pn177.pn.pn.pn.pn.pn.pn.pn = phi { ptr, i32 } [ %.pn177.pn.pn.pn.pn.pn.pn, %971 ], [ %267, %266 ]
  call void @llvm.lifetime.end.p0(i64 80, ptr nonnull %10) #7
  call void @_ZNSt5stackI13my_shared_ptrIN4urdf4LinkEESt5dequeIS3_SaIS3_EEED2Ev(ptr noundef nonnull align 8 dereferenceable(80) %9) #7
  br label %973

973:                                              ; preds = %972, %264
  %.pn177.pn.pn.pn.pn.pn.pn.pn.pn = phi { ptr, i32 } [ %.pn177.pn.pn.pn.pn.pn.pn.pn, %972 ], [ %265, %264 ]
  call void @llvm.lifetime.end.p0(i64 80, ptr nonnull %9) #7
  call void @_ZNSt6vectorINSt7__cxx1112basic_stringIcSt11char_traitsIcESaIcEEESaIS5_EED2Ev(ptr noundef nonnull align 8 dereferenceable(24) %8) #7
  call void @llvm.lifetime.end.p0(i64 24, ptr nonnull %8) #7
  br label %974

974:                                              ; preds = %973, %262
  %.pn177.pn.pn.pn.pn.pn.pn.pn.pn.pn = phi { ptr, i32 } [ %.pn177.pn.pn.pn.pn.pn.pn.pn.pn, %973 ], [ %263, %262 ]
  call void @_ZNSt3mapINSt7__cxx1112basic_stringIcSt11char_traitsIcESaIcEEE13my_shared_ptrIN4urdf5JointEESt4lessIS5_ESaISt4pairIKS5_S9_EEED2Ev(ptr noundef nonnull align 8 dereferenceable(48) %7) #7
  call void @llvm.lifetime.end.p0(i64 48, ptr nonnull %7) #7
  br label %975

975:                                              ; preds = %974, %260
  %.pn177.pn.pn.pn.pn.pn.pn.pn.pn.pn.pn = phi { ptr, i32 } [ %.pn177.pn.pn.pn.pn.pn.pn.pn.pn.pn, %974 ], [ %261, %260 ]
  call void @_ZNSt3mapINSt7__cxx1112basic_stringIcSt11char_traitsIcESaIcEEE13my_shared_ptrIN4urdf4LinkEESt4lessIS5_ESaISt4pairIKS5_S9_EEED2Ev(ptr noundef nonnull align 8 dereferenceable(48) %6) #7
  call void @llvm.lifetime.end.p0(i64 48, ptr nonnull %6) #7
  call void @_ZN13my_shared_ptrIN4urdf4LinkEED2Ev(ptr noundef nonnull align 8 dereferenceable(16) %5) #7
  call void @llvm.lifetime.end.p0(i64 16, ptr nonnull %5) #7
  resume { ptr, i32 } %.pn177.pn.pn.pn.pn.pn.pn.pn.pn.pn.pn
}

; Function Attrs: mustprogress nocallback nofree nosync nounwind willreturn memory(argmem: readwrite)
declare void @llvm.lifetime.start.p0(i64 immarg, ptr nocapture) #1

; Function Attrs: mustprogress nounwind uwtable
declare dso_local void @_ZN13my_shared_ptrIN4urdf4LinkEEC2Ev(ptr noundef nonnull align 8 dereferenceable(16)) unnamed_addr #2 align 2

; Function Attrs: mustprogress nounwind uwtable
declare dso_local void @_ZNSt3mapINSt7__cxx1112basic_stringIcSt11char_traitsIcESaIcEEE13my_shared_ptrIN4urdf4LinkEESt4lessIS5_ESaISt4pairIKS5_S9_EEEC2Ev(ptr noundef nonnull align 8 dereferenceable(48)) unnamed_addr #2 align 2

; Function Attrs: mustprogress nounwind uwtable
declare dso_local noundef ptr @_ZN13my_shared_ptrIN4urdf14ModelInterfaceEEptEv(ptr noundef nonnull align 8 dereferenceable(16)) local_unnamed_addr #2 align 2

declare i32 @__gxx_personality_v0(...)

; Function Attrs: mustprogress uwtable
declare dso_local noundef nonnull align 8 dereferenceable(48) ptr @_ZNSt3mapINSt7__cxx1112basic_stringIcSt11char_traitsIcESaIcEEE13my_shared_ptrIN4urdf4LinkEESt4lessIS5_ESaISt4pairIKS5_S9_EEEaSERKSG_(ptr noundef nonnull align 8 dereferenceable(48), ptr noundef nonnull align 8 dereferenceable(48)) local_unnamed_addr #0 align 2

; Function Attrs: mustprogress nounwind uwtable
declare dso_local void @_ZNSt3mapINSt7__cxx1112basic_stringIcSt11char_traitsIcESaIcEEE13my_shared_ptrIN4urdf5JointEESt4lessIS5_ESaISt4pairIKS5_S9_EEEC2Ev(ptr noundef nonnull align 8 dereferenceable(48)) unnamed_addr #2 align 2

; Function Attrs: mustprogress uwtable
declare dso_local noundef nonnull align 8 dereferenceable(48) ptr @_ZNSt3mapINSt7__cxx1112basic_stringIcSt11char_traitsIcESaIcEEE13my_shared_ptrIN4urdf5JointEESt4lessIS5_ESaISt4pairIKS5_S9_EEEaSERKSG_(ptr noundef nonnull align 8 dereferenceable(48), ptr noundef nonnull align 8 dereferenceable(48)) local_unnamed_addr #0 align 2

; Function Attrs: mustprogress nounwind uwtable
declare dso_local void @_ZNSt6vectorINSt7__cxx1112basic_stringIcSt11char_traitsIcESaIcEEESaIS5_EEC2Ev(ptr noundef nonnull align 8 dereferenceable(24)) unnamed_addr #2 align 2

; Function Attrs: mustprogress uwtable
declare dso_local void @_ZNSt5stackI13my_shared_ptrIN4urdf4LinkEESt5dequeIS3_SaIS3_EEEC2IS6_vEEv(ptr noundef nonnull align 8 dereferenceable(80)) unnamed_addr #0 align 2

; Function Attrs: mustprogress uwtable
declare dso_local void @_ZNSt5stackIiSt5dequeIiSaIiEEEC2IS2_vEEv(ptr noundef nonnull align 8 dereferenceable(80)) unnamed_addr #0 align 2

; Function Attrs: mustprogress uwtable
declare dso_local void @_ZNSt5stackI13my_shared_ptrIN4urdf4LinkEESt5dequeIS3_SaIS3_EEE4pushERKS3_(ptr noundef nonnull align 8 dereferenceable(80), ptr noundef nonnull align 8 dereferenceable(16)) local_unnamed_addr #0 align 2

; Function Attrs: mustprogress uwtable
declare dso_local noundef nonnull align 8 dereferenceable(16) ptr @_ZNSt3mapINSt7__cxx1112basic_stringIcSt11char_traitsIcESaIcEEE13my_shared_ptrIN4urdf4LinkEESt4lessIS5_ESaISt4pairIKS5_S9_EEEixERSD_(ptr noundef nonnull align 8 dereferenceable(48), ptr noundef nonnull align 8 dereferenceable(32)) local_unnamed_addr #0 align 2

; Function Attrs: mustprogress uwtable
declare dso_local void @_ZNK4urdf14ModelInterface7getRootEv(ptr dead_on_unwind noalias writable sret(%class.my_shared_ptr.28) align 8, ptr noundef nonnull align 8 dereferenceable(200)) local_unnamed_addr #0 align 2

; Function Attrs: mustprogress nounwind uwtable
declare dso_local noundef ptr @_ZN13my_shared_ptrIKN4urdf4LinkEEptEv(ptr noundef nonnull align 8 dereferenceable(16)) local_unnamed_addr #2 align 2

; Function Attrs: mustprogress nounwind uwtable
declare dso_local void @_ZN13my_shared_ptrIKN4urdf4LinkEED2Ev(ptr noundef nonnull align 8 dereferenceable(16)) unnamed_addr #2 align 2

; Function Attrs: mustprogress nocallback nofree nosync nounwind willreturn memory(argmem: readwrite)
declare void @llvm.lifetime.end.p0(i64 immarg, ptr nocapture) #1

; Function Attrs: inlinehint mustprogress uwtable
declare dso_local void @_ZN9Vector3_tC2Ev(ptr noundef nonnull align 8 dereferenceable(24)) unnamed_addr #3 align 2

; Function Attrs: inlinehint mustprogress uwtable
declare dso_local void @_ZN9Matrix3_tC2Ev(ptr noundef nonnull align 8 dereferenceable(72)) unnamed_addr #3 align 2

; Function Attrs: mustprogress nounwind uwtable
declare dso_local noundef ptr @_ZNK13my_shared_ptrIKN4urdf4LinkEEptEv(ptr noundef nonnull align 8 dereferenceable(16)) local_unnamed_addr #2 align 2

; Function Attrs: mustprogress nounwind uwtable
declare dso_local noundef zeroext i1 @_ZNK13my_shared_ptrIN4urdf8InertialEEcvbEv(ptr noundef nonnull align 8 dereferenceable(16)) local_unnamed_addr #2 align 2

; Function Attrs: mustprogress nounwind uwtable
declare dso_local noundef ptr @_ZNK13my_shared_ptrIN4urdf8InertialEEptEv(ptr noundef nonnull align 8 dereferenceable(16)) local_unnamed_addr #2 align 2

; Function Attrs: mustprogress uwtable
declare dso_local void @_ZN9Vector3_t3setERKdS1_S1_(ptr noundef nonnull align 8 dereferenceable(24), ptr noundef nonnull align 8 dereferenceable(8), ptr noundef nonnull align 8 dereferenceable(8), ptr noundef nonnull align 8 dereferenceable(8)) local_unnamed_addr #0 align 2

; Function Attrs: inlinehint mustprogress uwtable
declare dso_local noundef nonnull align 8 dereferenceable(8) ptr @_ZN5Eigen15DenseCoeffsBaseINS_6MatrixIdLi3ELi3ELi0ELi3ELi3EEELi1EEclEll(ptr noundef nonnull align 1 dereferenceable(1), i64 noundef, i64 noundef) local_unnamed_addr #3 align 2

; Function Attrs: mustprogress nounwind uwtable
declare dso_local void @_ZNK4urdf8Rotation6getRPYERdS1_S1_(ptr noundef nonnull align 8 dereferenceable(32), ptr noundef nonnull align 8 dereferenceable(8), ptr noundef nonnull align 8 dereferenceable(8), ptr noundef nonnull align 8 dereferenceable(8)) local_unnamed_addr #2 align 2

; Function Attrs: inlinehint mustprogress uwtable
declare dso_local noundef nonnull align 8 dereferenceable(8) ptr @_ZN5Eigen15DenseCoeffsBaseINS_6MatrixIdLi3ELi1ELi0ELi3ELi1EEELi1EEixEl(ptr noundef nonnull align 1 dereferenceable(1), i64 noundef) local_unnamed_addr #3 align 2

; Function Attrs: mustprogress uwtable
declare dso_local void @_ZN17RigidBodyDynamics4BodyC2ERKdRK9Vector3_tRK9Matrix3_t(ptr noundef nonnull align 8 dereferenceable(105), ptr noundef nonnull align 8 dereferenceable(8), ptr noundef nonnull align 8 dereferenceable(24), ptr noundef nonnull align 8 dereferenceable(72)) unnamed_addr #0 align 2

; Function Attrs: mustprogress uwtable
declare dso_local void @_ZN17RigidBodyDynamics5JointC2ENS_9JointTypeE(ptr noundef nonnull align 8 dereferenceable(24), i32 noundef) unnamed_addr #0 align 2

; Function Attrs: mustprogress uwtable
declare dso_local noundef nonnull align 8 dereferenceable(24) ptr @_ZN17RigidBodyDynamics5JointaSERKS0_(ptr noundef nonnull align 8 dereferenceable(24), ptr noundef nonnull align 8 dereferenceable(24)) local_unnamed_addr #0 align 2

; Function Attrs: mustprogress nounwind uwtable
declare dso_local void @_ZN17RigidBodyDynamics5JointD2Ev(ptr noundef nonnull align 8 dereferenceable(24)) unnamed_addr #2 align 2

; Function Attrs: mustprogress uwtable
declare dso_local void @_ZN17RigidBodyDynamics4Math16SpatialTransformC2Ev(ptr noundef nonnull align 8 dereferenceable(96)) unnamed_addr #0 align 2

; Function Attrs: inlinehint mustprogress uwtable
declare noundef nonnull align 8 dereferenceable(8) ptr @_ZStlsISt11char_traitsIcEERSt13basic_ostreamIcT_ES5_PKc(ptr noundef nonnull align 8 dereferenceable(8), ptr noundef) local_unnamed_addr #3

; Function Attrs: mustprogress uwtable
declare noundef nonnull align 8 dereferenceable(8) ptr @_ZNSolsEPFRSoS_E(ptr noundef nonnull align 8 dereferenceable(8), ptr noundef) local_unnamed_addr #0 align 2

; Function Attrs: inlinehint mustprogress uwtable
declare noundef nonnull align 8 dereferenceable(8) ptr @_ZSt4endlIcSt11char_traitsIcEERSt13basic_ostreamIT_T0_ES6_(ptr noundef nonnull align 8 dereferenceable(8)) #3

; Function Attrs: inlinehint mustprogress uwtable
declare dso_local noundef nonnull align 8 dereferenceable(8) ptr @_ZN17RigidBodyDynamics4MathlsERSoRKNS0_16SpatialTransformE(ptr noundef nonnull align 8 dereferenceable(8), ptr noundef nonnull align 8 dereferenceable(96)) local_unnamed_addr #3

; Function Attrs: mustprogress uwtable
declare dso_local noundef nonnull align 8 dereferenceable(8) ptr @_ZN5EigenlsINS_6MatrixIdLi3ELi3ELi0ELi3ELi3EEEEERSoS3_RKNS_9DenseBaseIT_EE(ptr noundef nonnull align 8 dereferenceable(8), ptr noundef nonnull align 1 dereferenceable(1)) local_unnamed_addr #0

; Function Attrs: mustprogress uwtable
declare noundef nonnull align 8 dereferenceable(8) ptr @_ZNSolsEd(ptr noundef nonnull align 8 dereferenceable(8), double noundef) local_unnamed_addr #0 align 2

; Function Attrs: inlinehint mustprogress uwtable
declare noundef nonnull align 8 dereferenceable(8) ptr @_ZStlsIcSt11char_traitsIcESaIcEERSt13basic_ostreamIT_T0_ES7_RKNSt7__cxx1112basic_stringIS4_S5_T1_EE(ptr noundef nonnull align 8 dereferenceable(8), ptr noundef nonnull align 8 dereferenceable(32)) local_unnamed_addr #3

declare noundef i32 @_ZN17RigidBodyDynamics5Model10AppendBodyERKNS_4Math16SpatialTransformERKNS_5JointERKNS_4BodyENSt7__cxx1112basic_stringIcSt11char_traitsIcESaIcEEE(ptr noundef nonnull align 8 dereferenceable(872), ptr noundef nonnull align 8 dereferenceable(96), ptr noundef nonnull align 8 dereferenceable(24), ptr noundef nonnull align 8 dereferenceable(105), ptr noundef) local_unnamed_addr #4

; Function Attrs: mustprogress uwtable
declare void @_ZNSt7__cxx1112basic_stringIcSt11char_traitsIcESaIcEEC2ERKS4_(ptr noundef nonnull align 8 dereferenceable(32), ptr noundef nonnull align 8 dereferenceable(32)) unnamed_addr #0 align 2

; Function Attrs: mustprogress nounwind uwtable
declare void @_ZNSt7__cxx1112basic_stringIcSt11char_traitsIcESaIcEED2Ev(ptr noundef nonnull align 8 dereferenceable(32)) unnamed_addr #2 align 2

; Function Attrs: mustprogress nounwind uwtable
declare dso_local void @_ZN17RigidBodyDynamics4BodyD2Ev(ptr noundef nonnull align 8 dereferenceable(105)) unnamed_addr #2 align 2

; Function Attrs: mustprogress uwtable
declare dso_local void @_ZNSt5stackIiSt5dequeIiSaIiEEE4pushEOi(ptr noundef nonnull align 8 dereferenceable(80), ptr noundef nonnull align 4 dereferenceable(4)) local_unnamed_addr #0 align 2

; Function Attrs: mustprogress nounwind uwtable
declare dso_local noundef i64 @_ZNKSt5stackI13my_shared_ptrIN4urdf4LinkEESt5dequeIS3_SaIS3_EEE4sizeEv(ptr noundef nonnull align 8 dereferenceable(80)) local_unnamed_addr #2 align 2

; Function Attrs: mustprogress nounwind uwtable
declare dso_local noundef nonnull align 8 dereferenceable(16) ptr @_ZNSt5stackI13my_shared_ptrIN4urdf4LinkEESt5dequeIS3_SaIS3_EEE3topEv(ptr noundef nonnull align 8 dereferenceable(80)) local_unnamed_addr #2 align 2

; Function Attrs: mustprogress nounwind uwtable
declare dso_local void @_ZN13my_shared_ptrIN4urdf4LinkEEC2ERKS2_(ptr noundef nonnull align 8 dereferenceable(16), ptr noundef nonnull align 8 dereferenceable(16)) unnamed_addr #2 align 2

; Function Attrs: mustprogress nounwind uwtable
declare dso_local noundef nonnull align 4 dereferenceable(4) ptr @_ZNSt5stackIiSt5dequeIiSaIiEEE3topEv(ptr noundef nonnull align 8 dereferenceable(80)) local_unnamed_addr #2 align 2

; Function Attrs: mustprogress nounwind uwtable
declare dso_local noundef ptr @_ZN13my_shared_ptrIN4urdf4LinkEEptEv(ptr noundef nonnull align 8 dereferenceable(16)) local_unnamed_addr #2 align 2

; Function Attrs: mustprogress nounwind uwtable
declare dso_local noundef i64 @_ZNKSt6vectorI13my_shared_ptrIN4urdf5JointEESaIS3_EE4sizeEv(ptr noundef nonnull align 8 dereferenceable(24)) local_unnamed_addr #2 align 2

; Function Attrs: mustprogress nounwind uwtable
declare dso_local noundef nonnull align 8 dereferenceable(16) ptr @_ZNSt6vectorI13my_shared_ptrIN4urdf5JointEESaIS3_EEixEm(ptr noundef nonnull align 8 dereferenceable(24), i64 noundef) local_unnamed_addr #2 align 2

; Function Attrs: mustprogress nounwind uwtable
declare dso_local void @_ZN13my_shared_ptrIN4urdf5JointEEC2ERKS2_(ptr noundef nonnull align 8 dereferenceable(16), ptr noundef nonnull align 8 dereferenceable(16)) unnamed_addr #2 align 2

; Function Attrs: mustprogress nounwind uwtable
declare dso_local void @_ZNSt5stackIiSt5dequeIiSaIiEEE3popEv(ptr noundef nonnull align 8 dereferenceable(80)) local_unnamed_addr #2 align 2

; Function Attrs: mustprogress nounwind uwtable
declare dso_local noundef ptr @_ZN13my_shared_ptrIN4urdf5JointEEptEv(ptr noundef nonnull align 8 dereferenceable(16)) local_unnamed_addr #2 align 2

; Function Attrs: mustprogress nounwind uwtable
declare dso_local noundef i64 @_ZNKSt5stackIiSt5dequeIiSaIiEEE4sizeEv(ptr noundef nonnull align 8 dereferenceable(80)) local_unnamed_addr #2 align 2

declare noundef nonnull align 8 dereferenceable(8) ptr @_ZNSolsEi(ptr noundef nonnull align 8 dereferenceable(8), i32 noundef) local_unnamed_addr #4

; Function Attrs: mustprogress uwtable
declare dso_local void @_ZNSt6vectorINSt7__cxx1112basic_stringIcSt11char_traitsIcESaIcEEESaIS5_EE9push_backERKS5_(ptr noundef nonnull align 8 dereferenceable(24), ptr noundef nonnull align 8 dereferenceable(32)) local_unnamed_addr #0 align 2

; Function Attrs: mustprogress nounwind uwtable
declare dso_local void @_ZN13my_shared_ptrIN4urdf5JointEED2Ev(ptr noundef nonnull align 8 dereferenceable(16)) unnamed_addr #2 align 2

; Function Attrs: mustprogress nounwind uwtable
declare dso_local void @_ZNSt5stackI13my_shared_ptrIN4urdf4LinkEESt5dequeIS3_SaIS3_EEE3popEv(ptr noundef nonnull align 8 dereferenceable(80)) local_unnamed_addr #2 align 2

; Function Attrs: mustprogress nounwind uwtable
declare dso_local void @_ZN13my_shared_ptrIN4urdf4LinkEED2Ev(ptr noundef nonnull align 8 dereferenceable(16)) unnamed_addr #2 align 2

; Function Attrs: mustprogress nounwind uwtable
declare dso_local noundef i64 @_ZNKSt6vectorINSt7__cxx1112basic_stringIcSt11char_traitsIcESaIcEEESaIS5_EE4sizeEv(ptr noundef nonnull align 8 dereferenceable(24)) local_unnamed_addr #2 align 2

; Function Attrs: mustprogress uwtable
declare dso_local noundef nonnull align 8 dereferenceable(16) ptr @_ZNSt3mapINSt7__cxx1112basic_stringIcSt11char_traitsIcESaIcEEE13my_shared_ptrIN4urdf5JointEESt4lessIS5_ESaISt4pairIKS5_S9_EEEixERSD_(ptr noundef nonnull align 8 dereferenceable(48), ptr noundef nonnull align 8 dereferenceable(32)) local_unnamed_addr #0 align 2

; Function Attrs: mustprogress nounwind uwtable
declare dso_local noundef nonnull align 8 dereferenceable(32) ptr @_ZNSt6vectorINSt7__cxx1112basic_stringIcSt11char_traitsIcESaIcEEESaIS5_EEixEm(ptr noundef nonnull align 8 dereferenceable(24), i64 noundef) local_unnamed_addr #2 align 2

; Function Attrs: mustprogress uwtable
declare dso_local noundef i32 @_ZNK17RigidBodyDynamics5Model9GetBodyIdEPKc(ptr noundef nonnull align 8 dereferenceable(872), ptr noundef) local_unnamed_addr #0 align 2

; Function Attrs: mustprogress nounwind uwtable
declare noundef ptr @_ZNKSt7__cxx1112basic_stringIcSt11char_traitsIcESaIcEE5c_strEv(ptr noundef nonnull align 8 dereferenceable(32)) local_unnamed_addr #2 align 2

; Function Attrs: mustprogress nounwind uwtable
declare dso_local noundef i32 @_ZNSt14numeric_limitsIjE3maxEv() local_unnamed_addr #2 align 2

; Function Attrs: mustprogress nounwind uwtable
declare dso_local void @_ZN17RigidBodyDynamics5JointC2Ev(ptr noundef nonnull align 8 dereferenceable(24)) unnamed_addr #2 align 2

; Function Attrs: inlinehint mustprogress uwtable
declare dso_local void @_ZN9Vector3_tC2ERKdS1_S1_(ptr noundef nonnull align 8 dereferenceable(24), ptr noundef nonnull align 8 dereferenceable(8), ptr noundef nonnull align 8 dereferenceable(8), ptr noundef nonnull align 8 dereferenceable(8)) unnamed_addr #3 align 2

; Function Attrs: inlinehint mustprogress uwtable
declare dso_local noundef double @_ZNK5Eigen10MatrixBaseINS_6MatrixIdLi3ELi1ELi0ELi3ELi1EEEE3dotIS2_EENS_20ScalarBinaryOpTraitsIdNS_8internal6traitsIT_E6ScalarENS6_17scalar_product_opIdSA_EEE10ReturnTypeERKNS0_IS8_EE(ptr noundef nonnull align 1 dereferenceable(1), ptr noundef nonnull align 1 dereferenceable(1)) local_unnamed_addr #3 align 2

; Function Attrs: mustprogress nocallback nofree nosync nounwind speculatable willreturn memory(none)
declare double @llvm.fabs.f64(double) #5

; Function Attrs: mustprogress nounwind uwtable
declare dso_local noundef double @_ZNSt14numeric_limitsIdE7epsilonEv() local_unnamed_addr #2 align 2

; Function Attrs: mustprogress uwtable
declare dso_local void @_ZN17RigidBodyDynamics5JointC2ENS_9JointTypeERK9Vector3_t(ptr noundef nonnull align 8 dereferenceable(24), i32 noundef, ptr noundef nonnull align 8 dereferenceable(24)) unnamed_addr #0 align 2

; Function Attrs: inlinehint mustprogress uwtable
declare dso_local void @_ZN15SpatialVector_tC2ERKdS1_S1_S1_S1_S1_(ptr noundef nonnull align 16 dereferenceable(48), ptr noundef nonnull align 8 dereferenceable(8), ptr noundef nonnull align 8 dereferenceable(8), ptr noundef nonnull align 8 dereferenceable(8), ptr noundef nonnull align 8 dereferenceable(8), ptr noundef nonnull align 8 dereferenceable(8), ptr noundef nonnull align 8 dereferenceable(8)) unnamed_addr #3 align 2

; Function Attrs: mustprogress uwtable
declare dso_local void @_ZN17RigidBodyDynamics5JointC2ERK15SpatialVector_tS3_S3_S3_S3_S3_(ptr noundef nonnull align 8 dereferenceable(24), ptr noundef nonnull align 16 dereferenceable(48), ptr noundef nonnull align 16 dereferenceable(48), ptr noundef nonnull align 16 dereferenceable(48), ptr noundef nonnull align 16 dereferenceable(48), ptr noundef nonnull align 16 dereferenceable(48), ptr noundef nonnull align 16 dereferenceable(48)) unnamed_addr #0 align 2

; Function Attrs: inlinehint mustprogress uwtable
declare dso_local void @_ZN17RigidBodyDynamics4Math4XrotEdRK9Vector3_t(ptr dead_on_unwind noalias writable sret(%"struct.RigidBodyDynamics::Math::SpatialTransform") align 8, double noundef, ptr noundef nonnull align 8 dereferenceable(24)) local_unnamed_addr #3

; Function Attrs: mustprogress uwtable
declare dso_local void @_ZNK17RigidBodyDynamics4Math16SpatialTransformmlERKS1_(ptr dead_on_unwind noalias writable sret(%"struct.RigidBodyDynamics::Math::SpatialTransform") align 8, ptr noundef nonnull align 8 dereferenceable(96), ptr noundef nonnull align 8 dereferenceable(96)) local_unnamed_addr #0 align 2

; Function Attrs: inlinehint mustprogress uwtable
declare dso_local void @_ZN17RigidBodyDynamics4Math6XtransERK9Vector3_t(ptr dead_on_unwind noalias writable sret(%"struct.RigidBodyDynamics::Math::SpatialTransform") align 8, ptr noundef nonnull align 8 dereferenceable(24)) local_unnamed_addr #3

; Function Attrs: inlinehint mustprogress uwtable
declare dso_local void @_ZN9Vector3_tC2ERKS_(ptr noundef nonnull align 8 dereferenceable(24), ptr noundef nonnull align 8 dereferenceable(24)) unnamed_addr #3 align 2

; Function Attrs: inlinehint mustprogress uwtable
declare dso_local void @_ZN5Eigen9DenseBaseINS_6MatrixIdLi3ELi3ELi0ELi3ELi3EEEE4ZeroEv(ptr dead_on_unwind noalias writable sret(%"class.Eigen::CwiseNullaryOp") align 8) local_unnamed_addr #3 align 2

; Function Attrs: mustprogress uwtable
declare dso_local void @_ZN9Matrix3_tC2IN5Eigen14CwiseNullaryOpINS1_8internal18scalar_constant_opIdEENS1_6MatrixIdLi3ELi3ELi0ELi3ELi3EEEEEEERKNS1_10MatrixBaseIT_EE(ptr noundef nonnull align 8 dereferenceable(72), ptr noundef nonnull align 1 dereferenceable(1)) unnamed_addr #0 align 2

; Function Attrs: mustprogress nounwind uwtable
declare dso_local noundef ptr @_ZN13my_shared_ptrIN4urdf8InertialEEptEv(ptr noundef nonnull align 8 dereferenceable(16)) local_unnamed_addr #2 align 2

; Function Attrs: inlinehint mustprogress uwtable
declare dso_local noundef zeroext i1 @_ZNK5Eigen10MatrixBaseINS_6MatrixIdLi3ELi1ELi0ELi3ELi1EEEEneIS2_EEbRKNS0_IT_EE(ptr noundef nonnull align 1 dereferenceable(1), ptr noundef nonnull align 1 dereferenceable(1)) local_unnamed_addr #3 align 2

; Function Attrs: mustprogress uwtable
declare noundef nonnull align 8 dereferenceable(8) ptr @_ZNSolsEj(ptr noundef nonnull align 8 dereferenceable(8), i32 noundef) local_unnamed_addr #0 align 2

; Function Attrs: mustprogress uwtable
declare dso_local noundef nonnull align 8 dereferenceable(8) ptr @_ZN5EigenlsINS_9TransposeINS_6MatrixIdLi6ELi1ELi0ELi6ELi1EEEEEEERSoS5_RKNS_9DenseBaseIT_EE(ptr noundef nonnull align 8 dereferenceable(8), ptr noundef nonnull align 1 dereferenceable(1)) local_unnamed_addr #0

; Function Attrs: inlinehint mustprogress uwtable
declare dso_local ptr @_ZN5Eigen9DenseBaseINS_6MatrixIdLi6ELi1ELi0ELi6ELi1EEEE9transposeEv(ptr noundef nonnull align 1 dereferenceable(1)) local_unnamed_addr #3 align 2

; Function Attrs: inlinehint mustprogress uwtable
declare dso_local void @_ZN5Eigen9DenseBaseINS_6MatrixIdLi3ELi1ELi0ELi3ELi1EEEE4ZeroEl(ptr dead_on_unwind noalias writable sret(%"class.Eigen::CwiseNullaryOp.181") align 8, i64 noundef) local_unnamed_addr #3 align 2

; Function Attrs: mustprogress uwtable
declare dso_local void @_ZN9Vector3_tC2IN5Eigen14CwiseNullaryOpINS1_8internal18scalar_constant_opIdEENS1_6MatrixIdLi3ELi1ELi0ELi3ELi1EEEEEEERKNS1_10MatrixBaseIT_EE(ptr noundef nonnull align 8 dereferenceable(24), ptr noundef nonnull align 1 dereferenceable(1)) unnamed_addr #0 align 2

; Function Attrs: inlinehint mustprogress uwtable
declare dso_local void @_ZStplIcSt11char_traitsIcESaIcEENSt7__cxx1112basic_stringIT_T0_T1_EERKS8_PKS5_(ptr dead_on_unwind noalias writable sret(%"class.std::__cxx11::basic_string") align 8, ptr noundef nonnull align 8 dereferenceable(32), ptr noundef) local_unnamed_addr #3

declare noundef i32 @_ZN17RigidBodyDynamics5Model7AddBodyEjRKNS_4Math16SpatialTransformERKNS_5JointERKNS_4BodyENSt7__cxx1112basic_stringIcSt11char_traitsIcESaIcEEE(ptr noundef nonnull align 8 dereferenceable(872), i32 noundef, ptr noundef nonnull align 8 dereferenceable(96), ptr noundef nonnull align 8 dereferenceable(24), ptr noundef nonnull align 8 dereferenceable(105), ptr noundef) local_unnamed_addr #4

; Function Attrs: inlinehint mustprogress nounwind uwtable
declare dso_local void @_ZNSt5stackIiSt5dequeIiSaIiEEED2Ev(ptr noundef nonnull align 8 dereferenceable(80)) unnamed_addr #6 align 2

; Function Attrs: inlinehint mustprogress nounwind uwtable
declare dso_local void @_ZNSt5stackI13my_shared_ptrIN4urdf4LinkEESt5dequeIS3_SaIS3_EEED2Ev(ptr noundef nonnull align 8 dereferenceable(80)) unnamed_addr #6 align 2

; Function Attrs: mustprogress nounwind uwtable
declare dso_local void @_ZNSt6vectorINSt7__cxx1112basic_stringIcSt11char_traitsIcESaIcEEESaIS5_EED2Ev(ptr noundef nonnull align 8 dereferenceable(24)) unnamed_addr #2 align 2

; Function Attrs: mustprogress nounwind uwtable
declare dso_local void @_ZNSt3mapINSt7__cxx1112basic_stringIcSt11char_traitsIcESaIcEEE13my_shared_ptrIN4urdf5JointEESt4lessIS5_ESaISt4pairIKS5_S9_EEED2Ev(ptr noundef nonnull align 8 dereferenceable(48)) unnamed_addr #2 align 2

; Function Attrs: mustprogress nounwind uwtable
declare dso_local void @_ZNSt3mapINSt7__cxx1112basic_stringIcSt11char_traitsIcESaIcEEE13my_shared_ptrIN4urdf4LinkEESt4lessIS5_ESaISt4pairIKS5_S9_EEED2Ev(ptr noundef nonnull align 8 dereferenceable(48)) unnamed_addr #2 align 2

attributes #0 = { mustprogress uwtable "min-legal-vector-width"="0" "no-trapping-math"="true" "stack-protector-buffer-size"="8" "target-cpu"="x86-64" "target-features"="+cmov,+cx8,+fxsr,+mmx,+sse,+sse2,+x87" "tune-cpu"="generic" }
attributes #1 = { mustprogress nocallback nofree nosync nounwind willreturn memory(argmem: readwrite) }
attributes #2 = { mustprogress nounwind uwtable "min-legal-vector-width"="0" "no-trapping-math"="true" "stack-protector-buffer-size"="8" "target-cpu"="x86-64" "target-features"="+cmov,+cx8,+fxsr,+mmx,+sse,+sse2,+x87" "tune-cpu"="generic" }
attributes #3 = { inlinehint mustprogress uwtable "min-legal-vector-width"="0" "no-trapping-math"="true" "stack-protector-buffer-size"="8" "target-cpu"="x86-64" "target-features"="+cmov,+cx8,+fxsr,+mmx,+sse,+sse2,+x87" "tune-cpu"="generic" }
attributes #4 = { "no-trapping-math"="true" "stack-protector-buffer-size"="8" "target-cpu"="x86-64" "target-features"="+cmov,+cx8,+fxsr,+mmx,+sse,+sse2,+x87" "tune-cpu"="generic" }
attributes #5 = { mustprogress nocallback nofree nosync nounwind speculatable willreturn memory(none) }
attributes #6 = { inlinehint mustprogress nounwind uwtable "min-legal-vector-width"="0" "no-trapping-math"="true" "stack-protector-buffer-size"="8" "target-cpu"="x86-64" "target-features"="+cmov,+cx8,+fxsr,+mmx,+sse,+sse2,+x87" "tune-cpu"="generic" }
attributes #7 = { nounwind }

!llvm.module.flags = !{!0, !1, !2, !3}
!llvm.ident = !{!4}

!0 = !{i32 1, !"wchar_size", i32 4}
!1 = !{i32 8, !"PIC Level", i32 2}
!2 = !{i32 7, !"PIE Level", i32 2}
!3 = !{i32 7, !"uwtable", i32 2}
!4 = !{!"clang version 19.1.0 (git@github.com:llvm/llvm-project.git a4bf6cd7cfb1a1421ba92bca9d017b49936c55e4)"}
!5 = !{!6, !9, i64 56}
!6 = !{!"_ZTSN4urdf8InertialE", !7, i64 0, !9, i64 56, !9, i64 64, !9, i64 72, !9, i64 80, !9, i64 88, !9, i64 96, !9, i64 104}
!7 = !{!"_ZTSN4urdf4PoseE", !8, i64 0, !12, i64 24}
!8 = !{!"_ZTSN4urdf7Vector3E", !9, i64 0, !9, i64 8, !9, i64 16}
!9 = !{!"double", !10, i64 0}
!10 = !{!"omnipotent char", !11, i64 0}
!11 = !{!"Simple C++ TBAA"}
!12 = !{!"_ZTSN4urdf8RotationE", !9, i64 0, !9, i64 8, !9, i64 16, !9, i64 24}
!13 = !{!9, !9, i64 0}
!14 = !{!6, !9, i64 64}
!15 = !{!6, !9, i64 72}
!16 = !{!6, !9, i64 80}
!17 = !{!6, !9, i64 88}
!18 = !{!6, !9, i64 96}
!19 = !{!6, !9, i64 104}
!20 = !{!21, !9, i64 0}
!21 = !{!"_ZTSN17RigidBodyDynamics4BodyE", !9, i64 0, !22, i64 8, !27, i64 32, !32, i64 104}
!22 = !{!"_ZTS9Vector3_t", !23, i64 0}
!23 = !{!"_ZTSN5Eigen6MatrixIdLi3ELi1ELi0ELi3ELi1EEE", !24, i64 0}
!24 = !{!"_ZTSN5Eigen15PlainObjectBaseINS_6MatrixIdLi3ELi1ELi0ELi3ELi1EEEEE", !25, i64 0}
!25 = !{!"_ZTSN5Eigen12DenseStorageIdLi3ELi3ELi1ELi0EEE", !26, i64 0}
!26 = !{!"_ZTSN5Eigen8internal11plain_arrayIdLi3ELi0ELi0EEE", !10, i64 0}
!27 = !{!"_ZTS9Matrix3_t", !28, i64 0}
!28 = !{!"_ZTSN5Eigen6MatrixIdLi3ELi3ELi0ELi3ELi3EEE", !29, i64 0}
!29 = !{!"_ZTSN5Eigen15PlainObjectBaseINS_6MatrixIdLi3ELi3ELi0ELi3ELi3EEEEE", !30, i64 0}
!30 = !{!"_ZTSN5Eigen12DenseStorageIdLi9ELi3ELi3ELi0EEE", !31, i64 0}
!31 = !{!"_ZTSN5Eigen8internal11plain_arrayIdLi9ELi0ELi0EEE", !10, i64 0}
!32 = !{!"bool", !10, i64 0}
!33 = !{!34, !34, i64 0}
!34 = !{!"int", !10, i64 0}
!35 = distinct !{!35, !36}
!36 = !{!"llvm.loop.mustprogress"}
!37 = !{!38, !43, i64 32}
!38 = !{!"_ZTSN4urdf5JointE", !39, i64 0, !43, i64 32, !8, i64 40, !39, i64 64, !39, i64 96, !7, i64 128, !44, i64 184, !45, i64 200, !46, i64 216, !47, i64 232, !48, i64 248}
!39 = !{!"_ZTSNSt7__cxx1112basic_stringIcSt11char_traitsIcESaIcEEE", !40, i64 0, !42, i64 8, !10, i64 16}
!40 = !{!"_ZTSNSt7__cxx1112basic_stringIcSt11char_traitsIcESaIcEE12_Alloc_hiderE", !41, i64 0}
!41 = !{!"any pointer", !10, i64 0}
!42 = !{!"long", !10, i64 0}
!43 = !{!"_ZTSN4urdf5JointUt_E", !10, i64 0}
!44 = !{!"_ZTS13my_shared_ptrIN4urdf13JointDynamicsEE", !41, i64 0, !41, i64 8}
!45 = !{!"_ZTS13my_shared_ptrIN4urdf11JointLimitsEE", !41, i64 0, !41, i64 8}
!46 = !{!"_ZTS13my_shared_ptrIN4urdf11JointSafetyEE", !41, i64 0, !41, i64 8}
!47 = !{!"_ZTS13my_shared_ptrIN4urdf16JointCalibrationEE", !41, i64 0, !41, i64 8}
!48 = !{!"_ZTS13my_shared_ptrIN4urdf10JointMimicEE", !41, i64 0, !41, i64 8}
!49 = distinct !{!49, !36}
!50 = distinct !{!50, !36}
!51 = !{!52, !34, i64 12}
!52 = !{!"_ZTSN17RigidBodyDynamics5JointE", !41, i64 0, !53, i64 8, !34, i64 12, !34, i64 16, !34, i64 20}
!53 = !{!"_ZTSN17RigidBodyDynamics9JointTypeE", !10, i64 0}
!54 = !{!52, !41, i64 0}
!55 = distinct !{!55, !36}


; Effective O3 passes: CorrelatedValuePropagationPass,EarlyCSEPass,GVNPass,GlobalOptPass,IPSCCPPass,IndVarSimplifyPass,InferFunctionAttrsPass,InstCombinePass,InstSimplifyPass,JumpThreadingPass,LCSSAPass,LICMPass,LoopRotatePass,LoopSimplifyPass,LoopUnrollPass,SROAPass,SimplifyCFGPass