diff --git a/body_force_estimator/CMakeLists.txt b/body_force_estimator/CMakeLists.txt new file mode 100644 index 000000000..65b5123ad --- /dev/null +++ b/body_force_estimator/CMakeLists.txt @@ -0,0 +1,213 @@ +cmake_minimum_required(VERSION 3.0.2) +project(body_force_estimator) + +## Compile as C++14, supported in ROS Melodic and newer +add_compile_options(-std=c++14) + +## Set default cmake build type to release +if(NOT CMAKE_CONFIGURATION_TYPES AND NOT CMAKE_BUILD_TYPE) + set(CMAKE_BUILD_TYPE Release) +endif() + +## Find catkin macros and libraries +## if COMPONENTS list like find_package(catkin REQUIRED COMPONENTS xyz) +## is used, also find other catkin packages +find_package(catkin REQUIRED COMPONENTS + roscpp + std_msgs + quad_msgs + quad_utils +) + +## System dependencies are found with CMake's conventions +# find_package(Boost REQUIRED COMPONENTS system) + + +## Uncomment this if the package has a setup.py. This macro ensures +## modules and global scripts declared therein get installed +## See http://ros.org/doc/api/catkin/html/user_guide/setup_dot_py.html +# catkin_python_setup() + +################################################ +## Declare ROS messages, services and actions ## +################################################ + +## To declare and build messages, services or actions from within this +## package, follow these steps: +## * Let MSG_DEP_SET be the set of packages whose message types you use in +## your messages/services/actions (e.g. std_msgs, actionlib_msgs, ...). +## * In the file package.xml: +## * add a build_depend tag for "message_generation" +## * add a build_depend and a exec_depend tag for each package in MSG_DEP_SET +## * If MSG_DEP_SET isn't empty the following dependency has been pulled in +## but can be declared for certainty nonetheless: +## * add a exec_depend tag for "message_runtime" +## * In this file (CMakeLists.txt): +## * add "message_generation" and every package in MSG_DEP_SET to +## find_package(catkin REQUIRED COMPONENTS ...) +## * add "message_runtime" and every package in MSG_DEP_SET to +## catkin_package(CATKIN_DEPENDS ...) +## * uncomment the add_*_files sections below as needed +## and list every .msg/.srv/.action file to be processed +## * uncomment the generate_messages entry below +## * add every package in MSG_DEP_SET to generate_messages(DEPENDENCIES ...) + +## Generate messages in the 'msg' folder +# add_message_files( +# FILES +# Message1.msg +# Message2.msg +# ) + +## Generate services in the 'srv' folder +# add_service_files( +# FILES +# Service1.srv +# Service2.srv +# ) + +## Generate actions in the 'action' folder +# add_action_files( +# FILES +# Action1.action +# Action2.action +# ) + +## Generate added messages and services with any dependencies listed here +# generate_messages( +# DEPENDENCIES +# std_msgs +# ) + +################################################ +## Declare ROS dynamic reconfigure parameters ## +################################################ + +## To declare and build dynamic reconfigure parameters within this +## package, follow these steps: +## * In the file package.xml: +## * add a build_depend and a exec_depend tag for "dynamic_reconfigure" +## * In this file (CMakeLists.txt): +## * add "dynamic_reconfigure" to +## find_package(catkin REQUIRED COMPONENTS ...) +## * uncomment the "generate_dynamic_reconfigure_options" section below +## and list every .cfg file to be processed + +## Generate dynamic reconfigure parameters in the 'cfg' folder +# generate_dynamic_reconfigure_options( +# cfg/DynReconf1.cfg +# cfg/DynReconf2.cfg +# ) + +################################### +## catkin specific configuration ## +################################### +## The catkin_package macro generates cmake config files for your package +## Declare things to be passed to dependent projects +## INCLUDE_DIRS: uncomment this if your package contains header files +## LIBRARIES: libraries you create in this project that dependent projects also need +## CATKIN_DEPENDS: catkin_packages dependent projects also need +## DEPENDS: system dependencies of this project that dependent projects also need +catkin_package( + INCLUDE_DIRS include +# LIBRARIES body_force_estimator + CATKIN_DEPENDS roscpp std_msgs quad_msgs quad_utils +# DEPENDS system_lib +) + +########### +## Build ## +########### + +## Specify additional locations of header files +## Your package locations should be listed before other locations +include_directories( + include + ${catkin_INCLUDE_DIRS} +) + +## Declare a C++ library +add_library(${PROJECT_NAME} + src/body_force_estimator.cpp + src/body_force_estimator_dynamics.cpp +) + +## Add cmake target dependencies of the library +## as an example, code may need to be generated before libraries +## either from message generation or dynamic reconfigure +add_dependencies(${PROJECT_NAME} ${${PROJECT_NAME}_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS}) + +## Declare a C++ executable +## With catkin_make all packages are built within a single CMake context +## The recommended prefix ensures that target names across packages don't collide +add_executable(${PROJECT_NAME}_node src/body_force_estimator_node.cpp) + +## Rename C++ executable without prefix +## The above recommended prefix causes long target names, the following renames the +## target back to the shorter version for ease of user use +## e.g. "rosrun someones_pkg node" instead of "rosrun someones_pkg someones_pkg_node" +# set_target_properties(${PROJECT_NAME}_node PROPERTIES OUTPUT_NAME node PREFIX "") + +## Add cmake target dependencies of the executable +## same as for the library above +# add_dependencies(${PROJECT_NAME}_node ${${PROJECT_NAME}_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS}) + +## Specify libraries to link a library or executable target against +target_link_libraries(body_force_estimator_node + body_force_estimator + ${catkin_LIBRARIES} +) + +############# +## Install ## +############# + +# all install targets should use catkin DESTINATION variables +# See http://ros.org/doc/api/catkin/html/adv_user_guide/variables.html + +## Mark executable scripts (Python etc.) for installation +## in contrast to setup.py, you can choose the destination +catkin_install_python(PROGRAMS + scripts/leg_flail.py + scripts/path_following.py + DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION} +) + +## Mark executables for installation +## See http://docs.ros.org/melodic/api/catkin/html/howto/format1/building_executables.html +# install(TARGETS ${PROJECT_NAME}_node +# RUNTIME DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION} +# ) + +## Mark libraries for installation +## See http://docs.ros.org/melodic/api/catkin/html/howto/format1/building_libraries.html +# install(TARGETS ${PROJECT_NAME} +# ARCHIVE DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} +# LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} +# RUNTIME DESTINATION ${CATKIN_GLOBAL_BIN_DESTINATION} +# ) + +## Mark cpp header files for installation +# install(DIRECTORY include/${PROJECT_NAME}/ +# DESTINATION ${CATKIN_PACKAGE_INCLUDE_DESTINATION} +# FILES_MATCHING PATTERN "*.h" +# PATTERN ".svn" EXCLUDE +# ) + +## Mark other files for installation (e.g. launch and bag files, etc.) +# install(FILES +# # myfile1 +# # myfile2 +# DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION} +# ) + +############# +## Testing ## +############# + +## Add gtest based cpp test target and link libraries +catkin_add_gtest(${PROJECT_NAME}-test test/test_body_force_estimator.cpp) +target_link_libraries(${PROJECT_NAME}-test ${PROJECT_NAME} ${catkin_LIBRARIES}) + +## Add folders to be run by python nosetests +# catkin_add_nosetests(test) diff --git a/body_force_estimator/body_force_estimator.yaml b/body_force_estimator/body_force_estimator.yaml new file mode 100644 index 000000000..a5e265cc8 --- /dev/null +++ b/body_force_estimator/body_force_estimator.yaml @@ -0,0 +1,4 @@ +body_force_estimator: + update_rate: 250 + K_O: 25 # 50 + cancel_friction: 1 \ No newline at end of file diff --git a/body_force_estimator/include/body_force_estimator/body_force_estimator.h b/body_force_estimator/include/body_force_estimator/body_force_estimator.h new file mode 100644 index 000000000..6146fd790 --- /dev/null +++ b/body_force_estimator/include/body_force_estimator/body_force_estimator.h @@ -0,0 +1,96 @@ +#ifndef BODY_FORCE_ESTIMATOR_H +#define BODY_FORCE_ESTIMATOR_H + +#include +#include +#include +#include +#include +#include +#include + +//! Estimates body contact forces +/*! + BodyForceEstimator is a container for all logic used in estimating force from + contacts distrbuted across all links of the robot. It requires robot state + estimates and motor commands and exposes an update method. +*/ +class BodyForceEstimator { + public: + /** + * @brief Constructor for BodyForceEstimator Class + * @param[in] nh ROS NodeHandle to publish and subscribe from + * @return Constructed object of type BodyForceEstimator + */ + BodyForceEstimator(ros::NodeHandle nh); + + /** + * @brief Calls ros spinOnce and pubs data at set frequency + */ + void spin(); + + /** + * @brief Callback function to handle new state estimates + * @param[in] Robot state message contining position and velocity for each + * joint and robot body + */ + void robotStateCallback(const quad_msgs::RobotState::ConstPtr& msg); + + /** + * @brief Callback function to handle new local plan (states and GRFs) + * @param[in] msg input message contining the local plan + */ + void localPlanCallback(const quad_msgs::RobotPlan::ConstPtr& msg); + + /** + * @brief Compute the momentum observer external force estimation update. + */ + void update(); + + /** + * @brief Publish body force force estimates + */ + void publishBodyForce(); + + /// ROS subscriber for the robot state + ros::Subscriber robot_state_sub_; + + /// ROS subscriber for local plan + ros::Subscriber local_plan_sub_; + + /// ROS publisher for body force force estimates + ros::Publisher body_force_pub_; + + /// ROS publisher for toe force estimates + ros::Publisher toe_force_pub_; + + /// Nodehandle to pub to and sub from + ros::NodeHandle nh_; + + /// Update rate for sending and receiving data; + double update_rate_; + + /// Momentum observer gain + double K_O_; + + /// Momentum observer cancel friction or not + int cancel_friction_; + + private: + /// External torque estimate + double r_mom[12]; + + /// Momentum estimate + double p_hat[12]; + + /// Most recent local plan + quad_msgs::RobotPlan::ConstPtr last_local_plan_msg_; + + /// Previous foot state + quad_msgs::MultiFootState past_feet_state_; + + // Robot state estimate + quad_msgs::RobotState::ConstPtr last_state_msg_; +}; + +#endif // BODY_FORCE_ESTIMATOR_H diff --git a/body_force_estimator/include/body_force_estimator/body_force_estimator_dynamics.h b/body_force_estimator/include/body_force_estimator/body_force_estimator_dynamics.h new file mode 100644 index 000000000..c2536c4b9 --- /dev/null +++ b/body_force_estimator/include/body_force_estimator/body_force_estimator_dynamics.h @@ -0,0 +1,19 @@ +#ifndef FORCE_ESTIMATOR_DYNAMICS_H +#define FORCE_ESTIMATOR_DYNAMICS_H + +#include +#include + +#include + +namespace force_estimation_dynamics { +void f_M(Eigen::Vector3d q, int RL, Eigen::Matrix3d &F); +void f_beta(Eigen::Vector3d q, Eigen::Vector3d qd, int RL, Eigen::Vector3d &F); +void f_J_MO(Eigen::Vector3d q, int RL, Eigen::Matrix3d &F); + +extern double MO_fric[3]; +extern double MO_damp[3]; +extern double MO_ktau[3]; +} // namespace force_estimation_dynamics + +#endif // FORCE_ESTIMATOR_DYNAMICS_H diff --git a/body_force_estimator/package.xml b/body_force_estimator/package.xml new file mode 100644 index 000000000..eaabcddfd --- /dev/null +++ b/body_force_estimator/package.xml @@ -0,0 +1,74 @@ + + + body_force_estimator + 0.0.0 + The body_force_estimator package + + + + + Justin Yim + + + + + + MIT + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + catkin + roscpp + std_msgs + sensor_msgs + quad_msgs + quad_utils + roscpp + std_msgs + sensor_msgs + quad_msgs + quad_utils + roscpp + std_msgs + sensor_msgs + quad_msgs + quad_utils + + + + + + + + diff --git a/body_force_estimator/scripts/leg_flail.py b/body_force_estimator/scripts/leg_flail.py new file mode 100755 index 000000000..d2e64a8b9 --- /dev/null +++ b/body_force_estimator/scripts/leg_flail.py @@ -0,0 +1,33 @@ +#!/usr/bin/env python + +import rospy +from geometry_msgs.msg import Vector3 +from quad_msgs.msg import RobotState + +last_state_msg_ = RobotState() + +def callback(data): + last_state_msg_ = data + +def leg_flail(): + pub = rospy.Publisher('/robot_1/control/single_joint_command', Vector3, queue_size=10) + #rospy.Subscriber('/robot_1/state/ground_truth', RobotState, callback) + rospy.init_node('leg_flail') + rate = rospy.Rate(100) + while not rospy.is_shutdown(): + cmd = Vector3() + #print(last_state_msg_) + for i in range(0,4): + for j in range(0,3): + cmd.x = i + cmd.y = j + cmd.z = 10 + + pub.publish(cmd) + rate.sleep() + +if __name__ == '__main__': + try: + leg_flail() + except rospy.ROSInterruptException: + pass diff --git a/body_force_estimator/scripts/path_following.py b/body_force_estimator/scripts/path_following.py new file mode 100755 index 000000000..1fdb00cae --- /dev/null +++ b/body_force_estimator/scripts/path_following.py @@ -0,0 +1,77 @@ +#!/usr/bin/env python + +# Command the robot to walk along a straight line parallel to the x-axis with +# zero yaw + +import rospy +import math +from geometry_msgs.msg import Twist +from geometry_msgs.msg import Pose +from geometry_msgs.msg import Point +from quad_msgs.msg import RobotState +from tf.transformations import euler_from_quaternion, quaternion_from_euler + +# Initialization +last_state_msg_ = RobotState() # Robot state message + +def callback(data): + global last_state_msg_ + last_state_msg_ = data + +def path_following(): + global last_state_msg_ + + pub = rospy.Publisher('/robot_1/cmd_vel', Twist, queue_size=10) + rospy.Subscriber('/robot_1/state/ground_truth', RobotState, callback) + rospy.init_node('path_following') + + # Parameters + rate = 100 + ros_rate = rospy.Rate(rate) # Rate + speed = 0.5 # Forward velocity + y_pt = 0.0 # Lateral shift + yaw_pt = 0.0 #math.pi # Yaw angle + y_gain = 1.0 # Lateral P gain in m/s per m (or Hz) + yaw_gain = 3.0 # 1.0 # Yaw P gain in rad/s per rad (or Hz) + speed_i = 0.0 # Integration rate + + speed_integral = 0.0 # Initialize integrator + speed_sign = (speed > 0)*1 + (speed < 0)*-1 + + while last_state_msg_.header.seq == 0: + print('path_following: no state message') + ros_rate.sleep() + + while not rospy.is_shutdown(): + cmd = Twist() # Initialize twist command + q = last_state_msg_.body.pose.orientation + euler = euler_from_quaternion([q.x, q.y, q.z, q.w]) + + # x velocity error integral + speed_error = last_state_msg_.body.twist.linear.x - speed + if speed_error*speed_sign < 0: + speed_integral = speed_integral - speed_i/rate*speed_error + else: + speed_integral = speed_integral - speed_i/rate*speed_error*10 + + # Yaw angle + yaw_error = euler[2] - yaw_pt + if yaw_error > math.pi: + yaw_error = yaw_error - 2*math.pi + elif yaw_error < -math.pi: + yaw_error = yaw_error + 2*math.pi + + # Velocity commands: proportional feedback on y and yaw and integral in x + cmd.linear.x = speed + speed_integral + cmd.linear.y = -speed_sign*y_gain*(last_state_msg_.body.pose.position.y - y_pt) + cmd.angular.z = -yaw_gain*yaw_error + + pub.publish(cmd) + print(cmd) + ros_rate.sleep() + +if __name__ == '__main__': + try: + path_following() + except rospy.ROSInterruptException: + pass diff --git a/body_force_estimator/setup_deps.sh b/body_force_estimator/setup_deps.sh new file mode 100755 index 000000000..e69de29bb diff --git a/body_force_estimator/src/body_force_estimator.cpp b/body_force_estimator/src/body_force_estimator.cpp new file mode 100644 index 000000000..cd7e8e3bd --- /dev/null +++ b/body_force_estimator/src/body_force_estimator.cpp @@ -0,0 +1,226 @@ +#include "body_force_estimator/body_force_estimator.h" + +#include "body_force_estimator/body_force_estimator_dynamics.h" + +using namespace force_estimation_dynamics; + +int joint_inds[12] = {8, 0, 1, 9, 2, 3, 10, 4, 5, 11, 6, 7}; + +// Effective toe force estimate +double f_toe_MO[12]; +// Planned liftoff time +double t_up[4]; + +BodyForceEstimator::BodyForceEstimator(ros::NodeHandle nh) { + nh_ = nh; + + // Load rosparams from parameter server + std::string robot_state_topic, body_force_topic, toe_force_topic, + local_plan_topic; + quad_utils::loadROSParam(nh_, "topics/state/ground_truth", robot_state_topic); + quad_utils::loadROSParam(nh_, "topics/local_plan", local_plan_topic); + quad_utils::loadROSParam(nh_, "topics/body_force/joint_torques", + body_force_topic); + quad_utils::loadROSParam(nh_, "topics/body_force/toe_forces", + toe_force_topic); + nh.param( + "/body_force_estimator/update_rate", update_rate_, + 250); // add a param for your package instead of using the estimator one + nh.param("/body_force_estimator/K_O", K_O_, 50); + nh.param("/body_force_estimator/cancel_friction", cancel_friction_, 1); + + // Setup pubs and subs + robot_state_sub_ = nh_.subscribe( + robot_state_topic, 1, &BodyForceEstimator::robotStateCallback, this, + ros::TransportHints().tcpNoDelay(true)); + local_plan_sub_ = nh_.subscribe(local_plan_topic, 1, + &BodyForceEstimator::localPlanCallback, this); + body_force_pub_ = + nh_.advertise(body_force_topic, 1); + toe_force_pub_ = nh_.advertise(toe_force_topic, 1); +} + +void BodyForceEstimator::robotStateCallback( + const quad_msgs::RobotState::ConstPtr& msg) { + // ROS_INFO("In robotStateCallback"); + last_state_msg_ = msg; +} + +void BodyForceEstimator::localPlanCallback( + const quad_msgs::RobotPlan::ConstPtr& msg) { + last_local_plan_msg_ = msg; +} + +void BodyForceEstimator::update() { + Eigen::Matrix3d M; + Eigen::Vector3d beta; + Eigen::Matrix3d J_MO; // Jacobian + + Eigen::Vector3d q; // joint positions + Eigen::Vector3d qd; // joint velocities + Eigen::Vector3d tau; // joint actuator torques + Eigen::Vector3d re; // momentum observer external torque estimates + Eigen::Vector3d pe; // momentum observer momentum estimate + Eigen::Vector3d fe_toe; // effective toe forces + + // Momentum observer gain (todo: this should be precomputed once) + Eigen::Matrix3d K_O; + K_O << K_O_, 0, 0, 0, K_O_, 0, 0, 0, K_O_; + + // Joint directions (todo: make this a parameter or read the URDF) + int joint_dirs[3] = {1, -1, 1}; + + if (last_state_msg_ == NULL || last_local_plan_msg_ == NULL) { + return; + } + + // Interpolate the local plan to get the reference state + quad_msgs::RobotState ref_state_msg; + ros::Time t_first_state = last_local_plan_msg_->states.front().header.stamp; + double t_now = (ros::Time::now() - last_local_plan_msg_->state_timestamp) + .toSec(); // Use time of state - RECOMMENDED + if ((t_now < + (last_local_plan_msg_->states.front().header.stamp - t_first_state) + .toSec()) || + (t_now > + (last_local_plan_msg_->states.back().header.stamp - t_first_state) + .toSec())) { + ROS_ERROR("ID node couldn't find the correct ref state!"); + std::cout << "t_now " << t_now << ", plan front " + << (last_local_plan_msg_->states.front().header.stamp - + t_first_state) + .toSec() + << ", plan back " + << (last_local_plan_msg_->states.back().header.stamp - + t_first_state) + .toSec() + << std::endl; + } + for (int i = 0; i < last_local_plan_msg_->states.size() - 1; i++) { + if ((t_now >= (last_local_plan_msg_->states[i].header.stamp - t_first_state) + .toSec()) && + (t_now < + (last_local_plan_msg_->states[i + 1].header.stamp - t_first_state) + .toSec())) { + double t_interp = + (t_now - + (last_local_plan_msg_->states[i].header.stamp - t_first_state) + .toSec()) / + (last_local_plan_msg_->states[i + 1].header.stamp.toSec() - + last_local_plan_msg_->states[i].header.stamp.toSec()); + + // Linearly interpolate between states + quad_utils::interpRobotState(last_local_plan_msg_->states[i], + last_local_plan_msg_->states[i + 1], + t_interp, ref_state_msg); + continue; + } + } + + if (past_feet_state_.feet.empty()) { + past_feet_state_ = ref_state_msg.feet; + } + + for (int i = 0; i < 4; i++) { + // Loop over four legs: FL, BL, FR, BR + + // Zero the torque estimates when the foot first lifts off the ground + if (ref_state_msg.feet.feet[i].contact) { + // Record the time that the foot was last in stance + t_up[i] = (ros::Time::now()).toSec(); + } + if (!ref_state_msg.feet.feet[i].contact && + ((ros::Time::now()).toSec() - t_up[i] < 0.03)) { + // on liftoff reset momentum observer state to 0s + for (int j = 0; j < 3; j++) { + p_hat[3 * i + j] = 0; + r_mom[3 * i + j] = 0; + } + } else { + // Compute joint torque estimates with momentum observer + + for (int j = 0; j < 3; j++) { + // read joint data from message + int ind = 3 * i + j; + q[j] = joint_dirs[j] * last_state_msg_->joints.position[ind]; + qd[j] = joint_dirs[j] * last_state_msg_->joints.velocity[ind]; + tau[j] = + MO_ktau[j] * joint_dirs[j] * last_state_msg_->joints.effort[ind]; + + if (cancel_friction_) { + // Compensate for friction with estimated dry and viscous parameters + tau[j] += (qd[j] > 0 ? 1 : -1) * MO_fric[j] + qd[j] * MO_damp[j]; + } + + // read this leg's estimates + re[j] = r_mom[3 * i + j]; + pe[j] = p_hat[3 * i + j]; + } + // Compute dynamics matrices and vectors + int RL = i < 2 ? -1 : 1; + f_M(q, RL, M); + f_beta(q, qd, RL, beta); + f_J_MO(q, RL, J_MO); + + // Momentum observer update + Eigen::Vector3d p = M * qd; + Eigen::Vector3d pd_hat = tau - beta + re; + p = p - pe; + re = K_O * p; + pe = pe + pd_hat / update_rate_; + + // Effective toe forces + fe_toe = J_MO.transpose().colPivHouseholderQr().solve(re); + + for (int j = 0; j < 3; j++) { + r_mom[3 * i + j] = re[j]; + p_hat[3 * i + j] = pe[j]; + f_toe_MO[3 * i + j] = fe_toe[j]; + } + } + } + + past_feet_state_ = ref_state_msg.feet; +} + +void BodyForceEstimator::publishBodyForce() { + // ROS_INFO("In BodyForce"); + quad_msgs::BodyForceEstimate msg; + quad_msgs::GRFArray msg_toe; + + for (int i = 0; i < 4; i++) { + geometry_msgs::Vector3 ft; + + ft.x = f_toe_MO[3 * i + 0]; + ft.y = f_toe_MO[3 * i + 1]; + ft.z = f_toe_MO[3 * i + 2]; + + msg_toe.vectors.push_back(ft); + } + for (int i = 0; i < 12; i++) { + msg.joint_torques.push_back(r_mom[i]); + } + + msg.header.stamp = ros::Time::now(); + msg_toe.header.stamp = ros::Time::now(); + + body_force_pub_.publish(msg); + toe_force_pub_.publish(msg_toe); +} + +void BodyForceEstimator::spin() { + ros::Rate r(update_rate_); + while (ros::ok()) { + // Collect new messages on subscriber topics + + // Compute new estimate + this->update(); + + // Publish new estimate + publishBodyForce(); + + ros::spinOnce(); + // Enforce update rate + r.sleep(); + } +} diff --git a/body_force_estimator/src/body_force_estimator_dynamics.cpp b/body_force_estimator/src/body_force_estimator_dynamics.cpp new file mode 100644 index 000000000..bf681cacf --- /dev/null +++ b/body_force_estimator/src/body_force_estimator_dynamics.cpp @@ -0,0 +1,19449 @@ +#include "body_force_estimator/body_force_estimator_dynamics.h" + +namespace force_estimation_dynamics { + +double MO_fric[3] = {-0.1977, -0.2242, -0.4360}; +double MO_damp[3] = {0.0354, 0.0451, -0.0265}; +double MO_ktau[3] = {0.546, 0.546, 1.092}; + +double l[15] = { + 0, -0.101, 0, -0.2060, 0, 0, 0.2060, 0, 0, 0, 0, 0, 0, 0, 0 // 0, 0.050, 0 +}; +double G[3] = {6, 6, 12}; + +// Spirit40 Quadruped Identified parameters +double phi[60] = { + 0.40835, -8.2577e-22, -0.010685, -0.0047235, 0.00037337, + 0.00012421, 0.00034915, -2.4585e-23, -0.00012359, -1.4203e-23, + 0.64165, -0.031586, 0.024924, 0.0035579, 0.0014187, + 0.0081689, 0.0091453, 5.4006e-05, -2.1884e-05, 0.00082911, + 0.111, 0.01221, 7.6675e-11, -7.3791e-12, 1.3032e-05, + 0.0020514, 0.0020549, -6.4727e-05, -3.289e-06, -3.6995e-05, + 0.15, 0, -1.6363e-11, 2.9759e-11, 8.5705e-05, + 4.7851e-05, 4.7851e-05, 0, 0, 0, + 0.15, -3.6488e-11, -0.0008677, -4.5619e-11, 5.2199e-05, + 8.5705e-05, 5.2199e-05, 5.9888e-12, -1.2269e-10, 5.6103e-11, + 0.15, 9.2005e-11, 0.00016055, 9.6905e-12, 6.6689e-05, + 0.00012438, 6.6689e-05, -1.9005e-10, -1.1627e-10, -3.8405e-11, +}; + +void f_M(Eigen::Vector3d q, int RL, Eigen::Matrix3d& F) { + F(0 * 3 + 0) = + phi[0 * 10 + 4] + + l[3 * 3 + 2] * (l[3 * 3 + 2] * phi[4 * 10 + 0] + + phi[4 * 10 + 3] * cos(G[0 * 3 + 1] * q[0 * 3 + 1]) - + phi[4 * 10 + 1] * sin(G[0 * 3 + 1] * q[0 * 3 + 1])) + + (G[0 * 3 + 0] * G[0 * 3 + 0]) * + (phi[3 * 10 + 4] - + phi[3 * 10 + 0] * + (1.0 / (phi[3 * 10 + 0] * phi[3 * 10 + 0]) * + (phi[3 * 10 + 3] * phi[3 * 10 + 3]) + + (RL * RL) * 1.0 / (phi[3 * 10 + 0] * phi[3 * 10 + 0]) * + (phi[3 * 10 + 2] * phi[3 * 10 + 2])) + + (phi[3 * 10 + 3] * phi[3 * 10 + 3]) / phi[3 * 10 + 0] + + ((RL * RL) * (phi[3 * 10 + 2] * phi[3 * 10 + 2])) / + phi[3 * 10 + 0]) + + cos(G[0 * 3 + 1] * q[0 * 3 + 1]) * + (cos(G[0 * 3 + 1] * q[0 * 3 + 1]) * + (phi[4 * 10 + 4] - + phi[4 * 10 + 0] * + (1.0 / (phi[4 * 10 + 0] * phi[4 * 10 + 0]) * + (phi[4 * 10 + 3] * phi[4 * 10 + 3]) + + (RL * RL) * 1.0 / (phi[4 * 10 + 0] * phi[4 * 10 + 0]) * + (phi[4 * 10 + 2] * phi[4 * 10 + 2])) + + (phi[4 * 10 + 3] * phi[4 * 10 + 3]) / phi[4 * 10 + 0] + + ((RL * RL) * (phi[4 * 10 + 2] * phi[4 * 10 + 2])) / + phi[4 * 10 + 0]) + + l[3 * 3 + 2] * phi[4 * 10 + 3] + + phi[4 * 10 + 9] * sin(G[0 * 3 + 1] * q[0 * 3 + 1]) + + (RL * RL) * l[3 * 3 + 1] * phi[4 * 10 + 2] * + cos(G[0 * 3 + 1] * q[0 * 3 + 1])) + + cos(q[0 * 3 + 1]) * + (-sin(q[0 * 3 + 1]) * + (-phi[1 * 10 + 9] + + l[4 * 3 + 2] * + (l[4 * 3 + 0] * phi[5 * 10 + 0] + + phi[5 * 10 + 1] * cos(G[0 * 3 + 2] * q[0 * 3 + 2]) + + phi[5 * 10 + 3] * sin(G[0 * 3 + 2] * q[0 * 3 + 2])) - + sin(G[0 * 3 + 2] * q[0 * 3 + 2]) * + (cos(G[0 * 3 + 2] * q[0 * 3 + 2]) * + (phi[5 * 10 + 6] - + phi[5 * 10 + 0] * + (1.0 / (phi[5 * 10 + 0] * phi[5 * 10 + 0]) * + (phi[5 * 10 + 1] * phi[5 * 10 + 1]) + + (RL * RL) * 1.0 / + (phi[5 * 10 + 0] * phi[5 * 10 + 0]) * + (phi[5 * 10 + 2] * phi[5 * 10 + 2])) + + (phi[5 * 10 + 1] * phi[5 * 10 + 1]) / + phi[5 * 10 + 0] + + ((RL * RL) * (phi[5 * 10 + 2] * phi[5 * 10 + 2])) / + phi[5 * 10 + 0]) + + l[4 * 3 + 0] * phi[5 * 10 + 1] - + phi[5 * 10 + 9] * sin(G[0 * 3 + 2] * q[0 * 3 + 2]) + + (RL * RL) * l[4 * 3 + 1] * phi[5 * 10 + 2] * + cos(G[0 * 3 + 2] * q[0 * 3 + 2])) + + cos(G[0 * 3 + 2] * q[0 * 3 + 2]) * + (l[4 * 3 + 0] * phi[5 * 10 + 3] + + sin(G[0 * 3 + 2] * q[0 * 3 + 2]) * + (phi[5 * 10 + 4] - + phi[5 * 10 + 0] * + (1.0 / (phi[5 * 10 + 0] * phi[5 * 10 + 0]) * + (phi[5 * 10 + 3] * phi[5 * 10 + 3]) + + (RL * RL) * 1.0 / + (phi[5 * 10 + 0] * phi[5 * 10 + 0]) * + (phi[5 * 10 + 2] * phi[5 * 10 + 2])) + + (phi[5 * 10 + 3] * phi[5 * 10 + 3]) / + phi[5 * 10 + 0] + + ((RL * RL) * (phi[5 * 10 + 2] * phi[5 * 10 + 2])) / + phi[5 * 10 + 0]) - + phi[5 * 10 + 9] * cos(G[0 * 3 + 2] * q[0 * 3 + 2]) + + (RL * RL) * l[4 * 3 + 1] * phi[5 * 10 + 2] * + sin(G[0 * 3 + 2] * q[0 * 3 + 2])) + + l[1 * 3 + 2] * (l[1 * 3 + 0] * phi[2 * 10 + 0] + + phi[2 * 10 + 1] * cos(q[0 * 3 + 2]) + + phi[2 * 10 + 3] * sin(q[0 * 3 + 2])) - + sin(q[0 * 3 + 2]) * + (l[1 * 3 + 0] * phi[2 * 10 + 1] + + cos(q[0 * 3 + 2]) * + (phi[2 * 10 + 6] - + phi[2 * 10 + 0] * + (1.0 / (phi[2 * 10 + 0] * phi[2 * 10 + 0]) * + (phi[2 * 10 + 1] * phi[2 * 10 + 1]) + + (RL * RL) * 1.0 / + (phi[2 * 10 + 0] * phi[2 * 10 + 0]) * + (phi[2 * 10 + 2] * phi[2 * 10 + 2])) + + (phi[2 * 10 + 1] * phi[2 * 10 + 1]) / + phi[2 * 10 + 0] + + ((RL * RL) * (phi[2 * 10 + 2] * phi[2 * 10 + 2])) / + phi[2 * 10 + 0]) - + phi[2 * 10 + 9] * sin(q[0 * 3 + 2]) + + (RL * RL) * l[1 * 3 + 1] * phi[2 * 10 + 2] * + cos(q[0 * 3 + 2])) + + cos(q[0 * 3 + 2]) * + (l[1 * 3 + 0] * phi[2 * 10 + 3] + + sin(q[0 * 3 + 2]) * + (phi[2 * 10 + 4] - + phi[2 * 10 + 0] * + (1.0 / (phi[2 * 10 + 0] * phi[2 * 10 + 0]) * + (phi[2 * 10 + 3] * phi[2 * 10 + 3]) + + (RL * RL) * 1.0 / + (phi[2 * 10 + 0] * phi[2 * 10 + 0]) * + (phi[2 * 10 + 2] * phi[2 * 10 + 2])) + + (phi[2 * 10 + 3] * phi[2 * 10 + 3]) / + phi[2 * 10 + 0] + + ((RL * RL) * (phi[2 * 10 + 2] * phi[2 * 10 + 2])) / + phi[2 * 10 + 0]) - + phi[2 * 10 + 9] * cos(q[0 * 3 + 2]) + + (RL * RL) * l[1 * 3 + 1] * phi[2 * 10 + 2] * + sin(q[0 * 3 + 2])) - + RL * l[4 * 3 + 1] * sin(G[0 * 3 + 2] * q[0 * 3 + 2]) * + (RL * phi[5 * 10 + 2] * cos(G[0 * 3 + 2] * q[0 * 3 + 2]) + + RL * l[4 * 3 + 1] * phi[5 * 10 + 0] * + cos(G[0 * 3 + 2] * q[0 * 3 + 2])) - + RL * l[1 * 3 + 1] * sin(q[0 * 3 + 2]) * + (RL * phi[2 * 10 + 2] * cos(q[0 * 3 + 2]) + + RL * l[1 * 3 + 1] * phi[2 * 10 + 0] * + cos(q[0 * 3 + 2])) + + RL * l[4 * 3 + 1] * cos(G[0 * 3 + 2] * q[0 * 3 + 2]) * + (RL * phi[5 * 10 + 2] * sin(G[0 * 3 + 2] * q[0 * 3 + 2]) + + RL * l[4 * 3 + 1] * phi[5 * 10 + 0] * + sin(G[0 * 3 + 2] * q[0 * 3 + 2])) + + RL * l[1 * 3 + 1] * cos(q[0 * 3 + 2]) * + (RL * phi[2 * 10 + 2] * sin(q[0 * 3 + 2]) + + RL * l[1 * 3 + 1] * phi[2 * 10 + 0] * + sin(q[0 * 3 + 2]))) + + l[0 * 3 + 2] * + (phi[1 * 10 + 3] + l[1 * 3 + 2] * phi[2 * 10 + 0] + + l[4 * 3 + 2] * phi[5 * 10 + 0] + + phi[5 * 10 + 3] * cos(G[0 * 3 + 2] * q[0 * 3 + 2]) - + phi[5 * 10 + 1] * sin(G[0 * 3 + 2] * q[0 * 3 + 2]) + + phi[2 * 10 + 3] * cos(q[0 * 3 + 2]) - + phi[2 * 10 + 1] * sin(q[0 * 3 + 2])) + + cos(q[0 * 3 + 1]) * + (phi[1 * 10 + 4] + + l[4 * 3 + 2] * + (l[4 * 3 + 2] * phi[5 * 10 + 0] + + phi[5 * 10 + 3] * cos(G[0 * 3 + 2] * q[0 * 3 + 2]) - + phi[5 * 10 + 1] * sin(G[0 * 3 + 2] * q[0 * 3 + 2])) + + cos(G[0 * 3 + 2] * q[0 * 3 + 2]) * + (cos(G[0 * 3 + 2] * q[0 * 3 + 2]) * + (phi[5 * 10 + 4] - + phi[5 * 10 + 0] * + (1.0 / (phi[5 * 10 + 0] * phi[5 * 10 + 0]) * + (phi[5 * 10 + 3] * phi[5 * 10 + 3]) + + (RL * RL) * 1.0 / + (phi[5 * 10 + 0] * phi[5 * 10 + 0]) * + (phi[5 * 10 + 2] * phi[5 * 10 + 2])) + + (phi[5 * 10 + 3] * phi[5 * 10 + 3]) / + phi[5 * 10 + 0] + + ((RL * RL) * (phi[5 * 10 + 2] * phi[5 * 10 + 2])) / + phi[5 * 10 + 0]) + + l[4 * 3 + 2] * phi[5 * 10 + 3] + + phi[5 * 10 + 9] * sin(G[0 * 3 + 2] * q[0 * 3 + 2]) + + (RL * RL) * l[4 * 3 + 1] * phi[5 * 10 + 2] * + cos(G[0 * 3 + 2] * q[0 * 3 + 2])) + + sin(G[0 * 3 + 2] * q[0 * 3 + 2]) * + (-l[4 * 3 + 2] * phi[5 * 10 + 1] + + sin(G[0 * 3 + 2] * q[0 * 3 + 2]) * + (phi[5 * 10 + 6] - + phi[5 * 10 + 0] * + (1.0 / (phi[5 * 10 + 0] * phi[5 * 10 + 0]) * + (phi[5 * 10 + 1] * phi[5 * 10 + 1]) + + (RL * RL) * 1.0 / + (phi[5 * 10 + 0] * phi[5 * 10 + 0]) * + (phi[5 * 10 + 2] * phi[5 * 10 + 2])) + + (phi[5 * 10 + 1] * phi[5 * 10 + 1]) / + phi[5 * 10 + 0] + + ((RL * RL) * (phi[5 * 10 + 2] * phi[5 * 10 + 2])) / + phi[5 * 10 + 0]) + + phi[5 * 10 + 9] * cos(G[0 * 3 + 2] * q[0 * 3 + 2]) + + (RL * RL) * l[4 * 3 + 1] * phi[5 * 10 + 2] * + sin(G[0 * 3 + 2] * q[0 * 3 + 2])) + + l[1 * 3 + 2] * (l[1 * 3 + 2] * phi[2 * 10 + 0] + + phi[2 * 10 + 3] * cos(q[0 * 3 + 2]) - + phi[2 * 10 + 1] * sin(q[0 * 3 + 2])) + + cos(q[0 * 3 + 2]) * + (l[1 * 3 + 2] * phi[2 * 10 + 3] + + cos(q[0 * 3 + 2]) * + (phi[2 * 10 + 4] - + phi[2 * 10 + 0] * + (1.0 / (phi[2 * 10 + 0] * phi[2 * 10 + 0]) * + (phi[2 * 10 + 3] * phi[2 * 10 + 3]) + + (RL * RL) * 1.0 / + (phi[2 * 10 + 0] * phi[2 * 10 + 0]) * + (phi[2 * 10 + 2] * phi[2 * 10 + 2])) + + (phi[2 * 10 + 3] * phi[2 * 10 + 3]) / + phi[2 * 10 + 0] + + ((RL * RL) * (phi[2 * 10 + 2] * phi[2 * 10 + 2])) / + phi[2 * 10 + 0]) + + phi[2 * 10 + 9] * sin(q[0 * 3 + 2]) + + (RL * RL) * l[1 * 3 + 1] * phi[2 * 10 + 2] * + cos(q[0 * 3 + 2])) - + phi[1 * 10 + 0] * + (1.0 / (phi[1 * 10 + 0] * phi[1 * 10 + 0]) * + (phi[1 * 10 + 3] * phi[1 * 10 + 3]) + + (RL * RL) * 1.0 / (phi[1 * 10 + 0] * phi[1 * 10 + 0]) * + (phi[1 * 10 + 2] * phi[1 * 10 + 2])) + + sin(q[0 * 3 + 2]) * + (-l[1 * 3 + 2] * phi[2 * 10 + 1] + + sin(q[0 * 3 + 2]) * + (phi[2 * 10 + 6] - + phi[2 * 10 + 0] * + (1.0 / (phi[2 * 10 + 0] * phi[2 * 10 + 0]) * + (phi[2 * 10 + 1] * phi[2 * 10 + 1]) + + (RL * RL) * 1.0 / + (phi[2 * 10 + 0] * phi[2 * 10 + 0]) * + (phi[2 * 10 + 2] * phi[2 * 10 + 2])) + + (phi[2 * 10 + 1] * phi[2 * 10 + 1]) / + phi[2 * 10 + 0] + + ((RL * RL) * (phi[2 * 10 + 2] * phi[2 * 10 + 2])) / + phi[2 * 10 + 0]) + + phi[2 * 10 + 9] * cos(q[0 * 3 + 2]) + + (RL * RL) * l[1 * 3 + 1] * phi[2 * 10 + 2] * + sin(q[0 * 3 + 2])) + + (phi[1 * 10 + 3] * phi[1 * 10 + 3]) / phi[1 * 10 + 0] + + ((RL * RL) * (phi[1 * 10 + 2] * phi[1 * 10 + 2])) / + phi[1 * 10 + 0] + + RL * l[4 * 3 + 1] * cos(G[0 * 3 + 2] * q[0 * 3 + 2]) * + (RL * phi[5 * 10 + 2] * cos(G[0 * 3 + 2] * q[0 * 3 + 2]) + + RL * l[4 * 3 + 1] * phi[5 * 10 + 0] * + cos(G[0 * 3 + 2] * q[0 * 3 + 2])) + + RL * l[1 * 3 + 1] * cos(q[0 * 3 + 2]) * + (RL * phi[2 * 10 + 2] * cos(q[0 * 3 + 2]) + + RL * l[1 * 3 + 1] * phi[2 * 10 + 0] * + cos(q[0 * 3 + 2])) + + RL * l[4 * 3 + 1] * sin(G[0 * 3 + 2] * q[0 * 3 + 2]) * + (RL * phi[5 * 10 + 2] * sin(G[0 * 3 + 2] * q[0 * 3 + 2]) + + RL * l[4 * 3 + 1] * phi[5 * 10 + 0] * + sin(G[0 * 3 + 2] * q[0 * 3 + 2])) + + RL * l[1 * 3 + 1] * sin(q[0 * 3 + 2]) * + (RL * phi[2 * 10 + 2] * sin(q[0 * 3 + 2]) + + RL * l[1 * 3 + 1] * phi[2 * 10 + 0] * + sin(q[0 * 3 + 2]))) + + RL * l[0 * 3 + 1] * cos(q[0 * 3 + 1]) * + (RL * phi[1 * 10 + 2] + + RL * phi[5 * 10 + 2] * + pow(sin(G[0 * 3 + 2] * q[0 * 3 + 2]), 2.0) + + RL * phi[2 * 10 + 2] * pow(cos(q[0 * 3 + 2]), 2.0) + + RL * phi[2 * 10 + 2] * pow(sin(q[0 * 3 + 2]), 2.0) + + RL * phi[5 * 10 + 2] * + pow(cos(G[0 * 3 + 2] * q[0 * 3 + 2]), 2.0) + + RL * l[4 * 3 + 1] * phi[5 * 10 + 0] * + pow(cos(G[0 * 3 + 2] * q[0 * 3 + 2]), 2.0) + + RL * l[4 * 3 + 1] * phi[5 * 10 + 0] * + pow(sin(G[0 * 3 + 2] * q[0 * 3 + 2]), 2.0) + + RL * l[1 * 3 + 1] * phi[2 * 10 + 0] * + pow(cos(q[0 * 3 + 2]), 2.0) + + RL * l[1 * 3 + 1] * phi[2 * 10 + 0] * + pow(sin(q[0 * 3 + 2]), 2.0))) + + sin(G[0 * 3 + 1] * q[0 * 3 + 1]) * + (-l[3 * 3 + 2] * phi[4 * 10 + 1] + + sin(G[0 * 3 + 1] * q[0 * 3 + 1]) * + (phi[4 * 10 + 6] - + phi[4 * 10 + 0] * + (1.0 / (phi[4 * 10 + 0] * phi[4 * 10 + 0]) * + (phi[4 * 10 + 1] * phi[4 * 10 + 1]) + + (RL * RL) * 1.0 / (phi[4 * 10 + 0] * phi[4 * 10 + 0]) * + (phi[4 * 10 + 2] * phi[4 * 10 + 2])) + + (phi[4 * 10 + 1] * phi[4 * 10 + 1]) / phi[4 * 10 + 0] + + ((RL * RL) * (phi[4 * 10 + 2] * phi[4 * 10 + 2])) / + phi[4 * 10 + 0]) + + phi[4 * 10 + 9] * cos(G[0 * 3 + 1] * q[0 * 3 + 1]) + + (RL * RL) * l[3 * 3 + 1] * phi[4 * 10 + 2] * + sin(G[0 * 3 + 1] * q[0 * 3 + 1])) - + sin(q[0 * 3 + 1]) * + (l[0 * 3 + 2] * + (phi[1 * 10 + 1] + l[1 * 3 + 0] * phi[2 * 10 + 0] + + l[4 * 3 + 0] * phi[5 * 10 + 0] + + phi[5 * 10 + 1] * cos(G[0 * 3 + 2] * q[0 * 3 + 2]) + + phi[5 * 10 + 3] * sin(G[0 * 3 + 2] * q[0 * 3 + 2]) + + phi[2 * 10 + 1] * cos(q[0 * 3 + 2]) + + phi[2 * 10 + 3] * sin(q[0 * 3 + 2])) - + sin(q[0 * 3 + 1]) * + (phi[1 * 10 + 6] + + l[4 * 3 + 0] * + (l[4 * 3 + 0] * phi[5 * 10 + 0] + + phi[5 * 10 + 1] * cos(G[0 * 3 + 2] * q[0 * 3 + 2]) + + phi[5 * 10 + 3] * sin(G[0 * 3 + 2] * q[0 * 3 + 2])) + + cos(G[0 * 3 + 2] * q[0 * 3 + 2]) * + (cos(G[0 * 3 + 2] * q[0 * 3 + 2]) * + (phi[5 * 10 + 6] - + phi[5 * 10 + 0] * + (1.0 / (phi[5 * 10 + 0] * phi[5 * 10 + 0]) * + (phi[5 * 10 + 1] * phi[5 * 10 + 1]) + + (RL * RL) * 1.0 / + (phi[5 * 10 + 0] * phi[5 * 10 + 0]) * + (phi[5 * 10 + 2] * phi[5 * 10 + 2])) + + (phi[5 * 10 + 1] * phi[5 * 10 + 1]) / + phi[5 * 10 + 0] + + ((RL * RL) * (phi[5 * 10 + 2] * phi[5 * 10 + 2])) / + phi[5 * 10 + 0]) + + l[4 * 3 + 0] * phi[5 * 10 + 1] - + phi[5 * 10 + 9] * sin(G[0 * 3 + 2] * q[0 * 3 + 2]) + + (RL * RL) * l[4 * 3 + 1] * phi[5 * 10 + 2] * + cos(G[0 * 3 + 2] * q[0 * 3 + 2])) + + sin(G[0 * 3 + 2] * q[0 * 3 + 2]) * + (l[4 * 3 + 0] * phi[5 * 10 + 3] + + sin(G[0 * 3 + 2] * q[0 * 3 + 2]) * + (phi[5 * 10 + 4] - + phi[5 * 10 + 0] * + (1.0 / (phi[5 * 10 + 0] * phi[5 * 10 + 0]) * + (phi[5 * 10 + 3] * phi[5 * 10 + 3]) + + (RL * RL) * 1.0 / + (phi[5 * 10 + 0] * phi[5 * 10 + 0]) * + (phi[5 * 10 + 2] * phi[5 * 10 + 2])) + + (phi[5 * 10 + 3] * phi[5 * 10 + 3]) / + phi[5 * 10 + 0] + + ((RL * RL) * (phi[5 * 10 + 2] * phi[5 * 10 + 2])) / + phi[5 * 10 + 0]) - + phi[5 * 10 + 9] * cos(G[0 * 3 + 2] * q[0 * 3 + 2]) + + (RL * RL) * l[4 * 3 + 1] * phi[5 * 10 + 2] * + sin(G[0 * 3 + 2] * q[0 * 3 + 2])) + + l[1 * 3 + 0] * (l[1 * 3 + 0] * phi[2 * 10 + 0] + + phi[2 * 10 + 1] * cos(q[0 * 3 + 2]) + + phi[2 * 10 + 3] * sin(q[0 * 3 + 2])) + + cos(q[0 * 3 + 2]) * + (l[1 * 3 + 0] * phi[2 * 10 + 1] + + cos(q[0 * 3 + 2]) * + (phi[2 * 10 + 6] - + phi[2 * 10 + 0] * + (1.0 / (phi[2 * 10 + 0] * phi[2 * 10 + 0]) * + (phi[2 * 10 + 1] * phi[2 * 10 + 1]) + + (RL * RL) * 1.0 / + (phi[2 * 10 + 0] * phi[2 * 10 + 0]) * + (phi[2 * 10 + 2] * phi[2 * 10 + 2])) + + (phi[2 * 10 + 1] * phi[2 * 10 + 1]) / + phi[2 * 10 + 0] + + ((RL * RL) * (phi[2 * 10 + 2] * phi[2 * 10 + 2])) / + phi[2 * 10 + 0]) - + phi[2 * 10 + 9] * sin(q[0 * 3 + 2]) + + (RL * RL) * l[1 * 3 + 1] * phi[2 * 10 + 2] * + cos(q[0 * 3 + 2])) - + phi[1 * 10 + 0] * + (1.0 / (phi[1 * 10 + 0] * phi[1 * 10 + 0]) * + (phi[1 * 10 + 1] * phi[1 * 10 + 1]) + + (RL * RL) * 1.0 / (phi[1 * 10 + 0] * phi[1 * 10 + 0]) * + (phi[1 * 10 + 2] * phi[1 * 10 + 2])) + + sin(q[0 * 3 + 2]) * + (l[1 * 3 + 0] * phi[2 * 10 + 3] + + sin(q[0 * 3 + 2]) * + (phi[2 * 10 + 4] - + phi[2 * 10 + 0] * + (1.0 / (phi[2 * 10 + 0] * phi[2 * 10 + 0]) * + (phi[2 * 10 + 3] * phi[2 * 10 + 3]) + + (RL * RL) * 1.0 / + (phi[2 * 10 + 0] * phi[2 * 10 + 0]) * + (phi[2 * 10 + 2] * phi[2 * 10 + 2])) + + (phi[2 * 10 + 3] * phi[2 * 10 + 3]) / + phi[2 * 10 + 0] + + ((RL * RL) * (phi[2 * 10 + 2] * phi[2 * 10 + 2])) / + phi[2 * 10 + 0]) - + phi[2 * 10 + 9] * cos(q[0 * 3 + 2]) + + (RL * RL) * l[1 * 3 + 1] * phi[2 * 10 + 2] * + sin(q[0 * 3 + 2])) + + (phi[1 * 10 + 1] * phi[1 * 10 + 1]) / phi[1 * 10 + 0] + + ((RL * RL) * (phi[1 * 10 + 2] * phi[1 * 10 + 2])) / + phi[1 * 10 + 0] + + RL * l[4 * 3 + 1] * cos(G[0 * 3 + 2] * q[0 * 3 + 2]) * + (RL * phi[5 * 10 + 2] * cos(G[0 * 3 + 2] * q[0 * 3 + 2]) + + RL * l[4 * 3 + 1] * phi[5 * 10 + 0] * + cos(G[0 * 3 + 2] * q[0 * 3 + 2])) + + RL * l[1 * 3 + 1] * cos(q[0 * 3 + 2]) * + (RL * phi[2 * 10 + 2] * cos(q[0 * 3 + 2]) + + RL * l[1 * 3 + 1] * phi[2 * 10 + 0] * + cos(q[0 * 3 + 2])) + + RL * l[4 * 3 + 1] * sin(G[0 * 3 + 2] * q[0 * 3 + 2]) * + (RL * phi[5 * 10 + 2] * sin(G[0 * 3 + 2] * q[0 * 3 + 2]) + + RL * l[4 * 3 + 1] * phi[5 * 10 + 0] * + sin(G[0 * 3 + 2] * q[0 * 3 + 2])) + + RL * l[1 * 3 + 1] * sin(q[0 * 3 + 2]) * + (RL * phi[2 * 10 + 2] * sin(q[0 * 3 + 2]) + + RL * l[1 * 3 + 1] * phi[2 * 10 + 0] * + sin(q[0 * 3 + 2]))) + + cos(q[0 * 3 + 1]) * + (-phi[1 * 10 + 9] + + l[4 * 3 + 0] * + (l[4 * 3 + 2] * phi[5 * 10 + 0] + + phi[5 * 10 + 3] * cos(G[0 * 3 + 2] * q[0 * 3 + 2]) - + phi[5 * 10 + 1] * sin(G[0 * 3 + 2] * q[0 * 3 + 2])) - + cos(G[0 * 3 + 2] * q[0 * 3 + 2]) * + (-l[4 * 3 + 2] * phi[5 * 10 + 1] + + sin(G[0 * 3 + 2] * q[0 * 3 + 2]) * + (phi[5 * 10 + 6] - + phi[5 * 10 + 0] * + (1.0 / (phi[5 * 10 + 0] * phi[5 * 10 + 0]) * + (phi[5 * 10 + 1] * phi[5 * 10 + 1]) + + (RL * RL) * 1.0 / + (phi[5 * 10 + 0] * phi[5 * 10 + 0]) * + (phi[5 * 10 + 2] * phi[5 * 10 + 2])) + + (phi[5 * 10 + 1] * phi[5 * 10 + 1]) / + phi[5 * 10 + 0] + + ((RL * RL) * (phi[5 * 10 + 2] * phi[5 * 10 + 2])) / + phi[5 * 10 + 0]) + + phi[5 * 10 + 9] * cos(G[0 * 3 + 2] * q[0 * 3 + 2]) + + (RL * RL) * l[4 * 3 + 1] * phi[5 * 10 + 2] * + sin(G[0 * 3 + 2] * q[0 * 3 + 2])) + + sin(G[0 * 3 + 2] * q[0 * 3 + 2]) * + (cos(G[0 * 3 + 2] * q[0 * 3 + 2]) * + (phi[5 * 10 + 4] - + phi[5 * 10 + 0] * + (1.0 / (phi[5 * 10 + 0] * phi[5 * 10 + 0]) * + (phi[5 * 10 + 3] * phi[5 * 10 + 3]) + + (RL * RL) * 1.0 / + (phi[5 * 10 + 0] * phi[5 * 10 + 0]) * + (phi[5 * 10 + 2] * phi[5 * 10 + 2])) + + (phi[5 * 10 + 3] * phi[5 * 10 + 3]) / + phi[5 * 10 + 0] + + ((RL * RL) * (phi[5 * 10 + 2] * phi[5 * 10 + 2])) / + phi[5 * 10 + 0]) + + l[4 * 3 + 2] * phi[5 * 10 + 3] + + phi[5 * 10 + 9] * sin(G[0 * 3 + 2] * q[0 * 3 + 2]) + + (RL * RL) * l[4 * 3 + 1] * phi[5 * 10 + 2] * + cos(G[0 * 3 + 2] * q[0 * 3 + 2])) + + l[1 * 3 + 0] * (l[1 * 3 + 2] * phi[2 * 10 + 0] + + phi[2 * 10 + 3] * cos(q[0 * 3 + 2]) - + phi[2 * 10 + 1] * sin(q[0 * 3 + 2])) - + cos(q[0 * 3 + 2]) * + (-l[1 * 3 + 2] * phi[2 * 10 + 1] + + sin(q[0 * 3 + 2]) * + (phi[2 * 10 + 6] - + phi[2 * 10 + 0] * + (1.0 / (phi[2 * 10 + 0] * phi[2 * 10 + 0]) * + (phi[2 * 10 + 1] * phi[2 * 10 + 1]) + + (RL * RL) * 1.0 / + (phi[2 * 10 + 0] * phi[2 * 10 + 0]) * + (phi[2 * 10 + 2] * phi[2 * 10 + 2])) + + (phi[2 * 10 + 1] * phi[2 * 10 + 1]) / + phi[2 * 10 + 0] + + ((RL * RL) * (phi[2 * 10 + 2] * phi[2 * 10 + 2])) / + phi[2 * 10 + 0]) + + phi[2 * 10 + 9] * cos(q[0 * 3 + 2]) + + (RL * RL) * l[1 * 3 + 1] * phi[2 * 10 + 2] * + sin(q[0 * 3 + 2])) + + sin(q[0 * 3 + 2]) * + (l[1 * 3 + 2] * phi[2 * 10 + 3] + + cos(q[0 * 3 + 2]) * + (phi[2 * 10 + 4] - + phi[2 * 10 + 0] * + (1.0 / (phi[2 * 10 + 0] * phi[2 * 10 + 0]) * + (phi[2 * 10 + 3] * phi[2 * 10 + 3]) + + (RL * RL) * 1.0 / + (phi[2 * 10 + 0] * phi[2 * 10 + 0]) * + (phi[2 * 10 + 2] * phi[2 * 10 + 2])) + + (phi[2 * 10 + 3] * phi[2 * 10 + 3]) / + phi[2 * 10 + 0] + + ((RL * RL) * (phi[2 * 10 + 2] * phi[2 * 10 + 2])) / + phi[2 * 10 + 0]) + + phi[2 * 10 + 9] * sin(q[0 * 3 + 2]) + + (RL * RL) * l[1 * 3 + 1] * phi[2 * 10 + 2] * + cos(q[0 * 3 + 2])) + + RL * l[4 * 3 + 1] * sin(G[0 * 3 + 2] * q[0 * 3 + 2]) * + (RL * phi[5 * 10 + 2] * cos(G[0 * 3 + 2] * q[0 * 3 + 2]) + + RL * l[4 * 3 + 1] * phi[5 * 10 + 0] * + cos(G[0 * 3 + 2] * q[0 * 3 + 2])) + + RL * l[1 * 3 + 1] * sin(q[0 * 3 + 2]) * + (RL * phi[2 * 10 + 2] * cos(q[0 * 3 + 2]) + + RL * l[1 * 3 + 1] * phi[2 * 10 + 0] * + cos(q[0 * 3 + 2])) - + RL * l[4 * 3 + 1] * cos(G[0 * 3 + 2] * q[0 * 3 + 2]) * + (RL * phi[5 * 10 + 2] * sin(G[0 * 3 + 2] * q[0 * 3 + 2]) + + RL * l[4 * 3 + 1] * phi[5 * 10 + 0] * + sin(G[0 * 3 + 2] * q[0 * 3 + 2])) - + RL * l[1 * 3 + 1] * cos(q[0 * 3 + 2]) * + (RL * phi[2 * 10 + 2] * sin(q[0 * 3 + 2]) + + RL * l[1 * 3 + 1] * phi[2 * 10 + 0] * + sin(q[0 * 3 + 2]))) - + RL * l[0 * 3 + 1] * sin(q[0 * 3 + 1]) * + (RL * phi[1 * 10 + 2] + + RL * phi[5 * 10 + 2] * + pow(sin(G[0 * 3 + 2] * q[0 * 3 + 2]), 2.0) + + RL * phi[2 * 10 + 2] * pow(cos(q[0 * 3 + 2]), 2.0) + + RL * phi[2 * 10 + 2] * pow(sin(q[0 * 3 + 2]), 2.0) + + RL * phi[5 * 10 + 2] * + pow(cos(G[0 * 3 + 2] * q[0 * 3 + 2]), 2.0) + + RL * l[4 * 3 + 1] * phi[5 * 10 + 0] * + pow(cos(G[0 * 3 + 2] * q[0 * 3 + 2]), 2.0) + + RL * l[4 * 3 + 1] * phi[5 * 10 + 0] * + pow(sin(G[0 * 3 + 2] * q[0 * 3 + 2]), 2.0) + + RL * l[1 * 3 + 1] * phi[2 * 10 + 0] * + pow(cos(q[0 * 3 + 2]), 2.0) + + RL * l[1 * 3 + 1] * phi[2 * 10 + 0] * + pow(sin(q[0 * 3 + 2]), 2.0))) + + l[0 * 3 + 2] * + (cos(q[0 * 3 + 1]) * + (phi[1 * 10 + 3] + l[1 * 3 + 2] * phi[2 * 10 + 0] + + l[4 * 3 + 2] * phi[5 * 10 + 0] + + phi[5 * 10 + 3] * cos(G[0 * 3 + 2] * q[0 * 3 + 2]) - + phi[5 * 10 + 1] * sin(G[0 * 3 + 2] * q[0 * 3 + 2]) + + phi[2 * 10 + 3] * cos(q[0 * 3 + 2]) - + phi[2 * 10 + 1] * sin(q[0 * 3 + 2])) + + l[0 * 3 + 2] * + (phi[1 * 10 + 0] + phi[2 * 10 + 0] + phi[5 * 10 + 0]) - + sin(q[0 * 3 + 1]) * + (phi[1 * 10 + 1] + l[1 * 3 + 0] * phi[2 * 10 + 0] + + l[4 * 3 + 0] * phi[5 * 10 + 0] + + phi[5 * 10 + 1] * cos(G[0 * 3 + 2] * q[0 * 3 + 2]) + + phi[5 * 10 + 3] * sin(G[0 * 3 + 2] * q[0 * 3 + 2]) + + phi[2 * 10 + 1] * cos(q[0 * 3 + 2]) + + phi[2 * 10 + 3] * sin(q[0 * 3 + 2]))) - + phi[0 * 10 + 0] * + (1.0 / (phi[0 * 10 + 0] * phi[0 * 10 + 0]) * + (phi[0 * 10 + 3] * phi[0 * 10 + 3]) + + (RL * RL) * 1.0 / (phi[0 * 10 + 0] * phi[0 * 10 + 0]) * + (phi[0 * 10 + 2] * phi[0 * 10 + 2])) + + (phi[0 * 10 + 3] * phi[0 * 10 + 3]) / phi[0 * 10 + 0] + + ((RL * RL) * (phi[0 * 10 + 2] * phi[0 * 10 + 2])) / phi[0 * 10 + 0] + + RL * l[3 * 3 + 1] * cos(G[0 * 3 + 1] * q[0 * 3 + 1]) * + (RL * phi[4 * 10 + 2] * cos(G[0 * 3 + 1] * q[0 * 3 + 1]) + + RL * l[3 * 3 + 1] * phi[4 * 10 + 0] * + cos(G[0 * 3 + 1] * q[0 * 3 + 1])) + + RL * l[3 * 3 + 1] * sin(G[0 * 3 + 1] * q[0 * 3 + 1]) * + (RL * phi[4 * 10 + 2] * sin(G[0 * 3 + 1] * q[0 * 3 + 1]) + + RL * l[3 * 3 + 1] * phi[4 * 10 + 0] * + sin(G[0 * 3 + 1] * q[0 * 3 + 1])) + + RL * l[0 * 3 + 1] * cos(q[0 * 3 + 1]) * + (-sin(q[0 * 3 + 1]) * + (cos(q[0 * 3 + 2]) * + (RL * phi[2 * 10 + 2] * sin(q[0 * 3 + 2]) + + RL * l[1 * 3 + 1] * phi[2 * 10 + 0] * + sin(q[0 * 3 + 2])) - + sin(G[0 * 3 + 2] * q[0 * 3 + 2]) * + (RL * phi[5 * 10 + 2] * cos(G[0 * 3 + 2] * q[0 * 3 + 2]) + + RL * l[4 * 3 + 1] * phi[5 * 10 + 0] * + cos(G[0 * 3 + 2] * q[0 * 3 + 2])) - + sin(q[0 * 3 + 2]) * + (RL * phi[2 * 10 + 2] * cos(q[0 * 3 + 2]) + + RL * l[1 * 3 + 1] * phi[2 * 10 + 0] * + cos(q[0 * 3 + 2])) + + cos(G[0 * 3 + 2] * q[0 * 3 + 2]) * + (RL * phi[5 * 10 + 2] * sin(G[0 * 3 + 2] * q[0 * 3 + 2]) + + RL * l[4 * 3 + 1] * phi[5 * 10 + 0] * + sin(G[0 * 3 + 2] * q[0 * 3 + 2]))) + + cos(q[0 * 3 + 1]) * + (sin(G[0 * 3 + 2] * q[0 * 3 + 2]) * + (RL * phi[5 * 10 + 2] * sin(G[0 * 3 + 2] * q[0 * 3 + 2]) + + RL * l[4 * 3 + 1] * phi[5 * 10 + 0] * + sin(G[0 * 3 + 2] * q[0 * 3 + 2])) + + sin(q[0 * 3 + 2]) * + (RL * phi[2 * 10 + 2] * sin(q[0 * 3 + 2]) + + RL * l[1 * 3 + 1] * phi[2 * 10 + 0] * + sin(q[0 * 3 + 2])) + + RL * phi[1 * 10 + 2] + + cos(G[0 * 3 + 2] * q[0 * 3 + 2]) * + (RL * phi[5 * 10 + 2] * cos(G[0 * 3 + 2] * q[0 * 3 + 2]) + + RL * l[4 * 3 + 1] * phi[5 * 10 + 0] * + cos(G[0 * 3 + 2] * q[0 * 3 + 2])) + + cos(q[0 * 3 + 2]) * + (RL * phi[2 * 10 + 2] * cos(q[0 * 3 + 2]) + + RL * l[1 * 3 + 1] * phi[2 * 10 + 0] * + cos(q[0 * 3 + 2]))) + + RL * l[0 * 3 + 1] * cos(q[0 * 3 + 1]) * + (phi[1 * 10 + 0] + + phi[2 * 10 + 0] * pow(cos(q[0 * 3 + 2]), 2.0) + + phi[2 * 10 + 0] * pow(sin(q[0 * 3 + 2]), 2.0) + + phi[5 * 10 + 0] * pow(cos(G[0 * 3 + 2] * q[0 * 3 + 2]), 2.0) + + phi[5 * 10 + 0] * + pow(sin(G[0 * 3 + 2] * q[0 * 3 + 2]), 2.0))) + + RL * l[0 * 3 + 1] * sin(q[0 * 3 + 1]) * + (cos(q[0 * 3 + 1]) * + (cos(q[0 * 3 + 2]) * + (RL * phi[2 * 10 + 2] * sin(q[0 * 3 + 2]) + + RL * l[1 * 3 + 1] * phi[2 * 10 + 0] * + sin(q[0 * 3 + 2])) - + sin(G[0 * 3 + 2] * q[0 * 3 + 2]) * + (RL * phi[5 * 10 + 2] * cos(G[0 * 3 + 2] * q[0 * 3 + 2]) + + RL * l[4 * 3 + 1] * phi[5 * 10 + 0] * + cos(G[0 * 3 + 2] * q[0 * 3 + 2])) - + sin(q[0 * 3 + 2]) * + (RL * phi[2 * 10 + 2] * cos(q[0 * 3 + 2]) + + RL * l[1 * 3 + 1] * phi[2 * 10 + 0] * + cos(q[0 * 3 + 2])) + + cos(G[0 * 3 + 2] * q[0 * 3 + 2]) * + (RL * phi[5 * 10 + 2] * sin(G[0 * 3 + 2] * q[0 * 3 + 2]) + + RL * l[4 * 3 + 1] * phi[5 * 10 + 0] * + sin(G[0 * 3 + 2] * q[0 * 3 + 2]))) + + sin(q[0 * 3 + 1]) * + (sin(G[0 * 3 + 2] * q[0 * 3 + 2]) * + (RL * phi[5 * 10 + 2] * sin(G[0 * 3 + 2] * q[0 * 3 + 2]) + + RL * l[4 * 3 + 1] * phi[5 * 10 + 0] * + sin(G[0 * 3 + 2] * q[0 * 3 + 2])) + + sin(q[0 * 3 + 2]) * + (RL * phi[2 * 10 + 2] * sin(q[0 * 3 + 2]) + + RL * l[1 * 3 + 1] * phi[2 * 10 + 0] * + sin(q[0 * 3 + 2])) + + RL * phi[1 * 10 + 2] + + cos(G[0 * 3 + 2] * q[0 * 3 + 2]) * + (RL * phi[5 * 10 + 2] * cos(G[0 * 3 + 2] * q[0 * 3 + 2]) + + RL * l[4 * 3 + 1] * phi[5 * 10 + 0] * + cos(G[0 * 3 + 2] * q[0 * 3 + 2])) + + cos(q[0 * 3 + 2]) * + (RL * phi[2 * 10 + 2] * cos(q[0 * 3 + 2]) + + RL * l[1 * 3 + 1] * phi[2 * 10 + 0] * + cos(q[0 * 3 + 2]))) + + RL * l[0 * 3 + 1] * sin(q[0 * 3 + 1]) * + (phi[1 * 10 + 0] + + phi[2 * 10 + 0] * pow(cos(q[0 * 3 + 2]), 2.0) + + phi[2 * 10 + 0] * pow(sin(q[0 * 3 + 2]), 2.0) + + phi[5 * 10 + 0] * pow(cos(G[0 * 3 + 2] * q[0 * 3 + 2]), 2.0) + + phi[5 * 10 + 0] * + pow(sin(G[0 * 3 + 2] * q[0 * 3 + 2]), 2.0))); + F(1 * 3 + 0) = + G[0 * 3 + 0] * + (RL * phi[4 * 10 + 7] * cos(G[0 * 3 + 1] * q[0 * 3 + 1]) + + RL * phi[4 * 10 + 8] * sin(G[0 * 3 + 1] * q[0 * 3 + 1]) - + RL * l[3 * 3 + 1] * phi[4 * 10 + 1] * + cos(G[0 * 3 + 1] * q[0 * 3 + 1]) - + RL * l[3 * 3 + 1] * phi[4 * 10 + 3] * + sin(G[0 * 3 + 1] * q[0 * 3 + 1])) - + cos(q[0 * 3 + 1]) * + (-RL * phi[1 * 10 + 7] + + (RL * phi[5 * 10 + 2] * cos(G[0 * 3 + 2] * q[0 * 3 + 2]) + + RL * l[4 * 3 + 1] * phi[5 * 10 + 0] * + cos(G[0 * 3 + 2] * q[0 * 3 + 2])) * + (l[4 * 3 + 0] * cos(G[0 * 3 + 2] * q[0 * 3 + 2]) - + l[4 * 3 + 2] * sin(G[0 * 3 + 2] * q[0 * 3 + 2])) + + (RL * phi[5 * 10 + 2] * sin(G[0 * 3 + 2] * q[0 * 3 + 2]) + + RL * l[4 * 3 + 1] * phi[5 * 10 + 0] * + sin(G[0 * 3 + 2] * q[0 * 3 + 2])) * + (l[4 * 3 + 2] * cos(G[0 * 3 + 2] * q[0 * 3 + 2]) + + l[4 * 3 + 0] * sin(G[0 * 3 + 2] * q[0 * 3 + 2])) + + (l[1 * 3 + 0] * cos(q[0 * 3 + 2]) - + l[1 * 3 + 2] * sin(q[0 * 3 + 2])) * + (RL * phi[2 * 10 + 2] * cos(q[0 * 3 + 2]) + + RL * l[1 * 3 + 1] * phi[2 * 10 + 0] * cos(q[0 * 3 + 2])) + + (l[1 * 3 + 2] * cos(q[0 * 3 + 2]) + + l[1 * 3 + 0] * sin(q[0 * 3 + 2])) * + (RL * phi[2 * 10 + 2] * sin(q[0 * 3 + 2]) + + RL * l[1 * 3 + 1] * phi[2 * 10 + 0] * sin(q[0 * 3 + 2])) - + RL * phi[2 * 10 + 8] * sin(q[0 * 3 + 2]) - + RL * phi[5 * 10 + 7] * cos(G[0 * 3 + 2] * q[0 * 3 + 2]) - + RL * phi[5 * 10 + 8] * sin(G[0 * 3 + 2] * q[0 * 3 + 2]) - + RL * phi[2 * 10 + 7] * cos(q[0 * 3 + 2]) + + RL * l[4 * 3 + 1] * phi[5 * 10 + 1] * + cos(G[0 * 3 + 2] * q[0 * 3 + 2]) + + RL * l[4 * 3 + 1] * phi[5 * 10 + 3] * + sin(G[0 * 3 + 2] * q[0 * 3 + 2]) + + RL * l[1 * 3 + 1] * phi[2 * 10 + 1] * cos(q[0 * 3 + 2]) + + RL * l[1 * 3 + 1] * phi[2 * 10 + 3] * sin(q[0 * 3 + 2])) + + sin(q[0 * 3 + 1]) * + (RL * phi[1 * 10 + 8] - + (RL * phi[5 * 10 + 2] * cos(G[0 * 3 + 2] * q[0 * 3 + 2]) + + RL * l[4 * 3 + 1] * phi[5 * 10 + 0] * + cos(G[0 * 3 + 2] * q[0 * 3 + 2])) * + (l[4 * 3 + 2] * cos(G[0 * 3 + 2] * q[0 * 3 + 2]) + + l[4 * 3 + 0] * sin(G[0 * 3 + 2] * q[0 * 3 + 2])) + + (RL * phi[5 * 10 + 2] * sin(G[0 * 3 + 2] * q[0 * 3 + 2]) + + RL * l[4 * 3 + 1] * phi[5 * 10 + 0] * + sin(G[0 * 3 + 2] * q[0 * 3 + 2])) * + (l[4 * 3 + 0] * cos(G[0 * 3 + 2] * q[0 * 3 + 2]) - + l[4 * 3 + 2] * sin(G[0 * 3 + 2] * q[0 * 3 + 2])) - + (l[1 * 3 + 2] * cos(q[0 * 3 + 2]) + + l[1 * 3 + 0] * sin(q[0 * 3 + 2])) * + (RL * phi[2 * 10 + 2] * cos(q[0 * 3 + 2]) + + RL * l[1 * 3 + 1] * phi[2 * 10 + 0] * cos(q[0 * 3 + 2])) + + (l[1 * 3 + 0] * cos(q[0 * 3 + 2]) - + l[1 * 3 + 2] * sin(q[0 * 3 + 2])) * + (RL * phi[2 * 10 + 2] * sin(q[0 * 3 + 2]) + + RL * l[1 * 3 + 1] * phi[2 * 10 + 0] * sin(q[0 * 3 + 2])) - + RL * phi[2 * 10 + 7] * sin(q[0 * 3 + 2]) + + RL * phi[5 * 10 + 8] * cos(G[0 * 3 + 2] * q[0 * 3 + 2]) - + RL * phi[5 * 10 + 7] * sin(G[0 * 3 + 2] * q[0 * 3 + 2]) + + RL * phi[2 * 10 + 8] * cos(q[0 * 3 + 2]) - + RL * l[4 * 3 + 1] * phi[5 * 10 + 3] * + cos(G[0 * 3 + 2] * q[0 * 3 + 2]) + + RL * l[4 * 3 + 1] * phi[5 * 10 + 1] * + sin(G[0 * 3 + 2] * q[0 * 3 + 2]) - + RL * l[1 * 3 + 1] * phi[2 * 10 + 3] * cos(q[0 * 3 + 2]) + + RL * l[1 * 3 + 1] * phi[2 * 10 + 1] * sin(q[0 * 3 + 2])) - + RL * l[0 * 3 + 1] * cos(q[0 * 3 + 1]) * + (phi[1 * 10 + 1] + + phi[5 * 10 + 1] * cos(G[0 * 3 + 2] * q[0 * 3 + 2]) + + phi[5 * 10 + 3] * sin(G[0 * 3 + 2] * q[0 * 3 + 2]) + + phi[2 * 10 + 1] * cos(q[0 * 3 + 2]) + + phi[2 * 10 + 3] * sin(q[0 * 3 + 2]) + + phi[5 * 10 + 0] * cos(G[0 * 3 + 2] * q[0 * 3 + 2]) * + (l[4 * 3 + 0] * cos(G[0 * 3 + 2] * q[0 * 3 + 2]) - + l[4 * 3 + 2] * sin(G[0 * 3 + 2] * q[0 * 3 + 2])) + + phi[2 * 10 + 0] * cos(q[0 * 3 + 2]) * + (l[1 * 3 + 0] * cos(q[0 * 3 + 2]) - + l[1 * 3 + 2] * sin(q[0 * 3 + 2])) + + phi[5 * 10 + 0] * sin(G[0 * 3 + 2] * q[0 * 3 + 2]) * + (l[4 * 3 + 2] * cos(G[0 * 3 + 2] * q[0 * 3 + 2]) + + l[4 * 3 + 0] * sin(G[0 * 3 + 2] * q[0 * 3 + 2])) + + phi[2 * 10 + 0] * sin(q[0 * 3 + 2]) * + (l[1 * 3 + 2] * cos(q[0 * 3 + 2]) + + l[1 * 3 + 0] * sin(q[0 * 3 + 2]))) - + RL * l[0 * 3 + 1] * sin(q[0 * 3 + 1]) * + (phi[1 * 10 + 3] + + phi[5 * 10 + 3] * cos(G[0 * 3 + 2] * q[0 * 3 + 2]) - + phi[5 * 10 + 1] * sin(G[0 * 3 + 2] * q[0 * 3 + 2]) + + phi[2 * 10 + 3] * cos(q[0 * 3 + 2]) - + phi[2 * 10 + 1] * sin(q[0 * 3 + 2]) + + phi[5 * 10 + 0] * cos(G[0 * 3 + 2] * q[0 * 3 + 2]) * + (l[4 * 3 + 2] * cos(G[0 * 3 + 2] * q[0 * 3 + 2]) + + l[4 * 3 + 0] * sin(G[0 * 3 + 2] * q[0 * 3 + 2])) + + phi[2 * 10 + 0] * cos(q[0 * 3 + 2]) * + (l[1 * 3 + 2] * cos(q[0 * 3 + 2]) + + l[1 * 3 + 0] * sin(q[0 * 3 + 2])) - + phi[5 * 10 + 0] * sin(G[0 * 3 + 2] * q[0 * 3 + 2]) * + (l[4 * 3 + 0] * cos(G[0 * 3 + 2] * q[0 * 3 + 2]) - + l[4 * 3 + 2] * sin(G[0 * 3 + 2] * q[0 * 3 + 2])) - + phi[2 * 10 + 0] * sin(q[0 * 3 + 2]) * + (l[1 * 3 + 0] * cos(q[0 * 3 + 2]) - + l[1 * 3 + 2] * sin(q[0 * 3 + 2]))); + F(2 * 3 + 0) = + G[0 * 3 + 0] * + (sin(q[0 * 3 + 1]) * + (RL * phi[5 * 10 + 8] * cos(G[0 * 3 + 2] * q[0 * 3 + 2]) - + RL * phi[5 * 10 + 7] * sin(G[0 * 3 + 2] * q[0 * 3 + 2]) - + RL * l[4 * 3 + 1] * phi[5 * 10 + 3] * + cos(G[0 * 3 + 2] * q[0 * 3 + 2]) + + RL * l[4 * 3 + 1] * phi[5 * 10 + 1] * + sin(G[0 * 3 + 2] * q[0 * 3 + 2])) + + cos(q[0 * 3 + 1]) * + (RL * phi[5 * 10 + 7] * cos(G[0 * 3 + 2] * q[0 * 3 + 2]) + + RL * phi[5 * 10 + 8] * sin(G[0 * 3 + 2] * q[0 * 3 + 2]) - + RL * l[4 * 3 + 1] * phi[5 * 10 + 1] * + cos(G[0 * 3 + 2] * q[0 * 3 + 2]) - + RL * l[4 * 3 + 1] * phi[5 * 10 + 3] * + sin(G[0 * 3 + 2] * q[0 * 3 + 2])) - + RL * l[0 * 3 + 1] * sin(q[0 * 3 + 1]) * + (phi[5 * 10 + 3] * cos(G[0 * 3 + 2] * q[0 * 3 + 2]) - + phi[5 * 10 + 1] * sin(G[0 * 3 + 2] * q[0 * 3 + 2])) - + RL * l[0 * 3 + 1] * cos(q[0 * 3 + 1]) * + (phi[5 * 10 + 1] * cos(G[0 * 3 + 2] * q[0 * 3 + 2]) + + phi[5 * 10 + 3] * sin(G[0 * 3 + 2] * q[0 * 3 + 2]))) + + cos(q[0 * 3 + 1]) * + (RL * phi[2 * 10 + 8] * sin(q[0 * 3 + 2]) + + RL * phi[2 * 10 + 7] * cos(q[0 * 3 + 2]) - + RL * l[1 * 3 + 1] * phi[2 * 10 + 1] * cos(q[0 * 3 + 2]) - + RL * l[1 * 3 + 1] * phi[2 * 10 + 3] * sin(q[0 * 3 + 2])) - + sin(q[0 * 3 + 1]) * + (RL * phi[2 * 10 + 7] * sin(q[0 * 3 + 2]) - + RL * phi[2 * 10 + 8] * cos(q[0 * 3 + 2]) + + RL * l[1 * 3 + 1] * phi[2 * 10 + 3] * cos(q[0 * 3 + 2]) - + RL * l[1 * 3 + 1] * phi[2 * 10 + 1] * sin(q[0 * 3 + 2])) - + RL * l[0 * 3 + 1] * cos(q[0 * 3 + 1]) * + (phi[2 * 10 + 1] * cos(q[0 * 3 + 2]) + + phi[2 * 10 + 3] * sin(q[0 * 3 + 2])) - + RL * l[0 * 3 + 1] * sin(q[0 * 3 + 1]) * + (phi[2 * 10 + 3] * cos(q[0 * 3 + 2]) - + phi[2 * 10 + 1] * sin(q[0 * 3 + 2])); + F(0 * 3 + 1) = + G[0 * 3 + 1] * + (RL * phi[4 * 10 + 7] * cos(G[0 * 3 + 1] * q[0 * 3 + 1]) + + RL * phi[4 * 10 + 8] * sin(G[0 * 3 + 1] * q[0 * 3 + 1]) - + RL * l[3 * 3 + 1] * phi[4 * 10 + 1] * + cos(G[0 * 3 + 1] * q[0 * 3 + 1]) - + RL * l[3 * 3 + 1] * phi[4 * 10 + 3] * + sin(G[0 * 3 + 1] * q[0 * 3 + 1])) - + cos(q[0 * 3 + 1]) * + (-RL * phi[1 * 10 + 7] + + (RL * phi[5 * 10 + 2] * cos(G[0 * 3 + 2] * q[0 * 3 + 2]) + + RL * l[4 * 3 + 1] * phi[5 * 10 + 0] * + cos(G[0 * 3 + 2] * q[0 * 3 + 2])) * + (l[4 * 3 + 0] * cos(G[0 * 3 + 2] * q[0 * 3 + 2]) - + l[4 * 3 + 2] * sin(G[0 * 3 + 2] * q[0 * 3 + 2])) + + (RL * phi[5 * 10 + 2] * sin(G[0 * 3 + 2] * q[0 * 3 + 2]) + + RL * l[4 * 3 + 1] * phi[5 * 10 + 0] * + sin(G[0 * 3 + 2] * q[0 * 3 + 2])) * + (l[4 * 3 + 2] * cos(G[0 * 3 + 2] * q[0 * 3 + 2]) + + l[4 * 3 + 0] * sin(G[0 * 3 + 2] * q[0 * 3 + 2])) + + (l[1 * 3 + 0] * cos(q[0 * 3 + 2]) - + l[1 * 3 + 2] * sin(q[0 * 3 + 2])) * + (RL * phi[2 * 10 + 2] * cos(q[0 * 3 + 2]) + + RL * l[1 * 3 + 1] * phi[2 * 10 + 0] * cos(q[0 * 3 + 2])) + + (l[1 * 3 + 2] * cos(q[0 * 3 + 2]) + + l[1 * 3 + 0] * sin(q[0 * 3 + 2])) * + (RL * phi[2 * 10 + 2] * sin(q[0 * 3 + 2]) + + RL * l[1 * 3 + 1] * phi[2 * 10 + 0] * sin(q[0 * 3 + 2])) - + RL * phi[2 * 10 + 8] * sin(q[0 * 3 + 2]) - + RL * phi[5 * 10 + 7] * cos(G[0 * 3 + 2] * q[0 * 3 + 2]) - + RL * phi[5 * 10 + 8] * sin(G[0 * 3 + 2] * q[0 * 3 + 2]) - + RL * phi[2 * 10 + 7] * cos(q[0 * 3 + 2]) + + RL * l[4 * 3 + 1] * phi[5 * 10 + 1] * + cos(G[0 * 3 + 2] * q[0 * 3 + 2]) + + RL * l[4 * 3 + 1] * phi[5 * 10 + 3] * + sin(G[0 * 3 + 2] * q[0 * 3 + 2]) + + RL * l[1 * 3 + 1] * phi[2 * 10 + 1] * cos(q[0 * 3 + 2]) + + RL * l[1 * 3 + 1] * phi[2 * 10 + 3] * sin(q[0 * 3 + 2])) + + sin(q[0 * 3 + 1]) * + (RL * phi[1 * 10 + 8] - + (RL * phi[5 * 10 + 2] * cos(G[0 * 3 + 2] * q[0 * 3 + 2]) + + RL * l[4 * 3 + 1] * phi[5 * 10 + 0] * + cos(G[0 * 3 + 2] * q[0 * 3 + 2])) * + (l[4 * 3 + 2] * cos(G[0 * 3 + 2] * q[0 * 3 + 2]) + + l[4 * 3 + 0] * sin(G[0 * 3 + 2] * q[0 * 3 + 2])) + + (RL * phi[5 * 10 + 2] * sin(G[0 * 3 + 2] * q[0 * 3 + 2]) + + RL * l[4 * 3 + 1] * phi[5 * 10 + 0] * + sin(G[0 * 3 + 2] * q[0 * 3 + 2])) * + (l[4 * 3 + 0] * cos(G[0 * 3 + 2] * q[0 * 3 + 2]) - + l[4 * 3 + 2] * sin(G[0 * 3 + 2] * q[0 * 3 + 2])) - + (l[1 * 3 + 2] * cos(q[0 * 3 + 2]) + + l[1 * 3 + 0] * sin(q[0 * 3 + 2])) * + (RL * phi[2 * 10 + 2] * cos(q[0 * 3 + 2]) + + RL * l[1 * 3 + 1] * phi[2 * 10 + 0] * cos(q[0 * 3 + 2])) + + (l[1 * 3 + 0] * cos(q[0 * 3 + 2]) - + l[1 * 3 + 2] * sin(q[0 * 3 + 2])) * + (RL * phi[2 * 10 + 2] * sin(q[0 * 3 + 2]) + + RL * l[1 * 3 + 1] * phi[2 * 10 + 0] * sin(q[0 * 3 + 2])) - + RL * phi[2 * 10 + 7] * sin(q[0 * 3 + 2]) + + RL * phi[5 * 10 + 8] * cos(G[0 * 3 + 2] * q[0 * 3 + 2]) - + RL * phi[5 * 10 + 7] * sin(G[0 * 3 + 2] * q[0 * 3 + 2]) + + RL * phi[2 * 10 + 8] * cos(q[0 * 3 + 2]) - + RL * l[4 * 3 + 1] * phi[5 * 10 + 3] * + cos(G[0 * 3 + 2] * q[0 * 3 + 2]) + + RL * l[4 * 3 + 1] * phi[5 * 10 + 1] * + sin(G[0 * 3 + 2] * q[0 * 3 + 2]) - + RL * l[1 * 3 + 1] * phi[2 * 10 + 3] * cos(q[0 * 3 + 2]) + + RL * l[1 * 3 + 1] * phi[2 * 10 + 1] * sin(q[0 * 3 + 2])) - + RL * l[0 * 3 + 1] * cos(q[0 * 3 + 1]) * + (phi[1 * 10 + 1] + + phi[5 * 10 + 1] * cos(G[0 * 3 + 2] * q[0 * 3 + 2]) + + phi[5 * 10 + 3] * sin(G[0 * 3 + 2] * q[0 * 3 + 2]) + + phi[2 * 10 + 1] * cos(q[0 * 3 + 2]) + + phi[2 * 10 + 3] * sin(q[0 * 3 + 2]) + + phi[5 * 10 + 0] * cos(G[0 * 3 + 2] * q[0 * 3 + 2]) * + (l[4 * 3 + 0] * cos(G[0 * 3 + 2] * q[0 * 3 + 2]) - + l[4 * 3 + 2] * sin(G[0 * 3 + 2] * q[0 * 3 + 2])) + + phi[2 * 10 + 0] * cos(q[0 * 3 + 2]) * + (l[1 * 3 + 0] * cos(q[0 * 3 + 2]) - + l[1 * 3 + 2] * sin(q[0 * 3 + 2])) + + phi[5 * 10 + 0] * sin(G[0 * 3 + 2] * q[0 * 3 + 2]) * + (l[4 * 3 + 2] * cos(G[0 * 3 + 2] * q[0 * 3 + 2]) + + l[4 * 3 + 0] * sin(G[0 * 3 + 2] * q[0 * 3 + 2])) + + phi[2 * 10 + 0] * sin(q[0 * 3 + 2]) * + (l[1 * 3 + 2] * cos(q[0 * 3 + 2]) + + l[1 * 3 + 0] * sin(q[0 * 3 + 2]))) - + RL * l[0 * 3 + 1] * sin(q[0 * 3 + 1]) * + (phi[1 * 10 + 3] + + phi[5 * 10 + 3] * cos(G[0 * 3 + 2] * q[0 * 3 + 2]) - + phi[5 * 10 + 1] * sin(G[0 * 3 + 2] * q[0 * 3 + 2]) + + phi[2 * 10 + 3] * cos(q[0 * 3 + 2]) - + phi[2 * 10 + 1] * sin(q[0 * 3 + 2]) + + phi[5 * 10 + 0] * cos(G[0 * 3 + 2] * q[0 * 3 + 2]) * + (l[4 * 3 + 2] * cos(G[0 * 3 + 2] * q[0 * 3 + 2]) + + l[4 * 3 + 0] * sin(G[0 * 3 + 2] * q[0 * 3 + 2])) + + phi[2 * 10 + 0] * cos(q[0 * 3 + 2]) * + (l[1 * 3 + 2] * cos(q[0 * 3 + 2]) + + l[1 * 3 + 0] * sin(q[0 * 3 + 2])) - + phi[5 * 10 + 0] * sin(G[0 * 3 + 2] * q[0 * 3 + 2]) * + (l[4 * 3 + 0] * cos(G[0 * 3 + 2] * q[0 * 3 + 2]) - + l[4 * 3 + 2] * sin(G[0 * 3 + 2] * q[0 * 3 + 2])) - + phi[2 * 10 + 0] * sin(q[0 * 3 + 2]) * + (l[1 * 3 + 0] * cos(q[0 * 3 + 2]) - + l[1 * 3 + 2] * sin(q[0 * 3 + 2]))); + F(1 * 3 + 1) = + phi[1 * 10 + 5] + phi[2 * 10 + 5] + phi[5 * 10 + 5] + + (G[0 * 3 + 1] * G[0 * 3 + 1]) * + (phi[4 * 10 + 5] - + phi[4 * 10 + 0] * (1.0 / (phi[4 * 10 + 0] * phi[4 * 10 + 0]) * + (phi[4 * 10 + 1] * phi[4 * 10 + 1]) + + 1.0 / (phi[4 * 10 + 0] * phi[4 * 10 + 0]) * + (phi[4 * 10 + 3] * phi[4 * 10 + 3])) + + (phi[4 * 10 + 1] * phi[4 * 10 + 1]) / phi[4 * 10 + 0] + + (phi[4 * 10 + 3] * phi[4 * 10 + 3]) / phi[4 * 10 + 0]) - + phi[1 * 10 + 0] * (1.0 / (phi[1 * 10 + 0] * phi[1 * 10 + 0]) * + (phi[1 * 10 + 1] * phi[1 * 10 + 1]) + + 1.0 / (phi[1 * 10 + 0] * phi[1 * 10 + 0]) * + (phi[1 * 10 + 3] * phi[1 * 10 + 3])) - + phi[2 * 10 + 0] * (1.0 / (phi[2 * 10 + 0] * phi[2 * 10 + 0]) * + (phi[2 * 10 + 1] * phi[2 * 10 + 1]) + + 1.0 / (phi[2 * 10 + 0] * phi[2 * 10 + 0]) * + (phi[2 * 10 + 3] * phi[2 * 10 + 3])) - + phi[5 * 10 + 0] * (1.0 / (phi[5 * 10 + 0] * phi[5 * 10 + 0]) * + (phi[5 * 10 + 1] * phi[5 * 10 + 1]) + + 1.0 / (phi[5 * 10 + 0] * phi[5 * 10 + 0]) * + (phi[5 * 10 + 3] * phi[5 * 10 + 3])) + + (phi[2 * 10 + 1] + + phi[2 * 10 + 0] * (l[1 * 3 + 0] * cos(q[0 * 3 + 2]) - + l[1 * 3 + 2] * sin(q[0 * 3 + 2]))) * + (l[1 * 3 + 0] * cos(q[0 * 3 + 2]) - + l[1 * 3 + 2] * sin(q[0 * 3 + 2])) + + (phi[2 * 10 + 3] + + phi[2 * 10 + 0] * (l[1 * 3 + 2] * cos(q[0 * 3 + 2]) + + l[1 * 3 + 0] * sin(q[0 * 3 + 2]))) * + (l[1 * 3 + 2] * cos(q[0 * 3 + 2]) + + l[1 * 3 + 0] * sin(q[0 * 3 + 2])) + + phi[2 * 10 + 1] * (l[1 * 3 + 0] * cos(q[0 * 3 + 2]) - + l[1 * 3 + 2] * sin(q[0 * 3 + 2])) + + phi[2 * 10 + 3] * (l[1 * 3 + 2] * cos(q[0 * 3 + 2]) + + l[1 * 3 + 0] * sin(q[0 * 3 + 2])) + + phi[5 * 10 + 1] * (l[4 * 3 + 0] * cos(G[0 * 3 + 2] * q[0 * 3 + 2]) - + l[4 * 3 + 2] * sin(G[0 * 3 + 2] * q[0 * 3 + 2])) + + phi[5 * 10 + 3] * (l[4 * 3 + 2] * cos(G[0 * 3 + 2] * q[0 * 3 + 2]) + + l[4 * 3 + 0] * sin(G[0 * 3 + 2] * q[0 * 3 + 2])) + + (phi[5 * 10 + 1] + + phi[5 * 10 + 0] * (l[4 * 3 + 0] * cos(G[0 * 3 + 2] * q[0 * 3 + 2]) - + l[4 * 3 + 2] * sin(G[0 * 3 + 2] * q[0 * 3 + 2]))) * + (l[4 * 3 + 0] * cos(G[0 * 3 + 2] * q[0 * 3 + 2]) - + l[4 * 3 + 2] * sin(G[0 * 3 + 2] * q[0 * 3 + 2])) + + (phi[5 * 10 + 3] + + phi[5 * 10 + 0] * (l[4 * 3 + 2] * cos(G[0 * 3 + 2] * q[0 * 3 + 2]) + + l[4 * 3 + 0] * sin(G[0 * 3 + 2] * q[0 * 3 + 2]))) * + (l[4 * 3 + 2] * cos(G[0 * 3 + 2] * q[0 * 3 + 2]) + + l[4 * 3 + 0] * sin(G[0 * 3 + 2] * q[0 * 3 + 2])) + + (phi[1 * 10 + 1] * phi[1 * 10 + 1]) / phi[1 * 10 + 0] + + (phi[2 * 10 + 1] * phi[2 * 10 + 1]) / phi[2 * 10 + 0] + + (phi[5 * 10 + 1] * phi[5 * 10 + 1]) / phi[5 * 10 + 0] + + (phi[1 * 10 + 3] * phi[1 * 10 + 3]) / phi[1 * 10 + 0] + + (phi[2 * 10 + 3] * phi[2 * 10 + 3]) / phi[2 * 10 + 0] + + (phi[5 * 10 + 3] * phi[5 * 10 + 3]) / phi[5 * 10 + 0]; + F(2 * 3 + 1) = + phi[2 * 10 + 5] - + phi[2 * 10 + 0] * (1.0 / (phi[2 * 10 + 0] * phi[2 * 10 + 0]) * + (phi[2 * 10 + 1] * phi[2 * 10 + 1]) + + 1.0 / (phi[2 * 10 + 0] * phi[2 * 10 + 0]) * + (phi[2 * 10 + 3] * phi[2 * 10 + 3])) + + G[0 * 3 + 1] * + (phi[5 * 10 + 5] - + phi[5 * 10 + 0] * (1.0 / (phi[5 * 10 + 0] * phi[5 * 10 + 0]) * + (phi[5 * 10 + 1] * phi[5 * 10 + 1]) + + 1.0 / (phi[5 * 10 + 0] * phi[5 * 10 + 0]) * + (phi[5 * 10 + 3] * phi[5 * 10 + 3])) + + phi[5 * 10 + 1] * + (l[4 * 3 + 0] * cos(G[0 * 3 + 2] * q[0 * 3 + 2]) - + l[4 * 3 + 2] * sin(G[0 * 3 + 2] * q[0 * 3 + 2])) + + phi[5 * 10 + 3] * + (l[4 * 3 + 2] * cos(G[0 * 3 + 2] * q[0 * 3 + 2]) + + l[4 * 3 + 0] * sin(G[0 * 3 + 2] * q[0 * 3 + 2])) + + (phi[5 * 10 + 1] * phi[5 * 10 + 1]) / phi[5 * 10 + 0] + + (phi[5 * 10 + 3] * phi[5 * 10 + 3]) / phi[5 * 10 + 0]) + + phi[2 * 10 + 1] * (l[1 * 3 + 0] * cos(q[0 * 3 + 2]) - + l[1 * 3 + 2] * sin(q[0 * 3 + 2])) + + phi[2 * 10 + 3] * (l[1 * 3 + 2] * cos(q[0 * 3 + 2]) + + l[1 * 3 + 0] * sin(q[0 * 3 + 2])) + + (phi[2 * 10 + 1] * phi[2 * 10 + 1]) / phi[2 * 10 + 0] + + (phi[2 * 10 + 3] * phi[2 * 10 + 3]) / phi[2 * 10 + 0]; + F(0 * 3 + 2) = + G[0 * 3 + 2] * + (sin(q[0 * 3 + 1]) * + (RL * phi[5 * 10 + 8] * cos(G[0 * 3 + 2] * q[0 * 3 + 2]) - + RL * phi[5 * 10 + 7] * sin(G[0 * 3 + 2] * q[0 * 3 + 2]) - + RL * l[4 * 3 + 1] * phi[5 * 10 + 3] * + cos(G[0 * 3 + 2] * q[0 * 3 + 2]) + + RL * l[4 * 3 + 1] * phi[5 * 10 + 1] * + sin(G[0 * 3 + 2] * q[0 * 3 + 2])) + + cos(q[0 * 3 + 1]) * + (RL * phi[5 * 10 + 7] * cos(G[0 * 3 + 2] * q[0 * 3 + 2]) + + RL * phi[5 * 10 + 8] * sin(G[0 * 3 + 2] * q[0 * 3 + 2]) - + RL * l[4 * 3 + 1] * phi[5 * 10 + 1] * + cos(G[0 * 3 + 2] * q[0 * 3 + 2]) - + RL * l[4 * 3 + 1] * phi[5 * 10 + 3] * + sin(G[0 * 3 + 2] * q[0 * 3 + 2])) - + RL * l[0 * 3 + 1] * sin(q[0 * 3 + 1]) * + (phi[5 * 10 + 3] * cos(G[0 * 3 + 2] * q[0 * 3 + 2]) - + phi[5 * 10 + 1] * sin(G[0 * 3 + 2] * q[0 * 3 + 2])) - + RL * l[0 * 3 + 1] * cos(q[0 * 3 + 1]) * + (phi[5 * 10 + 1] * cos(G[0 * 3 + 2] * q[0 * 3 + 2]) + + phi[5 * 10 + 3] * sin(G[0 * 3 + 2] * q[0 * 3 + 2]))) + + cos(q[0 * 3 + 1]) * + (RL * phi[2 * 10 + 8] * sin(q[0 * 3 + 2]) + + RL * phi[2 * 10 + 7] * cos(q[0 * 3 + 2]) - + RL * l[1 * 3 + 1] * phi[2 * 10 + 1] * cos(q[0 * 3 + 2]) - + RL * l[1 * 3 + 1] * phi[2 * 10 + 3] * sin(q[0 * 3 + 2])) - + sin(q[0 * 3 + 1]) * + (RL * phi[2 * 10 + 7] * sin(q[0 * 3 + 2]) - + RL * phi[2 * 10 + 8] * cos(q[0 * 3 + 2]) + + RL * l[1 * 3 + 1] * phi[2 * 10 + 3] * cos(q[0 * 3 + 2]) - + RL * l[1 * 3 + 1] * phi[2 * 10 + 1] * sin(q[0 * 3 + 2])) - + RL * l[0 * 3 + 1] * cos(q[0 * 3 + 1]) * + (phi[2 * 10 + 1] * cos(q[0 * 3 + 2]) + + phi[2 * 10 + 3] * sin(q[0 * 3 + 2])) - + RL * l[0 * 3 + 1] * sin(q[0 * 3 + 1]) * + (phi[2 * 10 + 3] * cos(q[0 * 3 + 2]) - + phi[2 * 10 + 1] * sin(q[0 * 3 + 2])); + F(1 * 3 + 2) = + phi[2 * 10 + 5] - + phi[2 * 10 + 0] * (1.0 / (phi[2 * 10 + 0] * phi[2 * 10 + 0]) * + (phi[2 * 10 + 1] * phi[2 * 10 + 1]) + + 1.0 / (phi[2 * 10 + 0] * phi[2 * 10 + 0]) * + (phi[2 * 10 + 3] * phi[2 * 10 + 3])) + + G[0 * 3 + 2] * + (phi[5 * 10 + 5] - + phi[5 * 10 + 0] * (1.0 / (phi[5 * 10 + 0] * phi[5 * 10 + 0]) * + (phi[5 * 10 + 1] * phi[5 * 10 + 1]) + + 1.0 / (phi[5 * 10 + 0] * phi[5 * 10 + 0]) * + (phi[5 * 10 + 3] * phi[5 * 10 + 3])) + + phi[5 * 10 + 1] * + (l[4 * 3 + 0] * cos(G[0 * 3 + 2] * q[0 * 3 + 2]) - + l[4 * 3 + 2] * sin(G[0 * 3 + 2] * q[0 * 3 + 2])) + + phi[5 * 10 + 3] * + (l[4 * 3 + 2] * cos(G[0 * 3 + 2] * q[0 * 3 + 2]) + + l[4 * 3 + 0] * sin(G[0 * 3 + 2] * q[0 * 3 + 2])) + + (phi[5 * 10 + 1] * phi[5 * 10 + 1]) / phi[5 * 10 + 0] + + (phi[5 * 10 + 3] * phi[5 * 10 + 3]) / phi[5 * 10 + 0]) + + phi[2 * 10 + 1] * (l[1 * 3 + 0] * cos(q[0 * 3 + 2]) - + l[1 * 3 + 2] * sin(q[0 * 3 + 2])) + + phi[2 * 10 + 3] * (l[1 * 3 + 2] * cos(q[0 * 3 + 2]) + + l[1 * 3 + 0] * sin(q[0 * 3 + 2])) + + (phi[2 * 10 + 1] * phi[2 * 10 + 1]) / phi[2 * 10 + 0] + + (phi[2 * 10 + 3] * phi[2 * 10 + 3]) / phi[2 * 10 + 0]; + F(2 * 3 + 2) = + phi[2 * 10 + 5] + + (G[0 * 3 + 2] * G[0 * 3 + 2]) * + (phi[5 * 10 + 5] - + phi[5 * 10 + 0] * (1.0 / (phi[5 * 10 + 0] * phi[5 * 10 + 0]) * + (phi[5 * 10 + 1] * phi[5 * 10 + 1]) + + 1.0 / (phi[5 * 10 + 0] * phi[5 * 10 + 0]) * + (phi[5 * 10 + 3] * phi[5 * 10 + 3])) + + (phi[5 * 10 + 1] * phi[5 * 10 + 1]) / phi[5 * 10 + 0] + + (phi[5 * 10 + 3] * phi[5 * 10 + 3]) / phi[5 * 10 + 0]) - + phi[2 * 10 + 0] * (1.0 / (phi[2 * 10 + 0] * phi[2 * 10 + 0]) * + (phi[2 * 10 + 1] * phi[2 * 10 + 1]) + + 1.0 / (phi[2 * 10 + 0] * phi[2 * 10 + 0]) * + (phi[2 * 10 + 3] * phi[2 * 10 + 3])) + + (phi[2 * 10 + 1] * phi[2 * 10 + 1]) / phi[2 * 10 + 0] + + (phi[2 * 10 + 3] * phi[2 * 10 + 3]) / phi[2 * 10 + 0]; +} + +void f_beta(Eigen::Vector3d q, Eigen::Vector3d qd, int RL, Eigen::Vector3d& F) { + F(0 * 3 + 0) = + -qd[0 * 3 + 1] * + (qd[0 * 3 + 0] * + ((l[0 * 3 + 2] * + (cos(q[0 * 3 + 1]) * + (phi[1 * 10 + 1] + l[1 * 3 + 0] * phi[2 * 10 + 0] + + l[4 * 3 + 0] * phi[5 * 10 + 0] + + phi[5 * 10 + 1] * cos(G[0 * 3 + 2] * q[0 * 3 + 2]) + + phi[5 * 10 + 3] * sin(G[0 * 3 + 2] * q[0 * 3 + 2]) + + phi[2 * 10 + 1] * cos(q[0 * 3 + 2]) + + phi[2 * 10 + 3] * sin(q[0 * 3 + 2])) + + sin(q[0 * 3 + 1]) * + (phi[1 * 10 + 3] + l[1 * 3 + 2] * phi[2 * 10 + 0] + + l[4 * 3 + 2] * phi[5 * 10 + 0] + + phi[5 * 10 + 3] * cos(G[0 * 3 + 2] * q[0 * 3 + 2]) - + phi[5 * 10 + 1] * sin(G[0 * 3 + 2] * q[0 * 3 + 2]) + + phi[2 * 10 + 3] * cos(q[0 * 3 + 2]) - + phi[2 * 10 + 1] * sin(q[0 * 3 + 2])))) / + 2.0 - + (sin(q[0 * 3 + 1]) * + (sin(q[0 * 3 + 1]) * + (-phi[1 * 10 + 9] + + l[4 * 3 + 0] * (l[4 * 3 + 2] * phi[5 * 10 + 0] + + phi[5 * 10 + 3] * + cos(G[0 * 3 + 2] * q[0 * 3 + 2]) - + phi[5 * 10 + 1] * + sin(G[0 * 3 + 2] * q[0 * 3 + 2])) - + cos(G[0 * 3 + 2] * q[0 * 3 + 2]) * + (-l[4 * 3 + 2] * phi[5 * 10 + 1] + + sin(G[0 * 3 + 2] * q[0 * 3 + 2]) * + (phi[5 * 10 + 6] - + phi[5 * 10 + 0] * + (1.0 / + (phi[5 * 10 + 0] * + phi[5 * 10 + 0]) * + (phi[5 * 10 + 1] * + phi[5 * 10 + 1]) + + (RL * RL) * 1.0 / + (phi[5 * 10 + 0] * + phi[5 * 10 + 0]) * + (phi[5 * 10 + 2] * + phi[5 * 10 + 2])) + + (phi[5 * 10 + 1] * phi[5 * 10 + 1]) / + phi[5 * 10 + 0] + + ((RL * RL) * + (phi[5 * 10 + 2] * phi[5 * 10 + 2])) / + phi[5 * 10 + 0]) + + phi[5 * 10 + 9] * + cos(G[0 * 3 + 2] * q[0 * 3 + 2]) + + (RL * RL) * l[4 * 3 + 1] * phi[5 * 10 + 2] * + sin(G[0 * 3 + 2] * q[0 * 3 + 2])) + + sin(G[0 * 3 + 2] * q[0 * 3 + 2]) * + (cos(G[0 * 3 + 2] * q[0 * 3 + 2]) * + (phi[5 * 10 + 4] - + phi[5 * 10 + 0] * + (1.0 / + (phi[5 * 10 + 0] * + phi[5 * 10 + 0]) * + (phi[5 * 10 + 3] * + phi[5 * 10 + 3]) + + (RL * RL) * 1.0 / + (phi[5 * 10 + 0] * + phi[5 * 10 + 0]) * + (phi[5 * 10 + 2] * + phi[5 * 10 + 2])) + + (phi[5 * 10 + 3] * phi[5 * 10 + 3]) / + phi[5 * 10 + 0] + + ((RL * RL) * + (phi[5 * 10 + 2] * phi[5 * 10 + 2])) / + phi[5 * 10 + 0]) + + l[4 * 3 + 2] * phi[5 * 10 + 3] + + phi[5 * 10 + 9] * + sin(G[0 * 3 + 2] * q[0 * 3 + 2]) + + (RL * RL) * l[4 * 3 + 1] * phi[5 * 10 + 2] * + cos(G[0 * 3 + 2] * q[0 * 3 + 2])) + + l[1 * 3 + 0] * (l[1 * 3 + 2] * phi[2 * 10 + 0] + + phi[2 * 10 + 3] * cos(q[0 * 3 + 2]) - + phi[2 * 10 + 1] * sin(q[0 * 3 + 2])) - + cos(q[0 * 3 + 2]) * + (-l[1 * 3 + 2] * phi[2 * 10 + 1] + + sin(q[0 * 3 + 2]) * + (phi[2 * 10 + 6] - + phi[2 * 10 + 0] * + (1.0 / + (phi[2 * 10 + 0] * + phi[2 * 10 + 0]) * + (phi[2 * 10 + 1] * + phi[2 * 10 + 1]) + + (RL * RL) * 1.0 / + (phi[2 * 10 + 0] * + phi[2 * 10 + 0]) * + (phi[2 * 10 + 2] * + phi[2 * 10 + 2])) + + (phi[2 * 10 + 1] * phi[2 * 10 + 1]) / + phi[2 * 10 + 0] + + ((RL * RL) * + (phi[2 * 10 + 2] * phi[2 * 10 + 2])) / + phi[2 * 10 + 0]) + + phi[2 * 10 + 9] * cos(q[0 * 3 + 2]) + + (RL * RL) * l[1 * 3 + 1] * phi[2 * 10 + 2] * + sin(q[0 * 3 + 2])) + + sin(q[0 * 3 + 2]) * + (l[1 * 3 + 2] * phi[2 * 10 + 3] + + cos(q[0 * 3 + 2]) * + (phi[2 * 10 + 4] - + phi[2 * 10 + 0] * + (1.0 / + (phi[2 * 10 + 0] * + phi[2 * 10 + 0]) * + (phi[2 * 10 + 3] * + phi[2 * 10 + 3]) + + (RL * RL) * 1.0 / + (phi[2 * 10 + 0] * + phi[2 * 10 + 0]) * + (phi[2 * 10 + 2] * + phi[2 * 10 + 2])) + + (phi[2 * 10 + 3] * phi[2 * 10 + 3]) / + phi[2 * 10 + 0] + + ((RL * RL) * + (phi[2 * 10 + 2] * phi[2 * 10 + 2])) / + phi[2 * 10 + 0]) + + phi[2 * 10 + 9] * sin(q[0 * 3 + 2]) + + (RL * RL) * l[1 * 3 + 1] * phi[2 * 10 + 2] * + cos(q[0 * 3 + 2])) + + RL * l[4 * 3 + 1] * sin(G[0 * 3 + 2] * q[0 * 3 + 2]) * + (RL * phi[5 * 10 + 2] * + cos(G[0 * 3 + 2] * q[0 * 3 + 2]) + + RL * l[4 * 3 + 1] * phi[5 * 10 + 0] * + cos(G[0 * 3 + 2] * q[0 * 3 + 2])) + + RL * l[1 * 3 + 1] * sin(q[0 * 3 + 2]) * + (RL * phi[2 * 10 + 2] * cos(q[0 * 3 + 2]) + + RL * l[1 * 3 + 1] * phi[2 * 10 + 0] * + cos(q[0 * 3 + 2])) - + RL * l[4 * 3 + 1] * cos(G[0 * 3 + 2] * q[0 * 3 + 2]) * + (RL * phi[5 * 10 + 2] * + sin(G[0 * 3 + 2] * q[0 * 3 + 2]) + + RL * l[4 * 3 + 1] * phi[5 * 10 + 0] * + sin(G[0 * 3 + 2] * q[0 * 3 + 2])) - + RL * l[1 * 3 + 1] * cos(q[0 * 3 + 2]) * + (RL * phi[2 * 10 + 2] * sin(q[0 * 3 + 2]) + + RL * l[1 * 3 + 1] * phi[2 * 10 + 0] * + sin(q[0 * 3 + 2]))) + + cos(q[0 * 3 + 1]) * + (phi[1 * 10 + 6] + + l[4 * 3 + 0] * (l[4 * 3 + 0] * phi[5 * 10 + 0] + + phi[5 * 10 + 1] * + cos(G[0 * 3 + 2] * q[0 * 3 + 2]) + + phi[5 * 10 + 3] * + sin(G[0 * 3 + 2] * q[0 * 3 + 2])) + + cos(G[0 * 3 + 2] * q[0 * 3 + 2]) * + (cos(G[0 * 3 + 2] * q[0 * 3 + 2]) * + (phi[5 * 10 + 6] - + phi[5 * 10 + 0] * + (1.0 / + (phi[5 * 10 + 0] * + phi[5 * 10 + 0]) * + (phi[5 * 10 + 1] * + phi[5 * 10 + 1]) + + (RL * RL) * 1.0 / + (phi[5 * 10 + 0] * + phi[5 * 10 + 0]) * + (phi[5 * 10 + 2] * + phi[5 * 10 + 2])) + + (phi[5 * 10 + 1] * phi[5 * 10 + 1]) / + phi[5 * 10 + 0] + + ((RL * RL) * + (phi[5 * 10 + 2] * phi[5 * 10 + 2])) / + phi[5 * 10 + 0]) + + l[4 * 3 + 0] * phi[5 * 10 + 1] - + phi[5 * 10 + 9] * + sin(G[0 * 3 + 2] * q[0 * 3 + 2]) + + (RL * RL) * l[4 * 3 + 1] * phi[5 * 10 + 2] * + cos(G[0 * 3 + 2] * q[0 * 3 + 2])) + + sin(G[0 * 3 + 2] * q[0 * 3 + 2]) * + (l[4 * 3 + 0] * phi[5 * 10 + 3] + + sin(G[0 * 3 + 2] * q[0 * 3 + 2]) * + (phi[5 * 10 + 4] - + phi[5 * 10 + 0] * + (1.0 / + (phi[5 * 10 + 0] * + phi[5 * 10 + 0]) * + (phi[5 * 10 + 3] * + phi[5 * 10 + 3]) + + (RL * RL) * 1.0 / + (phi[5 * 10 + 0] * + phi[5 * 10 + 0]) * + (phi[5 * 10 + 2] * + phi[5 * 10 + 2])) + + (phi[5 * 10 + 3] * phi[5 * 10 + 3]) / + phi[5 * 10 + 0] + + ((RL * RL) * + (phi[5 * 10 + 2] * phi[5 * 10 + 2])) / + phi[5 * 10 + 0]) - + phi[5 * 10 + 9] * + cos(G[0 * 3 + 2] * q[0 * 3 + 2]) + + (RL * RL) * l[4 * 3 + 1] * phi[5 * 10 + 2] * + sin(G[0 * 3 + 2] * q[0 * 3 + 2])) + + l[1 * 3 + 0] * (l[1 * 3 + 0] * phi[2 * 10 + 0] + + phi[2 * 10 + 1] * cos(q[0 * 3 + 2]) + + phi[2 * 10 + 3] * sin(q[0 * 3 + 2])) + + cos(q[0 * 3 + 2]) * + (l[1 * 3 + 0] * phi[2 * 10 + 1] + + cos(q[0 * 3 + 2]) * + (phi[2 * 10 + 6] - + phi[2 * 10 + 0] * + (1.0 / + (phi[2 * 10 + 0] * + phi[2 * 10 + 0]) * + (phi[2 * 10 + 1] * + phi[2 * 10 + 1]) + + (RL * RL) * 1.0 / + (phi[2 * 10 + 0] * + phi[2 * 10 + 0]) * + (phi[2 * 10 + 2] * + phi[2 * 10 + 2])) + + (phi[2 * 10 + 1] * phi[2 * 10 + 1]) / + phi[2 * 10 + 0] + + ((RL * RL) * + (phi[2 * 10 + 2] * phi[2 * 10 + 2])) / + phi[2 * 10 + 0]) - + phi[2 * 10 + 9] * sin(q[0 * 3 + 2]) + + (RL * RL) * l[1 * 3 + 1] * phi[2 * 10 + 2] * + cos(q[0 * 3 + 2])) - + phi[1 * 10 + 0] * + (1.0 / (phi[1 * 10 + 0] * phi[1 * 10 + 0]) * + (phi[1 * 10 + 1] * phi[1 * 10 + 1]) + + (RL * RL) * 1.0 / + (phi[1 * 10 + 0] * phi[1 * 10 + 0]) * + (phi[1 * 10 + 2] * phi[1 * 10 + 2])) + + sin(q[0 * 3 + 2]) * + (l[1 * 3 + 0] * phi[2 * 10 + 3] + + sin(q[0 * 3 + 2]) * + (phi[2 * 10 + 4] - + phi[2 * 10 + 0] * + (1.0 / + (phi[2 * 10 + 0] * + phi[2 * 10 + 0]) * + (phi[2 * 10 + 3] * + phi[2 * 10 + 3]) + + (RL * RL) * 1.0 / + (phi[2 * 10 + 0] * + phi[2 * 10 + 0]) * + (phi[2 * 10 + 2] * + phi[2 * 10 + 2])) + + (phi[2 * 10 + 3] * phi[2 * 10 + 3]) / + phi[2 * 10 + 0] + + ((RL * RL) * + (phi[2 * 10 + 2] * phi[2 * 10 + 2])) / + phi[2 * 10 + 0]) - + phi[2 * 10 + 9] * cos(q[0 * 3 + 2]) + + (RL * RL) * l[1 * 3 + 1] * phi[2 * 10 + 2] * + sin(q[0 * 3 + 2])) + + (phi[1 * 10 + 1] * phi[1 * 10 + 1]) / phi[1 * 10 + 0] + + ((RL * RL) * (phi[1 * 10 + 2] * phi[1 * 10 + 2])) / + phi[1 * 10 + 0] + + RL * l[4 * 3 + 1] * cos(G[0 * 3 + 2] * q[0 * 3 + 2]) * + (RL * phi[5 * 10 + 2] * + cos(G[0 * 3 + 2] * q[0 * 3 + 2]) + + RL * l[4 * 3 + 1] * phi[5 * 10 + 0] * + cos(G[0 * 3 + 2] * q[0 * 3 + 2])) + + RL * l[1 * 3 + 1] * cos(q[0 * 3 + 2]) * + (RL * phi[2 * 10 + 2] * cos(q[0 * 3 + 2]) + + RL * l[1 * 3 + 1] * phi[2 * 10 + 0] * + cos(q[0 * 3 + 2])) + + RL * l[4 * 3 + 1] * sin(G[0 * 3 + 2] * q[0 * 3 + 2]) * + (RL * phi[5 * 10 + 2] * + sin(G[0 * 3 + 2] * q[0 * 3 + 2]) + + RL * l[4 * 3 + 1] * phi[5 * 10 + 0] * + sin(G[0 * 3 + 2] * q[0 * 3 + 2])) + + RL * l[1 * 3 + 1] * sin(q[0 * 3 + 2]) * + (RL * phi[2 * 10 + 2] * sin(q[0 * 3 + 2]) + + RL * l[1 * 3 + 1] * phi[2 * 10 + 0] * + sin(q[0 * 3 + 2]))) + + RL * l[0 * 3 + 1] * cos(q[0 * 3 + 1]) * + (RL * phi[1 * 10 + 2] + + RL * phi[5 * 10 + 2] * + pow(sin(G[0 * 3 + 2] * q[0 * 3 + 2]), 2.0) + + RL * phi[2 * 10 + 2] * pow(cos(q[0 * 3 + 2]), 2.0) + + RL * phi[2 * 10 + 2] * pow(sin(q[0 * 3 + 2]), 2.0) + + RL * phi[5 * 10 + 2] * + pow(cos(G[0 * 3 + 2] * q[0 * 3 + 2]), 2.0) + + RL * l[4 * 3 + 1] * phi[5 * 10 + 0] * + pow(cos(G[0 * 3 + 2] * q[0 * 3 + 2]), 2.0) + + RL * l[4 * 3 + 1] * phi[5 * 10 + 0] * + pow(sin(G[0 * 3 + 2] * q[0 * 3 + 2]), 2.0) + + RL * l[1 * 3 + 1] * phi[2 * 10 + 0] * + pow(cos(q[0 * 3 + 2]), 2.0) + + RL * l[1 * 3 + 1] * phi[2 * 10 + 0] * + pow(sin(q[0 * 3 + 2]), 2.0)))) / + 2.0 + + (cos(q[0 * 3 + 1]) * + (sin(q[0 * 3 + 1]) * + (phi[1 * 10 + 4] + + l[4 * 3 + 2] * (l[4 * 3 + 2] * phi[5 * 10 + 0] + + phi[5 * 10 + 3] * + cos(G[0 * 3 + 2] * q[0 * 3 + 2]) - + phi[5 * 10 + 1] * + sin(G[0 * 3 + 2] * q[0 * 3 + 2])) + + cos(G[0 * 3 + 2] * q[0 * 3 + 2]) * + (cos(G[0 * 3 + 2] * q[0 * 3 + 2]) * + (phi[5 * 10 + 4] - + phi[5 * 10 + 0] * + (1.0 / + (phi[5 * 10 + 0] * + phi[5 * 10 + 0]) * + (phi[5 * 10 + 3] * + phi[5 * 10 + 3]) + + (RL * RL) * 1.0 / + (phi[5 * 10 + 0] * + phi[5 * 10 + 0]) * + (phi[5 * 10 + 2] * + phi[5 * 10 + 2])) + + (phi[5 * 10 + 3] * phi[5 * 10 + 3]) / + phi[5 * 10 + 0] + + ((RL * RL) * + (phi[5 * 10 + 2] * phi[5 * 10 + 2])) / + phi[5 * 10 + 0]) + + l[4 * 3 + 2] * phi[5 * 10 + 3] + + phi[5 * 10 + 9] * + sin(G[0 * 3 + 2] * q[0 * 3 + 2]) + + (RL * RL) * l[4 * 3 + 1] * phi[5 * 10 + 2] * + cos(G[0 * 3 + 2] * q[0 * 3 + 2])) + + sin(G[0 * 3 + 2] * q[0 * 3 + 2]) * + (-l[4 * 3 + 2] * phi[5 * 10 + 1] + + sin(G[0 * 3 + 2] * q[0 * 3 + 2]) * + (phi[5 * 10 + 6] - + phi[5 * 10 + 0] * + (1.0 / + (phi[5 * 10 + 0] * + phi[5 * 10 + 0]) * + (phi[5 * 10 + 1] * + phi[5 * 10 + 1]) + + (RL * RL) * 1.0 / + (phi[5 * 10 + 0] * + phi[5 * 10 + 0]) * + (phi[5 * 10 + 2] * + phi[5 * 10 + 2])) + + (phi[5 * 10 + 1] * phi[5 * 10 + 1]) / + phi[5 * 10 + 0] + + ((RL * RL) * + (phi[5 * 10 + 2] * phi[5 * 10 + 2])) / + phi[5 * 10 + 0]) + + phi[5 * 10 + 9] * + cos(G[0 * 3 + 2] * q[0 * 3 + 2]) + + (RL * RL) * l[4 * 3 + 1] * phi[5 * 10 + 2] * + sin(G[0 * 3 + 2] * q[0 * 3 + 2])) + + l[1 * 3 + 2] * (l[1 * 3 + 2] * phi[2 * 10 + 0] + + phi[2 * 10 + 3] * cos(q[0 * 3 + 2]) - + phi[2 * 10 + 1] * sin(q[0 * 3 + 2])) + + cos(q[0 * 3 + 2]) * + (l[1 * 3 + 2] * phi[2 * 10 + 3] + + cos(q[0 * 3 + 2]) * + (phi[2 * 10 + 4] - + phi[2 * 10 + 0] * + (1.0 / + (phi[2 * 10 + 0] * + phi[2 * 10 + 0]) * + (phi[2 * 10 + 3] * + phi[2 * 10 + 3]) + + (RL * RL) * 1.0 / + (phi[2 * 10 + 0] * + phi[2 * 10 + 0]) * + (phi[2 * 10 + 2] * + phi[2 * 10 + 2])) + + (phi[2 * 10 + 3] * phi[2 * 10 + 3]) / + phi[2 * 10 + 0] + + ((RL * RL) * + (phi[2 * 10 + 2] * phi[2 * 10 + 2])) / + phi[2 * 10 + 0]) + + phi[2 * 10 + 9] * sin(q[0 * 3 + 2]) + + (RL * RL) * l[1 * 3 + 1] * phi[2 * 10 + 2] * + cos(q[0 * 3 + 2])) - + phi[1 * 10 + 0] * + (1.0 / (phi[1 * 10 + 0] * phi[1 * 10 + 0]) * + (phi[1 * 10 + 3] * phi[1 * 10 + 3]) + + (RL * RL) * 1.0 / + (phi[1 * 10 + 0] * phi[1 * 10 + 0]) * + (phi[1 * 10 + 2] * phi[1 * 10 + 2])) + + sin(q[0 * 3 + 2]) * + (-l[1 * 3 + 2] * phi[2 * 10 + 1] + + sin(q[0 * 3 + 2]) * + (phi[2 * 10 + 6] - + phi[2 * 10 + 0] * + (1.0 / + (phi[2 * 10 + 0] * + phi[2 * 10 + 0]) * + (phi[2 * 10 + 1] * + phi[2 * 10 + 1]) + + (RL * RL) * 1.0 / + (phi[2 * 10 + 0] * + phi[2 * 10 + 0]) * + (phi[2 * 10 + 2] * + phi[2 * 10 + 2])) + + (phi[2 * 10 + 1] * phi[2 * 10 + 1]) / + phi[2 * 10 + 0] + + ((RL * RL) * + (phi[2 * 10 + 2] * phi[2 * 10 + 2])) / + phi[2 * 10 + 0]) + + phi[2 * 10 + 9] * cos(q[0 * 3 + 2]) + + (RL * RL) * l[1 * 3 + 1] * phi[2 * 10 + 2] * + sin(q[0 * 3 + 2])) + + (phi[1 * 10 + 3] * phi[1 * 10 + 3]) / phi[1 * 10 + 0] + + ((RL * RL) * (phi[1 * 10 + 2] * phi[1 * 10 + 2])) / + phi[1 * 10 + 0] + + RL * l[4 * 3 + 1] * cos(G[0 * 3 + 2] * q[0 * 3 + 2]) * + (RL * phi[5 * 10 + 2] * + cos(G[0 * 3 + 2] * q[0 * 3 + 2]) + + RL * l[4 * 3 + 1] * phi[5 * 10 + 0] * + cos(G[0 * 3 + 2] * q[0 * 3 + 2])) + + RL * l[1 * 3 + 1] * cos(q[0 * 3 + 2]) * + (RL * phi[2 * 10 + 2] * cos(q[0 * 3 + 2]) + + RL * l[1 * 3 + 1] * phi[2 * 10 + 0] * + cos(q[0 * 3 + 2])) + + RL * l[4 * 3 + 1] * sin(G[0 * 3 + 2] * q[0 * 3 + 2]) * + (RL * phi[5 * 10 + 2] * + sin(G[0 * 3 + 2] * q[0 * 3 + 2]) + + RL * l[4 * 3 + 1] * phi[5 * 10 + 0] * + sin(G[0 * 3 + 2] * q[0 * 3 + 2])) + + RL * l[1 * 3 + 1] * sin(q[0 * 3 + 2]) * + (RL * phi[2 * 10 + 2] * sin(q[0 * 3 + 2]) + + RL * l[1 * 3 + 1] * phi[2 * 10 + 0] * + sin(q[0 * 3 + 2]))) + + cos(q[0 * 3 + 1]) * + (-phi[1 * 10 + 9] + + l[4 * 3 + 2] * (l[4 * 3 + 0] * phi[5 * 10 + 0] + + phi[5 * 10 + 1] * + cos(G[0 * 3 + 2] * q[0 * 3 + 2]) + + phi[5 * 10 + 3] * + sin(G[0 * 3 + 2] * q[0 * 3 + 2])) - + sin(G[0 * 3 + 2] * q[0 * 3 + 2]) * + (cos(G[0 * 3 + 2] * q[0 * 3 + 2]) * + (phi[5 * 10 + 6] - + phi[5 * 10 + 0] * + (1.0 / + (phi[5 * 10 + 0] * + phi[5 * 10 + 0]) * + (phi[5 * 10 + 1] * + phi[5 * 10 + 1]) + + (RL * RL) * 1.0 / + (phi[5 * 10 + 0] * + phi[5 * 10 + 0]) * + (phi[5 * 10 + 2] * + phi[5 * 10 + 2])) + + (phi[5 * 10 + 1] * phi[5 * 10 + 1]) / + phi[5 * 10 + 0] + + ((RL * RL) * + (phi[5 * 10 + 2] * phi[5 * 10 + 2])) / + phi[5 * 10 + 0]) + + l[4 * 3 + 0] * phi[5 * 10 + 1] - + phi[5 * 10 + 9] * + sin(G[0 * 3 + 2] * q[0 * 3 + 2]) + + (RL * RL) * l[4 * 3 + 1] * phi[5 * 10 + 2] * + cos(G[0 * 3 + 2] * q[0 * 3 + 2])) + + cos(G[0 * 3 + 2] * q[0 * 3 + 2]) * + (l[4 * 3 + 0] * phi[5 * 10 + 3] + + sin(G[0 * 3 + 2] * q[0 * 3 + 2]) * + (phi[5 * 10 + 4] - + phi[5 * 10 + 0] * + (1.0 / + (phi[5 * 10 + 0] * + phi[5 * 10 + 0]) * + (phi[5 * 10 + 3] * + phi[5 * 10 + 3]) + + (RL * RL) * 1.0 / + (phi[5 * 10 + 0] * + phi[5 * 10 + 0]) * + (phi[5 * 10 + 2] * + phi[5 * 10 + 2])) + + (phi[5 * 10 + 3] * phi[5 * 10 + 3]) / + phi[5 * 10 + 0] + + ((RL * RL) * + (phi[5 * 10 + 2] * phi[5 * 10 + 2])) / + phi[5 * 10 + 0]) - + phi[5 * 10 + 9] * + cos(G[0 * 3 + 2] * q[0 * 3 + 2]) + + (RL * RL) * l[4 * 3 + 1] * phi[5 * 10 + 2] * + sin(G[0 * 3 + 2] * q[0 * 3 + 2])) + + l[1 * 3 + 2] * (l[1 * 3 + 0] * phi[2 * 10 + 0] + + phi[2 * 10 + 1] * cos(q[0 * 3 + 2]) + + phi[2 * 10 + 3] * sin(q[0 * 3 + 2])) - + sin(q[0 * 3 + 2]) * + (l[1 * 3 + 0] * phi[2 * 10 + 1] + + cos(q[0 * 3 + 2]) * + (phi[2 * 10 + 6] - + phi[2 * 10 + 0] * + (1.0 / + (phi[2 * 10 + 0] * + phi[2 * 10 + 0]) * + (phi[2 * 10 + 1] * + phi[2 * 10 + 1]) + + (RL * RL) * 1.0 / + (phi[2 * 10 + 0] * + phi[2 * 10 + 0]) * + (phi[2 * 10 + 2] * + phi[2 * 10 + 2])) + + (phi[2 * 10 + 1] * phi[2 * 10 + 1]) / + phi[2 * 10 + 0] + + ((RL * RL) * + (phi[2 * 10 + 2] * phi[2 * 10 + 2])) / + phi[2 * 10 + 0]) - + phi[2 * 10 + 9] * sin(q[0 * 3 + 2]) + + (RL * RL) * l[1 * 3 + 1] * phi[2 * 10 + 2] * + cos(q[0 * 3 + 2])) + + cos(q[0 * 3 + 2]) * + (l[1 * 3 + 0] * phi[2 * 10 + 3] + + sin(q[0 * 3 + 2]) * + (phi[2 * 10 + 4] - + phi[2 * 10 + 0] * + (1.0 / + (phi[2 * 10 + 0] * + phi[2 * 10 + 0]) * + (phi[2 * 10 + 3] * + phi[2 * 10 + 3]) + + (RL * RL) * 1.0 / + (phi[2 * 10 + 0] * + phi[2 * 10 + 0]) * + (phi[2 * 10 + 2] * + phi[2 * 10 + 2])) + + (phi[2 * 10 + 3] * phi[2 * 10 + 3]) / + phi[2 * 10 + 0] + + ((RL * RL) * + (phi[2 * 10 + 2] * phi[2 * 10 + 2])) / + phi[2 * 10 + 0]) - + phi[2 * 10 + 9] * cos(q[0 * 3 + 2]) + + (RL * RL) * l[1 * 3 + 1] * phi[2 * 10 + 2] * + sin(q[0 * 3 + 2])) - + RL * l[4 * 3 + 1] * sin(G[0 * 3 + 2] * q[0 * 3 + 2]) * + (RL * phi[5 * 10 + 2] * + cos(G[0 * 3 + 2] * q[0 * 3 + 2]) + + RL * l[4 * 3 + 1] * phi[5 * 10 + 0] * + cos(G[0 * 3 + 2] * q[0 * 3 + 2])) - + RL * l[1 * 3 + 1] * sin(q[0 * 3 + 2]) * + (RL * phi[2 * 10 + 2] * cos(q[0 * 3 + 2]) + + RL * l[1 * 3 + 1] * phi[2 * 10 + 0] * + cos(q[0 * 3 + 2])) + + RL * l[4 * 3 + 1] * cos(G[0 * 3 + 2] * q[0 * 3 + 2]) * + (RL * phi[5 * 10 + 2] * + sin(G[0 * 3 + 2] * q[0 * 3 + 2]) + + RL * l[4 * 3 + 1] * phi[5 * 10 + 0] * + sin(G[0 * 3 + 2] * q[0 * 3 + 2])) + + RL * l[1 * 3 + 1] * cos(q[0 * 3 + 2]) * + (RL * phi[2 * 10 + 2] * sin(q[0 * 3 + 2]) + + RL * l[1 * 3 + 1] * phi[2 * 10 + 0] * + sin(q[0 * 3 + 2]))) + + RL * l[0 * 3 + 1] * sin(q[0 * 3 + 1]) * + (RL * phi[1 * 10 + 2] + + RL * phi[5 * 10 + 2] * + pow(sin(G[0 * 3 + 2] * q[0 * 3 + 2]), 2.0) + + RL * phi[2 * 10 + 2] * pow(cos(q[0 * 3 + 2]), 2.0) + + RL * phi[2 * 10 + 2] * pow(sin(q[0 * 3 + 2]), 2.0) + + RL * phi[5 * 10 + 2] * + pow(cos(G[0 * 3 + 2] * q[0 * 3 + 2]), 2.0) + + RL * l[4 * 3 + 1] * phi[5 * 10 + 0] * + pow(cos(G[0 * 3 + 2] * q[0 * 3 + 2]), 2.0) + + RL * l[4 * 3 + 1] * phi[5 * 10 + 0] * + pow(sin(G[0 * 3 + 2] * q[0 * 3 + 2]), 2.0) + + RL * l[1 * 3 + 1] * phi[2 * 10 + 0] * + pow(cos(q[0 * 3 + 2]), 2.0) + + RL * l[1 * 3 + 1] * phi[2 * 10 + 0] * + pow(sin(q[0 * 3 + 2]), 2.0)))) / + 2.0 + + (cos(q[0 * 3 + 1]) * + (l[0 * 3 + 2] * + (phi[1 * 10 + 1] + l[1 * 3 + 0] * phi[2 * 10 + 0] + + l[4 * 3 + 0] * phi[5 * 10 + 0] + + phi[5 * 10 + 1] * cos(G[0 * 3 + 2] * q[0 * 3 + 2]) + + phi[5 * 10 + 3] * sin(G[0 * 3 + 2] * q[0 * 3 + 2]) + + phi[2 * 10 + 1] * cos(q[0 * 3 + 2]) + + phi[2 * 10 + 3] * sin(q[0 * 3 + 2])) - + sin(q[0 * 3 + 1]) * + (phi[1 * 10 + 6] + + l[4 * 3 + 0] * (l[4 * 3 + 0] * phi[5 * 10 + 0] + + phi[5 * 10 + 1] * + cos(G[0 * 3 + 2] * q[0 * 3 + 2]) + + phi[5 * 10 + 3] * + sin(G[0 * 3 + 2] * q[0 * 3 + 2])) + + cos(G[0 * 3 + 2] * q[0 * 3 + 2]) * + (cos(G[0 * 3 + 2] * q[0 * 3 + 2]) * + (phi[5 * 10 + 6] - + phi[5 * 10 + 0] * + (1.0 / + (phi[5 * 10 + 0] * + phi[5 * 10 + 0]) * + (phi[5 * 10 + 1] * + phi[5 * 10 + 1]) + + (RL * RL) * 1.0 / + (phi[5 * 10 + 0] * + phi[5 * 10 + 0]) * + (phi[5 * 10 + 2] * + phi[5 * 10 + 2])) + + (phi[5 * 10 + 1] * phi[5 * 10 + 1]) / + phi[5 * 10 + 0] + + ((RL * RL) * + (phi[5 * 10 + 2] * phi[5 * 10 + 2])) / + phi[5 * 10 + 0]) + + l[4 * 3 + 0] * phi[5 * 10 + 1] - + phi[5 * 10 + 9] * + sin(G[0 * 3 + 2] * q[0 * 3 + 2]) + + (RL * RL) * l[4 * 3 + 1] * phi[5 * 10 + 2] * + cos(G[0 * 3 + 2] * q[0 * 3 + 2])) + + sin(G[0 * 3 + 2] * q[0 * 3 + 2]) * + (l[4 * 3 + 0] * phi[5 * 10 + 3] + + sin(G[0 * 3 + 2] * q[0 * 3 + 2]) * + (phi[5 * 10 + 4] - + phi[5 * 10 + 0] * + (1.0 / + (phi[5 * 10 + 0] * + phi[5 * 10 + 0]) * + (phi[5 * 10 + 3] * + phi[5 * 10 + 3]) + + (RL * RL) * 1.0 / + (phi[5 * 10 + 0] * + phi[5 * 10 + 0]) * + (phi[5 * 10 + 2] * + phi[5 * 10 + 2])) + + (phi[5 * 10 + 3] * phi[5 * 10 + 3]) / + phi[5 * 10 + 0] + + ((RL * RL) * + (phi[5 * 10 + 2] * phi[5 * 10 + 2])) / + phi[5 * 10 + 0]) - + phi[5 * 10 + 9] * + cos(G[0 * 3 + 2] * q[0 * 3 + 2]) + + (RL * RL) * l[4 * 3 + 1] * phi[5 * 10 + 2] * + sin(G[0 * 3 + 2] * q[0 * 3 + 2])) + + l[1 * 3 + 0] * (l[1 * 3 + 0] * phi[2 * 10 + 0] + + phi[2 * 10 + 1] * cos(q[0 * 3 + 2]) + + phi[2 * 10 + 3] * sin(q[0 * 3 + 2])) + + cos(q[0 * 3 + 2]) * + (l[1 * 3 + 0] * phi[2 * 10 + 1] + + cos(q[0 * 3 + 2]) * + (phi[2 * 10 + 6] - + phi[2 * 10 + 0] * + (1.0 / + (phi[2 * 10 + 0] * + phi[2 * 10 + 0]) * + (phi[2 * 10 + 1] * + phi[2 * 10 + 1]) + + (RL * RL) * 1.0 / + (phi[2 * 10 + 0] * + phi[2 * 10 + 0]) * + (phi[2 * 10 + 2] * + phi[2 * 10 + 2])) + + (phi[2 * 10 + 1] * phi[2 * 10 + 1]) / + phi[2 * 10 + 0] + + ((RL * RL) * + (phi[2 * 10 + 2] * phi[2 * 10 + 2])) / + phi[2 * 10 + 0]) - + phi[2 * 10 + 9] * sin(q[0 * 3 + 2]) + + (RL * RL) * l[1 * 3 + 1] * phi[2 * 10 + 2] * + cos(q[0 * 3 + 2])) - + phi[1 * 10 + 0] * + (1.0 / (phi[1 * 10 + 0] * phi[1 * 10 + 0]) * + (phi[1 * 10 + 1] * phi[1 * 10 + 1]) + + (RL * RL) * 1.0 / + (phi[1 * 10 + 0] * phi[1 * 10 + 0]) * + (phi[1 * 10 + 2] * phi[1 * 10 + 2])) + + sin(q[0 * 3 + 2]) * + (l[1 * 3 + 0] * phi[2 * 10 + 3] + + sin(q[0 * 3 + 2]) * + (phi[2 * 10 + 4] - + phi[2 * 10 + 0] * + (1.0 / + (phi[2 * 10 + 0] * + phi[2 * 10 + 0]) * + (phi[2 * 10 + 3] * + phi[2 * 10 + 3]) + + (RL * RL) * 1.0 / + (phi[2 * 10 + 0] * + phi[2 * 10 + 0]) * + (phi[2 * 10 + 2] * + phi[2 * 10 + 2])) + + (phi[2 * 10 + 3] * phi[2 * 10 + 3]) / + phi[2 * 10 + 0] + + ((RL * RL) * + (phi[2 * 10 + 2] * phi[2 * 10 + 2])) / + phi[2 * 10 + 0]) - + phi[2 * 10 + 9] * cos(q[0 * 3 + 2]) + + (RL * RL) * l[1 * 3 + 1] * phi[2 * 10 + 2] * + sin(q[0 * 3 + 2])) + + (phi[1 * 10 + 1] * phi[1 * 10 + 1]) / phi[1 * 10 + 0] + + ((RL * RL) * (phi[1 * 10 + 2] * phi[1 * 10 + 2])) / + phi[1 * 10 + 0] + + RL * l[4 * 3 + 1] * cos(G[0 * 3 + 2] * q[0 * 3 + 2]) * + (RL * phi[5 * 10 + 2] * + cos(G[0 * 3 + 2] * q[0 * 3 + 2]) + + RL * l[4 * 3 + 1] * phi[5 * 10 + 0] * + cos(G[0 * 3 + 2] * q[0 * 3 + 2])) + + RL * l[1 * 3 + 1] * cos(q[0 * 3 + 2]) * + (RL * phi[2 * 10 + 2] * cos(q[0 * 3 + 2]) + + RL * l[1 * 3 + 1] * phi[2 * 10 + 0] * + cos(q[0 * 3 + 2])) + + RL * l[4 * 3 + 1] * sin(G[0 * 3 + 2] * q[0 * 3 + 2]) * + (RL * phi[5 * 10 + 2] * + sin(G[0 * 3 + 2] * q[0 * 3 + 2]) + + RL * l[4 * 3 + 1] * phi[5 * 10 + 0] * + sin(G[0 * 3 + 2] * q[0 * 3 + 2])) + + RL * l[1 * 3 + 1] * sin(q[0 * 3 + 2]) * + (RL * phi[2 * 10 + 2] * sin(q[0 * 3 + 2]) + + RL * l[1 * 3 + 1] * phi[2 * 10 + 0] * + sin(q[0 * 3 + 2]))) + + cos(q[0 * 3 + 1]) * + (-phi[1 * 10 + 9] + + l[4 * 3 + 0] * (l[4 * 3 + 2] * phi[5 * 10 + 0] + + phi[5 * 10 + 3] * + cos(G[0 * 3 + 2] * q[0 * 3 + 2]) - + phi[5 * 10 + 1] * + sin(G[0 * 3 + 2] * q[0 * 3 + 2])) - + cos(G[0 * 3 + 2] * q[0 * 3 + 2]) * + (-l[4 * 3 + 2] * phi[5 * 10 + 1] + + sin(G[0 * 3 + 2] * q[0 * 3 + 2]) * + (phi[5 * 10 + 6] - + phi[5 * 10 + 0] * + (1.0 / + (phi[5 * 10 + 0] * + phi[5 * 10 + 0]) * + (phi[5 * 10 + 1] * + phi[5 * 10 + 1]) + + (RL * RL) * 1.0 / + (phi[5 * 10 + 0] * + phi[5 * 10 + 0]) * + (phi[5 * 10 + 2] * + phi[5 * 10 + 2])) + + (phi[5 * 10 + 1] * phi[5 * 10 + 1]) / + phi[5 * 10 + 0] + + ((RL * RL) * + (phi[5 * 10 + 2] * phi[5 * 10 + 2])) / + phi[5 * 10 + 0]) + + phi[5 * 10 + 9] * + cos(G[0 * 3 + 2] * q[0 * 3 + 2]) + + (RL * RL) * l[4 * 3 + 1] * phi[5 * 10 + 2] * + sin(G[0 * 3 + 2] * q[0 * 3 + 2])) + + sin(G[0 * 3 + 2] * q[0 * 3 + 2]) * + (cos(G[0 * 3 + 2] * q[0 * 3 + 2]) * + (phi[5 * 10 + 4] - + phi[5 * 10 + 0] * + (1.0 / + (phi[5 * 10 + 0] * + phi[5 * 10 + 0]) * + (phi[5 * 10 + 3] * + phi[5 * 10 + 3]) + + (RL * RL) * 1.0 / + (phi[5 * 10 + 0] * + phi[5 * 10 + 0]) * + (phi[5 * 10 + 2] * + phi[5 * 10 + 2])) + + (phi[5 * 10 + 3] * phi[5 * 10 + 3]) / + phi[5 * 10 + 0] + + ((RL * RL) * + (phi[5 * 10 + 2] * phi[5 * 10 + 2])) / + phi[5 * 10 + 0]) + + l[4 * 3 + 2] * phi[5 * 10 + 3] + + phi[5 * 10 + 9] * + sin(G[0 * 3 + 2] * q[0 * 3 + 2]) + + (RL * RL) * l[4 * 3 + 1] * phi[5 * 10 + 2] * + cos(G[0 * 3 + 2] * q[0 * 3 + 2])) + + l[1 * 3 + 0] * (l[1 * 3 + 2] * phi[2 * 10 + 0] + + phi[2 * 10 + 3] * cos(q[0 * 3 + 2]) - + phi[2 * 10 + 1] * sin(q[0 * 3 + 2])) - + cos(q[0 * 3 + 2]) * + (-l[1 * 3 + 2] * phi[2 * 10 + 1] + + sin(q[0 * 3 + 2]) * + (phi[2 * 10 + 6] - + phi[2 * 10 + 0] * + (1.0 / + (phi[2 * 10 + 0] * + phi[2 * 10 + 0]) * + (phi[2 * 10 + 1] * + phi[2 * 10 + 1]) + + (RL * RL) * 1.0 / + (phi[2 * 10 + 0] * + phi[2 * 10 + 0]) * + (phi[2 * 10 + 2] * + phi[2 * 10 + 2])) + + (phi[2 * 10 + 1] * phi[2 * 10 + 1]) / + phi[2 * 10 + 0] + + ((RL * RL) * + (phi[2 * 10 + 2] * phi[2 * 10 + 2])) / + phi[2 * 10 + 0]) + + phi[2 * 10 + 9] * cos(q[0 * 3 + 2]) + + (RL * RL) * l[1 * 3 + 1] * phi[2 * 10 + 2] * + sin(q[0 * 3 + 2])) + + sin(q[0 * 3 + 2]) * + (l[1 * 3 + 2] * phi[2 * 10 + 3] + + cos(q[0 * 3 + 2]) * + (phi[2 * 10 + 4] - + phi[2 * 10 + 0] * + (1.0 / + (phi[2 * 10 + 0] * + phi[2 * 10 + 0]) * + (phi[2 * 10 + 3] * + phi[2 * 10 + 3]) + + (RL * RL) * 1.0 / + (phi[2 * 10 + 0] * + phi[2 * 10 + 0]) * + (phi[2 * 10 + 2] * + phi[2 * 10 + 2])) + + (phi[2 * 10 + 3] * phi[2 * 10 + 3]) / + phi[2 * 10 + 0] + + ((RL * RL) * + (phi[2 * 10 + 2] * phi[2 * 10 + 2])) / + phi[2 * 10 + 0]) + + phi[2 * 10 + 9] * sin(q[0 * 3 + 2]) + + (RL * RL) * l[1 * 3 + 1] * phi[2 * 10 + 2] * + cos(q[0 * 3 + 2])) + + RL * l[4 * 3 + 1] * sin(G[0 * 3 + 2] * q[0 * 3 + 2]) * + (RL * phi[5 * 10 + 2] * + cos(G[0 * 3 + 2] * q[0 * 3 + 2]) + + RL * l[4 * 3 + 1] * phi[5 * 10 + 0] * + cos(G[0 * 3 + 2] * q[0 * 3 + 2])) + + RL * l[1 * 3 + 1] * sin(q[0 * 3 + 2]) * + (RL * phi[2 * 10 + 2] * cos(q[0 * 3 + 2]) + + RL * l[1 * 3 + 1] * phi[2 * 10 + 0] * + cos(q[0 * 3 + 2])) - + RL * l[4 * 3 + 1] * cos(G[0 * 3 + 2] * q[0 * 3 + 2]) * + (RL * phi[5 * 10 + 2] * + sin(G[0 * 3 + 2] * q[0 * 3 + 2]) + + RL * l[4 * 3 + 1] * phi[5 * 10 + 0] * + sin(G[0 * 3 + 2] * q[0 * 3 + 2])) - + RL * l[1 * 3 + 1] * cos(q[0 * 3 + 2]) * + (RL * phi[2 * 10 + 2] * sin(q[0 * 3 + 2]) + + RL * l[1 * 3 + 1] * phi[2 * 10 + 0] * + sin(q[0 * 3 + 2]))) - + RL * l[0 * 3 + 1] * sin(q[0 * 3 + 1]) * + (RL * phi[1 * 10 + 2] + + RL * phi[5 * 10 + 2] * + pow(sin(G[0 * 3 + 2] * q[0 * 3 + 2]), 2.0) + + RL * phi[2 * 10 + 2] * pow(cos(q[0 * 3 + 2]), 2.0) + + RL * phi[2 * 10 + 2] * pow(sin(q[0 * 3 + 2]), 2.0) + + RL * phi[5 * 10 + 2] * + pow(cos(G[0 * 3 + 2] * q[0 * 3 + 2]), 2.0) + + RL * l[4 * 3 + 1] * phi[5 * 10 + 0] * + pow(cos(G[0 * 3 + 2] * q[0 * 3 + 2]), 2.0) + + RL * l[4 * 3 + 1] * phi[5 * 10 + 0] * + pow(sin(G[0 * 3 + 2] * q[0 * 3 + 2]), 2.0) + + RL * l[1 * 3 + 1] * phi[2 * 10 + 0] * + pow(cos(q[0 * 3 + 2]), 2.0) + + RL * l[1 * 3 + 1] * phi[2 * 10 + 0] * + pow(sin(q[0 * 3 + 2]), 2.0)))) / + 2.0 + + (sin(q[0 * 3 + 1]) * + (-sin(q[0 * 3 + 1]) * + (-phi[1 * 10 + 9] + + l[4 * 3 + 2] * (l[4 * 3 + 0] * phi[5 * 10 + 0] + + phi[5 * 10 + 1] * + cos(G[0 * 3 + 2] * q[0 * 3 + 2]) + + phi[5 * 10 + 3] * + sin(G[0 * 3 + 2] * q[0 * 3 + 2])) - + sin(G[0 * 3 + 2] * q[0 * 3 + 2]) * + (cos(G[0 * 3 + 2] * q[0 * 3 + 2]) * + (phi[5 * 10 + 6] - + phi[5 * 10 + 0] * + (1.0 / + (phi[5 * 10 + 0] * + phi[5 * 10 + 0]) * + (phi[5 * 10 + 1] * + phi[5 * 10 + 1]) + + (RL * RL) * 1.0 / + (phi[5 * 10 + 0] * + phi[5 * 10 + 0]) * + (phi[5 * 10 + 2] * + phi[5 * 10 + 2])) + + (phi[5 * 10 + 1] * phi[5 * 10 + 1]) / + phi[5 * 10 + 0] + + ((RL * RL) * + (phi[5 * 10 + 2] * phi[5 * 10 + 2])) / + phi[5 * 10 + 0]) + + l[4 * 3 + 0] * phi[5 * 10 + 1] - + phi[5 * 10 + 9] * + sin(G[0 * 3 + 2] * q[0 * 3 + 2]) + + (RL * RL) * l[4 * 3 + 1] * phi[5 * 10 + 2] * + cos(G[0 * 3 + 2] * q[0 * 3 + 2])) + + cos(G[0 * 3 + 2] * q[0 * 3 + 2]) * + (l[4 * 3 + 0] * phi[5 * 10 + 3] + + sin(G[0 * 3 + 2] * q[0 * 3 + 2]) * + (phi[5 * 10 + 4] - + phi[5 * 10 + 0] * + (1.0 / + (phi[5 * 10 + 0] * + phi[5 * 10 + 0]) * + (phi[5 * 10 + 3] * + phi[5 * 10 + 3]) + + (RL * RL) * 1.0 / + (phi[5 * 10 + 0] * + phi[5 * 10 + 0]) * + (phi[5 * 10 + 2] * + phi[5 * 10 + 2])) + + (phi[5 * 10 + 3] * phi[5 * 10 + 3]) / + phi[5 * 10 + 0] + + ((RL * RL) * + (phi[5 * 10 + 2] * phi[5 * 10 + 2])) / + phi[5 * 10 + 0]) - + phi[5 * 10 + 9] * + cos(G[0 * 3 + 2] * q[0 * 3 + 2]) + + (RL * RL) * l[4 * 3 + 1] * phi[5 * 10 + 2] * + sin(G[0 * 3 + 2] * q[0 * 3 + 2])) + + l[1 * 3 + 2] * (l[1 * 3 + 0] * phi[2 * 10 + 0] + + phi[2 * 10 + 1] * cos(q[0 * 3 + 2]) + + phi[2 * 10 + 3] * sin(q[0 * 3 + 2])) - + sin(q[0 * 3 + 2]) * + (l[1 * 3 + 0] * phi[2 * 10 + 1] + + cos(q[0 * 3 + 2]) * + (phi[2 * 10 + 6] - + phi[2 * 10 + 0] * + (1.0 / + (phi[2 * 10 + 0] * + phi[2 * 10 + 0]) * + (phi[2 * 10 + 1] * + phi[2 * 10 + 1]) + + (RL * RL) * 1.0 / + (phi[2 * 10 + 0] * + phi[2 * 10 + 0]) * + (phi[2 * 10 + 2] * + phi[2 * 10 + 2])) + + (phi[2 * 10 + 1] * phi[2 * 10 + 1]) / + phi[2 * 10 + 0] + + ((RL * RL) * + (phi[2 * 10 + 2] * phi[2 * 10 + 2])) / + phi[2 * 10 + 0]) - + phi[2 * 10 + 9] * sin(q[0 * 3 + 2]) + + (RL * RL) * l[1 * 3 + 1] * phi[2 * 10 + 2] * + cos(q[0 * 3 + 2])) + + cos(q[0 * 3 + 2]) * + (l[1 * 3 + 0] * phi[2 * 10 + 3] + + sin(q[0 * 3 + 2]) * + (phi[2 * 10 + 4] - + phi[2 * 10 + 0] * + (1.0 / + (phi[2 * 10 + 0] * + phi[2 * 10 + 0]) * + (phi[2 * 10 + 3] * + phi[2 * 10 + 3]) + + (RL * RL) * 1.0 / + (phi[2 * 10 + 0] * + phi[2 * 10 + 0]) * + (phi[2 * 10 + 2] * + phi[2 * 10 + 2])) + + (phi[2 * 10 + 3] * phi[2 * 10 + 3]) / + phi[2 * 10 + 0] + + ((RL * RL) * + (phi[2 * 10 + 2] * phi[2 * 10 + 2])) / + phi[2 * 10 + 0]) - + phi[2 * 10 + 9] * cos(q[0 * 3 + 2]) + + (RL * RL) * l[1 * 3 + 1] * phi[2 * 10 + 2] * + sin(q[0 * 3 + 2])) - + RL * l[4 * 3 + 1] * sin(G[0 * 3 + 2] * q[0 * 3 + 2]) * + (RL * phi[5 * 10 + 2] * + cos(G[0 * 3 + 2] * q[0 * 3 + 2]) + + RL * l[4 * 3 + 1] * phi[5 * 10 + 0] * + cos(G[0 * 3 + 2] * q[0 * 3 + 2])) - + RL * l[1 * 3 + 1] * sin(q[0 * 3 + 2]) * + (RL * phi[2 * 10 + 2] * cos(q[0 * 3 + 2]) + + RL * l[1 * 3 + 1] * phi[2 * 10 + 0] * + cos(q[0 * 3 + 2])) + + RL * l[4 * 3 + 1] * cos(G[0 * 3 + 2] * q[0 * 3 + 2]) * + (RL * phi[5 * 10 + 2] * + sin(G[0 * 3 + 2] * q[0 * 3 + 2]) + + RL * l[4 * 3 + 1] * phi[5 * 10 + 0] * + sin(G[0 * 3 + 2] * q[0 * 3 + 2])) + + RL * l[1 * 3 + 1] * cos(q[0 * 3 + 2]) * + (RL * phi[2 * 10 + 2] * sin(q[0 * 3 + 2]) + + RL * l[1 * 3 + 1] * phi[2 * 10 + 0] * + sin(q[0 * 3 + 2]))) + + l[0 * 3 + 2] * + (phi[1 * 10 + 3] + l[1 * 3 + 2] * phi[2 * 10 + 0] + + l[4 * 3 + 2] * phi[5 * 10 + 0] + + phi[5 * 10 + 3] * cos(G[0 * 3 + 2] * q[0 * 3 + 2]) - + phi[5 * 10 + 1] * sin(G[0 * 3 + 2] * q[0 * 3 + 2]) + + phi[2 * 10 + 3] * cos(q[0 * 3 + 2]) - + phi[2 * 10 + 1] * sin(q[0 * 3 + 2])) + + cos(q[0 * 3 + 1]) * + (phi[1 * 10 + 4] + + l[4 * 3 + 2] * (l[4 * 3 + 2] * phi[5 * 10 + 0] + + phi[5 * 10 + 3] * + cos(G[0 * 3 + 2] * q[0 * 3 + 2]) - + phi[5 * 10 + 1] * + sin(G[0 * 3 + 2] * q[0 * 3 + 2])) + + cos(G[0 * 3 + 2] * q[0 * 3 + 2]) * + (cos(G[0 * 3 + 2] * q[0 * 3 + 2]) * + (phi[5 * 10 + 4] - + phi[5 * 10 + 0] * + (1.0 / + (phi[5 * 10 + 0] * + phi[5 * 10 + 0]) * + (phi[5 * 10 + 3] * + phi[5 * 10 + 3]) + + (RL * RL) * 1.0 / + (phi[5 * 10 + 0] * + phi[5 * 10 + 0]) * + (phi[5 * 10 + 2] * + phi[5 * 10 + 2])) + + (phi[5 * 10 + 3] * phi[5 * 10 + 3]) / + phi[5 * 10 + 0] + + ((RL * RL) * + (phi[5 * 10 + 2] * phi[5 * 10 + 2])) / + phi[5 * 10 + 0]) + + l[4 * 3 + 2] * phi[5 * 10 + 3] + + phi[5 * 10 + 9] * + sin(G[0 * 3 + 2] * q[0 * 3 + 2]) + + (RL * RL) * l[4 * 3 + 1] * phi[5 * 10 + 2] * + cos(G[0 * 3 + 2] * q[0 * 3 + 2])) + + sin(G[0 * 3 + 2] * q[0 * 3 + 2]) * + (-l[4 * 3 + 2] * phi[5 * 10 + 1] + + sin(G[0 * 3 + 2] * q[0 * 3 + 2]) * + (phi[5 * 10 + 6] - + phi[5 * 10 + 0] * + (1.0 / + (phi[5 * 10 + 0] * + phi[5 * 10 + 0]) * + (phi[5 * 10 + 1] * + phi[5 * 10 + 1]) + + (RL * RL) * 1.0 / + (phi[5 * 10 + 0] * + phi[5 * 10 + 0]) * + (phi[5 * 10 + 2] * + phi[5 * 10 + 2])) + + (phi[5 * 10 + 1] * phi[5 * 10 + 1]) / + phi[5 * 10 + 0] + + ((RL * RL) * + (phi[5 * 10 + 2] * phi[5 * 10 + 2])) / + phi[5 * 10 + 0]) + + phi[5 * 10 + 9] * + cos(G[0 * 3 + 2] * q[0 * 3 + 2]) + + (RL * RL) * l[4 * 3 + 1] * phi[5 * 10 + 2] * + sin(G[0 * 3 + 2] * q[0 * 3 + 2])) + + l[1 * 3 + 2] * (l[1 * 3 + 2] * phi[2 * 10 + 0] + + phi[2 * 10 + 3] * cos(q[0 * 3 + 2]) - + phi[2 * 10 + 1] * sin(q[0 * 3 + 2])) + + cos(q[0 * 3 + 2]) * + (l[1 * 3 + 2] * phi[2 * 10 + 3] + + cos(q[0 * 3 + 2]) * + (phi[2 * 10 + 4] - + phi[2 * 10 + 0] * + (1.0 / + (phi[2 * 10 + 0] * + phi[2 * 10 + 0]) * + (phi[2 * 10 + 3] * + phi[2 * 10 + 3]) + + (RL * RL) * 1.0 / + (phi[2 * 10 + 0] * + phi[2 * 10 + 0]) * + (phi[2 * 10 + 2] * + phi[2 * 10 + 2])) + + (phi[2 * 10 + 3] * phi[2 * 10 + 3]) / + phi[2 * 10 + 0] + + ((RL * RL) * + (phi[2 * 10 + 2] * phi[2 * 10 + 2])) / + phi[2 * 10 + 0]) + + phi[2 * 10 + 9] * sin(q[0 * 3 + 2]) + + (RL * RL) * l[1 * 3 + 1] * phi[2 * 10 + 2] * + cos(q[0 * 3 + 2])) - + phi[1 * 10 + 0] * + (1.0 / (phi[1 * 10 + 0] * phi[1 * 10 + 0]) * + (phi[1 * 10 + 3] * phi[1 * 10 + 3]) + + (RL * RL) * 1.0 / + (phi[1 * 10 + 0] * phi[1 * 10 + 0]) * + (phi[1 * 10 + 2] * phi[1 * 10 + 2])) + + sin(q[0 * 3 + 2]) * + (-l[1 * 3 + 2] * phi[2 * 10 + 1] + + sin(q[0 * 3 + 2]) * + (phi[2 * 10 + 6] - + phi[2 * 10 + 0] * + (1.0 / + (phi[2 * 10 + 0] * + phi[2 * 10 + 0]) * + (phi[2 * 10 + 1] * + phi[2 * 10 + 1]) + + (RL * RL) * 1.0 / + (phi[2 * 10 + 0] * + phi[2 * 10 + 0]) * + (phi[2 * 10 + 2] * + phi[2 * 10 + 2])) + + (phi[2 * 10 + 1] * phi[2 * 10 + 1]) / + phi[2 * 10 + 0] + + ((RL * RL) * + (phi[2 * 10 + 2] * phi[2 * 10 + 2])) / + phi[2 * 10 + 0]) + + phi[2 * 10 + 9] * cos(q[0 * 3 + 2]) + + (RL * RL) * l[1 * 3 + 1] * phi[2 * 10 + 2] * + sin(q[0 * 3 + 2])) + + (phi[1 * 10 + 3] * phi[1 * 10 + 3]) / phi[1 * 10 + 0] + + ((RL * RL) * (phi[1 * 10 + 2] * phi[1 * 10 + 2])) / + phi[1 * 10 + 0] + + RL * l[4 * 3 + 1] * cos(G[0 * 3 + 2] * q[0 * 3 + 2]) * + (RL * phi[5 * 10 + 2] * + cos(G[0 * 3 + 2] * q[0 * 3 + 2]) + + RL * l[4 * 3 + 1] * phi[5 * 10 + 0] * + cos(G[0 * 3 + 2] * q[0 * 3 + 2])) + + RL * l[1 * 3 + 1] * cos(q[0 * 3 + 2]) * + (RL * phi[2 * 10 + 2] * cos(q[0 * 3 + 2]) + + RL * l[1 * 3 + 1] * phi[2 * 10 + 0] * + cos(q[0 * 3 + 2])) + + RL * l[4 * 3 + 1] * sin(G[0 * 3 + 2] * q[0 * 3 + 2]) * + (RL * phi[5 * 10 + 2] * + sin(G[0 * 3 + 2] * q[0 * 3 + 2]) + + RL * l[4 * 3 + 1] * phi[5 * 10 + 0] * + sin(G[0 * 3 + 2] * q[0 * 3 + 2])) + + RL * l[1 * 3 + 1] * sin(q[0 * 3 + 2]) * + (RL * phi[2 * 10 + 2] * sin(q[0 * 3 + 2]) + + RL * l[1 * 3 + 1] * phi[2 * 10 + 0] * + sin(q[0 * 3 + 2]))) + + RL * l[0 * 3 + 1] * cos(q[0 * 3 + 1]) * + (RL * phi[1 * 10 + 2] + + RL * phi[5 * 10 + 2] * + pow(sin(G[0 * 3 + 2] * q[0 * 3 + 2]), 2.0) + + RL * phi[2 * 10 + 2] * pow(cos(q[0 * 3 + 2]), 2.0) + + RL * phi[2 * 10 + 2] * pow(sin(q[0 * 3 + 2]), 2.0) + + RL * phi[5 * 10 + 2] * + pow(cos(G[0 * 3 + 2] * q[0 * 3 + 2]), 2.0) + + RL * l[4 * 3 + 1] * phi[5 * 10 + 0] * + pow(cos(G[0 * 3 + 2] * q[0 * 3 + 2]), 2.0) + + RL * l[4 * 3 + 1] * phi[5 * 10 + 0] * + pow(sin(G[0 * 3 + 2] * q[0 * 3 + 2]), 2.0) + + RL * l[1 * 3 + 1] * phi[2 * 10 + 0] * + pow(cos(q[0 * 3 + 2]), 2.0) + + RL * l[1 * 3 + 1] * phi[2 * 10 + 0] * + pow(sin(q[0 * 3 + 2]), 2.0)))) / + 2.0) + + G[0 * 3 + 1] * qd[0 * 3 + 0] * + (sin(G[0 * 3 + 1] * q[0 * 3 + 1]) * + (cos(G[0 * 3 + 1] * q[0 * 3 + 1]) * + (phi[4 * 10 + 6] - + phi[4 * 10 + 0] * + (1.0 / (phi[4 * 10 + 0] * phi[4 * 10 + 0]) * + (phi[4 * 10 + 1] * phi[4 * 10 + 1]) + + (RL * RL) * 1.0 / + (phi[4 * 10 + 0] * phi[4 * 10 + 0]) * + (phi[4 * 10 + 2] * phi[4 * 10 + 2])) + + (phi[4 * 10 + 1] * phi[4 * 10 + 1]) / + phi[4 * 10 + 0] + + ((RL * RL) * (phi[4 * 10 + 2] * phi[4 * 10 + 2])) / + phi[4 * 10 + 0]) - + phi[4 * 10 + 9] * sin(G[0 * 3 + 1] * q[0 * 3 + 1]) + + (RL * RL) * l[3 * 3 + 1] * phi[4 * 10 + 2] * + cos(G[0 * 3 + 1] * q[0 * 3 + 1])) * + (-1.0 / 2.0) + + (cos(G[0 * 3 + 1] * q[0 * 3 + 1]) * + (sin(G[0 * 3 + 1] * q[0 * 3 + 1]) * + (phi[4 * 10 + 4] - + phi[4 * 10 + 0] * + (1.0 / (phi[4 * 10 + 0] * phi[4 * 10 + 0]) * + (phi[4 * 10 + 3] * phi[4 * 10 + 3]) + + (RL * RL) * 1.0 / + (phi[4 * 10 + 0] * phi[4 * 10 + 0]) * + (phi[4 * 10 + 2] * phi[4 * 10 + 2])) + + (phi[4 * 10 + 3] * phi[4 * 10 + 3]) / phi[4 * 10 + 0] + + ((RL * RL) * (phi[4 * 10 + 2] * phi[4 * 10 + 2])) / + phi[4 * 10 + 0]) - + phi[4 * 10 + 9] * cos(G[0 * 3 + 1] * q[0 * 3 + 1]) + + (RL * RL) * l[3 * 3 + 1] * phi[4 * 10 + 2] * + sin(G[0 * 3 + 1] * q[0 * 3 + 1]))) / + 2.0 - + (cos(G[0 * 3 + 1] * q[0 * 3 + 1]) * + (-l[3 * 3 + 2] * phi[4 * 10 + 1] + + sin(G[0 * 3 + 1] * q[0 * 3 + 1]) * + (phi[4 * 10 + 6] - + phi[4 * 10 + 0] * + (1.0 / (phi[4 * 10 + 0] * phi[4 * 10 + 0]) * + (phi[4 * 10 + 1] * phi[4 * 10 + 1]) + + (RL * RL) * 1.0 / + (phi[4 * 10 + 0] * phi[4 * 10 + 0]) * + (phi[4 * 10 + 2] * phi[4 * 10 + 2])) + + (phi[4 * 10 + 1] * phi[4 * 10 + 1]) / phi[4 * 10 + 0] + + ((RL * RL) * (phi[4 * 10 + 2] * phi[4 * 10 + 2])) / + phi[4 * 10 + 0]) + + phi[4 * 10 + 9] * cos(G[0 * 3 + 1] * q[0 * 3 + 1]) + + (RL * RL) * l[3 * 3 + 1] * phi[4 * 10 + 2] * + sin(G[0 * 3 + 1] * q[0 * 3 + 1]))) / + 2.0 + + (sin(G[0 * 3 + 1] * q[0 * 3 + 1]) * + (cos(G[0 * 3 + 1] * q[0 * 3 + 1]) * + (phi[4 * 10 + 4] - + phi[4 * 10 + 0] * + (1.0 / (phi[4 * 10 + 0] * phi[4 * 10 + 0]) * + (phi[4 * 10 + 3] * phi[4 * 10 + 3]) + + (RL * RL) * 1.0 / + (phi[4 * 10 + 0] * phi[4 * 10 + 0]) * + (phi[4 * 10 + 2] * phi[4 * 10 + 2])) + + (phi[4 * 10 + 3] * phi[4 * 10 + 3]) / phi[4 * 10 + 0] + + ((RL * RL) * (phi[4 * 10 + 2] * phi[4 * 10 + 2])) / + phi[4 * 10 + 0]) + + l[3 * 3 + 2] * phi[4 * 10 + 3] + + phi[4 * 10 + 9] * sin(G[0 * 3 + 1] * q[0 * 3 + 1]) + + (RL * RL) * l[3 * 3 + 1] * phi[4 * 10 + 2] * + cos(G[0 * 3 + 1] * q[0 * 3 + 1]))) / + 2.0 + + (l[3 * 3 + 2] * + (phi[4 * 10 + 1] * cos(G[0 * 3 + 1] * q[0 * 3 + 1]) + + phi[4 * 10 + 3] * sin(G[0 * 3 + 1] * q[0 * 3 + 1]))) / + 2.0)) - + phi[0 * 10 + 3] * sin(q[0 * 3 + 0]) * (9.81E+2 / 1.0E+2) - + qd[0 * 3 + 2] * + (qd[0 * 3 + 0] * + ((l[0 * 3 + 2] * + (cos(q[0 * 3 + 1]) * (phi[2 * 10 + 1] * cos(q[0 * 3 + 2]) + + phi[2 * 10 + 3] * sin(q[0 * 3 + 2])) + + sin(q[0 * 3 + 1]) * + (phi[2 * 10 + 3] * cos(q[0 * 3 + 2]) - + phi[2 * 10 + 1] * sin(q[0 * 3 + 2])))) / + 2.0 + + (cos(q[0 * 3 + 1]) * + (cos(q[0 * 3 + 1]) * + (l[1 * 3 + 2] * (phi[2 * 10 + 1] * cos(q[0 * 3 + 2]) + + phi[2 * 10 + 3] * sin(q[0 * 3 + 2])) - + sin(q[0 * 3 + 2]) * + (cos(q[0 * 3 + 2]) * + (phi[2 * 10 + 6] - + phi[2 * 10 + 0] * + (1.0 / + (phi[2 * 10 + 0] * + phi[2 * 10 + 0]) * + (phi[2 * 10 + 1] * + phi[2 * 10 + 1]) + + (RL * RL) * 1.0 / + (phi[2 * 10 + 0] * + phi[2 * 10 + 0]) * + (phi[2 * 10 + 2] * + phi[2 * 10 + 2])) + + (phi[2 * 10 + 1] * phi[2 * 10 + 1]) / + phi[2 * 10 + 0] + + ((RL * RL) * + (phi[2 * 10 + 2] * phi[2 * 10 + 2])) / + phi[2 * 10 + 0]) - + phi[2 * 10 + 9] * sin(q[0 * 3 + 2]) + + (RL * RL) * l[1 * 3 + 1] * phi[2 * 10 + 2] * + cos(q[0 * 3 + 2])) + + cos(q[0 * 3 + 2]) * + (sin(q[0 * 3 + 2]) * + (phi[2 * 10 + 4] - + phi[2 * 10 + 0] * + (1.0 / + (phi[2 * 10 + 0] * + phi[2 * 10 + 0]) * + (phi[2 * 10 + 3] * + phi[2 * 10 + 3]) + + (RL * RL) * 1.0 / + (phi[2 * 10 + 0] * + phi[2 * 10 + 0]) * + (phi[2 * 10 + 2] * + phi[2 * 10 + 2])) + + (phi[2 * 10 + 3] * phi[2 * 10 + 3]) / + phi[2 * 10 + 0] + + ((RL * RL) * + (phi[2 * 10 + 2] * phi[2 * 10 + 2])) / + phi[2 * 10 + 0]) - + phi[2 * 10 + 9] * cos(q[0 * 3 + 2]) + + (RL * RL) * l[1 * 3 + 1] * phi[2 * 10 + 2] * + sin(q[0 * 3 + 2])) - + cos(q[0 * 3 + 2]) * + (-l[1 * 3 + 2] * phi[2 * 10 + 1] + + sin(q[0 * 3 + 2]) * + (phi[2 * 10 + 6] - + phi[2 * 10 + 0] * + (1.0 / + (phi[2 * 10 + 0] * + phi[2 * 10 + 0]) * + (phi[2 * 10 + 1] * + phi[2 * 10 + 1]) + + (RL * RL) * 1.0 / + (phi[2 * 10 + 0] * + phi[2 * 10 + 0]) * + (phi[2 * 10 + 2] * + phi[2 * 10 + 2])) + + (phi[2 * 10 + 1] * phi[2 * 10 + 1]) / + phi[2 * 10 + 0] + + ((RL * RL) * + (phi[2 * 10 + 2] * phi[2 * 10 + 2])) / + phi[2 * 10 + 0]) + + phi[2 * 10 + 9] * cos(q[0 * 3 + 2]) + + (RL * RL) * l[1 * 3 + 1] * phi[2 * 10 + 2] * + sin(q[0 * 3 + 2])) + + sin(q[0 * 3 + 2]) * + (l[1 * 3 + 2] * phi[2 * 10 + 3] + + cos(q[0 * 3 + 2]) * + (phi[2 * 10 + 4] - + phi[2 * 10 + 0] * + (1.0 / + (phi[2 * 10 + 0] * + phi[2 * 10 + 0]) * + (phi[2 * 10 + 3] * + phi[2 * 10 + 3]) + + (RL * RL) * 1.0 / + (phi[2 * 10 + 0] * + phi[2 * 10 + 0]) * + (phi[2 * 10 + 2] * + phi[2 * 10 + 2])) + + (phi[2 * 10 + 3] * phi[2 * 10 + 3]) / + phi[2 * 10 + 0] + + ((RL * RL) * + (phi[2 * 10 + 2] * phi[2 * 10 + 2])) / + phi[2 * 10 + 0]) + + phi[2 * 10 + 9] * sin(q[0 * 3 + 2]) + + (RL * RL) * l[1 * 3 + 1] * phi[2 * 10 + 2] * + cos(q[0 * 3 + 2]))) + + sin(q[0 * 3 + 1]) * + (l[1 * 3 + 2] * (phi[2 * 10 + 3] * cos(q[0 * 3 + 2]) - + phi[2 * 10 + 1] * sin(q[0 * 3 + 2])) + + cos(q[0 * 3 + 2]) * + (cos(q[0 * 3 + 2]) * + (phi[2 * 10 + 4] - + phi[2 * 10 + 0] * + (1.0 / + (phi[2 * 10 + 0] * + phi[2 * 10 + 0]) * + (phi[2 * 10 + 3] * + phi[2 * 10 + 3]) + + (RL * RL) * 1.0 / + (phi[2 * 10 + 0] * + phi[2 * 10 + 0]) * + (phi[2 * 10 + 2] * + phi[2 * 10 + 2])) + + (phi[2 * 10 + 3] * phi[2 * 10 + 3]) / + phi[2 * 10 + 0] + + ((RL * RL) * + (phi[2 * 10 + 2] * phi[2 * 10 + 2])) / + phi[2 * 10 + 0]) + + phi[2 * 10 + 9] * sin(q[0 * 3 + 2]) + + (RL * RL) * l[1 * 3 + 1] * phi[2 * 10 + 2] * + cos(q[0 * 3 + 2])) + + sin(q[0 * 3 + 2]) * + (sin(q[0 * 3 + 2]) * + (phi[2 * 10 + 6] - + phi[2 * 10 + 0] * + (1.0 / + (phi[2 * 10 + 0] * + phi[2 * 10 + 0]) * + (phi[2 * 10 + 1] * + phi[2 * 10 + 1]) + + (RL * RL) * 1.0 / + (phi[2 * 10 + 0] * + phi[2 * 10 + 0]) * + (phi[2 * 10 + 2] * + phi[2 * 10 + 2])) + + (phi[2 * 10 + 1] * phi[2 * 10 + 1]) / + phi[2 * 10 + 0] + + ((RL * RL) * + (phi[2 * 10 + 2] * phi[2 * 10 + 2])) / + phi[2 * 10 + 0]) + + phi[2 * 10 + 9] * cos(q[0 * 3 + 2]) + + (RL * RL) * l[1 * 3 + 1] * phi[2 * 10 + 2] * + sin(q[0 * 3 + 2])) - + cos(q[0 * 3 + 2]) * + (l[1 * 3 + 0] * phi[2 * 10 + 1] + + cos(q[0 * 3 + 2]) * + (phi[2 * 10 + 6] - + phi[2 * 10 + 0] * + (1.0 / + (phi[2 * 10 + 0] * + phi[2 * 10 + 0]) * + (phi[2 * 10 + 1] * + phi[2 * 10 + 1]) + + (RL * RL) * 1.0 / + (phi[2 * 10 + 0] * + phi[2 * 10 + 0]) * + (phi[2 * 10 + 2] * + phi[2 * 10 + 2])) + + (phi[2 * 10 + 1] * phi[2 * 10 + 1]) / + phi[2 * 10 + 0] + + ((RL * RL) * + (phi[2 * 10 + 2] * phi[2 * 10 + 2])) / + phi[2 * 10 + 0]) - + phi[2 * 10 + 9] * sin(q[0 * 3 + 2]) + + (RL * RL) * l[1 * 3 + 1] * phi[2 * 10 + 2] * + cos(q[0 * 3 + 2])) - + sin(q[0 * 3 + 2]) * + (l[1 * 3 + 0] * phi[2 * 10 + 3] + + sin(q[0 * 3 + 2]) * + (phi[2 * 10 + 4] - + phi[2 * 10 + 0] * + (1.0 / + (phi[2 * 10 + 0] * + phi[2 * 10 + 0]) * + (phi[2 * 10 + 3] * + phi[2 * 10 + 3]) + + (RL * RL) * 1.0 / + (phi[2 * 10 + 0] * + phi[2 * 10 + 0]) * + (phi[2 * 10 + 2] * + phi[2 * 10 + 2])) + + (phi[2 * 10 + 3] * phi[2 * 10 + 3]) / + phi[2 * 10 + 0] + + ((RL * RL) * + (phi[2 * 10 + 2] * phi[2 * 10 + 2])) / + phi[2 * 10 + 0]) - + phi[2 * 10 + 9] * cos(q[0 * 3 + 2]) + + (RL * RL) * l[1 * 3 + 1] * phi[2 * 10 + 2] * + sin(q[0 * 3 + 2]))) + + l[0 * 3 + 2] * (phi[2 * 10 + 1] * cos(q[0 * 3 + 2]) + + phi[2 * 10 + 3] * sin(q[0 * 3 + 2])))) / + 2.0 - + (sin(q[0 * 3 + 1]) * + (cos(q[0 * 3 + 1]) * + (l[1 * 3 + 0] * (phi[2 * 10 + 1] * cos(q[0 * 3 + 2]) + + phi[2 * 10 + 3] * sin(q[0 * 3 + 2])) + + cos(q[0 * 3 + 2]) * + (cos(q[0 * 3 + 2]) * + (phi[2 * 10 + 6] - + phi[2 * 10 + 0] * + (1.0 / + (phi[2 * 10 + 0] * + phi[2 * 10 + 0]) * + (phi[2 * 10 + 1] * + phi[2 * 10 + 1]) + + (RL * RL) * 1.0 / + (phi[2 * 10 + 0] * + phi[2 * 10 + 0]) * + (phi[2 * 10 + 2] * + phi[2 * 10 + 2])) + + (phi[2 * 10 + 1] * phi[2 * 10 + 1]) / + phi[2 * 10 + 0] + + ((RL * RL) * + (phi[2 * 10 + 2] * phi[2 * 10 + 2])) / + phi[2 * 10 + 0]) - + phi[2 * 10 + 9] * sin(q[0 * 3 + 2]) + + (RL * RL) * l[1 * 3 + 1] * phi[2 * 10 + 2] * + cos(q[0 * 3 + 2])) + + sin(q[0 * 3 + 2]) * + (sin(q[0 * 3 + 2]) * + (phi[2 * 10 + 4] - + phi[2 * 10 + 0] * + (1.0 / + (phi[2 * 10 + 0] * + phi[2 * 10 + 0]) * + (phi[2 * 10 + 3] * + phi[2 * 10 + 3]) + + (RL * RL) * 1.0 / + (phi[2 * 10 + 0] * + phi[2 * 10 + 0]) * + (phi[2 * 10 + 2] * + phi[2 * 10 + 2])) + + (phi[2 * 10 + 3] * phi[2 * 10 + 3]) / + phi[2 * 10 + 0] + + ((RL * RL) * + (phi[2 * 10 + 2] * phi[2 * 10 + 2])) / + phi[2 * 10 + 0]) - + phi[2 * 10 + 9] * cos(q[0 * 3 + 2]) + + (RL * RL) * l[1 * 3 + 1] * phi[2 * 10 + 2] * + sin(q[0 * 3 + 2])) - + cos(q[0 * 3 + 2]) * + (l[1 * 3 + 2] * phi[2 * 10 + 3] + + cos(q[0 * 3 + 2]) * + (phi[2 * 10 + 4] - + phi[2 * 10 + 0] * + (1.0 / + (phi[2 * 10 + 0] * + phi[2 * 10 + 0]) * + (phi[2 * 10 + 3] * + phi[2 * 10 + 3]) + + (RL * RL) * 1.0 / + (phi[2 * 10 + 0] * + phi[2 * 10 + 0]) * + (phi[2 * 10 + 2] * + phi[2 * 10 + 2])) + + (phi[2 * 10 + 3] * phi[2 * 10 + 3]) / + phi[2 * 10 + 0] + + ((RL * RL) * + (phi[2 * 10 + 2] * phi[2 * 10 + 2])) / + phi[2 * 10 + 0]) + + phi[2 * 10 + 9] * sin(q[0 * 3 + 2]) + + (RL * RL) * l[1 * 3 + 1] * phi[2 * 10 + 2] * + cos(q[0 * 3 + 2])) - + sin(q[0 * 3 + 2]) * + (-l[1 * 3 + 2] * phi[2 * 10 + 1] + + sin(q[0 * 3 + 2]) * + (phi[2 * 10 + 6] - + phi[2 * 10 + 0] * + (1.0 / + (phi[2 * 10 + 0] * + phi[2 * 10 + 0]) * + (phi[2 * 10 + 1] * + phi[2 * 10 + 1]) + + (RL * RL) * 1.0 / + (phi[2 * 10 + 0] * + phi[2 * 10 + 0]) * + (phi[2 * 10 + 2] * + phi[2 * 10 + 2])) + + (phi[2 * 10 + 1] * phi[2 * 10 + 1]) / + phi[2 * 10 + 0] + + ((RL * RL) * + (phi[2 * 10 + 2] * phi[2 * 10 + 2])) / + phi[2 * 10 + 0]) + + phi[2 * 10 + 9] * cos(q[0 * 3 + 2]) + + (RL * RL) * l[1 * 3 + 1] * phi[2 * 10 + 2] * + sin(q[0 * 3 + 2]))) + + sin(q[0 * 3 + 1]) * + (l[1 * 3 + 0] * (phi[2 * 10 + 3] * cos(q[0 * 3 + 2]) - + phi[2 * 10 + 1] * sin(q[0 * 3 + 2])) - + cos(q[0 * 3 + 2]) * + (sin(q[0 * 3 + 2]) * + (phi[2 * 10 + 6] - + phi[2 * 10 + 0] * + (1.0 / + (phi[2 * 10 + 0] * + phi[2 * 10 + 0]) * + (phi[2 * 10 + 1] * + phi[2 * 10 + 1]) + + (RL * RL) * 1.0 / + (phi[2 * 10 + 0] * + phi[2 * 10 + 0]) * + (phi[2 * 10 + 2] * + phi[2 * 10 + 2])) + + (phi[2 * 10 + 1] * phi[2 * 10 + 1]) / + phi[2 * 10 + 0] + + ((RL * RL) * + (phi[2 * 10 + 2] * phi[2 * 10 + 2])) / + phi[2 * 10 + 0]) + + phi[2 * 10 + 9] * cos(q[0 * 3 + 2]) + + (RL * RL) * l[1 * 3 + 1] * phi[2 * 10 + 2] * + sin(q[0 * 3 + 2])) + + sin(q[0 * 3 + 2]) * + (cos(q[0 * 3 + 2]) * + (phi[2 * 10 + 4] - + phi[2 * 10 + 0] * + (1.0 / + (phi[2 * 10 + 0] * + phi[2 * 10 + 0]) * + (phi[2 * 10 + 3] * + phi[2 * 10 + 3]) + + (RL * RL) * 1.0 / + (phi[2 * 10 + 0] * + phi[2 * 10 + 0]) * + (phi[2 * 10 + 2] * + phi[2 * 10 + 2])) + + (phi[2 * 10 + 3] * phi[2 * 10 + 3]) / + phi[2 * 10 + 0] + + ((RL * RL) * + (phi[2 * 10 + 2] * phi[2 * 10 + 2])) / + phi[2 * 10 + 0]) + + phi[2 * 10 + 9] * sin(q[0 * 3 + 2]) + + (RL * RL) * l[1 * 3 + 1] * phi[2 * 10 + 2] * + cos(q[0 * 3 + 2])) - + sin(q[0 * 3 + 2]) * + (l[1 * 3 + 0] * phi[2 * 10 + 1] + + cos(q[0 * 3 + 2]) * + (phi[2 * 10 + 6] - + phi[2 * 10 + 0] * + (1.0 / + (phi[2 * 10 + 0] * + phi[2 * 10 + 0]) * + (phi[2 * 10 + 1] * + phi[2 * 10 + 1]) + + (RL * RL) * 1.0 / + (phi[2 * 10 + 0] * + phi[2 * 10 + 0]) * + (phi[2 * 10 + 2] * + phi[2 * 10 + 2])) + + (phi[2 * 10 + 1] * phi[2 * 10 + 1]) / + phi[2 * 10 + 0] + + ((RL * RL) * + (phi[2 * 10 + 2] * phi[2 * 10 + 2])) / + phi[2 * 10 + 0]) - + phi[2 * 10 + 9] * sin(q[0 * 3 + 2]) + + (RL * RL) * l[1 * 3 + 1] * phi[2 * 10 + 2] * + cos(q[0 * 3 + 2])) + + cos(q[0 * 3 + 2]) * + (l[1 * 3 + 0] * phi[2 * 10 + 3] + + sin(q[0 * 3 + 2]) * + (phi[2 * 10 + 4] - + phi[2 * 10 + 0] * + (1.0 / + (phi[2 * 10 + 0] * + phi[2 * 10 + 0]) * + (phi[2 * 10 + 3] * + phi[2 * 10 + 3]) + + (RL * RL) * 1.0 / + (phi[2 * 10 + 0] * + phi[2 * 10 + 0]) * + (phi[2 * 10 + 2] * + phi[2 * 10 + 2])) + + (phi[2 * 10 + 3] * phi[2 * 10 + 3]) / + phi[2 * 10 + 0] + + ((RL * RL) * + (phi[2 * 10 + 2] * phi[2 * 10 + 2])) / + phi[2 * 10 + 0]) - + phi[2 * 10 + 9] * cos(q[0 * 3 + 2]) + + (RL * RL) * l[1 * 3 + 1] * phi[2 * 10 + 2] * + sin(q[0 * 3 + 2]))) - + l[0 * 3 + 2] * (phi[2 * 10 + 3] * cos(q[0 * 3 + 2]) - + phi[2 * 10 + 1] * sin(q[0 * 3 + 2])))) / + 2.0) + + G[0 * 3 + 2] * qd[0 * 3 + 0] * + ((cos(q[0 * 3 + 1]) * + (sin(q[0 * 3 + 1]) * + (cos(G[0 * 3 + 2] * q[0 * 3 + 2]) * + (cos(G[0 * 3 + 2] * q[0 * 3 + 2]) * + (phi[5 * 10 + 4] - + phi[5 * 10 + 0] * + (1.0 / + (phi[5 * 10 + 0] * + phi[5 * 10 + 0]) * + (phi[5 * 10 + 3] * + phi[5 * 10 + 3]) + + (RL * RL) * 1.0 / + (phi[5 * 10 + 0] * + phi[5 * 10 + 0]) * + (phi[5 * 10 + 2] * + phi[5 * 10 + 2])) + + (phi[5 * 10 + 3] * phi[5 * 10 + 3]) / + phi[5 * 10 + 0] + + ((RL * RL) * + (phi[5 * 10 + 2] * phi[5 * 10 + 2])) / + phi[5 * 10 + 0]) + + phi[5 * 10 + 9] * + sin(G[0 * 3 + 2] * q[0 * 3 + 2]) + + (RL * RL) * l[4 * 3 + 1] * phi[5 * 10 + 2] * + cos(G[0 * 3 + 2] * q[0 * 3 + 2])) + + sin(G[0 * 3 + 2] * q[0 * 3 + 2]) * + (sin(G[0 * 3 + 2] * q[0 * 3 + 2]) * + (phi[5 * 10 + 6] - + phi[5 * 10 + 0] * + (1.0 / + (phi[5 * 10 + 0] * + phi[5 * 10 + 0]) * + (phi[5 * 10 + 1] * + phi[5 * 10 + 1]) + + (RL * RL) * 1.0 / + (phi[5 * 10 + 0] * + phi[5 * 10 + 0]) * + (phi[5 * 10 + 2] * + phi[5 * 10 + 2])) + + (phi[5 * 10 + 1] * phi[5 * 10 + 1]) / + phi[5 * 10 + 0] + + ((RL * RL) * + (phi[5 * 10 + 2] * phi[5 * 10 + 2])) / + phi[5 * 10 + 0]) + + phi[5 * 10 + 9] * + cos(G[0 * 3 + 2] * q[0 * 3 + 2]) + + (RL * RL) * l[4 * 3 + 1] * phi[5 * 10 + 2] * + sin(G[0 * 3 + 2] * q[0 * 3 + 2])) - + cos(G[0 * 3 + 2] * q[0 * 3 + 2]) * + (cos(G[0 * 3 + 2] * q[0 * 3 + 2]) * + (phi[5 * 10 + 6] - + phi[5 * 10 + 0] * + (1.0 / + (phi[5 * 10 + 0] * + phi[5 * 10 + 0]) * + (phi[5 * 10 + 1] * + phi[5 * 10 + 1]) + + (RL * RL) * 1.0 / + (phi[5 * 10 + 0] * + phi[5 * 10 + 0]) * + (phi[5 * 10 + 2] * + phi[5 * 10 + 2])) + + (phi[5 * 10 + 1] * phi[5 * 10 + 1]) / + phi[5 * 10 + 0] + + ((RL * RL) * + (phi[5 * 10 + 2] * phi[5 * 10 + 2])) / + phi[5 * 10 + 0]) + + l[4 * 3 + 0] * phi[5 * 10 + 1] - + phi[5 * 10 + 9] * + sin(G[0 * 3 + 2] * q[0 * 3 + 2]) + + (RL * RL) * l[4 * 3 + 1] * phi[5 * 10 + 2] * + cos(G[0 * 3 + 2] * q[0 * 3 + 2])) - + sin(G[0 * 3 + 2] * q[0 * 3 + 2]) * + (l[4 * 3 + 0] * phi[5 * 10 + 3] + + sin(G[0 * 3 + 2] * q[0 * 3 + 2]) * + (phi[5 * 10 + 4] - + phi[5 * 10 + 0] * + (1.0 / + (phi[5 * 10 + 0] * + phi[5 * 10 + 0]) * + (phi[5 * 10 + 3] * + phi[5 * 10 + 3]) + + (RL * RL) * 1.0 / + (phi[5 * 10 + 0] * + phi[5 * 10 + 0]) * + (phi[5 * 10 + 2] * + phi[5 * 10 + 2])) + + (phi[5 * 10 + 3] * phi[5 * 10 + 3]) / + phi[5 * 10 + 0] + + ((RL * RL) * + (phi[5 * 10 + 2] * phi[5 * 10 + 2])) / + phi[5 * 10 + 0]) - + phi[5 * 10 + 9] * + cos(G[0 * 3 + 2] * q[0 * 3 + 2]) + + (RL * RL) * l[4 * 3 + 1] * phi[5 * 10 + 2] * + sin(G[0 * 3 + 2] * q[0 * 3 + 2])) + + l[4 * 3 + 2] * (phi[5 * 10 + 3] * + cos(G[0 * 3 + 2] * q[0 * 3 + 2]) - + phi[5 * 10 + 1] * sin(G[0 * 3 + 2] * + q[0 * 3 + 2]))) + + l[0 * 3 + 2] * + (phi[5 * 10 + 1] * cos(G[0 * 3 + 2] * q[0 * 3 + 2]) + + phi[5 * 10 + 3] * sin(G[0 * 3 + 2] * q[0 * 3 + 2])) + + cos(q[0 * 3 + 1]) * + (-sin(G[0 * 3 + 2] * q[0 * 3 + 2]) * + (cos(G[0 * 3 + 2] * q[0 * 3 + 2]) * + (phi[5 * 10 + 6] - + phi[5 * 10 + 0] * + (1.0 / + (phi[5 * 10 + 0] * + phi[5 * 10 + 0]) * + (phi[5 * 10 + 1] * + phi[5 * 10 + 1]) + + (RL * RL) * 1.0 / + (phi[5 * 10 + 0] * + phi[5 * 10 + 0]) * + (phi[5 * 10 + 2] * + phi[5 * 10 + 2])) + + (phi[5 * 10 + 1] * phi[5 * 10 + 1]) / + phi[5 * 10 + 0] + + ((RL * RL) * + (phi[5 * 10 + 2] * phi[5 * 10 + 2])) / + phi[5 * 10 + 0]) - + phi[5 * 10 + 9] * + sin(G[0 * 3 + 2] * q[0 * 3 + 2]) + + (RL * RL) * l[4 * 3 + 1] * phi[5 * 10 + 2] * + cos(G[0 * 3 + 2] * q[0 * 3 + 2])) + + cos(G[0 * 3 + 2] * q[0 * 3 + 2]) * + (sin(G[0 * 3 + 2] * q[0 * 3 + 2]) * + (phi[5 * 10 + 4] - + phi[5 * 10 + 0] * + (1.0 / + (phi[5 * 10 + 0] * + phi[5 * 10 + 0]) * + (phi[5 * 10 + 3] * + phi[5 * 10 + 3]) + + (RL * RL) * 1.0 / + (phi[5 * 10 + 0] * + phi[5 * 10 + 0]) * + (phi[5 * 10 + 2] * + phi[5 * 10 + 2])) + + (phi[5 * 10 + 3] * phi[5 * 10 + 3]) / + phi[5 * 10 + 0] + + ((RL * RL) * + (phi[5 * 10 + 2] * phi[5 * 10 + 2])) / + phi[5 * 10 + 0]) - + phi[5 * 10 + 9] * + cos(G[0 * 3 + 2] * q[0 * 3 + 2]) + + (RL * RL) * l[4 * 3 + 1] * phi[5 * 10 + 2] * + sin(G[0 * 3 + 2] * q[0 * 3 + 2])) - + cos(G[0 * 3 + 2] * q[0 * 3 + 2]) * + (-l[4 * 3 + 2] * phi[5 * 10 + 1] + + sin(G[0 * 3 + 2] * q[0 * 3 + 2]) * + (phi[5 * 10 + 6] - + phi[5 * 10 + 0] * + (1.0 / + (phi[5 * 10 + 0] * + phi[5 * 10 + 0]) * + (phi[5 * 10 + 1] * + phi[5 * 10 + 1]) + + (RL * RL) * 1.0 / + (phi[5 * 10 + 0] * + phi[5 * 10 + 0]) * + (phi[5 * 10 + 2] * + phi[5 * 10 + 2])) + + (phi[5 * 10 + 1] * phi[5 * 10 + 1]) / + phi[5 * 10 + 0] + + ((RL * RL) * + (phi[5 * 10 + 2] * phi[5 * 10 + 2])) / + phi[5 * 10 + 0]) + + phi[5 * 10 + 9] * + cos(G[0 * 3 + 2] * q[0 * 3 + 2]) + + (RL * RL) * l[4 * 3 + 1] * phi[5 * 10 + 2] * + sin(G[0 * 3 + 2] * q[0 * 3 + 2])) + + sin(G[0 * 3 + 2] * q[0 * 3 + 2]) * + (cos(G[0 * 3 + 2] * q[0 * 3 + 2]) * + (phi[5 * 10 + 4] - + phi[5 * 10 + 0] * + (1.0 / + (phi[5 * 10 + 0] * + phi[5 * 10 + 0]) * + (phi[5 * 10 + 3] * + phi[5 * 10 + 3]) + + (RL * RL) * 1.0 / + (phi[5 * 10 + 0] * + phi[5 * 10 + 0]) * + (phi[5 * 10 + 2] * + phi[5 * 10 + 2])) + + (phi[5 * 10 + 3] * phi[5 * 10 + 3]) / + phi[5 * 10 + 0] + + ((RL * RL) * + (phi[5 * 10 + 2] * phi[5 * 10 + 2])) / + phi[5 * 10 + 0]) + + l[4 * 3 + 2] * phi[5 * 10 + 3] + + phi[5 * 10 + 9] * + sin(G[0 * 3 + 2] * q[0 * 3 + 2]) + + (RL * RL) * l[4 * 3 + 1] * phi[5 * 10 + 2] * + cos(G[0 * 3 + 2] * q[0 * 3 + 2])) + + l[4 * 3 + 2] * + (phi[5 * 10 + 1] * + cos(G[0 * 3 + 2] * q[0 * 3 + 2]) + + phi[5 * 10 + 3] * + sin(G[0 * 3 + 2] * q[0 * 3 + 2]))))) / + 2.0 - + (sin(q[0 * 3 + 1]) * + (sin(q[0 * 3 + 1]) * + (-cos(G[0 * 3 + 2] * q[0 * 3 + 2]) * + (sin(G[0 * 3 + 2] * q[0 * 3 + 2]) * + (phi[5 * 10 + 6] - + phi[5 * 10 + 0] * + (1.0 / + (phi[5 * 10 + 0] * + phi[5 * 10 + 0]) * + (phi[5 * 10 + 1] * + phi[5 * 10 + 1]) + + (RL * RL) * 1.0 / + (phi[5 * 10 + 0] * + phi[5 * 10 + 0]) * + (phi[5 * 10 + 2] * + phi[5 * 10 + 2])) + + (phi[5 * 10 + 1] * phi[5 * 10 + 1]) / + phi[5 * 10 + 0] + + ((RL * RL) * + (phi[5 * 10 + 2] * phi[5 * 10 + 2])) / + phi[5 * 10 + 0]) + + phi[5 * 10 + 9] * + cos(G[0 * 3 + 2] * q[0 * 3 + 2]) + + (RL * RL) * l[4 * 3 + 1] * phi[5 * 10 + 2] * + sin(G[0 * 3 + 2] * q[0 * 3 + 2])) + + sin(G[0 * 3 + 2] * q[0 * 3 + 2]) * + (cos(G[0 * 3 + 2] * q[0 * 3 + 2]) * + (phi[5 * 10 + 4] - + phi[5 * 10 + 0] * + (1.0 / + (phi[5 * 10 + 0] * + phi[5 * 10 + 0]) * + (phi[5 * 10 + 3] * + phi[5 * 10 + 3]) + + (RL * RL) * 1.0 / + (phi[5 * 10 + 0] * + phi[5 * 10 + 0]) * + (phi[5 * 10 + 2] * + phi[5 * 10 + 2])) + + (phi[5 * 10 + 3] * phi[5 * 10 + 3]) / + phi[5 * 10 + 0] + + ((RL * RL) * + (phi[5 * 10 + 2] * phi[5 * 10 + 2])) / + phi[5 * 10 + 0]) + + phi[5 * 10 + 9] * + sin(G[0 * 3 + 2] * q[0 * 3 + 2]) + + (RL * RL) * l[4 * 3 + 1] * phi[5 * 10 + 2] * + cos(G[0 * 3 + 2] * q[0 * 3 + 2])) - + sin(G[0 * 3 + 2] * q[0 * 3 + 2]) * + (cos(G[0 * 3 + 2] * q[0 * 3 + 2]) * + (phi[5 * 10 + 6] - + phi[5 * 10 + 0] * + (1.0 / + (phi[5 * 10 + 0] * + phi[5 * 10 + 0]) * + (phi[5 * 10 + 1] * + phi[5 * 10 + 1]) + + (RL * RL) * 1.0 / + (phi[5 * 10 + 0] * + phi[5 * 10 + 0]) * + (phi[5 * 10 + 2] * + phi[5 * 10 + 2])) + + (phi[5 * 10 + 1] * phi[5 * 10 + 1]) / + phi[5 * 10 + 0] + + ((RL * RL) * + (phi[5 * 10 + 2] * phi[5 * 10 + 2])) / + phi[5 * 10 + 0]) + + l[4 * 3 + 0] * phi[5 * 10 + 1] - + phi[5 * 10 + 9] * + sin(G[0 * 3 + 2] * q[0 * 3 + 2]) + + (RL * RL) * l[4 * 3 + 1] * phi[5 * 10 + 2] * + cos(G[0 * 3 + 2] * q[0 * 3 + 2])) + + cos(G[0 * 3 + 2] * q[0 * 3 + 2]) * + (l[4 * 3 + 0] * phi[5 * 10 + 3] + + sin(G[0 * 3 + 2] * q[0 * 3 + 2]) * + (phi[5 * 10 + 4] - + phi[5 * 10 + 0] * + (1.0 / + (phi[5 * 10 + 0] * + phi[5 * 10 + 0]) * + (phi[5 * 10 + 3] * + phi[5 * 10 + 3]) + + (RL * RL) * 1.0 / + (phi[5 * 10 + 0] * + phi[5 * 10 + 0]) * + (phi[5 * 10 + 2] * + phi[5 * 10 + 2])) + + (phi[5 * 10 + 3] * phi[5 * 10 + 3]) / + phi[5 * 10 + 0] + + ((RL * RL) * + (phi[5 * 10 + 2] * phi[5 * 10 + 2])) / + phi[5 * 10 + 0]) - + phi[5 * 10 + 9] * + cos(G[0 * 3 + 2] * q[0 * 3 + 2]) + + (RL * RL) * l[4 * 3 + 1] * phi[5 * 10 + 2] * + sin(G[0 * 3 + 2] * q[0 * 3 + 2])) + + l[4 * 3 + 0] * (phi[5 * 10 + 3] * + cos(G[0 * 3 + 2] * q[0 * 3 + 2]) - + phi[5 * 10 + 1] * sin(G[0 * 3 + 2] * + q[0 * 3 + 2]))) - + l[0 * 3 + 2] * + (phi[5 * 10 + 3] * cos(G[0 * 3 + 2] * q[0 * 3 + 2]) - + phi[5 * 10 + 1] * sin(G[0 * 3 + 2] * q[0 * 3 + 2])) + + cos(q[0 * 3 + 1]) * + (cos(G[0 * 3 + 2] * q[0 * 3 + 2]) * + (cos(G[0 * 3 + 2] * q[0 * 3 + 2]) * + (phi[5 * 10 + 6] - + phi[5 * 10 + 0] * + (1.0 / + (phi[5 * 10 + 0] * + phi[5 * 10 + 0]) * + (phi[5 * 10 + 1] * + phi[5 * 10 + 1]) + + (RL * RL) * 1.0 / + (phi[5 * 10 + 0] * + phi[5 * 10 + 0]) * + (phi[5 * 10 + 2] * + phi[5 * 10 + 2])) + + (phi[5 * 10 + 1] * phi[5 * 10 + 1]) / + phi[5 * 10 + 0] + + ((RL * RL) * + (phi[5 * 10 + 2] * phi[5 * 10 + 2])) / + phi[5 * 10 + 0]) - + phi[5 * 10 + 9] * + sin(G[0 * 3 + 2] * q[0 * 3 + 2]) + + (RL * RL) * l[4 * 3 + 1] * phi[5 * 10 + 2] * + cos(G[0 * 3 + 2] * q[0 * 3 + 2])) + + sin(G[0 * 3 + 2] * q[0 * 3 + 2]) * + (sin(G[0 * 3 + 2] * q[0 * 3 + 2]) * + (phi[5 * 10 + 4] - + phi[5 * 10 + 0] * + (1.0 / + (phi[5 * 10 + 0] * + phi[5 * 10 + 0]) * + (phi[5 * 10 + 3] * + phi[5 * 10 + 3]) + + (RL * RL) * 1.0 / + (phi[5 * 10 + 0] * + phi[5 * 10 + 0]) * + (phi[5 * 10 + 2] * + phi[5 * 10 + 2])) + + (phi[5 * 10 + 3] * phi[5 * 10 + 3]) / + phi[5 * 10 + 0] + + ((RL * RL) * + (phi[5 * 10 + 2] * phi[5 * 10 + 2])) / + phi[5 * 10 + 0]) - + phi[5 * 10 + 9] * + cos(G[0 * 3 + 2] * q[0 * 3 + 2]) + + (RL * RL) * l[4 * 3 + 1] * phi[5 * 10 + 2] * + sin(G[0 * 3 + 2] * q[0 * 3 + 2])) - + cos(G[0 * 3 + 2] * q[0 * 3 + 2]) * + (cos(G[0 * 3 + 2] * q[0 * 3 + 2]) * + (phi[5 * 10 + 4] - + phi[5 * 10 + 0] * + (1.0 / + (phi[5 * 10 + 0] * + phi[5 * 10 + 0]) * + (phi[5 * 10 + 3] * + phi[5 * 10 + 3]) + + (RL * RL) * 1.0 / + (phi[5 * 10 + 0] * + phi[5 * 10 + 0]) * + (phi[5 * 10 + 2] * + phi[5 * 10 + 2])) + + (phi[5 * 10 + 3] * phi[5 * 10 + 3]) / + phi[5 * 10 + 0] + + ((RL * RL) * + (phi[5 * 10 + 2] * phi[5 * 10 + 2])) / + phi[5 * 10 + 0]) + + l[4 * 3 + 2] * phi[5 * 10 + 3] + + phi[5 * 10 + 9] * + sin(G[0 * 3 + 2] * q[0 * 3 + 2]) + + (RL * RL) * l[4 * 3 + 1] * phi[5 * 10 + 2] * + cos(G[0 * 3 + 2] * q[0 * 3 + 2])) - + sin(G[0 * 3 + 2] * q[0 * 3 + 2]) * + (-l[4 * 3 + 2] * phi[5 * 10 + 1] + + sin(G[0 * 3 + 2] * q[0 * 3 + 2]) * + (phi[5 * 10 + 6] - + phi[5 * 10 + 0] * + (1.0 / + (phi[5 * 10 + 0] * + phi[5 * 10 + 0]) * + (phi[5 * 10 + 1] * + phi[5 * 10 + 1]) + + (RL * RL) * 1.0 / + (phi[5 * 10 + 0] * + phi[5 * 10 + 0]) * + (phi[5 * 10 + 2] * + phi[5 * 10 + 2])) + + (phi[5 * 10 + 1] * phi[5 * 10 + 1]) / + phi[5 * 10 + 0] + + ((RL * RL) * + (phi[5 * 10 + 2] * phi[5 * 10 + 2])) / + phi[5 * 10 + 0]) + + phi[5 * 10 + 9] * + cos(G[0 * 3 + 2] * q[0 * 3 + 2]) + + (RL * RL) * l[4 * 3 + 1] * phi[5 * 10 + 2] * + sin(G[0 * 3 + 2] * q[0 * 3 + 2])) + + l[4 * 3 + 0] * + (phi[5 * 10 + 1] * + cos(G[0 * 3 + 2] * q[0 * 3 + 2]) + + phi[5 * 10 + 3] * + sin(G[0 * 3 + 2] * q[0 * 3 + 2]))))) / + 2.0 + + (l[0 * 3 + 2] * + (cos(q[0 * 3 + 1]) * + (phi[5 * 10 + 1] * cos(G[0 * 3 + 2] * q[0 * 3 + 2]) + + phi[5 * 10 + 3] * sin(G[0 * 3 + 2] * q[0 * 3 + 2])) + + sin(q[0 * 3 + 1]) * + (phi[5 * 10 + 3] * cos(G[0 * 3 + 2] * q[0 * 3 + 2]) - + phi[5 * 10 + 1] * sin(G[0 * 3 + 2] * q[0 * 3 + 2])))) / + 2.0)) + + qd[0 * 3 + 0] * + (qd[0 * 3 + 1] * + ((l[0 * 3 + 2] * + (cos(q[0 * 3 + 1]) * + (phi[1 * 10 + 1] + l[1 * 3 + 0] * phi[2 * 10 + 0] + + l[4 * 3 + 0] * phi[5 * 10 + 0] + + phi[5 * 10 + 1] * cos(G[0 * 3 + 2] * q[0 * 3 + 2]) + + phi[5 * 10 + 3] * sin(G[0 * 3 + 2] * q[0 * 3 + 2]) + + phi[2 * 10 + 1] * cos(q[0 * 3 + 2]) + + phi[2 * 10 + 3] * sin(q[0 * 3 + 2])) + + sin(q[0 * 3 + 1]) * + (phi[1 * 10 + 3] + l[1 * 3 + 2] * phi[2 * 10 + 0] + + l[4 * 3 + 2] * phi[5 * 10 + 0] + + phi[5 * 10 + 3] * cos(G[0 * 3 + 2] * q[0 * 3 + 2]) - + phi[5 * 10 + 1] * sin(G[0 * 3 + 2] * q[0 * 3 + 2]) + + phi[2 * 10 + 3] * cos(q[0 * 3 + 2]) - + phi[2 * 10 + 1] * sin(q[0 * 3 + 2])))) / + 2.0 - + (sin(q[0 * 3 + 1]) * + (sin(q[0 * 3 + 1]) * + (-phi[1 * 10 + 9] + + l[4 * 3 + 0] * (l[4 * 3 + 2] * phi[5 * 10 + 0] + + phi[5 * 10 + 3] * + cos(G[0 * 3 + 2] * q[0 * 3 + 2]) - + phi[5 * 10 + 1] * + sin(G[0 * 3 + 2] * q[0 * 3 + 2])) - + cos(G[0 * 3 + 2] * q[0 * 3 + 2]) * + (-l[4 * 3 + 2] * phi[5 * 10 + 1] + + sin(G[0 * 3 + 2] * q[0 * 3 + 2]) * + (phi[5 * 10 + 6] - + phi[5 * 10 + 0] * + (1.0 / + (phi[5 * 10 + 0] * + phi[5 * 10 + 0]) * + (phi[5 * 10 + 1] * + phi[5 * 10 + 1]) + + (RL * RL) * 1.0 / + (phi[5 * 10 + 0] * + phi[5 * 10 + 0]) * + (phi[5 * 10 + 2] * + phi[5 * 10 + 2])) + + (phi[5 * 10 + 1] * phi[5 * 10 + 1]) / + phi[5 * 10 + 0] + + ((RL * RL) * + (phi[5 * 10 + 2] * phi[5 * 10 + 2])) / + phi[5 * 10 + 0]) + + phi[5 * 10 + 9] * + cos(G[0 * 3 + 2] * q[0 * 3 + 2]) + + (RL * RL) * l[4 * 3 + 1] * phi[5 * 10 + 2] * + sin(G[0 * 3 + 2] * q[0 * 3 + 2])) + + sin(G[0 * 3 + 2] * q[0 * 3 + 2]) * + (cos(G[0 * 3 + 2] * q[0 * 3 + 2]) * + (phi[5 * 10 + 4] - + phi[5 * 10 + 0] * + (1.0 / + (phi[5 * 10 + 0] * + phi[5 * 10 + 0]) * + (phi[5 * 10 + 3] * + phi[5 * 10 + 3]) + + (RL * RL) * 1.0 / + (phi[5 * 10 + 0] * + phi[5 * 10 + 0]) * + (phi[5 * 10 + 2] * + phi[5 * 10 + 2])) + + (phi[5 * 10 + 3] * phi[5 * 10 + 3]) / + phi[5 * 10 + 0] + + ((RL * RL) * + (phi[5 * 10 + 2] * phi[5 * 10 + 2])) / + phi[5 * 10 + 0]) + + l[4 * 3 + 2] * phi[5 * 10 + 3] + + phi[5 * 10 + 9] * + sin(G[0 * 3 + 2] * q[0 * 3 + 2]) + + (RL * RL) * l[4 * 3 + 1] * phi[5 * 10 + 2] * + cos(G[0 * 3 + 2] * q[0 * 3 + 2])) + + l[1 * 3 + 0] * (l[1 * 3 + 2] * phi[2 * 10 + 0] + + phi[2 * 10 + 3] * cos(q[0 * 3 + 2]) - + phi[2 * 10 + 1] * sin(q[0 * 3 + 2])) - + cos(q[0 * 3 + 2]) * + (-l[1 * 3 + 2] * phi[2 * 10 + 1] + + sin(q[0 * 3 + 2]) * + (phi[2 * 10 + 6] - + phi[2 * 10 + 0] * + (1.0 / + (phi[2 * 10 + 0] * + phi[2 * 10 + 0]) * + (phi[2 * 10 + 1] * + phi[2 * 10 + 1]) + + (RL * RL) * 1.0 / + (phi[2 * 10 + 0] * + phi[2 * 10 + 0]) * + (phi[2 * 10 + 2] * + phi[2 * 10 + 2])) + + (phi[2 * 10 + 1] * phi[2 * 10 + 1]) / + phi[2 * 10 + 0] + + ((RL * RL) * + (phi[2 * 10 + 2] * phi[2 * 10 + 2])) / + phi[2 * 10 + 0]) + + phi[2 * 10 + 9] * cos(q[0 * 3 + 2]) + + (RL * RL) * l[1 * 3 + 1] * phi[2 * 10 + 2] * + sin(q[0 * 3 + 2])) + + sin(q[0 * 3 + 2]) * + (l[1 * 3 + 2] * phi[2 * 10 + 3] + + cos(q[0 * 3 + 2]) * + (phi[2 * 10 + 4] - + phi[2 * 10 + 0] * + (1.0 / + (phi[2 * 10 + 0] * + phi[2 * 10 + 0]) * + (phi[2 * 10 + 3] * + phi[2 * 10 + 3]) + + (RL * RL) * 1.0 / + (phi[2 * 10 + 0] * + phi[2 * 10 + 0]) * + (phi[2 * 10 + 2] * + phi[2 * 10 + 2])) + + (phi[2 * 10 + 3] * phi[2 * 10 + 3]) / + phi[2 * 10 + 0] + + ((RL * RL) * + (phi[2 * 10 + 2] * phi[2 * 10 + 2])) / + phi[2 * 10 + 0]) + + phi[2 * 10 + 9] * sin(q[0 * 3 + 2]) + + (RL * RL) * l[1 * 3 + 1] * phi[2 * 10 + 2] * + cos(q[0 * 3 + 2])) + + RL * l[4 * 3 + 1] * sin(G[0 * 3 + 2] * q[0 * 3 + 2]) * + (RL * phi[5 * 10 + 2] * + cos(G[0 * 3 + 2] * q[0 * 3 + 2]) + + RL * l[4 * 3 + 1] * phi[5 * 10 + 0] * + cos(G[0 * 3 + 2] * q[0 * 3 + 2])) + + RL * l[1 * 3 + 1] * sin(q[0 * 3 + 2]) * + (RL * phi[2 * 10 + 2] * cos(q[0 * 3 + 2]) + + RL * l[1 * 3 + 1] * phi[2 * 10 + 0] * + cos(q[0 * 3 + 2])) - + RL * l[4 * 3 + 1] * cos(G[0 * 3 + 2] * q[0 * 3 + 2]) * + (RL * phi[5 * 10 + 2] * + sin(G[0 * 3 + 2] * q[0 * 3 + 2]) + + RL * l[4 * 3 + 1] * phi[5 * 10 + 0] * + sin(G[0 * 3 + 2] * q[0 * 3 + 2])) - + RL * l[1 * 3 + 1] * cos(q[0 * 3 + 2]) * + (RL * phi[2 * 10 + 2] * sin(q[0 * 3 + 2]) + + RL * l[1 * 3 + 1] * phi[2 * 10 + 0] * + sin(q[0 * 3 + 2]))) + + cos(q[0 * 3 + 1]) * + (phi[1 * 10 + 6] + + l[4 * 3 + 0] * (l[4 * 3 + 0] * phi[5 * 10 + 0] + + phi[5 * 10 + 1] * + cos(G[0 * 3 + 2] * q[0 * 3 + 2]) + + phi[5 * 10 + 3] * + sin(G[0 * 3 + 2] * q[0 * 3 + 2])) + + cos(G[0 * 3 + 2] * q[0 * 3 + 2]) * + (cos(G[0 * 3 + 2] * q[0 * 3 + 2]) * + (phi[5 * 10 + 6] - + phi[5 * 10 + 0] * + (1.0 / + (phi[5 * 10 + 0] * + phi[5 * 10 + 0]) * + (phi[5 * 10 + 1] * + phi[5 * 10 + 1]) + + (RL * RL) * 1.0 / + (phi[5 * 10 + 0] * + phi[5 * 10 + 0]) * + (phi[5 * 10 + 2] * + phi[5 * 10 + 2])) + + (phi[5 * 10 + 1] * phi[5 * 10 + 1]) / + phi[5 * 10 + 0] + + ((RL * RL) * + (phi[5 * 10 + 2] * phi[5 * 10 + 2])) / + phi[5 * 10 + 0]) + + l[4 * 3 + 0] * phi[5 * 10 + 1] - + phi[5 * 10 + 9] * + sin(G[0 * 3 + 2] * q[0 * 3 + 2]) + + (RL * RL) * l[4 * 3 + 1] * phi[5 * 10 + 2] * + cos(G[0 * 3 + 2] * q[0 * 3 + 2])) + + sin(G[0 * 3 + 2] * q[0 * 3 + 2]) * + (l[4 * 3 + 0] * phi[5 * 10 + 3] + + sin(G[0 * 3 + 2] * q[0 * 3 + 2]) * + (phi[5 * 10 + 4] - + phi[5 * 10 + 0] * + (1.0 / + (phi[5 * 10 + 0] * + phi[5 * 10 + 0]) * + (phi[5 * 10 + 3] * + phi[5 * 10 + 3]) + + (RL * RL) * 1.0 / + (phi[5 * 10 + 0] * + phi[5 * 10 + 0]) * + (phi[5 * 10 + 2] * + phi[5 * 10 + 2])) + + (phi[5 * 10 + 3] * phi[5 * 10 + 3]) / + phi[5 * 10 + 0] + + ((RL * RL) * + (phi[5 * 10 + 2] * phi[5 * 10 + 2])) / + phi[5 * 10 + 0]) - + phi[5 * 10 + 9] * + cos(G[0 * 3 + 2] * q[0 * 3 + 2]) + + (RL * RL) * l[4 * 3 + 1] * phi[5 * 10 + 2] * + sin(G[0 * 3 + 2] * q[0 * 3 + 2])) + + l[1 * 3 + 0] * (l[1 * 3 + 0] * phi[2 * 10 + 0] + + phi[2 * 10 + 1] * cos(q[0 * 3 + 2]) + + phi[2 * 10 + 3] * sin(q[0 * 3 + 2])) + + cos(q[0 * 3 + 2]) * + (l[1 * 3 + 0] * phi[2 * 10 + 1] + + cos(q[0 * 3 + 2]) * + (phi[2 * 10 + 6] - + phi[2 * 10 + 0] * + (1.0 / + (phi[2 * 10 + 0] * + phi[2 * 10 + 0]) * + (phi[2 * 10 + 1] * + phi[2 * 10 + 1]) + + (RL * RL) * 1.0 / + (phi[2 * 10 + 0] * + phi[2 * 10 + 0]) * + (phi[2 * 10 + 2] * + phi[2 * 10 + 2])) + + (phi[2 * 10 + 1] * phi[2 * 10 + 1]) / + phi[2 * 10 + 0] + + ((RL * RL) * + (phi[2 * 10 + 2] * phi[2 * 10 + 2])) / + phi[2 * 10 + 0]) - + phi[2 * 10 + 9] * sin(q[0 * 3 + 2]) + + (RL * RL) * l[1 * 3 + 1] * phi[2 * 10 + 2] * + cos(q[0 * 3 + 2])) - + phi[1 * 10 + 0] * + (1.0 / (phi[1 * 10 + 0] * phi[1 * 10 + 0]) * + (phi[1 * 10 + 1] * phi[1 * 10 + 1]) + + (RL * RL) * 1.0 / + (phi[1 * 10 + 0] * phi[1 * 10 + 0]) * + (phi[1 * 10 + 2] * phi[1 * 10 + 2])) + + sin(q[0 * 3 + 2]) * + (l[1 * 3 + 0] * phi[2 * 10 + 3] + + sin(q[0 * 3 + 2]) * + (phi[2 * 10 + 4] - + phi[2 * 10 + 0] * + (1.0 / + (phi[2 * 10 + 0] * + phi[2 * 10 + 0]) * + (phi[2 * 10 + 3] * + phi[2 * 10 + 3]) + + (RL * RL) * 1.0 / + (phi[2 * 10 + 0] * + phi[2 * 10 + 0]) * + (phi[2 * 10 + 2] * + phi[2 * 10 + 2])) + + (phi[2 * 10 + 3] * phi[2 * 10 + 3]) / + phi[2 * 10 + 0] + + ((RL * RL) * + (phi[2 * 10 + 2] * phi[2 * 10 + 2])) / + phi[2 * 10 + 0]) - + phi[2 * 10 + 9] * cos(q[0 * 3 + 2]) + + (RL * RL) * l[1 * 3 + 1] * phi[2 * 10 + 2] * + sin(q[0 * 3 + 2])) + + (phi[1 * 10 + 1] * phi[1 * 10 + 1]) / phi[1 * 10 + 0] + + ((RL * RL) * (phi[1 * 10 + 2] * phi[1 * 10 + 2])) / + phi[1 * 10 + 0] + + RL * l[4 * 3 + 1] * cos(G[0 * 3 + 2] * q[0 * 3 + 2]) * + (RL * phi[5 * 10 + 2] * + cos(G[0 * 3 + 2] * q[0 * 3 + 2]) + + RL * l[4 * 3 + 1] * phi[5 * 10 + 0] * + cos(G[0 * 3 + 2] * q[0 * 3 + 2])) + + RL * l[1 * 3 + 1] * cos(q[0 * 3 + 2]) * + (RL * phi[2 * 10 + 2] * cos(q[0 * 3 + 2]) + + RL * l[1 * 3 + 1] * phi[2 * 10 + 0] * + cos(q[0 * 3 + 2])) + + RL * l[4 * 3 + 1] * sin(G[0 * 3 + 2] * q[0 * 3 + 2]) * + (RL * phi[5 * 10 + 2] * + sin(G[0 * 3 + 2] * q[0 * 3 + 2]) + + RL * l[4 * 3 + 1] * phi[5 * 10 + 0] * + sin(G[0 * 3 + 2] * q[0 * 3 + 2])) + + RL * l[1 * 3 + 1] * sin(q[0 * 3 + 2]) * + (RL * phi[2 * 10 + 2] * sin(q[0 * 3 + 2]) + + RL * l[1 * 3 + 1] * phi[2 * 10 + 0] * + sin(q[0 * 3 + 2]))) + + RL * l[0 * 3 + 1] * cos(q[0 * 3 + 1]) * + (RL * phi[1 * 10 + 2] + + RL * phi[5 * 10 + 2] * + pow(sin(G[0 * 3 + 2] * q[0 * 3 + 2]), 2.0) + + RL * phi[2 * 10 + 2] * pow(cos(q[0 * 3 + 2]), 2.0) + + RL * phi[2 * 10 + 2] * pow(sin(q[0 * 3 + 2]), 2.0) + + RL * phi[5 * 10 + 2] * + pow(cos(G[0 * 3 + 2] * q[0 * 3 + 2]), 2.0) + + RL * l[4 * 3 + 1] * phi[5 * 10 + 0] * + pow(cos(G[0 * 3 + 2] * q[0 * 3 + 2]), 2.0) + + RL * l[4 * 3 + 1] * phi[5 * 10 + 0] * + pow(sin(G[0 * 3 + 2] * q[0 * 3 + 2]), 2.0) + + RL * l[1 * 3 + 1] * phi[2 * 10 + 0] * + pow(cos(q[0 * 3 + 2]), 2.0) + + RL * l[1 * 3 + 1] * phi[2 * 10 + 0] * + pow(sin(q[0 * 3 + 2]), 2.0)))) / + 2.0 + + (cos(q[0 * 3 + 1]) * + (sin(q[0 * 3 + 1]) * + (phi[1 * 10 + 4] + + l[4 * 3 + 2] * (l[4 * 3 + 2] * phi[5 * 10 + 0] + + phi[5 * 10 + 3] * + cos(G[0 * 3 + 2] * q[0 * 3 + 2]) - + phi[5 * 10 + 1] * + sin(G[0 * 3 + 2] * q[0 * 3 + 2])) + + cos(G[0 * 3 + 2] * q[0 * 3 + 2]) * + (cos(G[0 * 3 + 2] * q[0 * 3 + 2]) * + (phi[5 * 10 + 4] - + phi[5 * 10 + 0] * + (1.0 / + (phi[5 * 10 + 0] * + phi[5 * 10 + 0]) * + (phi[5 * 10 + 3] * + phi[5 * 10 + 3]) + + (RL * RL) * 1.0 / + (phi[5 * 10 + 0] * + phi[5 * 10 + 0]) * + (phi[5 * 10 + 2] * + phi[5 * 10 + 2])) + + (phi[5 * 10 + 3] * phi[5 * 10 + 3]) / + phi[5 * 10 + 0] + + ((RL * RL) * + (phi[5 * 10 + 2] * phi[5 * 10 + 2])) / + phi[5 * 10 + 0]) + + l[4 * 3 + 2] * phi[5 * 10 + 3] + + phi[5 * 10 + 9] * + sin(G[0 * 3 + 2] * q[0 * 3 + 2]) + + (RL * RL) * l[4 * 3 + 1] * phi[5 * 10 + 2] * + cos(G[0 * 3 + 2] * q[0 * 3 + 2])) + + sin(G[0 * 3 + 2] * q[0 * 3 + 2]) * + (-l[4 * 3 + 2] * phi[5 * 10 + 1] + + sin(G[0 * 3 + 2] * q[0 * 3 + 2]) * + (phi[5 * 10 + 6] - + phi[5 * 10 + 0] * + (1.0 / + (phi[5 * 10 + 0] * + phi[5 * 10 + 0]) * + (phi[5 * 10 + 1] * + phi[5 * 10 + 1]) + + (RL * RL) * 1.0 / + (phi[5 * 10 + 0] * + phi[5 * 10 + 0]) * + (phi[5 * 10 + 2] * + phi[5 * 10 + 2])) + + (phi[5 * 10 + 1] * phi[5 * 10 + 1]) / + phi[5 * 10 + 0] + + ((RL * RL) * + (phi[5 * 10 + 2] * phi[5 * 10 + 2])) / + phi[5 * 10 + 0]) + + phi[5 * 10 + 9] * + cos(G[0 * 3 + 2] * q[0 * 3 + 2]) + + (RL * RL) * l[4 * 3 + 1] * phi[5 * 10 + 2] * + sin(G[0 * 3 + 2] * q[0 * 3 + 2])) + + l[1 * 3 + 2] * (l[1 * 3 + 2] * phi[2 * 10 + 0] + + phi[2 * 10 + 3] * cos(q[0 * 3 + 2]) - + phi[2 * 10 + 1] * sin(q[0 * 3 + 2])) + + cos(q[0 * 3 + 2]) * + (l[1 * 3 + 2] * phi[2 * 10 + 3] + + cos(q[0 * 3 + 2]) * + (phi[2 * 10 + 4] - + phi[2 * 10 + 0] * + (1.0 / + (phi[2 * 10 + 0] * + phi[2 * 10 + 0]) * + (phi[2 * 10 + 3] * + phi[2 * 10 + 3]) + + (RL * RL) * 1.0 / + (phi[2 * 10 + 0] * + phi[2 * 10 + 0]) * + (phi[2 * 10 + 2] * + phi[2 * 10 + 2])) + + (phi[2 * 10 + 3] * phi[2 * 10 + 3]) / + phi[2 * 10 + 0] + + ((RL * RL) * + (phi[2 * 10 + 2] * phi[2 * 10 + 2])) / + phi[2 * 10 + 0]) + + phi[2 * 10 + 9] * sin(q[0 * 3 + 2]) + + (RL * RL) * l[1 * 3 + 1] * phi[2 * 10 + 2] * + cos(q[0 * 3 + 2])) - + phi[1 * 10 + 0] * + (1.0 / (phi[1 * 10 + 0] * phi[1 * 10 + 0]) * + (phi[1 * 10 + 3] * phi[1 * 10 + 3]) + + (RL * RL) * 1.0 / + (phi[1 * 10 + 0] * phi[1 * 10 + 0]) * + (phi[1 * 10 + 2] * phi[1 * 10 + 2])) + + sin(q[0 * 3 + 2]) * + (-l[1 * 3 + 2] * phi[2 * 10 + 1] + + sin(q[0 * 3 + 2]) * + (phi[2 * 10 + 6] - + phi[2 * 10 + 0] * + (1.0 / + (phi[2 * 10 + 0] * + phi[2 * 10 + 0]) * + (phi[2 * 10 + 1] * + phi[2 * 10 + 1]) + + (RL * RL) * 1.0 / + (phi[2 * 10 + 0] * + phi[2 * 10 + 0]) * + (phi[2 * 10 + 2] * + phi[2 * 10 + 2])) + + (phi[2 * 10 + 1] * phi[2 * 10 + 1]) / + phi[2 * 10 + 0] + + ((RL * RL) * + (phi[2 * 10 + 2] * phi[2 * 10 + 2])) / + phi[2 * 10 + 0]) + + phi[2 * 10 + 9] * cos(q[0 * 3 + 2]) + + (RL * RL) * l[1 * 3 + 1] * phi[2 * 10 + 2] * + sin(q[0 * 3 + 2])) + + (phi[1 * 10 + 3] * phi[1 * 10 + 3]) / phi[1 * 10 + 0] + + ((RL * RL) * (phi[1 * 10 + 2] * phi[1 * 10 + 2])) / + phi[1 * 10 + 0] + + RL * l[4 * 3 + 1] * cos(G[0 * 3 + 2] * q[0 * 3 + 2]) * + (RL * phi[5 * 10 + 2] * + cos(G[0 * 3 + 2] * q[0 * 3 + 2]) + + RL * l[4 * 3 + 1] * phi[5 * 10 + 0] * + cos(G[0 * 3 + 2] * q[0 * 3 + 2])) + + RL * l[1 * 3 + 1] * cos(q[0 * 3 + 2]) * + (RL * phi[2 * 10 + 2] * cos(q[0 * 3 + 2]) + + RL * l[1 * 3 + 1] * phi[2 * 10 + 0] * + cos(q[0 * 3 + 2])) + + RL * l[4 * 3 + 1] * sin(G[0 * 3 + 2] * q[0 * 3 + 2]) * + (RL * phi[5 * 10 + 2] * + sin(G[0 * 3 + 2] * q[0 * 3 + 2]) + + RL * l[4 * 3 + 1] * phi[5 * 10 + 0] * + sin(G[0 * 3 + 2] * q[0 * 3 + 2])) + + RL * l[1 * 3 + 1] * sin(q[0 * 3 + 2]) * + (RL * phi[2 * 10 + 2] * sin(q[0 * 3 + 2]) + + RL * l[1 * 3 + 1] * phi[2 * 10 + 0] * + sin(q[0 * 3 + 2]))) + + cos(q[0 * 3 + 1]) * + (-phi[1 * 10 + 9] + + l[4 * 3 + 2] * (l[4 * 3 + 0] * phi[5 * 10 + 0] + + phi[5 * 10 + 1] * + cos(G[0 * 3 + 2] * q[0 * 3 + 2]) + + phi[5 * 10 + 3] * + sin(G[0 * 3 + 2] * q[0 * 3 + 2])) - + sin(G[0 * 3 + 2] * q[0 * 3 + 2]) * + (cos(G[0 * 3 + 2] * q[0 * 3 + 2]) * + (phi[5 * 10 + 6] - + phi[5 * 10 + 0] * + (1.0 / + (phi[5 * 10 + 0] * + phi[5 * 10 + 0]) * + (phi[5 * 10 + 1] * + phi[5 * 10 + 1]) + + (RL * RL) * 1.0 / + (phi[5 * 10 + 0] * + phi[5 * 10 + 0]) * + (phi[5 * 10 + 2] * + phi[5 * 10 + 2])) + + (phi[5 * 10 + 1] * phi[5 * 10 + 1]) / + phi[5 * 10 + 0] + + ((RL * RL) * + (phi[5 * 10 + 2] * phi[5 * 10 + 2])) / + phi[5 * 10 + 0]) + + l[4 * 3 + 0] * phi[5 * 10 + 1] - + phi[5 * 10 + 9] * + sin(G[0 * 3 + 2] * q[0 * 3 + 2]) + + (RL * RL) * l[4 * 3 + 1] * phi[5 * 10 + 2] * + cos(G[0 * 3 + 2] * q[0 * 3 + 2])) + + cos(G[0 * 3 + 2] * q[0 * 3 + 2]) * + (l[4 * 3 + 0] * phi[5 * 10 + 3] + + sin(G[0 * 3 + 2] * q[0 * 3 + 2]) * + (phi[5 * 10 + 4] - + phi[5 * 10 + 0] * + (1.0 / + (phi[5 * 10 + 0] * + phi[5 * 10 + 0]) * + (phi[5 * 10 + 3] * + phi[5 * 10 + 3]) + + (RL * RL) * 1.0 / + (phi[5 * 10 + 0] * + phi[5 * 10 + 0]) * + (phi[5 * 10 + 2] * + phi[5 * 10 + 2])) + + (phi[5 * 10 + 3] * phi[5 * 10 + 3]) / + phi[5 * 10 + 0] + + ((RL * RL) * + (phi[5 * 10 + 2] * phi[5 * 10 + 2])) / + phi[5 * 10 + 0]) - + phi[5 * 10 + 9] * + cos(G[0 * 3 + 2] * q[0 * 3 + 2]) + + (RL * RL) * l[4 * 3 + 1] * phi[5 * 10 + 2] * + sin(G[0 * 3 + 2] * q[0 * 3 + 2])) + + l[1 * 3 + 2] * (l[1 * 3 + 0] * phi[2 * 10 + 0] + + phi[2 * 10 + 1] * cos(q[0 * 3 + 2]) + + phi[2 * 10 + 3] * sin(q[0 * 3 + 2])) - + sin(q[0 * 3 + 2]) * + (l[1 * 3 + 0] * phi[2 * 10 + 1] + + cos(q[0 * 3 + 2]) * + (phi[2 * 10 + 6] - + phi[2 * 10 + 0] * + (1.0 / + (phi[2 * 10 + 0] * + phi[2 * 10 + 0]) * + (phi[2 * 10 + 1] * + phi[2 * 10 + 1]) + + (RL * RL) * 1.0 / + (phi[2 * 10 + 0] * + phi[2 * 10 + 0]) * + (phi[2 * 10 + 2] * + phi[2 * 10 + 2])) + + (phi[2 * 10 + 1] * phi[2 * 10 + 1]) / + phi[2 * 10 + 0] + + ((RL * RL) * + (phi[2 * 10 + 2] * phi[2 * 10 + 2])) / + phi[2 * 10 + 0]) - + phi[2 * 10 + 9] * sin(q[0 * 3 + 2]) + + (RL * RL) * l[1 * 3 + 1] * phi[2 * 10 + 2] * + cos(q[0 * 3 + 2])) + + cos(q[0 * 3 + 2]) * + (l[1 * 3 + 0] * phi[2 * 10 + 3] + + sin(q[0 * 3 + 2]) * + (phi[2 * 10 + 4] - + phi[2 * 10 + 0] * + (1.0 / + (phi[2 * 10 + 0] * + phi[2 * 10 + 0]) * + (phi[2 * 10 + 3] * + phi[2 * 10 + 3]) + + (RL * RL) * 1.0 / + (phi[2 * 10 + 0] * + phi[2 * 10 + 0]) * + (phi[2 * 10 + 2] * + phi[2 * 10 + 2])) + + (phi[2 * 10 + 3] * phi[2 * 10 + 3]) / + phi[2 * 10 + 0] + + ((RL * RL) * + (phi[2 * 10 + 2] * phi[2 * 10 + 2])) / + phi[2 * 10 + 0]) - + phi[2 * 10 + 9] * cos(q[0 * 3 + 2]) + + (RL * RL) * l[1 * 3 + 1] * phi[2 * 10 + 2] * + sin(q[0 * 3 + 2])) - + RL * l[4 * 3 + 1] * sin(G[0 * 3 + 2] * q[0 * 3 + 2]) * + (RL * phi[5 * 10 + 2] * + cos(G[0 * 3 + 2] * q[0 * 3 + 2]) + + RL * l[4 * 3 + 1] * phi[5 * 10 + 0] * + cos(G[0 * 3 + 2] * q[0 * 3 + 2])) - + RL * l[1 * 3 + 1] * sin(q[0 * 3 + 2]) * + (RL * phi[2 * 10 + 2] * cos(q[0 * 3 + 2]) + + RL * l[1 * 3 + 1] * phi[2 * 10 + 0] * + cos(q[0 * 3 + 2])) + + RL * l[4 * 3 + 1] * cos(G[0 * 3 + 2] * q[0 * 3 + 2]) * + (RL * phi[5 * 10 + 2] * + sin(G[0 * 3 + 2] * q[0 * 3 + 2]) + + RL * l[4 * 3 + 1] * phi[5 * 10 + 0] * + sin(G[0 * 3 + 2] * q[0 * 3 + 2])) + + RL * l[1 * 3 + 1] * cos(q[0 * 3 + 2]) * + (RL * phi[2 * 10 + 2] * sin(q[0 * 3 + 2]) + + RL * l[1 * 3 + 1] * phi[2 * 10 + 0] * + sin(q[0 * 3 + 2]))) + + RL * l[0 * 3 + 1] * sin(q[0 * 3 + 1]) * + (RL * phi[1 * 10 + 2] + + RL * phi[5 * 10 + 2] * + pow(sin(G[0 * 3 + 2] * q[0 * 3 + 2]), 2.0) + + RL * phi[2 * 10 + 2] * pow(cos(q[0 * 3 + 2]), 2.0) + + RL * phi[2 * 10 + 2] * pow(sin(q[0 * 3 + 2]), 2.0) + + RL * phi[5 * 10 + 2] * + pow(cos(G[0 * 3 + 2] * q[0 * 3 + 2]), 2.0) + + RL * l[4 * 3 + 1] * phi[5 * 10 + 0] * + pow(cos(G[0 * 3 + 2] * q[0 * 3 + 2]), 2.0) + + RL * l[4 * 3 + 1] * phi[5 * 10 + 0] * + pow(sin(G[0 * 3 + 2] * q[0 * 3 + 2]), 2.0) + + RL * l[1 * 3 + 1] * phi[2 * 10 + 0] * + pow(cos(q[0 * 3 + 2]), 2.0) + + RL * l[1 * 3 + 1] * phi[2 * 10 + 0] * + pow(sin(q[0 * 3 + 2]), 2.0)))) / + 2.0 + + (cos(q[0 * 3 + 1]) * + (l[0 * 3 + 2] * + (phi[1 * 10 + 1] + l[1 * 3 + 0] * phi[2 * 10 + 0] + + l[4 * 3 + 0] * phi[5 * 10 + 0] + + phi[5 * 10 + 1] * cos(G[0 * 3 + 2] * q[0 * 3 + 2]) + + phi[5 * 10 + 3] * sin(G[0 * 3 + 2] * q[0 * 3 + 2]) + + phi[2 * 10 + 1] * cos(q[0 * 3 + 2]) + + phi[2 * 10 + 3] * sin(q[0 * 3 + 2])) - + sin(q[0 * 3 + 1]) * + (phi[1 * 10 + 6] + + l[4 * 3 + 0] * (l[4 * 3 + 0] * phi[5 * 10 + 0] + + phi[5 * 10 + 1] * + cos(G[0 * 3 + 2] * q[0 * 3 + 2]) + + phi[5 * 10 + 3] * + sin(G[0 * 3 + 2] * q[0 * 3 + 2])) + + cos(G[0 * 3 + 2] * q[0 * 3 + 2]) * + (cos(G[0 * 3 + 2] * q[0 * 3 + 2]) * + (phi[5 * 10 + 6] - + phi[5 * 10 + 0] * + (1.0 / + (phi[5 * 10 + 0] * + phi[5 * 10 + 0]) * + (phi[5 * 10 + 1] * + phi[5 * 10 + 1]) + + (RL * RL) * 1.0 / + (phi[5 * 10 + 0] * + phi[5 * 10 + 0]) * + (phi[5 * 10 + 2] * + phi[5 * 10 + 2])) + + (phi[5 * 10 + 1] * phi[5 * 10 + 1]) / + phi[5 * 10 + 0] + + ((RL * RL) * + (phi[5 * 10 + 2] * phi[5 * 10 + 2])) / + phi[5 * 10 + 0]) + + l[4 * 3 + 0] * phi[5 * 10 + 1] - + phi[5 * 10 + 9] * + sin(G[0 * 3 + 2] * q[0 * 3 + 2]) + + (RL * RL) * l[4 * 3 + 1] * phi[5 * 10 + 2] * + cos(G[0 * 3 + 2] * q[0 * 3 + 2])) + + sin(G[0 * 3 + 2] * q[0 * 3 + 2]) * + (l[4 * 3 + 0] * phi[5 * 10 + 3] + + sin(G[0 * 3 + 2] * q[0 * 3 + 2]) * + (phi[5 * 10 + 4] - + phi[5 * 10 + 0] * + (1.0 / + (phi[5 * 10 + 0] * + phi[5 * 10 + 0]) * + (phi[5 * 10 + 3] * + phi[5 * 10 + 3]) + + (RL * RL) * 1.0 / + (phi[5 * 10 + 0] * + phi[5 * 10 + 0]) * + (phi[5 * 10 + 2] * + phi[5 * 10 + 2])) + + (phi[5 * 10 + 3] * phi[5 * 10 + 3]) / + phi[5 * 10 + 0] + + ((RL * RL) * + (phi[5 * 10 + 2] * phi[5 * 10 + 2])) / + phi[5 * 10 + 0]) - + phi[5 * 10 + 9] * + cos(G[0 * 3 + 2] * q[0 * 3 + 2]) + + (RL * RL) * l[4 * 3 + 1] * phi[5 * 10 + 2] * + sin(G[0 * 3 + 2] * q[0 * 3 + 2])) + + l[1 * 3 + 0] * (l[1 * 3 + 0] * phi[2 * 10 + 0] + + phi[2 * 10 + 1] * cos(q[0 * 3 + 2]) + + phi[2 * 10 + 3] * sin(q[0 * 3 + 2])) + + cos(q[0 * 3 + 2]) * + (l[1 * 3 + 0] * phi[2 * 10 + 1] + + cos(q[0 * 3 + 2]) * + (phi[2 * 10 + 6] - + phi[2 * 10 + 0] * + (1.0 / + (phi[2 * 10 + 0] * + phi[2 * 10 + 0]) * + (phi[2 * 10 + 1] * + phi[2 * 10 + 1]) + + (RL * RL) * 1.0 / + (phi[2 * 10 + 0] * + phi[2 * 10 + 0]) * + (phi[2 * 10 + 2] * + phi[2 * 10 + 2])) + + (phi[2 * 10 + 1] * phi[2 * 10 + 1]) / + phi[2 * 10 + 0] + + ((RL * RL) * + (phi[2 * 10 + 2] * phi[2 * 10 + 2])) / + phi[2 * 10 + 0]) - + phi[2 * 10 + 9] * sin(q[0 * 3 + 2]) + + (RL * RL) * l[1 * 3 + 1] * phi[2 * 10 + 2] * + cos(q[0 * 3 + 2])) - + phi[1 * 10 + 0] * + (1.0 / (phi[1 * 10 + 0] * phi[1 * 10 + 0]) * + (phi[1 * 10 + 1] * phi[1 * 10 + 1]) + + (RL * RL) * 1.0 / + (phi[1 * 10 + 0] * phi[1 * 10 + 0]) * + (phi[1 * 10 + 2] * phi[1 * 10 + 2])) + + sin(q[0 * 3 + 2]) * + (l[1 * 3 + 0] * phi[2 * 10 + 3] + + sin(q[0 * 3 + 2]) * + (phi[2 * 10 + 4] - + phi[2 * 10 + 0] * + (1.0 / + (phi[2 * 10 + 0] * + phi[2 * 10 + 0]) * + (phi[2 * 10 + 3] * + phi[2 * 10 + 3]) + + (RL * RL) * 1.0 / + (phi[2 * 10 + 0] * + phi[2 * 10 + 0]) * + (phi[2 * 10 + 2] * + phi[2 * 10 + 2])) + + (phi[2 * 10 + 3] * phi[2 * 10 + 3]) / + phi[2 * 10 + 0] + + ((RL * RL) * + (phi[2 * 10 + 2] * phi[2 * 10 + 2])) / + phi[2 * 10 + 0]) - + phi[2 * 10 + 9] * cos(q[0 * 3 + 2]) + + (RL * RL) * l[1 * 3 + 1] * phi[2 * 10 + 2] * + sin(q[0 * 3 + 2])) + + (phi[1 * 10 + 1] * phi[1 * 10 + 1]) / phi[1 * 10 + 0] + + ((RL * RL) * (phi[1 * 10 + 2] * phi[1 * 10 + 2])) / + phi[1 * 10 + 0] + + RL * l[4 * 3 + 1] * cos(G[0 * 3 + 2] * q[0 * 3 + 2]) * + (RL * phi[5 * 10 + 2] * + cos(G[0 * 3 + 2] * q[0 * 3 + 2]) + + RL * l[4 * 3 + 1] * phi[5 * 10 + 0] * + cos(G[0 * 3 + 2] * q[0 * 3 + 2])) + + RL * l[1 * 3 + 1] * cos(q[0 * 3 + 2]) * + (RL * phi[2 * 10 + 2] * cos(q[0 * 3 + 2]) + + RL * l[1 * 3 + 1] * phi[2 * 10 + 0] * + cos(q[0 * 3 + 2])) + + RL * l[4 * 3 + 1] * sin(G[0 * 3 + 2] * q[0 * 3 + 2]) * + (RL * phi[5 * 10 + 2] * + sin(G[0 * 3 + 2] * q[0 * 3 + 2]) + + RL * l[4 * 3 + 1] * phi[5 * 10 + 0] * + sin(G[0 * 3 + 2] * q[0 * 3 + 2])) + + RL * l[1 * 3 + 1] * sin(q[0 * 3 + 2]) * + (RL * phi[2 * 10 + 2] * sin(q[0 * 3 + 2]) + + RL * l[1 * 3 + 1] * phi[2 * 10 + 0] * + sin(q[0 * 3 + 2]))) + + cos(q[0 * 3 + 1]) * + (-phi[1 * 10 + 9] + + l[4 * 3 + 0] * (l[4 * 3 + 2] * phi[5 * 10 + 0] + + phi[5 * 10 + 3] * + cos(G[0 * 3 + 2] * q[0 * 3 + 2]) - + phi[5 * 10 + 1] * + sin(G[0 * 3 + 2] * q[0 * 3 + 2])) - + cos(G[0 * 3 + 2] * q[0 * 3 + 2]) * + (-l[4 * 3 + 2] * phi[5 * 10 + 1] + + sin(G[0 * 3 + 2] * q[0 * 3 + 2]) * + (phi[5 * 10 + 6] - + phi[5 * 10 + 0] * + (1.0 / + (phi[5 * 10 + 0] * + phi[5 * 10 + 0]) * + (phi[5 * 10 + 1] * + phi[5 * 10 + 1]) + + (RL * RL) * 1.0 / + (phi[5 * 10 + 0] * + phi[5 * 10 + 0]) * + (phi[5 * 10 + 2] * + phi[5 * 10 + 2])) + + (phi[5 * 10 + 1] * phi[5 * 10 + 1]) / + phi[5 * 10 + 0] + + ((RL * RL) * + (phi[5 * 10 + 2] * phi[5 * 10 + 2])) / + phi[5 * 10 + 0]) + + phi[5 * 10 + 9] * + cos(G[0 * 3 + 2] * q[0 * 3 + 2]) + + (RL * RL) * l[4 * 3 + 1] * phi[5 * 10 + 2] * + sin(G[0 * 3 + 2] * q[0 * 3 + 2])) + + sin(G[0 * 3 + 2] * q[0 * 3 + 2]) * + (cos(G[0 * 3 + 2] * q[0 * 3 + 2]) * + (phi[5 * 10 + 4] - + phi[5 * 10 + 0] * + (1.0 / + (phi[5 * 10 + 0] * + phi[5 * 10 + 0]) * + (phi[5 * 10 + 3] * + phi[5 * 10 + 3]) + + (RL * RL) * 1.0 / + (phi[5 * 10 + 0] * + phi[5 * 10 + 0]) * + (phi[5 * 10 + 2] * + phi[5 * 10 + 2])) + + (phi[5 * 10 + 3] * phi[5 * 10 + 3]) / + phi[5 * 10 + 0] + + ((RL * RL) * + (phi[5 * 10 + 2] * phi[5 * 10 + 2])) / + phi[5 * 10 + 0]) + + l[4 * 3 + 2] * phi[5 * 10 + 3] + + phi[5 * 10 + 9] * + sin(G[0 * 3 + 2] * q[0 * 3 + 2]) + + (RL * RL) * l[4 * 3 + 1] * phi[5 * 10 + 2] * + cos(G[0 * 3 + 2] * q[0 * 3 + 2])) + + l[1 * 3 + 0] * (l[1 * 3 + 2] * phi[2 * 10 + 0] + + phi[2 * 10 + 3] * cos(q[0 * 3 + 2]) - + phi[2 * 10 + 1] * sin(q[0 * 3 + 2])) - + cos(q[0 * 3 + 2]) * + (-l[1 * 3 + 2] * phi[2 * 10 + 1] + + sin(q[0 * 3 + 2]) * + (phi[2 * 10 + 6] - + phi[2 * 10 + 0] * + (1.0 / + (phi[2 * 10 + 0] * + phi[2 * 10 + 0]) * + (phi[2 * 10 + 1] * + phi[2 * 10 + 1]) + + (RL * RL) * 1.0 / + (phi[2 * 10 + 0] * + phi[2 * 10 + 0]) * + (phi[2 * 10 + 2] * + phi[2 * 10 + 2])) + + (phi[2 * 10 + 1] * phi[2 * 10 + 1]) / + phi[2 * 10 + 0] + + ((RL * RL) * + (phi[2 * 10 + 2] * phi[2 * 10 + 2])) / + phi[2 * 10 + 0]) + + phi[2 * 10 + 9] * cos(q[0 * 3 + 2]) + + (RL * RL) * l[1 * 3 + 1] * phi[2 * 10 + 2] * + sin(q[0 * 3 + 2])) + + sin(q[0 * 3 + 2]) * + (l[1 * 3 + 2] * phi[2 * 10 + 3] + + cos(q[0 * 3 + 2]) * + (phi[2 * 10 + 4] - + phi[2 * 10 + 0] * + (1.0 / + (phi[2 * 10 + 0] * + phi[2 * 10 + 0]) * + (phi[2 * 10 + 3] * + phi[2 * 10 + 3]) + + (RL * RL) * 1.0 / + (phi[2 * 10 + 0] * + phi[2 * 10 + 0]) * + (phi[2 * 10 + 2] * + phi[2 * 10 + 2])) + + (phi[2 * 10 + 3] * phi[2 * 10 + 3]) / + phi[2 * 10 + 0] + + ((RL * RL) * + (phi[2 * 10 + 2] * phi[2 * 10 + 2])) / + phi[2 * 10 + 0]) + + phi[2 * 10 + 9] * sin(q[0 * 3 + 2]) + + (RL * RL) * l[1 * 3 + 1] * phi[2 * 10 + 2] * + cos(q[0 * 3 + 2])) + + RL * l[4 * 3 + 1] * sin(G[0 * 3 + 2] * q[0 * 3 + 2]) * + (RL * phi[5 * 10 + 2] * + cos(G[0 * 3 + 2] * q[0 * 3 + 2]) + + RL * l[4 * 3 + 1] * phi[5 * 10 + 0] * + cos(G[0 * 3 + 2] * q[0 * 3 + 2])) + + RL * l[1 * 3 + 1] * sin(q[0 * 3 + 2]) * + (RL * phi[2 * 10 + 2] * cos(q[0 * 3 + 2]) + + RL * l[1 * 3 + 1] * phi[2 * 10 + 0] * + cos(q[0 * 3 + 2])) - + RL * l[4 * 3 + 1] * cos(G[0 * 3 + 2] * q[0 * 3 + 2]) * + (RL * phi[5 * 10 + 2] * + sin(G[0 * 3 + 2] * q[0 * 3 + 2]) + + RL * l[4 * 3 + 1] * phi[5 * 10 + 0] * + sin(G[0 * 3 + 2] * q[0 * 3 + 2])) - + RL * l[1 * 3 + 1] * cos(q[0 * 3 + 2]) * + (RL * phi[2 * 10 + 2] * sin(q[0 * 3 + 2]) + + RL * l[1 * 3 + 1] * phi[2 * 10 + 0] * + sin(q[0 * 3 + 2]))) - + RL * l[0 * 3 + 1] * sin(q[0 * 3 + 1]) * + (RL * phi[1 * 10 + 2] + + RL * phi[5 * 10 + 2] * + pow(sin(G[0 * 3 + 2] * q[0 * 3 + 2]), 2.0) + + RL * phi[2 * 10 + 2] * pow(cos(q[0 * 3 + 2]), 2.0) + + RL * phi[2 * 10 + 2] * pow(sin(q[0 * 3 + 2]), 2.0) + + RL * phi[5 * 10 + 2] * + pow(cos(G[0 * 3 + 2] * q[0 * 3 + 2]), 2.0) + + RL * l[4 * 3 + 1] * phi[5 * 10 + 0] * + pow(cos(G[0 * 3 + 2] * q[0 * 3 + 2]), 2.0) + + RL * l[4 * 3 + 1] * phi[5 * 10 + 0] * + pow(sin(G[0 * 3 + 2] * q[0 * 3 + 2]), 2.0) + + RL * l[1 * 3 + 1] * phi[2 * 10 + 0] * + pow(cos(q[0 * 3 + 2]), 2.0) + + RL * l[1 * 3 + 1] * phi[2 * 10 + 0] * + pow(sin(q[0 * 3 + 2]), 2.0)))) / + 2.0 + + (sin(q[0 * 3 + 1]) * + (-sin(q[0 * 3 + 1]) * + (-phi[1 * 10 + 9] + + l[4 * 3 + 2] * (l[4 * 3 + 0] * phi[5 * 10 + 0] + + phi[5 * 10 + 1] * + cos(G[0 * 3 + 2] * q[0 * 3 + 2]) + + phi[5 * 10 + 3] * + sin(G[0 * 3 + 2] * q[0 * 3 + 2])) - + sin(G[0 * 3 + 2] * q[0 * 3 + 2]) * + (cos(G[0 * 3 + 2] * q[0 * 3 + 2]) * + (phi[5 * 10 + 6] - + phi[5 * 10 + 0] * + (1.0 / + (phi[5 * 10 + 0] * + phi[5 * 10 + 0]) * + (phi[5 * 10 + 1] * + phi[5 * 10 + 1]) + + (RL * RL) * 1.0 / + (phi[5 * 10 + 0] * + phi[5 * 10 + 0]) * + (phi[5 * 10 + 2] * + phi[5 * 10 + 2])) + + (phi[5 * 10 + 1] * phi[5 * 10 + 1]) / + phi[5 * 10 + 0] + + ((RL * RL) * + (phi[5 * 10 + 2] * phi[5 * 10 + 2])) / + phi[5 * 10 + 0]) + + l[4 * 3 + 0] * phi[5 * 10 + 1] - + phi[5 * 10 + 9] * + sin(G[0 * 3 + 2] * q[0 * 3 + 2]) + + (RL * RL) * l[4 * 3 + 1] * phi[5 * 10 + 2] * + cos(G[0 * 3 + 2] * q[0 * 3 + 2])) + + cos(G[0 * 3 + 2] * q[0 * 3 + 2]) * + (l[4 * 3 + 0] * phi[5 * 10 + 3] + + sin(G[0 * 3 + 2] * q[0 * 3 + 2]) * + (phi[5 * 10 + 4] - + phi[5 * 10 + 0] * + (1.0 / + (phi[5 * 10 + 0] * + phi[5 * 10 + 0]) * + (phi[5 * 10 + 3] * + phi[5 * 10 + 3]) + + (RL * RL) * 1.0 / + (phi[5 * 10 + 0] * + phi[5 * 10 + 0]) * + (phi[5 * 10 + 2] * + phi[5 * 10 + 2])) + + (phi[5 * 10 + 3] * phi[5 * 10 + 3]) / + phi[5 * 10 + 0] + + ((RL * RL) * + (phi[5 * 10 + 2] * phi[5 * 10 + 2])) / + phi[5 * 10 + 0]) - + phi[5 * 10 + 9] * + cos(G[0 * 3 + 2] * q[0 * 3 + 2]) + + (RL * RL) * l[4 * 3 + 1] * phi[5 * 10 + 2] * + sin(G[0 * 3 + 2] * q[0 * 3 + 2])) + + l[1 * 3 + 2] * (l[1 * 3 + 0] * phi[2 * 10 + 0] + + phi[2 * 10 + 1] * cos(q[0 * 3 + 2]) + + phi[2 * 10 + 3] * sin(q[0 * 3 + 2])) - + sin(q[0 * 3 + 2]) * + (l[1 * 3 + 0] * phi[2 * 10 + 1] + + cos(q[0 * 3 + 2]) * + (phi[2 * 10 + 6] - + phi[2 * 10 + 0] * + (1.0 / + (phi[2 * 10 + 0] * + phi[2 * 10 + 0]) * + (phi[2 * 10 + 1] * + phi[2 * 10 + 1]) + + (RL * RL) * 1.0 / + (phi[2 * 10 + 0] * + phi[2 * 10 + 0]) * + (phi[2 * 10 + 2] * + phi[2 * 10 + 2])) + + (phi[2 * 10 + 1] * phi[2 * 10 + 1]) / + phi[2 * 10 + 0] + + ((RL * RL) * + (phi[2 * 10 + 2] * phi[2 * 10 + 2])) / + phi[2 * 10 + 0]) - + phi[2 * 10 + 9] * sin(q[0 * 3 + 2]) + + (RL * RL) * l[1 * 3 + 1] * phi[2 * 10 + 2] * + cos(q[0 * 3 + 2])) + + cos(q[0 * 3 + 2]) * + (l[1 * 3 + 0] * phi[2 * 10 + 3] + + sin(q[0 * 3 + 2]) * + (phi[2 * 10 + 4] - + phi[2 * 10 + 0] * + (1.0 / + (phi[2 * 10 + 0] * + phi[2 * 10 + 0]) * + (phi[2 * 10 + 3] * + phi[2 * 10 + 3]) + + (RL * RL) * 1.0 / + (phi[2 * 10 + 0] * + phi[2 * 10 + 0]) * + (phi[2 * 10 + 2] * + phi[2 * 10 + 2])) + + (phi[2 * 10 + 3] * phi[2 * 10 + 3]) / + phi[2 * 10 + 0] + + ((RL * RL) * + (phi[2 * 10 + 2] * phi[2 * 10 + 2])) / + phi[2 * 10 + 0]) - + phi[2 * 10 + 9] * cos(q[0 * 3 + 2]) + + (RL * RL) * l[1 * 3 + 1] * phi[2 * 10 + 2] * + sin(q[0 * 3 + 2])) - + RL * l[4 * 3 + 1] * sin(G[0 * 3 + 2] * q[0 * 3 + 2]) * + (RL * phi[5 * 10 + 2] * + cos(G[0 * 3 + 2] * q[0 * 3 + 2]) + + RL * l[4 * 3 + 1] * phi[5 * 10 + 0] * + cos(G[0 * 3 + 2] * q[0 * 3 + 2])) - + RL * l[1 * 3 + 1] * sin(q[0 * 3 + 2]) * + (RL * phi[2 * 10 + 2] * cos(q[0 * 3 + 2]) + + RL * l[1 * 3 + 1] * phi[2 * 10 + 0] * + cos(q[0 * 3 + 2])) + + RL * l[4 * 3 + 1] * cos(G[0 * 3 + 2] * q[0 * 3 + 2]) * + (RL * phi[5 * 10 + 2] * + sin(G[0 * 3 + 2] * q[0 * 3 + 2]) + + RL * l[4 * 3 + 1] * phi[5 * 10 + 0] * + sin(G[0 * 3 + 2] * q[0 * 3 + 2])) + + RL * l[1 * 3 + 1] * cos(q[0 * 3 + 2]) * + (RL * phi[2 * 10 + 2] * sin(q[0 * 3 + 2]) + + RL * l[1 * 3 + 1] * phi[2 * 10 + 0] * + sin(q[0 * 3 + 2]))) + + l[0 * 3 + 2] * + (phi[1 * 10 + 3] + l[1 * 3 + 2] * phi[2 * 10 + 0] + + l[4 * 3 + 2] * phi[5 * 10 + 0] + + phi[5 * 10 + 3] * cos(G[0 * 3 + 2] * q[0 * 3 + 2]) - + phi[5 * 10 + 1] * sin(G[0 * 3 + 2] * q[0 * 3 + 2]) + + phi[2 * 10 + 3] * cos(q[0 * 3 + 2]) - + phi[2 * 10 + 1] * sin(q[0 * 3 + 2])) + + cos(q[0 * 3 + 1]) * + (phi[1 * 10 + 4] + + l[4 * 3 + 2] * (l[4 * 3 + 2] * phi[5 * 10 + 0] + + phi[5 * 10 + 3] * + cos(G[0 * 3 + 2] * q[0 * 3 + 2]) - + phi[5 * 10 + 1] * + sin(G[0 * 3 + 2] * q[0 * 3 + 2])) + + cos(G[0 * 3 + 2] * q[0 * 3 + 2]) * + (cos(G[0 * 3 + 2] * q[0 * 3 + 2]) * + (phi[5 * 10 + 4] - + phi[5 * 10 + 0] * + (1.0 / + (phi[5 * 10 + 0] * + phi[5 * 10 + 0]) * + (phi[5 * 10 + 3] * + phi[5 * 10 + 3]) + + (RL * RL) * 1.0 / + (phi[5 * 10 + 0] * + phi[5 * 10 + 0]) * + (phi[5 * 10 + 2] * + phi[5 * 10 + 2])) + + (phi[5 * 10 + 3] * phi[5 * 10 + 3]) / + phi[5 * 10 + 0] + + ((RL * RL) * + (phi[5 * 10 + 2] * phi[5 * 10 + 2])) / + phi[5 * 10 + 0]) + + l[4 * 3 + 2] * phi[5 * 10 + 3] + + phi[5 * 10 + 9] * + sin(G[0 * 3 + 2] * q[0 * 3 + 2]) + + (RL * RL) * l[4 * 3 + 1] * phi[5 * 10 + 2] * + cos(G[0 * 3 + 2] * q[0 * 3 + 2])) + + sin(G[0 * 3 + 2] * q[0 * 3 + 2]) * + (-l[4 * 3 + 2] * phi[5 * 10 + 1] + + sin(G[0 * 3 + 2] * q[0 * 3 + 2]) * + (phi[5 * 10 + 6] - + phi[5 * 10 + 0] * + (1.0 / + (phi[5 * 10 + 0] * + phi[5 * 10 + 0]) * + (phi[5 * 10 + 1] * + phi[5 * 10 + 1]) + + (RL * RL) * 1.0 / + (phi[5 * 10 + 0] * + phi[5 * 10 + 0]) * + (phi[5 * 10 + 2] * + phi[5 * 10 + 2])) + + (phi[5 * 10 + 1] * phi[5 * 10 + 1]) / + phi[5 * 10 + 0] + + ((RL * RL) * + (phi[5 * 10 + 2] * phi[5 * 10 + 2])) / + phi[5 * 10 + 0]) + + phi[5 * 10 + 9] * + cos(G[0 * 3 + 2] * q[0 * 3 + 2]) + + (RL * RL) * l[4 * 3 + 1] * phi[5 * 10 + 2] * + sin(G[0 * 3 + 2] * q[0 * 3 + 2])) + + l[1 * 3 + 2] * (l[1 * 3 + 2] * phi[2 * 10 + 0] + + phi[2 * 10 + 3] * cos(q[0 * 3 + 2]) - + phi[2 * 10 + 1] * sin(q[0 * 3 + 2])) + + cos(q[0 * 3 + 2]) * + (l[1 * 3 + 2] * phi[2 * 10 + 3] + + cos(q[0 * 3 + 2]) * + (phi[2 * 10 + 4] - + phi[2 * 10 + 0] * + (1.0 / + (phi[2 * 10 + 0] * + phi[2 * 10 + 0]) * + (phi[2 * 10 + 3] * + phi[2 * 10 + 3]) + + (RL * RL) * 1.0 / + (phi[2 * 10 + 0] * + phi[2 * 10 + 0]) * + (phi[2 * 10 + 2] * + phi[2 * 10 + 2])) + + (phi[2 * 10 + 3] * phi[2 * 10 + 3]) / + phi[2 * 10 + 0] + + ((RL * RL) * + (phi[2 * 10 + 2] * phi[2 * 10 + 2])) / + phi[2 * 10 + 0]) + + phi[2 * 10 + 9] * sin(q[0 * 3 + 2]) + + (RL * RL) * l[1 * 3 + 1] * phi[2 * 10 + 2] * + cos(q[0 * 3 + 2])) - + phi[1 * 10 + 0] * + (1.0 / (phi[1 * 10 + 0] * phi[1 * 10 + 0]) * + (phi[1 * 10 + 3] * phi[1 * 10 + 3]) + + (RL * RL) * 1.0 / + (phi[1 * 10 + 0] * phi[1 * 10 + 0]) * + (phi[1 * 10 + 2] * phi[1 * 10 + 2])) + + sin(q[0 * 3 + 2]) * + (-l[1 * 3 + 2] * phi[2 * 10 + 1] + + sin(q[0 * 3 + 2]) * + (phi[2 * 10 + 6] - + phi[2 * 10 + 0] * + (1.0 / + (phi[2 * 10 + 0] * + phi[2 * 10 + 0]) * + (phi[2 * 10 + 1] * + phi[2 * 10 + 1]) + + (RL * RL) * 1.0 / + (phi[2 * 10 + 0] * + phi[2 * 10 + 0]) * + (phi[2 * 10 + 2] * + phi[2 * 10 + 2])) + + (phi[2 * 10 + 1] * phi[2 * 10 + 1]) / + phi[2 * 10 + 0] + + ((RL * RL) * + (phi[2 * 10 + 2] * phi[2 * 10 + 2])) / + phi[2 * 10 + 0]) + + phi[2 * 10 + 9] * cos(q[0 * 3 + 2]) + + (RL * RL) * l[1 * 3 + 1] * phi[2 * 10 + 2] * + sin(q[0 * 3 + 2])) + + (phi[1 * 10 + 3] * phi[1 * 10 + 3]) / phi[1 * 10 + 0] + + ((RL * RL) * (phi[1 * 10 + 2] * phi[1 * 10 + 2])) / + phi[1 * 10 + 0] + + RL * l[4 * 3 + 1] * cos(G[0 * 3 + 2] * q[0 * 3 + 2]) * + (RL * phi[5 * 10 + 2] * + cos(G[0 * 3 + 2] * q[0 * 3 + 2]) + + RL * l[4 * 3 + 1] * phi[5 * 10 + 0] * + cos(G[0 * 3 + 2] * q[0 * 3 + 2])) + + RL * l[1 * 3 + 1] * cos(q[0 * 3 + 2]) * + (RL * phi[2 * 10 + 2] * cos(q[0 * 3 + 2]) + + RL * l[1 * 3 + 1] * phi[2 * 10 + 0] * + cos(q[0 * 3 + 2])) + + RL * l[4 * 3 + 1] * sin(G[0 * 3 + 2] * q[0 * 3 + 2]) * + (RL * phi[5 * 10 + 2] * + sin(G[0 * 3 + 2] * q[0 * 3 + 2]) + + RL * l[4 * 3 + 1] * phi[5 * 10 + 0] * + sin(G[0 * 3 + 2] * q[0 * 3 + 2])) + + RL * l[1 * 3 + 1] * sin(q[0 * 3 + 2]) * + (RL * phi[2 * 10 + 2] * sin(q[0 * 3 + 2]) + + RL * l[1 * 3 + 1] * phi[2 * 10 + 0] * + sin(q[0 * 3 + 2]))) + + RL * l[0 * 3 + 1] * cos(q[0 * 3 + 1]) * + (RL * phi[1 * 10 + 2] + + RL * phi[5 * 10 + 2] * + pow(sin(G[0 * 3 + 2] * q[0 * 3 + 2]), 2.0) + + RL * phi[2 * 10 + 2] * pow(cos(q[0 * 3 + 2]), 2.0) + + RL * phi[2 * 10 + 2] * pow(sin(q[0 * 3 + 2]), 2.0) + + RL * phi[5 * 10 + 2] * + pow(cos(G[0 * 3 + 2] * q[0 * 3 + 2]), 2.0) + + RL * l[4 * 3 + 1] * phi[5 * 10 + 0] * + pow(cos(G[0 * 3 + 2] * q[0 * 3 + 2]), 2.0) + + RL * l[4 * 3 + 1] * phi[5 * 10 + 0] * + pow(sin(G[0 * 3 + 2] * q[0 * 3 + 2]), 2.0) + + RL * l[1 * 3 + 1] * phi[2 * 10 + 0] * + pow(cos(q[0 * 3 + 2]), 2.0) + + RL * l[1 * 3 + 1] * phi[2 * 10 + 0] * + pow(sin(q[0 * 3 + 2]), 2.0)))) / + 2.0) + + qd[0 * 3 + 2] * + ((l[0 * 3 + 2] * + (cos(q[0 * 3 + 1]) * (phi[2 * 10 + 1] * cos(q[0 * 3 + 2]) + + phi[2 * 10 + 3] * sin(q[0 * 3 + 2])) + + sin(q[0 * 3 + 1]) * + (phi[2 * 10 + 3] * cos(q[0 * 3 + 2]) - + phi[2 * 10 + 1] * sin(q[0 * 3 + 2])))) / + 2.0 + + (cos(q[0 * 3 + 1]) * + (cos(q[0 * 3 + 1]) * + (l[1 * 3 + 2] * (phi[2 * 10 + 1] * cos(q[0 * 3 + 2]) + + phi[2 * 10 + 3] * sin(q[0 * 3 + 2])) - + sin(q[0 * 3 + 2]) * + (cos(q[0 * 3 + 2]) * + (phi[2 * 10 + 6] - + phi[2 * 10 + 0] * + (1.0 / + (phi[2 * 10 + 0] * + phi[2 * 10 + 0]) * + (phi[2 * 10 + 1] * + phi[2 * 10 + 1]) + + (RL * RL) * 1.0 / + (phi[2 * 10 + 0] * + phi[2 * 10 + 0]) * + (phi[2 * 10 + 2] * + phi[2 * 10 + 2])) + + (phi[2 * 10 + 1] * phi[2 * 10 + 1]) / + phi[2 * 10 + 0] + + ((RL * RL) * + (phi[2 * 10 + 2] * phi[2 * 10 + 2])) / + phi[2 * 10 + 0]) - + phi[2 * 10 + 9] * sin(q[0 * 3 + 2]) + + (RL * RL) * l[1 * 3 + 1] * phi[2 * 10 + 2] * + cos(q[0 * 3 + 2])) + + cos(q[0 * 3 + 2]) * + (sin(q[0 * 3 + 2]) * + (phi[2 * 10 + 4] - + phi[2 * 10 + 0] * + (1.0 / + (phi[2 * 10 + 0] * + phi[2 * 10 + 0]) * + (phi[2 * 10 + 3] * + phi[2 * 10 + 3]) + + (RL * RL) * 1.0 / + (phi[2 * 10 + 0] * + phi[2 * 10 + 0]) * + (phi[2 * 10 + 2] * + phi[2 * 10 + 2])) + + (phi[2 * 10 + 3] * phi[2 * 10 + 3]) / + phi[2 * 10 + 0] + + ((RL * RL) * + (phi[2 * 10 + 2] * phi[2 * 10 + 2])) / + phi[2 * 10 + 0]) - + phi[2 * 10 + 9] * cos(q[0 * 3 + 2]) + + (RL * RL) * l[1 * 3 + 1] * phi[2 * 10 + 2] * + sin(q[0 * 3 + 2])) - + cos(q[0 * 3 + 2]) * + (-l[1 * 3 + 2] * phi[2 * 10 + 1] + + sin(q[0 * 3 + 2]) * + (phi[2 * 10 + 6] - + phi[2 * 10 + 0] * + (1.0 / + (phi[2 * 10 + 0] * + phi[2 * 10 + 0]) * + (phi[2 * 10 + 1] * + phi[2 * 10 + 1]) + + (RL * RL) * 1.0 / + (phi[2 * 10 + 0] * + phi[2 * 10 + 0]) * + (phi[2 * 10 + 2] * + phi[2 * 10 + 2])) + + (phi[2 * 10 + 1] * phi[2 * 10 + 1]) / + phi[2 * 10 + 0] + + ((RL * RL) * + (phi[2 * 10 + 2] * phi[2 * 10 + 2])) / + phi[2 * 10 + 0]) + + phi[2 * 10 + 9] * cos(q[0 * 3 + 2]) + + (RL * RL) * l[1 * 3 + 1] * phi[2 * 10 + 2] * + sin(q[0 * 3 + 2])) + + sin(q[0 * 3 + 2]) * + (l[1 * 3 + 2] * phi[2 * 10 + 3] + + cos(q[0 * 3 + 2]) * + (phi[2 * 10 + 4] - + phi[2 * 10 + 0] * + (1.0 / + (phi[2 * 10 + 0] * + phi[2 * 10 + 0]) * + (phi[2 * 10 + 3] * + phi[2 * 10 + 3]) + + (RL * RL) * 1.0 / + (phi[2 * 10 + 0] * + phi[2 * 10 + 0]) * + (phi[2 * 10 + 2] * + phi[2 * 10 + 2])) + + (phi[2 * 10 + 3] * phi[2 * 10 + 3]) / + phi[2 * 10 + 0] + + ((RL * RL) * + (phi[2 * 10 + 2] * phi[2 * 10 + 2])) / + phi[2 * 10 + 0]) + + phi[2 * 10 + 9] * sin(q[0 * 3 + 2]) + + (RL * RL) * l[1 * 3 + 1] * phi[2 * 10 + 2] * + cos(q[0 * 3 + 2]))) + + sin(q[0 * 3 + 1]) * + (l[1 * 3 + 2] * (phi[2 * 10 + 3] * cos(q[0 * 3 + 2]) - + phi[2 * 10 + 1] * sin(q[0 * 3 + 2])) + + cos(q[0 * 3 + 2]) * + (cos(q[0 * 3 + 2]) * + (phi[2 * 10 + 4] - + phi[2 * 10 + 0] * + (1.0 / + (phi[2 * 10 + 0] * + phi[2 * 10 + 0]) * + (phi[2 * 10 + 3] * + phi[2 * 10 + 3]) + + (RL * RL) * 1.0 / + (phi[2 * 10 + 0] * + phi[2 * 10 + 0]) * + (phi[2 * 10 + 2] * + phi[2 * 10 + 2])) + + (phi[2 * 10 + 3] * phi[2 * 10 + 3]) / + phi[2 * 10 + 0] + + ((RL * RL) * + (phi[2 * 10 + 2] * phi[2 * 10 + 2])) / + phi[2 * 10 + 0]) + + phi[2 * 10 + 9] * sin(q[0 * 3 + 2]) + + (RL * RL) * l[1 * 3 + 1] * phi[2 * 10 + 2] * + cos(q[0 * 3 + 2])) + + sin(q[0 * 3 + 2]) * + (sin(q[0 * 3 + 2]) * + (phi[2 * 10 + 6] - + phi[2 * 10 + 0] * + (1.0 / + (phi[2 * 10 + 0] * + phi[2 * 10 + 0]) * + (phi[2 * 10 + 1] * + phi[2 * 10 + 1]) + + (RL * RL) * 1.0 / + (phi[2 * 10 + 0] * + phi[2 * 10 + 0]) * + (phi[2 * 10 + 2] * + phi[2 * 10 + 2])) + + (phi[2 * 10 + 1] * phi[2 * 10 + 1]) / + phi[2 * 10 + 0] + + ((RL * RL) * + (phi[2 * 10 + 2] * phi[2 * 10 + 2])) / + phi[2 * 10 + 0]) + + phi[2 * 10 + 9] * cos(q[0 * 3 + 2]) + + (RL * RL) * l[1 * 3 + 1] * phi[2 * 10 + 2] * + sin(q[0 * 3 + 2])) - + cos(q[0 * 3 + 2]) * + (l[1 * 3 + 0] * phi[2 * 10 + 1] + + cos(q[0 * 3 + 2]) * + (phi[2 * 10 + 6] - + phi[2 * 10 + 0] * + (1.0 / + (phi[2 * 10 + 0] * + phi[2 * 10 + 0]) * + (phi[2 * 10 + 1] * + phi[2 * 10 + 1]) + + (RL * RL) * 1.0 / + (phi[2 * 10 + 0] * + phi[2 * 10 + 0]) * + (phi[2 * 10 + 2] * + phi[2 * 10 + 2])) + + (phi[2 * 10 + 1] * phi[2 * 10 + 1]) / + phi[2 * 10 + 0] + + ((RL * RL) * + (phi[2 * 10 + 2] * phi[2 * 10 + 2])) / + phi[2 * 10 + 0]) - + phi[2 * 10 + 9] * sin(q[0 * 3 + 2]) + + (RL * RL) * l[1 * 3 + 1] * phi[2 * 10 + 2] * + cos(q[0 * 3 + 2])) - + sin(q[0 * 3 + 2]) * + (l[1 * 3 + 0] * phi[2 * 10 + 3] + + sin(q[0 * 3 + 2]) * + (phi[2 * 10 + 4] - + phi[2 * 10 + 0] * + (1.0 / + (phi[2 * 10 + 0] * + phi[2 * 10 + 0]) * + (phi[2 * 10 + 3] * + phi[2 * 10 + 3]) + + (RL * RL) * 1.0 / + (phi[2 * 10 + 0] * + phi[2 * 10 + 0]) * + (phi[2 * 10 + 2] * + phi[2 * 10 + 2])) + + (phi[2 * 10 + 3] * phi[2 * 10 + 3]) / + phi[2 * 10 + 0] + + ((RL * RL) * + (phi[2 * 10 + 2] * phi[2 * 10 + 2])) / + phi[2 * 10 + 0]) - + phi[2 * 10 + 9] * cos(q[0 * 3 + 2]) + + (RL * RL) * l[1 * 3 + 1] * phi[2 * 10 + 2] * + sin(q[0 * 3 + 2]))) + + l[0 * 3 + 2] * (phi[2 * 10 + 1] * cos(q[0 * 3 + 2]) + + phi[2 * 10 + 3] * sin(q[0 * 3 + 2])))) / + 2.0 - + (sin(q[0 * 3 + 1]) * + (cos(q[0 * 3 + 1]) * + (l[1 * 3 + 0] * (phi[2 * 10 + 1] * cos(q[0 * 3 + 2]) + + phi[2 * 10 + 3] * sin(q[0 * 3 + 2])) + + cos(q[0 * 3 + 2]) * + (cos(q[0 * 3 + 2]) * + (phi[2 * 10 + 6] - + phi[2 * 10 + 0] * + (1.0 / + (phi[2 * 10 + 0] * + phi[2 * 10 + 0]) * + (phi[2 * 10 + 1] * + phi[2 * 10 + 1]) + + (RL * RL) * 1.0 / + (phi[2 * 10 + 0] * + phi[2 * 10 + 0]) * + (phi[2 * 10 + 2] * + phi[2 * 10 + 2])) + + (phi[2 * 10 + 1] * phi[2 * 10 + 1]) / + phi[2 * 10 + 0] + + ((RL * RL) * + (phi[2 * 10 + 2] * phi[2 * 10 + 2])) / + phi[2 * 10 + 0]) - + phi[2 * 10 + 9] * sin(q[0 * 3 + 2]) + + (RL * RL) * l[1 * 3 + 1] * phi[2 * 10 + 2] * + cos(q[0 * 3 + 2])) + + sin(q[0 * 3 + 2]) * + (sin(q[0 * 3 + 2]) * + (phi[2 * 10 + 4] - + phi[2 * 10 + 0] * + (1.0 / + (phi[2 * 10 + 0] * + phi[2 * 10 + 0]) * + (phi[2 * 10 + 3] * + phi[2 * 10 + 3]) + + (RL * RL) * 1.0 / + (phi[2 * 10 + 0] * + phi[2 * 10 + 0]) * + (phi[2 * 10 + 2] * + phi[2 * 10 + 2])) + + (phi[2 * 10 + 3] * phi[2 * 10 + 3]) / + phi[2 * 10 + 0] + + ((RL * RL) * + (phi[2 * 10 + 2] * phi[2 * 10 + 2])) / + phi[2 * 10 + 0]) - + phi[2 * 10 + 9] * cos(q[0 * 3 + 2]) + + (RL * RL) * l[1 * 3 + 1] * phi[2 * 10 + 2] * + sin(q[0 * 3 + 2])) - + cos(q[0 * 3 + 2]) * + (l[1 * 3 + 2] * phi[2 * 10 + 3] + + cos(q[0 * 3 + 2]) * + (phi[2 * 10 + 4] - + phi[2 * 10 + 0] * + (1.0 / + (phi[2 * 10 + 0] * + phi[2 * 10 + 0]) * + (phi[2 * 10 + 3] * + phi[2 * 10 + 3]) + + (RL * RL) * 1.0 / + (phi[2 * 10 + 0] * + phi[2 * 10 + 0]) * + (phi[2 * 10 + 2] * + phi[2 * 10 + 2])) + + (phi[2 * 10 + 3] * phi[2 * 10 + 3]) / + phi[2 * 10 + 0] + + ((RL * RL) * + (phi[2 * 10 + 2] * phi[2 * 10 + 2])) / + phi[2 * 10 + 0]) + + phi[2 * 10 + 9] * sin(q[0 * 3 + 2]) + + (RL * RL) * l[1 * 3 + 1] * phi[2 * 10 + 2] * + cos(q[0 * 3 + 2])) - + sin(q[0 * 3 + 2]) * + (-l[1 * 3 + 2] * phi[2 * 10 + 1] + + sin(q[0 * 3 + 2]) * + (phi[2 * 10 + 6] - + phi[2 * 10 + 0] * + (1.0 / + (phi[2 * 10 + 0] * + phi[2 * 10 + 0]) * + (phi[2 * 10 + 1] * + phi[2 * 10 + 1]) + + (RL * RL) * 1.0 / + (phi[2 * 10 + 0] * + phi[2 * 10 + 0]) * + (phi[2 * 10 + 2] * + phi[2 * 10 + 2])) + + (phi[2 * 10 + 1] * phi[2 * 10 + 1]) / + phi[2 * 10 + 0] + + ((RL * RL) * + (phi[2 * 10 + 2] * phi[2 * 10 + 2])) / + phi[2 * 10 + 0]) + + phi[2 * 10 + 9] * cos(q[0 * 3 + 2]) + + (RL * RL) * l[1 * 3 + 1] * phi[2 * 10 + 2] * + sin(q[0 * 3 + 2]))) + + sin(q[0 * 3 + 1]) * + (l[1 * 3 + 0] * (phi[2 * 10 + 3] * cos(q[0 * 3 + 2]) - + phi[2 * 10 + 1] * sin(q[0 * 3 + 2])) - + cos(q[0 * 3 + 2]) * + (sin(q[0 * 3 + 2]) * + (phi[2 * 10 + 6] - + phi[2 * 10 + 0] * + (1.0 / + (phi[2 * 10 + 0] * + phi[2 * 10 + 0]) * + (phi[2 * 10 + 1] * + phi[2 * 10 + 1]) + + (RL * RL) * 1.0 / + (phi[2 * 10 + 0] * + phi[2 * 10 + 0]) * + (phi[2 * 10 + 2] * + phi[2 * 10 + 2])) + + (phi[2 * 10 + 1] * phi[2 * 10 + 1]) / + phi[2 * 10 + 0] + + ((RL * RL) * + (phi[2 * 10 + 2] * phi[2 * 10 + 2])) / + phi[2 * 10 + 0]) + + phi[2 * 10 + 9] * cos(q[0 * 3 + 2]) + + (RL * RL) * l[1 * 3 + 1] * phi[2 * 10 + 2] * + sin(q[0 * 3 + 2])) + + sin(q[0 * 3 + 2]) * + (cos(q[0 * 3 + 2]) * + (phi[2 * 10 + 4] - + phi[2 * 10 + 0] * + (1.0 / + (phi[2 * 10 + 0] * + phi[2 * 10 + 0]) * + (phi[2 * 10 + 3] * + phi[2 * 10 + 3]) + + (RL * RL) * 1.0 / + (phi[2 * 10 + 0] * + phi[2 * 10 + 0]) * + (phi[2 * 10 + 2] * + phi[2 * 10 + 2])) + + (phi[2 * 10 + 3] * phi[2 * 10 + 3]) / + phi[2 * 10 + 0] + + ((RL * RL) * + (phi[2 * 10 + 2] * phi[2 * 10 + 2])) / + phi[2 * 10 + 0]) + + phi[2 * 10 + 9] * sin(q[0 * 3 + 2]) + + (RL * RL) * l[1 * 3 + 1] * phi[2 * 10 + 2] * + cos(q[0 * 3 + 2])) - + sin(q[0 * 3 + 2]) * + (l[1 * 3 + 0] * phi[2 * 10 + 1] + + cos(q[0 * 3 + 2]) * + (phi[2 * 10 + 6] - + phi[2 * 10 + 0] * + (1.0 / + (phi[2 * 10 + 0] * + phi[2 * 10 + 0]) * + (phi[2 * 10 + 1] * + phi[2 * 10 + 1]) + + (RL * RL) * 1.0 / + (phi[2 * 10 + 0] * + phi[2 * 10 + 0]) * + (phi[2 * 10 + 2] * + phi[2 * 10 + 2])) + + (phi[2 * 10 + 1] * phi[2 * 10 + 1]) / + phi[2 * 10 + 0] + + ((RL * RL) * + (phi[2 * 10 + 2] * phi[2 * 10 + 2])) / + phi[2 * 10 + 0]) - + phi[2 * 10 + 9] * sin(q[0 * 3 + 2]) + + (RL * RL) * l[1 * 3 + 1] * phi[2 * 10 + 2] * + cos(q[0 * 3 + 2])) + + cos(q[0 * 3 + 2]) * + (l[1 * 3 + 0] * phi[2 * 10 + 3] + + sin(q[0 * 3 + 2]) * + (phi[2 * 10 + 4] - + phi[2 * 10 + 0] * + (1.0 / + (phi[2 * 10 + 0] * + phi[2 * 10 + 0]) * + (phi[2 * 10 + 3] * + phi[2 * 10 + 3]) + + (RL * RL) * 1.0 / + (phi[2 * 10 + 0] * + phi[2 * 10 + 0]) * + (phi[2 * 10 + 2] * + phi[2 * 10 + 2])) + + (phi[2 * 10 + 3] * phi[2 * 10 + 3]) / + phi[2 * 10 + 0] + + ((RL * RL) * + (phi[2 * 10 + 2] * phi[2 * 10 + 2])) / + phi[2 * 10 + 0]) - + phi[2 * 10 + 9] * cos(q[0 * 3 + 2]) + + (RL * RL) * l[1 * 3 + 1] * phi[2 * 10 + 2] * + sin(q[0 * 3 + 2]))) - + l[0 * 3 + 2] * (phi[2 * 10 + 3] * cos(q[0 * 3 + 2]) - + phi[2 * 10 + 1] * sin(q[0 * 3 + 2])))) / + 2.0) + + G[0 * 3 + 1] * qd[0 * 3 + 1] * + (sin(G[0 * 3 + 1] * q[0 * 3 + 1]) * + (cos(G[0 * 3 + 1] * q[0 * 3 + 1]) * + (phi[4 * 10 + 6] - + phi[4 * 10 + 0] * + (1.0 / (phi[4 * 10 + 0] * phi[4 * 10 + 0]) * + (phi[4 * 10 + 1] * phi[4 * 10 + 1]) + + (RL * RL) * 1.0 / + (phi[4 * 10 + 0] * phi[4 * 10 + 0]) * + (phi[4 * 10 + 2] * phi[4 * 10 + 2])) + + (phi[4 * 10 + 1] * phi[4 * 10 + 1]) / + phi[4 * 10 + 0] + + ((RL * RL) * (phi[4 * 10 + 2] * phi[4 * 10 + 2])) / + phi[4 * 10 + 0]) - + phi[4 * 10 + 9] * sin(G[0 * 3 + 1] * q[0 * 3 + 1]) + + (RL * RL) * l[3 * 3 + 1] * phi[4 * 10 + 2] * + cos(G[0 * 3 + 1] * q[0 * 3 + 1])) * + (-1.0 / 2.0) + + (cos(G[0 * 3 + 1] * q[0 * 3 + 1]) * + (sin(G[0 * 3 + 1] * q[0 * 3 + 1]) * + (phi[4 * 10 + 4] - + phi[4 * 10 + 0] * + (1.0 / (phi[4 * 10 + 0] * phi[4 * 10 + 0]) * + (phi[4 * 10 + 3] * phi[4 * 10 + 3]) + + (RL * RL) * 1.0 / + (phi[4 * 10 + 0] * phi[4 * 10 + 0]) * + (phi[4 * 10 + 2] * phi[4 * 10 + 2])) + + (phi[4 * 10 + 3] * phi[4 * 10 + 3]) / phi[4 * 10 + 0] + + ((RL * RL) * (phi[4 * 10 + 2] * phi[4 * 10 + 2])) / + phi[4 * 10 + 0]) - + phi[4 * 10 + 9] * cos(G[0 * 3 + 1] * q[0 * 3 + 1]) + + (RL * RL) * l[3 * 3 + 1] * phi[4 * 10 + 2] * + sin(G[0 * 3 + 1] * q[0 * 3 + 1]))) / + 2.0 - + (cos(G[0 * 3 + 1] * q[0 * 3 + 1]) * + (-l[3 * 3 + 2] * phi[4 * 10 + 1] + + sin(G[0 * 3 + 1] * q[0 * 3 + 1]) * + (phi[4 * 10 + 6] - + phi[4 * 10 + 0] * + (1.0 / (phi[4 * 10 + 0] * phi[4 * 10 + 0]) * + (phi[4 * 10 + 1] * phi[4 * 10 + 1]) + + (RL * RL) * 1.0 / + (phi[4 * 10 + 0] * phi[4 * 10 + 0]) * + (phi[4 * 10 + 2] * phi[4 * 10 + 2])) + + (phi[4 * 10 + 1] * phi[4 * 10 + 1]) / phi[4 * 10 + 0] + + ((RL * RL) * (phi[4 * 10 + 2] * phi[4 * 10 + 2])) / + phi[4 * 10 + 0]) + + phi[4 * 10 + 9] * cos(G[0 * 3 + 1] * q[0 * 3 + 1]) + + (RL * RL) * l[3 * 3 + 1] * phi[4 * 10 + 2] * + sin(G[0 * 3 + 1] * q[0 * 3 + 1]))) / + 2.0 + + (sin(G[0 * 3 + 1] * q[0 * 3 + 1]) * + (cos(G[0 * 3 + 1] * q[0 * 3 + 1]) * + (phi[4 * 10 + 4] - + phi[4 * 10 + 0] * + (1.0 / (phi[4 * 10 + 0] * phi[4 * 10 + 0]) * + (phi[4 * 10 + 3] * phi[4 * 10 + 3]) + + (RL * RL) * 1.0 / + (phi[4 * 10 + 0] * phi[4 * 10 + 0]) * + (phi[4 * 10 + 2] * phi[4 * 10 + 2])) + + (phi[4 * 10 + 3] * phi[4 * 10 + 3]) / phi[4 * 10 + 0] + + ((RL * RL) * (phi[4 * 10 + 2] * phi[4 * 10 + 2])) / + phi[4 * 10 + 0]) + + l[3 * 3 + 2] * phi[4 * 10 + 3] + + phi[4 * 10 + 9] * sin(G[0 * 3 + 1] * q[0 * 3 + 1]) + + (RL * RL) * l[3 * 3 + 1] * phi[4 * 10 + 2] * + cos(G[0 * 3 + 1] * q[0 * 3 + 1]))) / + 2.0 + + (l[3 * 3 + 2] * + (phi[4 * 10 + 1] * cos(G[0 * 3 + 1] * q[0 * 3 + 1]) + + phi[4 * 10 + 3] * sin(G[0 * 3 + 1] * q[0 * 3 + 1]))) / + 2.0) + + G[0 * 3 + 2] * qd[0 * 3 + 2] * + ((cos(q[0 * 3 + 1]) * + (sin(q[0 * 3 + 1]) * + (cos(G[0 * 3 + 2] * q[0 * 3 + 2]) * + (cos(G[0 * 3 + 2] * q[0 * 3 + 2]) * + (phi[5 * 10 + 4] - + phi[5 * 10 + 0] * + (1.0 / + (phi[5 * 10 + 0] * + phi[5 * 10 + 0]) * + (phi[5 * 10 + 3] * + phi[5 * 10 + 3]) + + (RL * RL) * 1.0 / + (phi[5 * 10 + 0] * + phi[5 * 10 + 0]) * + (phi[5 * 10 + 2] * + phi[5 * 10 + 2])) + + (phi[5 * 10 + 3] * phi[5 * 10 + 3]) / + phi[5 * 10 + 0] + + ((RL * RL) * + (phi[5 * 10 + 2] * phi[5 * 10 + 2])) / + phi[5 * 10 + 0]) + + phi[5 * 10 + 9] * + sin(G[0 * 3 + 2] * q[0 * 3 + 2]) + + (RL * RL) * l[4 * 3 + 1] * phi[5 * 10 + 2] * + cos(G[0 * 3 + 2] * q[0 * 3 + 2])) + + sin(G[0 * 3 + 2] * q[0 * 3 + 2]) * + (sin(G[0 * 3 + 2] * q[0 * 3 + 2]) * + (phi[5 * 10 + 6] - + phi[5 * 10 + 0] * + (1.0 / + (phi[5 * 10 + 0] * + phi[5 * 10 + 0]) * + (phi[5 * 10 + 1] * + phi[5 * 10 + 1]) + + (RL * RL) * 1.0 / + (phi[5 * 10 + 0] * + phi[5 * 10 + 0]) * + (phi[5 * 10 + 2] * + phi[5 * 10 + 2])) + + (phi[5 * 10 + 1] * phi[5 * 10 + 1]) / + phi[5 * 10 + 0] + + ((RL * RL) * + (phi[5 * 10 + 2] * phi[5 * 10 + 2])) / + phi[5 * 10 + 0]) + + phi[5 * 10 + 9] * + cos(G[0 * 3 + 2] * q[0 * 3 + 2]) + + (RL * RL) * l[4 * 3 + 1] * phi[5 * 10 + 2] * + sin(G[0 * 3 + 2] * q[0 * 3 + 2])) - + cos(G[0 * 3 + 2] * q[0 * 3 + 2]) * + (cos(G[0 * 3 + 2] * q[0 * 3 + 2]) * + (phi[5 * 10 + 6] - + phi[5 * 10 + 0] * + (1.0 / + (phi[5 * 10 + 0] * + phi[5 * 10 + 0]) * + (phi[5 * 10 + 1] * + phi[5 * 10 + 1]) + + (RL * RL) * 1.0 / + (phi[5 * 10 + 0] * + phi[5 * 10 + 0]) * + (phi[5 * 10 + 2] * + phi[5 * 10 + 2])) + + (phi[5 * 10 + 1] * phi[5 * 10 + 1]) / + phi[5 * 10 + 0] + + ((RL * RL) * + (phi[5 * 10 + 2] * phi[5 * 10 + 2])) / + phi[5 * 10 + 0]) + + l[4 * 3 + 0] * phi[5 * 10 + 1] - + phi[5 * 10 + 9] * + sin(G[0 * 3 + 2] * q[0 * 3 + 2]) + + (RL * RL) * l[4 * 3 + 1] * phi[5 * 10 + 2] * + cos(G[0 * 3 + 2] * q[0 * 3 + 2])) - + sin(G[0 * 3 + 2] * q[0 * 3 + 2]) * + (l[4 * 3 + 0] * phi[5 * 10 + 3] + + sin(G[0 * 3 + 2] * q[0 * 3 + 2]) * + (phi[5 * 10 + 4] - + phi[5 * 10 + 0] * + (1.0 / + (phi[5 * 10 + 0] * + phi[5 * 10 + 0]) * + (phi[5 * 10 + 3] * + phi[5 * 10 + 3]) + + (RL * RL) * 1.0 / + (phi[5 * 10 + 0] * + phi[5 * 10 + 0]) * + (phi[5 * 10 + 2] * + phi[5 * 10 + 2])) + + (phi[5 * 10 + 3] * phi[5 * 10 + 3]) / + phi[5 * 10 + 0] + + ((RL * RL) * + (phi[5 * 10 + 2] * phi[5 * 10 + 2])) / + phi[5 * 10 + 0]) - + phi[5 * 10 + 9] * + cos(G[0 * 3 + 2] * q[0 * 3 + 2]) + + (RL * RL) * l[4 * 3 + 1] * phi[5 * 10 + 2] * + sin(G[0 * 3 + 2] * q[0 * 3 + 2])) + + l[4 * 3 + 2] * (phi[5 * 10 + 3] * + cos(G[0 * 3 + 2] * q[0 * 3 + 2]) - + phi[5 * 10 + 1] * sin(G[0 * 3 + 2] * + q[0 * 3 + 2]))) + + l[0 * 3 + 2] * + (phi[5 * 10 + 1] * cos(G[0 * 3 + 2] * q[0 * 3 + 2]) + + phi[5 * 10 + 3] * sin(G[0 * 3 + 2] * q[0 * 3 + 2])) + + cos(q[0 * 3 + 1]) * + (-sin(G[0 * 3 + 2] * q[0 * 3 + 2]) * + (cos(G[0 * 3 + 2] * q[0 * 3 + 2]) * + (phi[5 * 10 + 6] - + phi[5 * 10 + 0] * + (1.0 / + (phi[5 * 10 + 0] * + phi[5 * 10 + 0]) * + (phi[5 * 10 + 1] * + phi[5 * 10 + 1]) + + (RL * RL) * 1.0 / + (phi[5 * 10 + 0] * + phi[5 * 10 + 0]) * + (phi[5 * 10 + 2] * + phi[5 * 10 + 2])) + + (phi[5 * 10 + 1] * phi[5 * 10 + 1]) / + phi[5 * 10 + 0] + + ((RL * RL) * + (phi[5 * 10 + 2] * phi[5 * 10 + 2])) / + phi[5 * 10 + 0]) - + phi[5 * 10 + 9] * + sin(G[0 * 3 + 2] * q[0 * 3 + 2]) + + (RL * RL) * l[4 * 3 + 1] * phi[5 * 10 + 2] * + cos(G[0 * 3 + 2] * q[0 * 3 + 2])) + + cos(G[0 * 3 + 2] * q[0 * 3 + 2]) * + (sin(G[0 * 3 + 2] * q[0 * 3 + 2]) * + (phi[5 * 10 + 4] - + phi[5 * 10 + 0] * + (1.0 / + (phi[5 * 10 + 0] * + phi[5 * 10 + 0]) * + (phi[5 * 10 + 3] * + phi[5 * 10 + 3]) + + (RL * RL) * 1.0 / + (phi[5 * 10 + 0] * + phi[5 * 10 + 0]) * + (phi[5 * 10 + 2] * + phi[5 * 10 + 2])) + + (phi[5 * 10 + 3] * phi[5 * 10 + 3]) / + phi[5 * 10 + 0] + + ((RL * RL) * + (phi[5 * 10 + 2] * phi[5 * 10 + 2])) / + phi[5 * 10 + 0]) - + phi[5 * 10 + 9] * + cos(G[0 * 3 + 2] * q[0 * 3 + 2]) + + (RL * RL) * l[4 * 3 + 1] * phi[5 * 10 + 2] * + sin(G[0 * 3 + 2] * q[0 * 3 + 2])) - + cos(G[0 * 3 + 2] * q[0 * 3 + 2]) * + (-l[4 * 3 + 2] * phi[5 * 10 + 1] + + sin(G[0 * 3 + 2] * q[0 * 3 + 2]) * + (phi[5 * 10 + 6] - + phi[5 * 10 + 0] * + (1.0 / + (phi[5 * 10 + 0] * + phi[5 * 10 + 0]) * + (phi[5 * 10 + 1] * + phi[5 * 10 + 1]) + + (RL * RL) * 1.0 / + (phi[5 * 10 + 0] * + phi[5 * 10 + 0]) * + (phi[5 * 10 + 2] * + phi[5 * 10 + 2])) + + (phi[5 * 10 + 1] * phi[5 * 10 + 1]) / + phi[5 * 10 + 0] + + ((RL * RL) * + (phi[5 * 10 + 2] * phi[5 * 10 + 2])) / + phi[5 * 10 + 0]) + + phi[5 * 10 + 9] * + cos(G[0 * 3 + 2] * q[0 * 3 + 2]) + + (RL * RL) * l[4 * 3 + 1] * phi[5 * 10 + 2] * + sin(G[0 * 3 + 2] * q[0 * 3 + 2])) + + sin(G[0 * 3 + 2] * q[0 * 3 + 2]) * + (cos(G[0 * 3 + 2] * q[0 * 3 + 2]) * + (phi[5 * 10 + 4] - + phi[5 * 10 + 0] * + (1.0 / + (phi[5 * 10 + 0] * + phi[5 * 10 + 0]) * + (phi[5 * 10 + 3] * + phi[5 * 10 + 3]) + + (RL * RL) * 1.0 / + (phi[5 * 10 + 0] * + phi[5 * 10 + 0]) * + (phi[5 * 10 + 2] * + phi[5 * 10 + 2])) + + (phi[5 * 10 + 3] * phi[5 * 10 + 3]) / + phi[5 * 10 + 0] + + ((RL * RL) * + (phi[5 * 10 + 2] * phi[5 * 10 + 2])) / + phi[5 * 10 + 0]) + + l[4 * 3 + 2] * phi[5 * 10 + 3] + + phi[5 * 10 + 9] * + sin(G[0 * 3 + 2] * q[0 * 3 + 2]) + + (RL * RL) * l[4 * 3 + 1] * phi[5 * 10 + 2] * + cos(G[0 * 3 + 2] * q[0 * 3 + 2])) + + l[4 * 3 + 2] * + (phi[5 * 10 + 1] * + cos(G[0 * 3 + 2] * q[0 * 3 + 2]) + + phi[5 * 10 + 3] * + sin(G[0 * 3 + 2] * q[0 * 3 + 2]))))) / + 2.0 - + (sin(q[0 * 3 + 1]) * + (sin(q[0 * 3 + 1]) * + (-cos(G[0 * 3 + 2] * q[0 * 3 + 2]) * + (sin(G[0 * 3 + 2] * q[0 * 3 + 2]) * + (phi[5 * 10 + 6] - + phi[5 * 10 + 0] * + (1.0 / + (phi[5 * 10 + 0] * + phi[5 * 10 + 0]) * + (phi[5 * 10 + 1] * + phi[5 * 10 + 1]) + + (RL * RL) * 1.0 / + (phi[5 * 10 + 0] * + phi[5 * 10 + 0]) * + (phi[5 * 10 + 2] * + phi[5 * 10 + 2])) + + (phi[5 * 10 + 1] * phi[5 * 10 + 1]) / + phi[5 * 10 + 0] + + ((RL * RL) * + (phi[5 * 10 + 2] * phi[5 * 10 + 2])) / + phi[5 * 10 + 0]) + + phi[5 * 10 + 9] * + cos(G[0 * 3 + 2] * q[0 * 3 + 2]) + + (RL * RL) * l[4 * 3 + 1] * phi[5 * 10 + 2] * + sin(G[0 * 3 + 2] * q[0 * 3 + 2])) + + sin(G[0 * 3 + 2] * q[0 * 3 + 2]) * + (cos(G[0 * 3 + 2] * q[0 * 3 + 2]) * + (phi[5 * 10 + 4] - + phi[5 * 10 + 0] * + (1.0 / + (phi[5 * 10 + 0] * + phi[5 * 10 + 0]) * + (phi[5 * 10 + 3] * + phi[5 * 10 + 3]) + + (RL * RL) * 1.0 / + (phi[5 * 10 + 0] * + phi[5 * 10 + 0]) * + (phi[5 * 10 + 2] * + phi[5 * 10 + 2])) + + (phi[5 * 10 + 3] * phi[5 * 10 + 3]) / + phi[5 * 10 + 0] + + ((RL * RL) * + (phi[5 * 10 + 2] * phi[5 * 10 + 2])) / + phi[5 * 10 + 0]) + + phi[5 * 10 + 9] * + sin(G[0 * 3 + 2] * q[0 * 3 + 2]) + + (RL * RL) * l[4 * 3 + 1] * phi[5 * 10 + 2] * + cos(G[0 * 3 + 2] * q[0 * 3 + 2])) - + sin(G[0 * 3 + 2] * q[0 * 3 + 2]) * + (cos(G[0 * 3 + 2] * q[0 * 3 + 2]) * + (phi[5 * 10 + 6] - + phi[5 * 10 + 0] * + (1.0 / + (phi[5 * 10 + 0] * + phi[5 * 10 + 0]) * + (phi[5 * 10 + 1] * + phi[5 * 10 + 1]) + + (RL * RL) * 1.0 / + (phi[5 * 10 + 0] * + phi[5 * 10 + 0]) * + (phi[5 * 10 + 2] * + phi[5 * 10 + 2])) + + (phi[5 * 10 + 1] * phi[5 * 10 + 1]) / + phi[5 * 10 + 0] + + ((RL * RL) * + (phi[5 * 10 + 2] * phi[5 * 10 + 2])) / + phi[5 * 10 + 0]) + + l[4 * 3 + 0] * phi[5 * 10 + 1] - + phi[5 * 10 + 9] * + sin(G[0 * 3 + 2] * q[0 * 3 + 2]) + + (RL * RL) * l[4 * 3 + 1] * phi[5 * 10 + 2] * + cos(G[0 * 3 + 2] * q[0 * 3 + 2])) + + cos(G[0 * 3 + 2] * q[0 * 3 + 2]) * + (l[4 * 3 + 0] * phi[5 * 10 + 3] + + sin(G[0 * 3 + 2] * q[0 * 3 + 2]) * + (phi[5 * 10 + 4] - + phi[5 * 10 + 0] * + (1.0 / + (phi[5 * 10 + 0] * + phi[5 * 10 + 0]) * + (phi[5 * 10 + 3] * + phi[5 * 10 + 3]) + + (RL * RL) * 1.0 / + (phi[5 * 10 + 0] * + phi[5 * 10 + 0]) * + (phi[5 * 10 + 2] * + phi[5 * 10 + 2])) + + (phi[5 * 10 + 3] * phi[5 * 10 + 3]) / + phi[5 * 10 + 0] + + ((RL * RL) * + (phi[5 * 10 + 2] * phi[5 * 10 + 2])) / + phi[5 * 10 + 0]) - + phi[5 * 10 + 9] * + cos(G[0 * 3 + 2] * q[0 * 3 + 2]) + + (RL * RL) * l[4 * 3 + 1] * phi[5 * 10 + 2] * + sin(G[0 * 3 + 2] * q[0 * 3 + 2])) + + l[4 * 3 + 0] * (phi[5 * 10 + 3] * + cos(G[0 * 3 + 2] * q[0 * 3 + 2]) - + phi[5 * 10 + 1] * sin(G[0 * 3 + 2] * + q[0 * 3 + 2]))) - + l[0 * 3 + 2] * + (phi[5 * 10 + 3] * cos(G[0 * 3 + 2] * q[0 * 3 + 2]) - + phi[5 * 10 + 1] * sin(G[0 * 3 + 2] * q[0 * 3 + 2])) + + cos(q[0 * 3 + 1]) * + (cos(G[0 * 3 + 2] * q[0 * 3 + 2]) * + (cos(G[0 * 3 + 2] * q[0 * 3 + 2]) * + (phi[5 * 10 + 6] - + phi[5 * 10 + 0] * + (1.0 / + (phi[5 * 10 + 0] * + phi[5 * 10 + 0]) * + (phi[5 * 10 + 1] * + phi[5 * 10 + 1]) + + (RL * RL) * 1.0 / + (phi[5 * 10 + 0] * + phi[5 * 10 + 0]) * + (phi[5 * 10 + 2] * + phi[5 * 10 + 2])) + + (phi[5 * 10 + 1] * phi[5 * 10 + 1]) / + phi[5 * 10 + 0] + + ((RL * RL) * + (phi[5 * 10 + 2] * phi[5 * 10 + 2])) / + phi[5 * 10 + 0]) - + phi[5 * 10 + 9] * + sin(G[0 * 3 + 2] * q[0 * 3 + 2]) + + (RL * RL) * l[4 * 3 + 1] * phi[5 * 10 + 2] * + cos(G[0 * 3 + 2] * q[0 * 3 + 2])) + + sin(G[0 * 3 + 2] * q[0 * 3 + 2]) * + (sin(G[0 * 3 + 2] * q[0 * 3 + 2]) * + (phi[5 * 10 + 4] - + phi[5 * 10 + 0] * + (1.0 / + (phi[5 * 10 + 0] * + phi[5 * 10 + 0]) * + (phi[5 * 10 + 3] * + phi[5 * 10 + 3]) + + (RL * RL) * 1.0 / + (phi[5 * 10 + 0] * + phi[5 * 10 + 0]) * + (phi[5 * 10 + 2] * + phi[5 * 10 + 2])) + + (phi[5 * 10 + 3] * phi[5 * 10 + 3]) / + phi[5 * 10 + 0] + + ((RL * RL) * + (phi[5 * 10 + 2] * phi[5 * 10 + 2])) / + phi[5 * 10 + 0]) - + phi[5 * 10 + 9] * + cos(G[0 * 3 + 2] * q[0 * 3 + 2]) + + (RL * RL) * l[4 * 3 + 1] * phi[5 * 10 + 2] * + sin(G[0 * 3 + 2] * q[0 * 3 + 2])) - + cos(G[0 * 3 + 2] * q[0 * 3 + 2]) * + (cos(G[0 * 3 + 2] * q[0 * 3 + 2]) * + (phi[5 * 10 + 4] - + phi[5 * 10 + 0] * + (1.0 / + (phi[5 * 10 + 0] * + phi[5 * 10 + 0]) * + (phi[5 * 10 + 3] * + phi[5 * 10 + 3]) + + (RL * RL) * 1.0 / + (phi[5 * 10 + 0] * + phi[5 * 10 + 0]) * + (phi[5 * 10 + 2] * + phi[5 * 10 + 2])) + + (phi[5 * 10 + 3] * phi[5 * 10 + 3]) / + phi[5 * 10 + 0] + + ((RL * RL) * + (phi[5 * 10 + 2] * phi[5 * 10 + 2])) / + phi[5 * 10 + 0]) + + l[4 * 3 + 2] * phi[5 * 10 + 3] + + phi[5 * 10 + 9] * + sin(G[0 * 3 + 2] * q[0 * 3 + 2]) + + (RL * RL) * l[4 * 3 + 1] * phi[5 * 10 + 2] * + cos(G[0 * 3 + 2] * q[0 * 3 + 2])) - + sin(G[0 * 3 + 2] * q[0 * 3 + 2]) * + (-l[4 * 3 + 2] * phi[5 * 10 + 1] + + sin(G[0 * 3 + 2] * q[0 * 3 + 2]) * + (phi[5 * 10 + 6] - + phi[5 * 10 + 0] * + (1.0 / + (phi[5 * 10 + 0] * + phi[5 * 10 + 0]) * + (phi[5 * 10 + 1] * + phi[5 * 10 + 1]) + + (RL * RL) * 1.0 / + (phi[5 * 10 + 0] * + phi[5 * 10 + 0]) * + (phi[5 * 10 + 2] * + phi[5 * 10 + 2])) + + (phi[5 * 10 + 1] * phi[5 * 10 + 1]) / + phi[5 * 10 + 0] + + ((RL * RL) * + (phi[5 * 10 + 2] * phi[5 * 10 + 2])) / + phi[5 * 10 + 0]) + + phi[5 * 10 + 9] * + cos(G[0 * 3 + 2] * q[0 * 3 + 2]) + + (RL * RL) * l[4 * 3 + 1] * phi[5 * 10 + 2] * + sin(G[0 * 3 + 2] * q[0 * 3 + 2])) + + l[4 * 3 + 0] * + (phi[5 * 10 + 1] * + cos(G[0 * 3 + 2] * q[0 * 3 + 2]) + + phi[5 * 10 + 3] * + sin(G[0 * 3 + 2] * q[0 * 3 + 2]))))) / + 2.0 + + (l[0 * 3 + 2] * + (cos(q[0 * 3 + 1]) * + (phi[5 * 10 + 1] * cos(G[0 * 3 + 2] * q[0 * 3 + 2]) + + phi[5 * 10 + 3] * sin(G[0 * 3 + 2] * q[0 * 3 + 2])) + + sin(q[0 * 3 + 1]) * + (phi[5 * 10 + 3] * cos(G[0 * 3 + 2] * q[0 * 3 + 2]) - + phi[5 * 10 + 1] * sin(G[0 * 3 + 2] * q[0 * 3 + 2])))) / + 2.0)) - + G[0 * 3 + 0] * + (phi[3 * 10 + 3] * sin(G[0 * 3 + 0] * q[0 * 3 + 0]) * + (9.81E+2 / 1.0E+2) - + RL * phi[3 * 10 + 2] * cos(G[0 * 3 + 0] * q[0 * 3 + 0]) * + (9.81E+2 / 1.0E+2)) - + l[0 * 3 + 2] * phi[1 * 10 + 0] * sin(q[0 * 3 + 0]) * + (9.81E+2 / 1.0E+2) - + l[0 * 3 + 2] * phi[2 * 10 + 0] * sin(q[0 * 3 + 0]) * + (9.81E+2 / 1.0E+2) - + l[0 * 3 + 2] * phi[5 * 10 + 0] * sin(q[0 * 3 + 0]) * + (9.81E+2 / 1.0E+2) - + l[3 * 3 + 2] * phi[4 * 10 + 0] * sin(q[0 * 3 + 0]) * + (9.81E+2 / 1.0E+2) - + phi[4 * 10 + 3] * cos(G[0 * 3 + 1] * q[0 * 3 + 1]) * sin(q[0 * 3 + 0]) * + (9.81E+2 / 1.0E+2) + + phi[4 * 10 + 1] * sin(G[0 * 3 + 1] * q[0 * 3 + 1]) * sin(q[0 * 3 + 0]) * + (9.81E+2 / 1.0E+2) - + phi[1 * 10 + 3] * cos(q[0 * 3 + 1]) * sin(q[0 * 3 + 0]) * + (9.81E+2 / 1.0E+2) + + phi[1 * 10 + 1] * sin(q[0 * 3 + 0]) * sin(q[0 * 3 + 1]) * + (9.81E+2 / 1.0E+2) + + RL * phi[0 * 10 + 2] * cos(q[0 * 3 + 0]) * (9.81E+2 / 1.0E+2) + + RL * phi[1 * 10 + 2] * cos(q[0 * 3 + 0]) * (9.81E+2 / 1.0E+2) + + RL * phi[2 * 10 + 2] * cos(q[0 * 3 + 0]) * (9.81E+2 / 1.0E+2) + + RL * phi[4 * 10 + 2] * cos(q[0 * 3 + 0]) * (9.81E+2 / 1.0E+2) + + RL * phi[5 * 10 + 2] * cos(q[0 * 3 + 0]) * (9.81E+2 / 1.0E+2) - + phi[5 * 10 + 3] * cos(G[0 * 3 + 2] * q[0 * 3 + 2]) * cos(q[0 * 3 + 1]) * + sin(q[0 * 3 + 0]) * (9.81E+2 / 1.0E+2) + + l[1 * 3 + 0] * phi[2 * 10 + 0] * sin(q[0 * 3 + 0]) * sin(q[0 * 3 + 1]) * + (9.81E+2 / 1.0E+2) + + l[4 * 3 + 0] * phi[5 * 10 + 0] * sin(q[0 * 3 + 0]) * sin(q[0 * 3 + 1]) * + (9.81E+2 / 1.0E+2) + + phi[5 * 10 + 1] * cos(G[0 * 3 + 2] * q[0 * 3 + 2]) * sin(q[0 * 3 + 0]) * + sin(q[0 * 3 + 1]) * (9.81E+2 / 1.0E+2) + + phi[5 * 10 + 1] * sin(G[0 * 3 + 2] * q[0 * 3 + 2]) * cos(q[0 * 3 + 1]) * + sin(q[0 * 3 + 0]) * (9.81E+2 / 1.0E+2) + + phi[5 * 10 + 3] * sin(G[0 * 3 + 2] * q[0 * 3 + 2]) * sin(q[0 * 3 + 0]) * + sin(q[0 * 3 + 1]) * (9.81E+2 / 1.0E+2) - + phi[2 * 10 + 3] * cos(q[0 * 3 + 1]) * cos(q[0 * 3 + 2]) * + sin(q[0 * 3 + 0]) * (9.81E+2 / 1.0E+2) + + RL * l[0 * 3 + 1] * phi[1 * 10 + 0] * cos(q[0 * 3 + 0]) * + (9.81E+2 / 1.0E+2) + + RL * l[0 * 3 + 1] * phi[2 * 10 + 0] * cos(q[0 * 3 + 0]) * + (9.81E+2 / 1.0E+2) + + RL * l[1 * 3 + 1] * phi[2 * 10 + 0] * cos(q[0 * 3 + 0]) * + (9.81E+2 / 1.0E+2) + + RL * l[0 * 3 + 1] * phi[5 * 10 + 0] * cos(q[0 * 3 + 0]) * + (9.81E+2 / 1.0E+2) + + RL * l[3 * 3 + 1] * phi[4 * 10 + 0] * cos(q[0 * 3 + 0]) * + (9.81E+2 / 1.0E+2) + + RL * l[4 * 3 + 1] * phi[5 * 10 + 0] * cos(q[0 * 3 + 0]) * + (9.81E+2 / 1.0E+2) + + phi[2 * 10 + 1] * cos(q[0 * 3 + 1]) * sin(q[0 * 3 + 0]) * + sin(q[0 * 3 + 2]) * (9.81E+2 / 1.0E+2) + + phi[2 * 10 + 1] * cos(q[0 * 3 + 2]) * sin(q[0 * 3 + 0]) * + sin(q[0 * 3 + 1]) * (9.81E+2 / 1.0E+2) + + phi[2 * 10 + 3] * sin(q[0 * 3 + 0]) * sin(q[0 * 3 + 1]) * + sin(q[0 * 3 + 2]) * (9.81E+2 / 1.0E+2) - + l[1 * 3 + 2] * phi[2 * 10 + 0] * cos(q[0 * 3 + 1]) * sin(q[0 * 3 + 0]) * + (9.81E+2 / 1.0E+2) - + l[4 * 3 + 2] * phi[5 * 10 + 0] * cos(q[0 * 3 + 1]) * sin(q[0 * 3 + 0]) * + (9.81E+2 / 1.0E+2); + F(0 * 3 + 1) = + -qd[0 * 3 + 2] * + (qd[0 * 3 + 1] * + (((phi[2 * 10 + 1] + + phi[2 * 10 + 0] * (l[1 * 3 + 0] * cos(q[0 * 3 + 2]) - + l[1 * 3 + 2] * sin(q[0 * 3 + 2]))) * + (l[1 * 3 + 2] * cos(q[0 * 3 + 2]) + + l[1 * 3 + 0] * sin(q[0 * 3 + 2]))) / + 2.0 - + ((phi[2 * 10 + 3] + + phi[2 * 10 + 0] * (l[1 * 3 + 2] * cos(q[0 * 3 + 2]) + + l[1 * 3 + 0] * sin(q[0 * 3 + 2]))) * + (l[1 * 3 + 0] * cos(q[0 * 3 + 2]) - + l[1 * 3 + 2] * sin(q[0 * 3 + 2]))) / + 2.0 + + (phi[2 * 10 + 1] * (l[1 * 3 + 2] * cos(q[0 * 3 + 2]) + + l[1 * 3 + 0] * sin(q[0 * 3 + 2]))) / + 2.0 - + (phi[2 * 10 + 3] * (l[1 * 3 + 0] * cos(q[0 * 3 + 2]) - + l[1 * 3 + 2] * sin(q[0 * 3 + 2]))) / + 2.0) + + G[0 * 3 + 2] * qd[0 * 3 + 1] * + ((phi[5 * 10 + 1] * + (l[4 * 3 + 2] * cos(G[0 * 3 + 2] * q[0 * 3 + 2]) + + l[4 * 3 + 0] * sin(G[0 * 3 + 2] * q[0 * 3 + 2]))) / + 2.0 - + (phi[5 * 10 + 3] * + (l[4 * 3 + 0] * cos(G[0 * 3 + 2] * q[0 * 3 + 2]) - + l[4 * 3 + 2] * sin(G[0 * 3 + 2] * q[0 * 3 + 2]))) / + 2.0 + + ((phi[5 * 10 + 1] + + phi[5 * 10 + 0] * + (l[4 * 3 + 0] * cos(G[0 * 3 + 2] * q[0 * 3 + 2]) - + l[4 * 3 + 2] * sin(G[0 * 3 + 2] * q[0 * 3 + 2]))) * + (l[4 * 3 + 2] * cos(G[0 * 3 + 2] * q[0 * 3 + 2]) + + l[4 * 3 + 0] * sin(G[0 * 3 + 2] * q[0 * 3 + 2]))) / + 2.0 - + ((phi[5 * 10 + 3] + + phi[5 * 10 + 0] * + (l[4 * 3 + 2] * cos(G[0 * 3 + 2] * q[0 * 3 + 2]) + + l[4 * 3 + 0] * sin(G[0 * 3 + 2] * q[0 * 3 + 2]))) * + (l[4 * 3 + 0] * cos(G[0 * 3 + 2] * q[0 * 3 + 2]) - + l[4 * 3 + 2] * sin(G[0 * 3 + 2] * q[0 * 3 + 2]))) / + 2.0)) + + qd[0 * 3 + 1] * + (qd[0 * 3 + 2] * + (((phi[2 * 10 + 1] + + phi[2 * 10 + 0] * (l[1 * 3 + 0] * cos(q[0 * 3 + 2]) - + l[1 * 3 + 2] * sin(q[0 * 3 + 2]))) * + (l[1 * 3 + 2] * cos(q[0 * 3 + 2]) + + l[1 * 3 + 0] * sin(q[0 * 3 + 2]))) / + 2.0 - + ((phi[2 * 10 + 3] + + phi[2 * 10 + 0] * (l[1 * 3 + 2] * cos(q[0 * 3 + 2]) + + l[1 * 3 + 0] * sin(q[0 * 3 + 2]))) * + (l[1 * 3 + 0] * cos(q[0 * 3 + 2]) - + l[1 * 3 + 2] * sin(q[0 * 3 + 2]))) / + 2.0 + + (phi[2 * 10 + 1] * (l[1 * 3 + 2] * cos(q[0 * 3 + 2]) + + l[1 * 3 + 0] * sin(q[0 * 3 + 2]))) / + 2.0 - + (phi[2 * 10 + 3] * (l[1 * 3 + 0] * cos(q[0 * 3 + 2]) - + l[1 * 3 + 2] * sin(q[0 * 3 + 2]))) / + 2.0) + + G[0 * 3 + 2] * qd[0 * 3 + 2] * + ((phi[5 * 10 + 1] * + (l[4 * 3 + 2] * cos(G[0 * 3 + 2] * q[0 * 3 + 2]) + + l[4 * 3 + 0] * sin(G[0 * 3 + 2] * q[0 * 3 + 2]))) / + 2.0 - + (phi[5 * 10 + 3] * + (l[4 * 3 + 0] * cos(G[0 * 3 + 2] * q[0 * 3 + 2]) - + l[4 * 3 + 2] * sin(G[0 * 3 + 2] * q[0 * 3 + 2]))) / + 2.0 + + ((phi[5 * 10 + 1] + + phi[5 * 10 + 0] * + (l[4 * 3 + 0] * cos(G[0 * 3 + 2] * q[0 * 3 + 2]) - + l[4 * 3 + 2] * sin(G[0 * 3 + 2] * q[0 * 3 + 2]))) * + (l[4 * 3 + 2] * cos(G[0 * 3 + 2] * q[0 * 3 + 2]) + + l[4 * 3 + 0] * sin(G[0 * 3 + 2] * q[0 * 3 + 2]))) / + 2.0 - + ((phi[5 * 10 + 3] + + phi[5 * 10 + 0] * + (l[4 * 3 + 2] * cos(G[0 * 3 + 2] * q[0 * 3 + 2]) + + l[4 * 3 + 0] * sin(G[0 * 3 + 2] * q[0 * 3 + 2]))) * + (l[4 * 3 + 0] * cos(G[0 * 3 + 2] * q[0 * 3 + 2]) - + l[4 * 3 + 2] * sin(G[0 * 3 + 2] * q[0 * 3 + 2]))) / + 2.0)) + + qd[0 * 3 + 0] * + (qd[0 * 3 + 0] * + ((l[0 * 3 + 2] * + (cos(q[0 * 3 + 1]) * + (phi[1 * 10 + 1] + l[1 * 3 + 0] * phi[2 * 10 + 0] + + l[4 * 3 + 0] * phi[5 * 10 + 0] + + phi[5 * 10 + 1] * cos(G[0 * 3 + 2] * q[0 * 3 + 2]) + + phi[5 * 10 + 3] * sin(G[0 * 3 + 2] * q[0 * 3 + 2]) + + phi[2 * 10 + 1] * cos(q[0 * 3 + 2]) + + phi[2 * 10 + 3] * sin(q[0 * 3 + 2])) + + sin(q[0 * 3 + 1]) * + (phi[1 * 10 + 3] + l[1 * 3 + 2] * phi[2 * 10 + 0] + + l[4 * 3 + 2] * phi[5 * 10 + 0] + + phi[5 * 10 + 3] * cos(G[0 * 3 + 2] * q[0 * 3 + 2]) - + phi[5 * 10 + 1] * sin(G[0 * 3 + 2] * q[0 * 3 + 2]) + + phi[2 * 10 + 3] * cos(q[0 * 3 + 2]) - + phi[2 * 10 + 1] * sin(q[0 * 3 + 2])))) / + 2.0 - + (sin(q[0 * 3 + 1]) * + (sin(q[0 * 3 + 1]) * + (-phi[1 * 10 + 9] + + l[4 * 3 + 0] * (l[4 * 3 + 2] * phi[5 * 10 + 0] + + phi[5 * 10 + 3] * + cos(G[0 * 3 + 2] * q[0 * 3 + 2]) - + phi[5 * 10 + 1] * + sin(G[0 * 3 + 2] * q[0 * 3 + 2])) - + cos(G[0 * 3 + 2] * q[0 * 3 + 2]) * + (-l[4 * 3 + 2] * phi[5 * 10 + 1] + + sin(G[0 * 3 + 2] * q[0 * 3 + 2]) * + (phi[5 * 10 + 6] - + phi[5 * 10 + 0] * + (1.0 / + (phi[5 * 10 + 0] * + phi[5 * 10 + 0]) * + (phi[5 * 10 + 1] * + phi[5 * 10 + 1]) + + (RL * RL) * 1.0 / + (phi[5 * 10 + 0] * + phi[5 * 10 + 0]) * + (phi[5 * 10 + 2] * + phi[5 * 10 + 2])) + + (phi[5 * 10 + 1] * phi[5 * 10 + 1]) / + phi[5 * 10 + 0] + + ((RL * RL) * + (phi[5 * 10 + 2] * phi[5 * 10 + 2])) / + phi[5 * 10 + 0]) + + phi[5 * 10 + 9] * + cos(G[0 * 3 + 2] * q[0 * 3 + 2]) + + (RL * RL) * l[4 * 3 + 1] * phi[5 * 10 + 2] * + sin(G[0 * 3 + 2] * q[0 * 3 + 2])) + + sin(G[0 * 3 + 2] * q[0 * 3 + 2]) * + (cos(G[0 * 3 + 2] * q[0 * 3 + 2]) * + (phi[5 * 10 + 4] - + phi[5 * 10 + 0] * + (1.0 / + (phi[5 * 10 + 0] * + phi[5 * 10 + 0]) * + (phi[5 * 10 + 3] * + phi[5 * 10 + 3]) + + (RL * RL) * 1.0 / + (phi[5 * 10 + 0] * + phi[5 * 10 + 0]) * + (phi[5 * 10 + 2] * + phi[5 * 10 + 2])) + + (phi[5 * 10 + 3] * phi[5 * 10 + 3]) / + phi[5 * 10 + 0] + + ((RL * RL) * + (phi[5 * 10 + 2] * phi[5 * 10 + 2])) / + phi[5 * 10 + 0]) + + l[4 * 3 + 2] * phi[5 * 10 + 3] + + phi[5 * 10 + 9] * + sin(G[0 * 3 + 2] * q[0 * 3 + 2]) + + (RL * RL) * l[4 * 3 + 1] * phi[5 * 10 + 2] * + cos(G[0 * 3 + 2] * q[0 * 3 + 2])) + + l[1 * 3 + 0] * (l[1 * 3 + 2] * phi[2 * 10 + 0] + + phi[2 * 10 + 3] * cos(q[0 * 3 + 2]) - + phi[2 * 10 + 1] * sin(q[0 * 3 + 2])) - + cos(q[0 * 3 + 2]) * + (-l[1 * 3 + 2] * phi[2 * 10 + 1] + + sin(q[0 * 3 + 2]) * + (phi[2 * 10 + 6] - + phi[2 * 10 + 0] * + (1.0 / + (phi[2 * 10 + 0] * + phi[2 * 10 + 0]) * + (phi[2 * 10 + 1] * + phi[2 * 10 + 1]) + + (RL * RL) * 1.0 / + (phi[2 * 10 + 0] * + phi[2 * 10 + 0]) * + (phi[2 * 10 + 2] * + phi[2 * 10 + 2])) + + (phi[2 * 10 + 1] * phi[2 * 10 + 1]) / + phi[2 * 10 + 0] + + ((RL * RL) * + (phi[2 * 10 + 2] * phi[2 * 10 + 2])) / + phi[2 * 10 + 0]) + + phi[2 * 10 + 9] * cos(q[0 * 3 + 2]) + + (RL * RL) * l[1 * 3 + 1] * phi[2 * 10 + 2] * + sin(q[0 * 3 + 2])) + + sin(q[0 * 3 + 2]) * + (l[1 * 3 + 2] * phi[2 * 10 + 3] + + cos(q[0 * 3 + 2]) * + (phi[2 * 10 + 4] - + phi[2 * 10 + 0] * + (1.0 / + (phi[2 * 10 + 0] * + phi[2 * 10 + 0]) * + (phi[2 * 10 + 3] * + phi[2 * 10 + 3]) + + (RL * RL) * 1.0 / + (phi[2 * 10 + 0] * + phi[2 * 10 + 0]) * + (phi[2 * 10 + 2] * + phi[2 * 10 + 2])) + + (phi[2 * 10 + 3] * phi[2 * 10 + 3]) / + phi[2 * 10 + 0] + + ((RL * RL) * + (phi[2 * 10 + 2] * phi[2 * 10 + 2])) / + phi[2 * 10 + 0]) + + phi[2 * 10 + 9] * sin(q[0 * 3 + 2]) + + (RL * RL) * l[1 * 3 + 1] * phi[2 * 10 + 2] * + cos(q[0 * 3 + 2])) + + RL * l[4 * 3 + 1] * sin(G[0 * 3 + 2] * q[0 * 3 + 2]) * + (RL * phi[5 * 10 + 2] * + cos(G[0 * 3 + 2] * q[0 * 3 + 2]) + + RL * l[4 * 3 + 1] * phi[5 * 10 + 0] * + cos(G[0 * 3 + 2] * q[0 * 3 + 2])) + + RL * l[1 * 3 + 1] * sin(q[0 * 3 + 2]) * + (RL * phi[2 * 10 + 2] * cos(q[0 * 3 + 2]) + + RL * l[1 * 3 + 1] * phi[2 * 10 + 0] * + cos(q[0 * 3 + 2])) - + RL * l[4 * 3 + 1] * cos(G[0 * 3 + 2] * q[0 * 3 + 2]) * + (RL * phi[5 * 10 + 2] * + sin(G[0 * 3 + 2] * q[0 * 3 + 2]) + + RL * l[4 * 3 + 1] * phi[5 * 10 + 0] * + sin(G[0 * 3 + 2] * q[0 * 3 + 2])) - + RL * l[1 * 3 + 1] * cos(q[0 * 3 + 2]) * + (RL * phi[2 * 10 + 2] * sin(q[0 * 3 + 2]) + + RL * l[1 * 3 + 1] * phi[2 * 10 + 0] * + sin(q[0 * 3 + 2]))) + + cos(q[0 * 3 + 1]) * + (phi[1 * 10 + 6] + + l[4 * 3 + 0] * (l[4 * 3 + 0] * phi[5 * 10 + 0] + + phi[5 * 10 + 1] * + cos(G[0 * 3 + 2] * q[0 * 3 + 2]) + + phi[5 * 10 + 3] * + sin(G[0 * 3 + 2] * q[0 * 3 + 2])) + + cos(G[0 * 3 + 2] * q[0 * 3 + 2]) * + (cos(G[0 * 3 + 2] * q[0 * 3 + 2]) * + (phi[5 * 10 + 6] - + phi[5 * 10 + 0] * + (1.0 / + (phi[5 * 10 + 0] * + phi[5 * 10 + 0]) * + (phi[5 * 10 + 1] * + phi[5 * 10 + 1]) + + (RL * RL) * 1.0 / + (phi[5 * 10 + 0] * + phi[5 * 10 + 0]) * + (phi[5 * 10 + 2] * + phi[5 * 10 + 2])) + + (phi[5 * 10 + 1] * phi[5 * 10 + 1]) / + phi[5 * 10 + 0] + + ((RL * RL) * + (phi[5 * 10 + 2] * phi[5 * 10 + 2])) / + phi[5 * 10 + 0]) + + l[4 * 3 + 0] * phi[5 * 10 + 1] - + phi[5 * 10 + 9] * + sin(G[0 * 3 + 2] * q[0 * 3 + 2]) + + (RL * RL) * l[4 * 3 + 1] * phi[5 * 10 + 2] * + cos(G[0 * 3 + 2] * q[0 * 3 + 2])) + + sin(G[0 * 3 + 2] * q[0 * 3 + 2]) * + (l[4 * 3 + 0] * phi[5 * 10 + 3] + + sin(G[0 * 3 + 2] * q[0 * 3 + 2]) * + (phi[5 * 10 + 4] - + phi[5 * 10 + 0] * + (1.0 / + (phi[5 * 10 + 0] * + phi[5 * 10 + 0]) * + (phi[5 * 10 + 3] * + phi[5 * 10 + 3]) + + (RL * RL) * 1.0 / + (phi[5 * 10 + 0] * + phi[5 * 10 + 0]) * + (phi[5 * 10 + 2] * + phi[5 * 10 + 2])) + + (phi[5 * 10 + 3] * phi[5 * 10 + 3]) / + phi[5 * 10 + 0] + + ((RL * RL) * + (phi[5 * 10 + 2] * phi[5 * 10 + 2])) / + phi[5 * 10 + 0]) - + phi[5 * 10 + 9] * + cos(G[0 * 3 + 2] * q[0 * 3 + 2]) + + (RL * RL) * l[4 * 3 + 1] * phi[5 * 10 + 2] * + sin(G[0 * 3 + 2] * q[0 * 3 + 2])) + + l[1 * 3 + 0] * (l[1 * 3 + 0] * phi[2 * 10 + 0] + + phi[2 * 10 + 1] * cos(q[0 * 3 + 2]) + + phi[2 * 10 + 3] * sin(q[0 * 3 + 2])) + + cos(q[0 * 3 + 2]) * + (l[1 * 3 + 0] * phi[2 * 10 + 1] + + cos(q[0 * 3 + 2]) * + (phi[2 * 10 + 6] - + phi[2 * 10 + 0] * + (1.0 / + (phi[2 * 10 + 0] * + phi[2 * 10 + 0]) * + (phi[2 * 10 + 1] * + phi[2 * 10 + 1]) + + (RL * RL) * 1.0 / + (phi[2 * 10 + 0] * + phi[2 * 10 + 0]) * + (phi[2 * 10 + 2] * + phi[2 * 10 + 2])) + + (phi[2 * 10 + 1] * phi[2 * 10 + 1]) / + phi[2 * 10 + 0] + + ((RL * RL) * + (phi[2 * 10 + 2] * phi[2 * 10 + 2])) / + phi[2 * 10 + 0]) - + phi[2 * 10 + 9] * sin(q[0 * 3 + 2]) + + (RL * RL) * l[1 * 3 + 1] * phi[2 * 10 + 2] * + cos(q[0 * 3 + 2])) - + phi[1 * 10 + 0] * + (1.0 / (phi[1 * 10 + 0] * phi[1 * 10 + 0]) * + (phi[1 * 10 + 1] * phi[1 * 10 + 1]) + + (RL * RL) * 1.0 / + (phi[1 * 10 + 0] * phi[1 * 10 + 0]) * + (phi[1 * 10 + 2] * phi[1 * 10 + 2])) + + sin(q[0 * 3 + 2]) * + (l[1 * 3 + 0] * phi[2 * 10 + 3] + + sin(q[0 * 3 + 2]) * + (phi[2 * 10 + 4] - + phi[2 * 10 + 0] * + (1.0 / + (phi[2 * 10 + 0] * + phi[2 * 10 + 0]) * + (phi[2 * 10 + 3] * + phi[2 * 10 + 3]) + + (RL * RL) * 1.0 / + (phi[2 * 10 + 0] * + phi[2 * 10 + 0]) * + (phi[2 * 10 + 2] * + phi[2 * 10 + 2])) + + (phi[2 * 10 + 3] * phi[2 * 10 + 3]) / + phi[2 * 10 + 0] + + ((RL * RL) * + (phi[2 * 10 + 2] * phi[2 * 10 + 2])) / + phi[2 * 10 + 0]) - + phi[2 * 10 + 9] * cos(q[0 * 3 + 2]) + + (RL * RL) * l[1 * 3 + 1] * phi[2 * 10 + 2] * + sin(q[0 * 3 + 2])) + + (phi[1 * 10 + 1] * phi[1 * 10 + 1]) / phi[1 * 10 + 0] + + ((RL * RL) * (phi[1 * 10 + 2] * phi[1 * 10 + 2])) / + phi[1 * 10 + 0] + + RL * l[4 * 3 + 1] * cos(G[0 * 3 + 2] * q[0 * 3 + 2]) * + (RL * phi[5 * 10 + 2] * + cos(G[0 * 3 + 2] * q[0 * 3 + 2]) + + RL * l[4 * 3 + 1] * phi[5 * 10 + 0] * + cos(G[0 * 3 + 2] * q[0 * 3 + 2])) + + RL * l[1 * 3 + 1] * cos(q[0 * 3 + 2]) * + (RL * phi[2 * 10 + 2] * cos(q[0 * 3 + 2]) + + RL * l[1 * 3 + 1] * phi[2 * 10 + 0] * + cos(q[0 * 3 + 2])) + + RL * l[4 * 3 + 1] * sin(G[0 * 3 + 2] * q[0 * 3 + 2]) * + (RL * phi[5 * 10 + 2] * + sin(G[0 * 3 + 2] * q[0 * 3 + 2]) + + RL * l[4 * 3 + 1] * phi[5 * 10 + 0] * + sin(G[0 * 3 + 2] * q[0 * 3 + 2])) + + RL * l[1 * 3 + 1] * sin(q[0 * 3 + 2]) * + (RL * phi[2 * 10 + 2] * sin(q[0 * 3 + 2]) + + RL * l[1 * 3 + 1] * phi[2 * 10 + 0] * + sin(q[0 * 3 + 2]))) + + RL * l[0 * 3 + 1] * cos(q[0 * 3 + 1]) * + (RL * phi[1 * 10 + 2] + + RL * phi[5 * 10 + 2] * + pow(sin(G[0 * 3 + 2] * q[0 * 3 + 2]), 2.0) + + RL * phi[2 * 10 + 2] * pow(cos(q[0 * 3 + 2]), 2.0) + + RL * phi[2 * 10 + 2] * pow(sin(q[0 * 3 + 2]), 2.0) + + RL * phi[5 * 10 + 2] * + pow(cos(G[0 * 3 + 2] * q[0 * 3 + 2]), 2.0) + + RL * l[4 * 3 + 1] * phi[5 * 10 + 0] * + pow(cos(G[0 * 3 + 2] * q[0 * 3 + 2]), 2.0) + + RL * l[4 * 3 + 1] * phi[5 * 10 + 0] * + pow(sin(G[0 * 3 + 2] * q[0 * 3 + 2]), 2.0) + + RL * l[1 * 3 + 1] * phi[2 * 10 + 0] * + pow(cos(q[0 * 3 + 2]), 2.0) + + RL * l[1 * 3 + 1] * phi[2 * 10 + 0] * + pow(sin(q[0 * 3 + 2]), 2.0)))) / + 2.0 + + (cos(q[0 * 3 + 1]) * + (sin(q[0 * 3 + 1]) * + (phi[1 * 10 + 4] + + l[4 * 3 + 2] * (l[4 * 3 + 2] * phi[5 * 10 + 0] + + phi[5 * 10 + 3] * + cos(G[0 * 3 + 2] * q[0 * 3 + 2]) - + phi[5 * 10 + 1] * + sin(G[0 * 3 + 2] * q[0 * 3 + 2])) + + cos(G[0 * 3 + 2] * q[0 * 3 + 2]) * + (cos(G[0 * 3 + 2] * q[0 * 3 + 2]) * + (phi[5 * 10 + 4] - + phi[5 * 10 + 0] * + (1.0 / + (phi[5 * 10 + 0] * + phi[5 * 10 + 0]) * + (phi[5 * 10 + 3] * + phi[5 * 10 + 3]) + + (RL * RL) * 1.0 / + (phi[5 * 10 + 0] * + phi[5 * 10 + 0]) * + (phi[5 * 10 + 2] * + phi[5 * 10 + 2])) + + (phi[5 * 10 + 3] * phi[5 * 10 + 3]) / + phi[5 * 10 + 0] + + ((RL * RL) * + (phi[5 * 10 + 2] * phi[5 * 10 + 2])) / + phi[5 * 10 + 0]) + + l[4 * 3 + 2] * phi[5 * 10 + 3] + + phi[5 * 10 + 9] * + sin(G[0 * 3 + 2] * q[0 * 3 + 2]) + + (RL * RL) * l[4 * 3 + 1] * phi[5 * 10 + 2] * + cos(G[0 * 3 + 2] * q[0 * 3 + 2])) + + sin(G[0 * 3 + 2] * q[0 * 3 + 2]) * + (-l[4 * 3 + 2] * phi[5 * 10 + 1] + + sin(G[0 * 3 + 2] * q[0 * 3 + 2]) * + (phi[5 * 10 + 6] - + phi[5 * 10 + 0] * + (1.0 / + (phi[5 * 10 + 0] * + phi[5 * 10 + 0]) * + (phi[5 * 10 + 1] * + phi[5 * 10 + 1]) + + (RL * RL) * 1.0 / + (phi[5 * 10 + 0] * + phi[5 * 10 + 0]) * + (phi[5 * 10 + 2] * + phi[5 * 10 + 2])) + + (phi[5 * 10 + 1] * phi[5 * 10 + 1]) / + phi[5 * 10 + 0] + + ((RL * RL) * + (phi[5 * 10 + 2] * phi[5 * 10 + 2])) / + phi[5 * 10 + 0]) + + phi[5 * 10 + 9] * + cos(G[0 * 3 + 2] * q[0 * 3 + 2]) + + (RL * RL) * l[4 * 3 + 1] * phi[5 * 10 + 2] * + sin(G[0 * 3 + 2] * q[0 * 3 + 2])) + + l[1 * 3 + 2] * (l[1 * 3 + 2] * phi[2 * 10 + 0] + + phi[2 * 10 + 3] * cos(q[0 * 3 + 2]) - + phi[2 * 10 + 1] * sin(q[0 * 3 + 2])) + + cos(q[0 * 3 + 2]) * + (l[1 * 3 + 2] * phi[2 * 10 + 3] + + cos(q[0 * 3 + 2]) * + (phi[2 * 10 + 4] - + phi[2 * 10 + 0] * + (1.0 / + (phi[2 * 10 + 0] * + phi[2 * 10 + 0]) * + (phi[2 * 10 + 3] * + phi[2 * 10 + 3]) + + (RL * RL) * 1.0 / + (phi[2 * 10 + 0] * + phi[2 * 10 + 0]) * + (phi[2 * 10 + 2] * + phi[2 * 10 + 2])) + + (phi[2 * 10 + 3] * phi[2 * 10 + 3]) / + phi[2 * 10 + 0] + + ((RL * RL) * + (phi[2 * 10 + 2] * phi[2 * 10 + 2])) / + phi[2 * 10 + 0]) + + phi[2 * 10 + 9] * sin(q[0 * 3 + 2]) + + (RL * RL) * l[1 * 3 + 1] * phi[2 * 10 + 2] * + cos(q[0 * 3 + 2])) - + phi[1 * 10 + 0] * + (1.0 / (phi[1 * 10 + 0] * phi[1 * 10 + 0]) * + (phi[1 * 10 + 3] * phi[1 * 10 + 3]) + + (RL * RL) * 1.0 / + (phi[1 * 10 + 0] * phi[1 * 10 + 0]) * + (phi[1 * 10 + 2] * phi[1 * 10 + 2])) + + sin(q[0 * 3 + 2]) * + (-l[1 * 3 + 2] * phi[2 * 10 + 1] + + sin(q[0 * 3 + 2]) * + (phi[2 * 10 + 6] - + phi[2 * 10 + 0] * + (1.0 / + (phi[2 * 10 + 0] * + phi[2 * 10 + 0]) * + (phi[2 * 10 + 1] * + phi[2 * 10 + 1]) + + (RL * RL) * 1.0 / + (phi[2 * 10 + 0] * + phi[2 * 10 + 0]) * + (phi[2 * 10 + 2] * + phi[2 * 10 + 2])) + + (phi[2 * 10 + 1] * phi[2 * 10 + 1]) / + phi[2 * 10 + 0] + + ((RL * RL) * + (phi[2 * 10 + 2] * phi[2 * 10 + 2])) / + phi[2 * 10 + 0]) + + phi[2 * 10 + 9] * cos(q[0 * 3 + 2]) + + (RL * RL) * l[1 * 3 + 1] * phi[2 * 10 + 2] * + sin(q[0 * 3 + 2])) + + (phi[1 * 10 + 3] * phi[1 * 10 + 3]) / phi[1 * 10 + 0] + + ((RL * RL) * (phi[1 * 10 + 2] * phi[1 * 10 + 2])) / + phi[1 * 10 + 0] + + RL * l[4 * 3 + 1] * cos(G[0 * 3 + 2] * q[0 * 3 + 2]) * + (RL * phi[5 * 10 + 2] * + cos(G[0 * 3 + 2] * q[0 * 3 + 2]) + + RL * l[4 * 3 + 1] * phi[5 * 10 + 0] * + cos(G[0 * 3 + 2] * q[0 * 3 + 2])) + + RL * l[1 * 3 + 1] * cos(q[0 * 3 + 2]) * + (RL * phi[2 * 10 + 2] * cos(q[0 * 3 + 2]) + + RL * l[1 * 3 + 1] * phi[2 * 10 + 0] * + cos(q[0 * 3 + 2])) + + RL * l[4 * 3 + 1] * sin(G[0 * 3 + 2] * q[0 * 3 + 2]) * + (RL * phi[5 * 10 + 2] * + sin(G[0 * 3 + 2] * q[0 * 3 + 2]) + + RL * l[4 * 3 + 1] * phi[5 * 10 + 0] * + sin(G[0 * 3 + 2] * q[0 * 3 + 2])) + + RL * l[1 * 3 + 1] * sin(q[0 * 3 + 2]) * + (RL * phi[2 * 10 + 2] * sin(q[0 * 3 + 2]) + + RL * l[1 * 3 + 1] * phi[2 * 10 + 0] * + sin(q[0 * 3 + 2]))) + + cos(q[0 * 3 + 1]) * + (-phi[1 * 10 + 9] + + l[4 * 3 + 2] * (l[4 * 3 + 0] * phi[5 * 10 + 0] + + phi[5 * 10 + 1] * + cos(G[0 * 3 + 2] * q[0 * 3 + 2]) + + phi[5 * 10 + 3] * + sin(G[0 * 3 + 2] * q[0 * 3 + 2])) - + sin(G[0 * 3 + 2] * q[0 * 3 + 2]) * + (cos(G[0 * 3 + 2] * q[0 * 3 + 2]) * + (phi[5 * 10 + 6] - + phi[5 * 10 + 0] * + (1.0 / + (phi[5 * 10 + 0] * + phi[5 * 10 + 0]) * + (phi[5 * 10 + 1] * + phi[5 * 10 + 1]) + + (RL * RL) * 1.0 / + (phi[5 * 10 + 0] * + phi[5 * 10 + 0]) * + (phi[5 * 10 + 2] * + phi[5 * 10 + 2])) + + (phi[5 * 10 + 1] * phi[5 * 10 + 1]) / + phi[5 * 10 + 0] + + ((RL * RL) * + (phi[5 * 10 + 2] * phi[5 * 10 + 2])) / + phi[5 * 10 + 0]) + + l[4 * 3 + 0] * phi[5 * 10 + 1] - + phi[5 * 10 + 9] * + sin(G[0 * 3 + 2] * q[0 * 3 + 2]) + + (RL * RL) * l[4 * 3 + 1] * phi[5 * 10 + 2] * + cos(G[0 * 3 + 2] * q[0 * 3 + 2])) + + cos(G[0 * 3 + 2] * q[0 * 3 + 2]) * + (l[4 * 3 + 0] * phi[5 * 10 + 3] + + sin(G[0 * 3 + 2] * q[0 * 3 + 2]) * + (phi[5 * 10 + 4] - + phi[5 * 10 + 0] * + (1.0 / + (phi[5 * 10 + 0] * + phi[5 * 10 + 0]) * + (phi[5 * 10 + 3] * + phi[5 * 10 + 3]) + + (RL * RL) * 1.0 / + (phi[5 * 10 + 0] * + phi[5 * 10 + 0]) * + (phi[5 * 10 + 2] * + phi[5 * 10 + 2])) + + (phi[5 * 10 + 3] * phi[5 * 10 + 3]) / + phi[5 * 10 + 0] + + ((RL * RL) * + (phi[5 * 10 + 2] * phi[5 * 10 + 2])) / + phi[5 * 10 + 0]) - + phi[5 * 10 + 9] * + cos(G[0 * 3 + 2] * q[0 * 3 + 2]) + + (RL * RL) * l[4 * 3 + 1] * phi[5 * 10 + 2] * + sin(G[0 * 3 + 2] * q[0 * 3 + 2])) + + l[1 * 3 + 2] * (l[1 * 3 + 0] * phi[2 * 10 + 0] + + phi[2 * 10 + 1] * cos(q[0 * 3 + 2]) + + phi[2 * 10 + 3] * sin(q[0 * 3 + 2])) - + sin(q[0 * 3 + 2]) * + (l[1 * 3 + 0] * phi[2 * 10 + 1] + + cos(q[0 * 3 + 2]) * + (phi[2 * 10 + 6] - + phi[2 * 10 + 0] * + (1.0 / + (phi[2 * 10 + 0] * + phi[2 * 10 + 0]) * + (phi[2 * 10 + 1] * + phi[2 * 10 + 1]) + + (RL * RL) * 1.0 / + (phi[2 * 10 + 0] * + phi[2 * 10 + 0]) * + (phi[2 * 10 + 2] * + phi[2 * 10 + 2])) + + (phi[2 * 10 + 1] * phi[2 * 10 + 1]) / + phi[2 * 10 + 0] + + ((RL * RL) * + (phi[2 * 10 + 2] * phi[2 * 10 + 2])) / + phi[2 * 10 + 0]) - + phi[2 * 10 + 9] * sin(q[0 * 3 + 2]) + + (RL * RL) * l[1 * 3 + 1] * phi[2 * 10 + 2] * + cos(q[0 * 3 + 2])) + + cos(q[0 * 3 + 2]) * + (l[1 * 3 + 0] * phi[2 * 10 + 3] + + sin(q[0 * 3 + 2]) * + (phi[2 * 10 + 4] - + phi[2 * 10 + 0] * + (1.0 / + (phi[2 * 10 + 0] * + phi[2 * 10 + 0]) * + (phi[2 * 10 + 3] * + phi[2 * 10 + 3]) + + (RL * RL) * 1.0 / + (phi[2 * 10 + 0] * + phi[2 * 10 + 0]) * + (phi[2 * 10 + 2] * + phi[2 * 10 + 2])) + + (phi[2 * 10 + 3] * phi[2 * 10 + 3]) / + phi[2 * 10 + 0] + + ((RL * RL) * + (phi[2 * 10 + 2] * phi[2 * 10 + 2])) / + phi[2 * 10 + 0]) - + phi[2 * 10 + 9] * cos(q[0 * 3 + 2]) + + (RL * RL) * l[1 * 3 + 1] * phi[2 * 10 + 2] * + sin(q[0 * 3 + 2])) - + RL * l[4 * 3 + 1] * sin(G[0 * 3 + 2] * q[0 * 3 + 2]) * + (RL * phi[5 * 10 + 2] * + cos(G[0 * 3 + 2] * q[0 * 3 + 2]) + + RL * l[4 * 3 + 1] * phi[5 * 10 + 0] * + cos(G[0 * 3 + 2] * q[0 * 3 + 2])) - + RL * l[1 * 3 + 1] * sin(q[0 * 3 + 2]) * + (RL * phi[2 * 10 + 2] * cos(q[0 * 3 + 2]) + + RL * l[1 * 3 + 1] * phi[2 * 10 + 0] * + cos(q[0 * 3 + 2])) + + RL * l[4 * 3 + 1] * cos(G[0 * 3 + 2] * q[0 * 3 + 2]) * + (RL * phi[5 * 10 + 2] * + sin(G[0 * 3 + 2] * q[0 * 3 + 2]) + + RL * l[4 * 3 + 1] * phi[5 * 10 + 0] * + sin(G[0 * 3 + 2] * q[0 * 3 + 2])) + + RL * l[1 * 3 + 1] * cos(q[0 * 3 + 2]) * + (RL * phi[2 * 10 + 2] * sin(q[0 * 3 + 2]) + + RL * l[1 * 3 + 1] * phi[2 * 10 + 0] * + sin(q[0 * 3 + 2]))) + + RL * l[0 * 3 + 1] * sin(q[0 * 3 + 1]) * + (RL * phi[1 * 10 + 2] + + RL * phi[5 * 10 + 2] * + pow(sin(G[0 * 3 + 2] * q[0 * 3 + 2]), 2.0) + + RL * phi[2 * 10 + 2] * pow(cos(q[0 * 3 + 2]), 2.0) + + RL * phi[2 * 10 + 2] * pow(sin(q[0 * 3 + 2]), 2.0) + + RL * phi[5 * 10 + 2] * + pow(cos(G[0 * 3 + 2] * q[0 * 3 + 2]), 2.0) + + RL * l[4 * 3 + 1] * phi[5 * 10 + 0] * + pow(cos(G[0 * 3 + 2] * q[0 * 3 + 2]), 2.0) + + RL * l[4 * 3 + 1] * phi[5 * 10 + 0] * + pow(sin(G[0 * 3 + 2] * q[0 * 3 + 2]), 2.0) + + RL * l[1 * 3 + 1] * phi[2 * 10 + 0] * + pow(cos(q[0 * 3 + 2]), 2.0) + + RL * l[1 * 3 + 1] * phi[2 * 10 + 0] * + pow(sin(q[0 * 3 + 2]), 2.0)))) / + 2.0 + + (cos(q[0 * 3 + 1]) * + (l[0 * 3 + 2] * + (phi[1 * 10 + 1] + l[1 * 3 + 0] * phi[2 * 10 + 0] + + l[4 * 3 + 0] * phi[5 * 10 + 0] + + phi[5 * 10 + 1] * cos(G[0 * 3 + 2] * q[0 * 3 + 2]) + + phi[5 * 10 + 3] * sin(G[0 * 3 + 2] * q[0 * 3 + 2]) + + phi[2 * 10 + 1] * cos(q[0 * 3 + 2]) + + phi[2 * 10 + 3] * sin(q[0 * 3 + 2])) - + sin(q[0 * 3 + 1]) * + (phi[1 * 10 + 6] + + l[4 * 3 + 0] * (l[4 * 3 + 0] * phi[5 * 10 + 0] + + phi[5 * 10 + 1] * + cos(G[0 * 3 + 2] * q[0 * 3 + 2]) + + phi[5 * 10 + 3] * + sin(G[0 * 3 + 2] * q[0 * 3 + 2])) + + cos(G[0 * 3 + 2] * q[0 * 3 + 2]) * + (cos(G[0 * 3 + 2] * q[0 * 3 + 2]) * + (phi[5 * 10 + 6] - + phi[5 * 10 + 0] * + (1.0 / + (phi[5 * 10 + 0] * + phi[5 * 10 + 0]) * + (phi[5 * 10 + 1] * + phi[5 * 10 + 1]) + + (RL * RL) * 1.0 / + (phi[5 * 10 + 0] * + phi[5 * 10 + 0]) * + (phi[5 * 10 + 2] * + phi[5 * 10 + 2])) + + (phi[5 * 10 + 1] * phi[5 * 10 + 1]) / + phi[5 * 10 + 0] + + ((RL * RL) * + (phi[5 * 10 + 2] * phi[5 * 10 + 2])) / + phi[5 * 10 + 0]) + + l[4 * 3 + 0] * phi[5 * 10 + 1] - + phi[5 * 10 + 9] * + sin(G[0 * 3 + 2] * q[0 * 3 + 2]) + + (RL * RL) * l[4 * 3 + 1] * phi[5 * 10 + 2] * + cos(G[0 * 3 + 2] * q[0 * 3 + 2])) + + sin(G[0 * 3 + 2] * q[0 * 3 + 2]) * + (l[4 * 3 + 0] * phi[5 * 10 + 3] + + sin(G[0 * 3 + 2] * q[0 * 3 + 2]) * + (phi[5 * 10 + 4] - + phi[5 * 10 + 0] * + (1.0 / + (phi[5 * 10 + 0] * + phi[5 * 10 + 0]) * + (phi[5 * 10 + 3] * + phi[5 * 10 + 3]) + + (RL * RL) * 1.0 / + (phi[5 * 10 + 0] * + phi[5 * 10 + 0]) * + (phi[5 * 10 + 2] * + phi[5 * 10 + 2])) + + (phi[5 * 10 + 3] * phi[5 * 10 + 3]) / + phi[5 * 10 + 0] + + ((RL * RL) * + (phi[5 * 10 + 2] * phi[5 * 10 + 2])) / + phi[5 * 10 + 0]) - + phi[5 * 10 + 9] * + cos(G[0 * 3 + 2] * q[0 * 3 + 2]) + + (RL * RL) * l[4 * 3 + 1] * phi[5 * 10 + 2] * + sin(G[0 * 3 + 2] * q[0 * 3 + 2])) + + l[1 * 3 + 0] * (l[1 * 3 + 0] * phi[2 * 10 + 0] + + phi[2 * 10 + 1] * cos(q[0 * 3 + 2]) + + phi[2 * 10 + 3] * sin(q[0 * 3 + 2])) + + cos(q[0 * 3 + 2]) * + (l[1 * 3 + 0] * phi[2 * 10 + 1] + + cos(q[0 * 3 + 2]) * + (phi[2 * 10 + 6] - + phi[2 * 10 + 0] * + (1.0 / + (phi[2 * 10 + 0] * + phi[2 * 10 + 0]) * + (phi[2 * 10 + 1] * + phi[2 * 10 + 1]) + + (RL * RL) * 1.0 / + (phi[2 * 10 + 0] * + phi[2 * 10 + 0]) * + (phi[2 * 10 + 2] * + phi[2 * 10 + 2])) + + (phi[2 * 10 + 1] * phi[2 * 10 + 1]) / + phi[2 * 10 + 0] + + ((RL * RL) * + (phi[2 * 10 + 2] * phi[2 * 10 + 2])) / + phi[2 * 10 + 0]) - + phi[2 * 10 + 9] * sin(q[0 * 3 + 2]) + + (RL * RL) * l[1 * 3 + 1] * phi[2 * 10 + 2] * + cos(q[0 * 3 + 2])) - + phi[1 * 10 + 0] * + (1.0 / (phi[1 * 10 + 0] * phi[1 * 10 + 0]) * + (phi[1 * 10 + 1] * phi[1 * 10 + 1]) + + (RL * RL) * 1.0 / + (phi[1 * 10 + 0] * phi[1 * 10 + 0]) * + (phi[1 * 10 + 2] * phi[1 * 10 + 2])) + + sin(q[0 * 3 + 2]) * + (l[1 * 3 + 0] * phi[2 * 10 + 3] + + sin(q[0 * 3 + 2]) * + (phi[2 * 10 + 4] - + phi[2 * 10 + 0] * + (1.0 / + (phi[2 * 10 + 0] * + phi[2 * 10 + 0]) * + (phi[2 * 10 + 3] * + phi[2 * 10 + 3]) + + (RL * RL) * 1.0 / + (phi[2 * 10 + 0] * + phi[2 * 10 + 0]) * + (phi[2 * 10 + 2] * + phi[2 * 10 + 2])) + + (phi[2 * 10 + 3] * phi[2 * 10 + 3]) / + phi[2 * 10 + 0] + + ((RL * RL) * + (phi[2 * 10 + 2] * phi[2 * 10 + 2])) / + phi[2 * 10 + 0]) - + phi[2 * 10 + 9] * cos(q[0 * 3 + 2]) + + (RL * RL) * l[1 * 3 + 1] * phi[2 * 10 + 2] * + sin(q[0 * 3 + 2])) + + (phi[1 * 10 + 1] * phi[1 * 10 + 1]) / phi[1 * 10 + 0] + + ((RL * RL) * (phi[1 * 10 + 2] * phi[1 * 10 + 2])) / + phi[1 * 10 + 0] + + RL * l[4 * 3 + 1] * cos(G[0 * 3 + 2] * q[0 * 3 + 2]) * + (RL * phi[5 * 10 + 2] * + cos(G[0 * 3 + 2] * q[0 * 3 + 2]) + + RL * l[4 * 3 + 1] * phi[5 * 10 + 0] * + cos(G[0 * 3 + 2] * q[0 * 3 + 2])) + + RL * l[1 * 3 + 1] * cos(q[0 * 3 + 2]) * + (RL * phi[2 * 10 + 2] * cos(q[0 * 3 + 2]) + + RL * l[1 * 3 + 1] * phi[2 * 10 + 0] * + cos(q[0 * 3 + 2])) + + RL * l[4 * 3 + 1] * sin(G[0 * 3 + 2] * q[0 * 3 + 2]) * + (RL * phi[5 * 10 + 2] * + sin(G[0 * 3 + 2] * q[0 * 3 + 2]) + + RL * l[4 * 3 + 1] * phi[5 * 10 + 0] * + sin(G[0 * 3 + 2] * q[0 * 3 + 2])) + + RL * l[1 * 3 + 1] * sin(q[0 * 3 + 2]) * + (RL * phi[2 * 10 + 2] * sin(q[0 * 3 + 2]) + + RL * l[1 * 3 + 1] * phi[2 * 10 + 0] * + sin(q[0 * 3 + 2]))) + + cos(q[0 * 3 + 1]) * + (-phi[1 * 10 + 9] + + l[4 * 3 + 0] * (l[4 * 3 + 2] * phi[5 * 10 + 0] + + phi[5 * 10 + 3] * + cos(G[0 * 3 + 2] * q[0 * 3 + 2]) - + phi[5 * 10 + 1] * + sin(G[0 * 3 + 2] * q[0 * 3 + 2])) - + cos(G[0 * 3 + 2] * q[0 * 3 + 2]) * + (-l[4 * 3 + 2] * phi[5 * 10 + 1] + + sin(G[0 * 3 + 2] * q[0 * 3 + 2]) * + (phi[5 * 10 + 6] - + phi[5 * 10 + 0] * + (1.0 / + (phi[5 * 10 + 0] * + phi[5 * 10 + 0]) * + (phi[5 * 10 + 1] * + phi[5 * 10 + 1]) + + (RL * RL) * 1.0 / + (phi[5 * 10 + 0] * + phi[5 * 10 + 0]) * + (phi[5 * 10 + 2] * + phi[5 * 10 + 2])) + + (phi[5 * 10 + 1] * phi[5 * 10 + 1]) / + phi[5 * 10 + 0] + + ((RL * RL) * + (phi[5 * 10 + 2] * phi[5 * 10 + 2])) / + phi[5 * 10 + 0]) + + phi[5 * 10 + 9] * + cos(G[0 * 3 + 2] * q[0 * 3 + 2]) + + (RL * RL) * l[4 * 3 + 1] * phi[5 * 10 + 2] * + sin(G[0 * 3 + 2] * q[0 * 3 + 2])) + + sin(G[0 * 3 + 2] * q[0 * 3 + 2]) * + (cos(G[0 * 3 + 2] * q[0 * 3 + 2]) * + (phi[5 * 10 + 4] - + phi[5 * 10 + 0] * + (1.0 / + (phi[5 * 10 + 0] * + phi[5 * 10 + 0]) * + (phi[5 * 10 + 3] * + phi[5 * 10 + 3]) + + (RL * RL) * 1.0 / + (phi[5 * 10 + 0] * + phi[5 * 10 + 0]) * + (phi[5 * 10 + 2] * + phi[5 * 10 + 2])) + + (phi[5 * 10 + 3] * phi[5 * 10 + 3]) / + phi[5 * 10 + 0] + + ((RL * RL) * + (phi[5 * 10 + 2] * phi[5 * 10 + 2])) / + phi[5 * 10 + 0]) + + l[4 * 3 + 2] * phi[5 * 10 + 3] + + phi[5 * 10 + 9] * + sin(G[0 * 3 + 2] * q[0 * 3 + 2]) + + (RL * RL) * l[4 * 3 + 1] * phi[5 * 10 + 2] * + cos(G[0 * 3 + 2] * q[0 * 3 + 2])) + + l[1 * 3 + 0] * (l[1 * 3 + 2] * phi[2 * 10 + 0] + + phi[2 * 10 + 3] * cos(q[0 * 3 + 2]) - + phi[2 * 10 + 1] * sin(q[0 * 3 + 2])) - + cos(q[0 * 3 + 2]) * + (-l[1 * 3 + 2] * phi[2 * 10 + 1] + + sin(q[0 * 3 + 2]) * + (phi[2 * 10 + 6] - + phi[2 * 10 + 0] * + (1.0 / + (phi[2 * 10 + 0] * + phi[2 * 10 + 0]) * + (phi[2 * 10 + 1] * + phi[2 * 10 + 1]) + + (RL * RL) * 1.0 / + (phi[2 * 10 + 0] * + phi[2 * 10 + 0]) * + (phi[2 * 10 + 2] * + phi[2 * 10 + 2])) + + (phi[2 * 10 + 1] * phi[2 * 10 + 1]) / + phi[2 * 10 + 0] + + ((RL * RL) * + (phi[2 * 10 + 2] * phi[2 * 10 + 2])) / + phi[2 * 10 + 0]) + + phi[2 * 10 + 9] * cos(q[0 * 3 + 2]) + + (RL * RL) * l[1 * 3 + 1] * phi[2 * 10 + 2] * + sin(q[0 * 3 + 2])) + + sin(q[0 * 3 + 2]) * + (l[1 * 3 + 2] * phi[2 * 10 + 3] + + cos(q[0 * 3 + 2]) * + (phi[2 * 10 + 4] - + phi[2 * 10 + 0] * + (1.0 / + (phi[2 * 10 + 0] * + phi[2 * 10 + 0]) * + (phi[2 * 10 + 3] * + phi[2 * 10 + 3]) + + (RL * RL) * 1.0 / + (phi[2 * 10 + 0] * + phi[2 * 10 + 0]) * + (phi[2 * 10 + 2] * + phi[2 * 10 + 2])) + + (phi[2 * 10 + 3] * phi[2 * 10 + 3]) / + phi[2 * 10 + 0] + + ((RL * RL) * + (phi[2 * 10 + 2] * phi[2 * 10 + 2])) / + phi[2 * 10 + 0]) + + phi[2 * 10 + 9] * sin(q[0 * 3 + 2]) + + (RL * RL) * l[1 * 3 + 1] * phi[2 * 10 + 2] * + cos(q[0 * 3 + 2])) + + RL * l[4 * 3 + 1] * sin(G[0 * 3 + 2] * q[0 * 3 + 2]) * + (RL * phi[5 * 10 + 2] * + cos(G[0 * 3 + 2] * q[0 * 3 + 2]) + + RL * l[4 * 3 + 1] * phi[5 * 10 + 0] * + cos(G[0 * 3 + 2] * q[0 * 3 + 2])) + + RL * l[1 * 3 + 1] * sin(q[0 * 3 + 2]) * + (RL * phi[2 * 10 + 2] * cos(q[0 * 3 + 2]) + + RL * l[1 * 3 + 1] * phi[2 * 10 + 0] * + cos(q[0 * 3 + 2])) - + RL * l[4 * 3 + 1] * cos(G[0 * 3 + 2] * q[0 * 3 + 2]) * + (RL * phi[5 * 10 + 2] * + sin(G[0 * 3 + 2] * q[0 * 3 + 2]) + + RL * l[4 * 3 + 1] * phi[5 * 10 + 0] * + sin(G[0 * 3 + 2] * q[0 * 3 + 2])) - + RL * l[1 * 3 + 1] * cos(q[0 * 3 + 2]) * + (RL * phi[2 * 10 + 2] * sin(q[0 * 3 + 2]) + + RL * l[1 * 3 + 1] * phi[2 * 10 + 0] * + sin(q[0 * 3 + 2]))) - + RL * l[0 * 3 + 1] * sin(q[0 * 3 + 1]) * + (RL * phi[1 * 10 + 2] + + RL * phi[5 * 10 + 2] * + pow(sin(G[0 * 3 + 2] * q[0 * 3 + 2]), 2.0) + + RL * phi[2 * 10 + 2] * pow(cos(q[0 * 3 + 2]), 2.0) + + RL * phi[2 * 10 + 2] * pow(sin(q[0 * 3 + 2]), 2.0) + + RL * phi[5 * 10 + 2] * + pow(cos(G[0 * 3 + 2] * q[0 * 3 + 2]), 2.0) + + RL * l[4 * 3 + 1] * phi[5 * 10 + 0] * + pow(cos(G[0 * 3 + 2] * q[0 * 3 + 2]), 2.0) + + RL * l[4 * 3 + 1] * phi[5 * 10 + 0] * + pow(sin(G[0 * 3 + 2] * q[0 * 3 + 2]), 2.0) + + RL * l[1 * 3 + 1] * phi[2 * 10 + 0] * + pow(cos(q[0 * 3 + 2]), 2.0) + + RL * l[1 * 3 + 1] * phi[2 * 10 + 0] * + pow(sin(q[0 * 3 + 2]), 2.0)))) / + 2.0 + + (sin(q[0 * 3 + 1]) * + (-sin(q[0 * 3 + 1]) * + (-phi[1 * 10 + 9] + + l[4 * 3 + 2] * (l[4 * 3 + 0] * phi[5 * 10 + 0] + + phi[5 * 10 + 1] * + cos(G[0 * 3 + 2] * q[0 * 3 + 2]) + + phi[5 * 10 + 3] * + sin(G[0 * 3 + 2] * q[0 * 3 + 2])) - + sin(G[0 * 3 + 2] * q[0 * 3 + 2]) * + (cos(G[0 * 3 + 2] * q[0 * 3 + 2]) * + (phi[5 * 10 + 6] - + phi[5 * 10 + 0] * + (1.0 / + (phi[5 * 10 + 0] * + phi[5 * 10 + 0]) * + (phi[5 * 10 + 1] * + phi[5 * 10 + 1]) + + (RL * RL) * 1.0 / + (phi[5 * 10 + 0] * + phi[5 * 10 + 0]) * + (phi[5 * 10 + 2] * + phi[5 * 10 + 2])) + + (phi[5 * 10 + 1] * phi[5 * 10 + 1]) / + phi[5 * 10 + 0] + + ((RL * RL) * + (phi[5 * 10 + 2] * phi[5 * 10 + 2])) / + phi[5 * 10 + 0]) + + l[4 * 3 + 0] * phi[5 * 10 + 1] - + phi[5 * 10 + 9] * + sin(G[0 * 3 + 2] * q[0 * 3 + 2]) + + (RL * RL) * l[4 * 3 + 1] * phi[5 * 10 + 2] * + cos(G[0 * 3 + 2] * q[0 * 3 + 2])) + + cos(G[0 * 3 + 2] * q[0 * 3 + 2]) * + (l[4 * 3 + 0] * phi[5 * 10 + 3] + + sin(G[0 * 3 + 2] * q[0 * 3 + 2]) * + (phi[5 * 10 + 4] - + phi[5 * 10 + 0] * + (1.0 / + (phi[5 * 10 + 0] * + phi[5 * 10 + 0]) * + (phi[5 * 10 + 3] * + phi[5 * 10 + 3]) + + (RL * RL) * 1.0 / + (phi[5 * 10 + 0] * + phi[5 * 10 + 0]) * + (phi[5 * 10 + 2] * + phi[5 * 10 + 2])) + + (phi[5 * 10 + 3] * phi[5 * 10 + 3]) / + phi[5 * 10 + 0] + + ((RL * RL) * + (phi[5 * 10 + 2] * phi[5 * 10 + 2])) / + phi[5 * 10 + 0]) - + phi[5 * 10 + 9] * + cos(G[0 * 3 + 2] * q[0 * 3 + 2]) + + (RL * RL) * l[4 * 3 + 1] * phi[5 * 10 + 2] * + sin(G[0 * 3 + 2] * q[0 * 3 + 2])) + + l[1 * 3 + 2] * (l[1 * 3 + 0] * phi[2 * 10 + 0] + + phi[2 * 10 + 1] * cos(q[0 * 3 + 2]) + + phi[2 * 10 + 3] * sin(q[0 * 3 + 2])) - + sin(q[0 * 3 + 2]) * + (l[1 * 3 + 0] * phi[2 * 10 + 1] + + cos(q[0 * 3 + 2]) * + (phi[2 * 10 + 6] - + phi[2 * 10 + 0] * + (1.0 / + (phi[2 * 10 + 0] * + phi[2 * 10 + 0]) * + (phi[2 * 10 + 1] * + phi[2 * 10 + 1]) + + (RL * RL) * 1.0 / + (phi[2 * 10 + 0] * + phi[2 * 10 + 0]) * + (phi[2 * 10 + 2] * + phi[2 * 10 + 2])) + + (phi[2 * 10 + 1] * phi[2 * 10 + 1]) / + phi[2 * 10 + 0] + + ((RL * RL) * + (phi[2 * 10 + 2] * phi[2 * 10 + 2])) / + phi[2 * 10 + 0]) - + phi[2 * 10 + 9] * sin(q[0 * 3 + 2]) + + (RL * RL) * l[1 * 3 + 1] * phi[2 * 10 + 2] * + cos(q[0 * 3 + 2])) + + cos(q[0 * 3 + 2]) * + (l[1 * 3 + 0] * phi[2 * 10 + 3] + + sin(q[0 * 3 + 2]) * + (phi[2 * 10 + 4] - + phi[2 * 10 + 0] * + (1.0 / + (phi[2 * 10 + 0] * + phi[2 * 10 + 0]) * + (phi[2 * 10 + 3] * + phi[2 * 10 + 3]) + + (RL * RL) * 1.0 / + (phi[2 * 10 + 0] * + phi[2 * 10 + 0]) * + (phi[2 * 10 + 2] * + phi[2 * 10 + 2])) + + (phi[2 * 10 + 3] * phi[2 * 10 + 3]) / + phi[2 * 10 + 0] + + ((RL * RL) * + (phi[2 * 10 + 2] * phi[2 * 10 + 2])) / + phi[2 * 10 + 0]) - + phi[2 * 10 + 9] * cos(q[0 * 3 + 2]) + + (RL * RL) * l[1 * 3 + 1] * phi[2 * 10 + 2] * + sin(q[0 * 3 + 2])) - + RL * l[4 * 3 + 1] * sin(G[0 * 3 + 2] * q[0 * 3 + 2]) * + (RL * phi[5 * 10 + 2] * + cos(G[0 * 3 + 2] * q[0 * 3 + 2]) + + RL * l[4 * 3 + 1] * phi[5 * 10 + 0] * + cos(G[0 * 3 + 2] * q[0 * 3 + 2])) - + RL * l[1 * 3 + 1] * sin(q[0 * 3 + 2]) * + (RL * phi[2 * 10 + 2] * cos(q[0 * 3 + 2]) + + RL * l[1 * 3 + 1] * phi[2 * 10 + 0] * + cos(q[0 * 3 + 2])) + + RL * l[4 * 3 + 1] * cos(G[0 * 3 + 2] * q[0 * 3 + 2]) * + (RL * phi[5 * 10 + 2] * + sin(G[0 * 3 + 2] * q[0 * 3 + 2]) + + RL * l[4 * 3 + 1] * phi[5 * 10 + 0] * + sin(G[0 * 3 + 2] * q[0 * 3 + 2])) + + RL * l[1 * 3 + 1] * cos(q[0 * 3 + 2]) * + (RL * phi[2 * 10 + 2] * sin(q[0 * 3 + 2]) + + RL * l[1 * 3 + 1] * phi[2 * 10 + 0] * + sin(q[0 * 3 + 2]))) + + l[0 * 3 + 2] * + (phi[1 * 10 + 3] + l[1 * 3 + 2] * phi[2 * 10 + 0] + + l[4 * 3 + 2] * phi[5 * 10 + 0] + + phi[5 * 10 + 3] * cos(G[0 * 3 + 2] * q[0 * 3 + 2]) - + phi[5 * 10 + 1] * sin(G[0 * 3 + 2] * q[0 * 3 + 2]) + + phi[2 * 10 + 3] * cos(q[0 * 3 + 2]) - + phi[2 * 10 + 1] * sin(q[0 * 3 + 2])) + + cos(q[0 * 3 + 1]) * + (phi[1 * 10 + 4] + + l[4 * 3 + 2] * (l[4 * 3 + 2] * phi[5 * 10 + 0] + + phi[5 * 10 + 3] * + cos(G[0 * 3 + 2] * q[0 * 3 + 2]) - + phi[5 * 10 + 1] * + sin(G[0 * 3 + 2] * q[0 * 3 + 2])) + + cos(G[0 * 3 + 2] * q[0 * 3 + 2]) * + (cos(G[0 * 3 + 2] * q[0 * 3 + 2]) * + (phi[5 * 10 + 4] - + phi[5 * 10 + 0] * + (1.0 / + (phi[5 * 10 + 0] * + phi[5 * 10 + 0]) * + (phi[5 * 10 + 3] * + phi[5 * 10 + 3]) + + (RL * RL) * 1.0 / + (phi[5 * 10 + 0] * + phi[5 * 10 + 0]) * + (phi[5 * 10 + 2] * + phi[5 * 10 + 2])) + + (phi[5 * 10 + 3] * phi[5 * 10 + 3]) / + phi[5 * 10 + 0] + + ((RL * RL) * + (phi[5 * 10 + 2] * phi[5 * 10 + 2])) / + phi[5 * 10 + 0]) + + l[4 * 3 + 2] * phi[5 * 10 + 3] + + phi[5 * 10 + 9] * + sin(G[0 * 3 + 2] * q[0 * 3 + 2]) + + (RL * RL) * l[4 * 3 + 1] * phi[5 * 10 + 2] * + cos(G[0 * 3 + 2] * q[0 * 3 + 2])) + + sin(G[0 * 3 + 2] * q[0 * 3 + 2]) * + (-l[4 * 3 + 2] * phi[5 * 10 + 1] + + sin(G[0 * 3 + 2] * q[0 * 3 + 2]) * + (phi[5 * 10 + 6] - + phi[5 * 10 + 0] * + (1.0 / + (phi[5 * 10 + 0] * + phi[5 * 10 + 0]) * + (phi[5 * 10 + 1] * + phi[5 * 10 + 1]) + + (RL * RL) * 1.0 / + (phi[5 * 10 + 0] * + phi[5 * 10 + 0]) * + (phi[5 * 10 + 2] * + phi[5 * 10 + 2])) + + (phi[5 * 10 + 1] * phi[5 * 10 + 1]) / + phi[5 * 10 + 0] + + ((RL * RL) * + (phi[5 * 10 + 2] * phi[5 * 10 + 2])) / + phi[5 * 10 + 0]) + + phi[5 * 10 + 9] * + cos(G[0 * 3 + 2] * q[0 * 3 + 2]) + + (RL * RL) * l[4 * 3 + 1] * phi[5 * 10 + 2] * + sin(G[0 * 3 + 2] * q[0 * 3 + 2])) + + l[1 * 3 + 2] * (l[1 * 3 + 2] * phi[2 * 10 + 0] + + phi[2 * 10 + 3] * cos(q[0 * 3 + 2]) - + phi[2 * 10 + 1] * sin(q[0 * 3 + 2])) + + cos(q[0 * 3 + 2]) * + (l[1 * 3 + 2] * phi[2 * 10 + 3] + + cos(q[0 * 3 + 2]) * + (phi[2 * 10 + 4] - + phi[2 * 10 + 0] * + (1.0 / + (phi[2 * 10 + 0] * + phi[2 * 10 + 0]) * + (phi[2 * 10 + 3] * + phi[2 * 10 + 3]) + + (RL * RL) * 1.0 / + (phi[2 * 10 + 0] * + phi[2 * 10 + 0]) * + (phi[2 * 10 + 2] * + phi[2 * 10 + 2])) + + (phi[2 * 10 + 3] * phi[2 * 10 + 3]) / + phi[2 * 10 + 0] + + ((RL * RL) * + (phi[2 * 10 + 2] * phi[2 * 10 + 2])) / + phi[2 * 10 + 0]) + + phi[2 * 10 + 9] * sin(q[0 * 3 + 2]) + + (RL * RL) * l[1 * 3 + 1] * phi[2 * 10 + 2] * + cos(q[0 * 3 + 2])) - + phi[1 * 10 + 0] * + (1.0 / (phi[1 * 10 + 0] * phi[1 * 10 + 0]) * + (phi[1 * 10 + 3] * phi[1 * 10 + 3]) + + (RL * RL) * 1.0 / + (phi[1 * 10 + 0] * phi[1 * 10 + 0]) * + (phi[1 * 10 + 2] * phi[1 * 10 + 2])) + + sin(q[0 * 3 + 2]) * + (-l[1 * 3 + 2] * phi[2 * 10 + 1] + + sin(q[0 * 3 + 2]) * + (phi[2 * 10 + 6] - + phi[2 * 10 + 0] * + (1.0 / + (phi[2 * 10 + 0] * + phi[2 * 10 + 0]) * + (phi[2 * 10 + 1] * + phi[2 * 10 + 1]) + + (RL * RL) * 1.0 / + (phi[2 * 10 + 0] * + phi[2 * 10 + 0]) * + (phi[2 * 10 + 2] * + phi[2 * 10 + 2])) + + (phi[2 * 10 + 1] * phi[2 * 10 + 1]) / + phi[2 * 10 + 0] + + ((RL * RL) * + (phi[2 * 10 + 2] * phi[2 * 10 + 2])) / + phi[2 * 10 + 0]) + + phi[2 * 10 + 9] * cos(q[0 * 3 + 2]) + + (RL * RL) * l[1 * 3 + 1] * phi[2 * 10 + 2] * + sin(q[0 * 3 + 2])) + + (phi[1 * 10 + 3] * phi[1 * 10 + 3]) / phi[1 * 10 + 0] + + ((RL * RL) * (phi[1 * 10 + 2] * phi[1 * 10 + 2])) / + phi[1 * 10 + 0] + + RL * l[4 * 3 + 1] * cos(G[0 * 3 + 2] * q[0 * 3 + 2]) * + (RL * phi[5 * 10 + 2] * + cos(G[0 * 3 + 2] * q[0 * 3 + 2]) + + RL * l[4 * 3 + 1] * phi[5 * 10 + 0] * + cos(G[0 * 3 + 2] * q[0 * 3 + 2])) + + RL * l[1 * 3 + 1] * cos(q[0 * 3 + 2]) * + (RL * phi[2 * 10 + 2] * cos(q[0 * 3 + 2]) + + RL * l[1 * 3 + 1] * phi[2 * 10 + 0] * + cos(q[0 * 3 + 2])) + + RL * l[4 * 3 + 1] * sin(G[0 * 3 + 2] * q[0 * 3 + 2]) * + (RL * phi[5 * 10 + 2] * + sin(G[0 * 3 + 2] * q[0 * 3 + 2]) + + RL * l[4 * 3 + 1] * phi[5 * 10 + 0] * + sin(G[0 * 3 + 2] * q[0 * 3 + 2])) + + RL * l[1 * 3 + 1] * sin(q[0 * 3 + 2]) * + (RL * phi[2 * 10 + 2] * sin(q[0 * 3 + 2]) + + RL * l[1 * 3 + 1] * phi[2 * 10 + 0] * + sin(q[0 * 3 + 2]))) + + RL * l[0 * 3 + 1] * cos(q[0 * 3 + 1]) * + (RL * phi[1 * 10 + 2] + + RL * phi[5 * 10 + 2] * + pow(sin(G[0 * 3 + 2] * q[0 * 3 + 2]), 2.0) + + RL * phi[2 * 10 + 2] * pow(cos(q[0 * 3 + 2]), 2.0) + + RL * phi[2 * 10 + 2] * pow(sin(q[0 * 3 + 2]), 2.0) + + RL * phi[5 * 10 + 2] * + pow(cos(G[0 * 3 + 2] * q[0 * 3 + 2]), 2.0) + + RL * l[4 * 3 + 1] * phi[5 * 10 + 0] * + pow(cos(G[0 * 3 + 2] * q[0 * 3 + 2]), 2.0) + + RL * l[4 * 3 + 1] * phi[5 * 10 + 0] * + pow(sin(G[0 * 3 + 2] * q[0 * 3 + 2]), 2.0) + + RL * l[1 * 3 + 1] * phi[2 * 10 + 0] * + pow(cos(q[0 * 3 + 2]), 2.0) + + RL * l[1 * 3 + 1] * phi[2 * 10 + 0] * + pow(sin(q[0 * 3 + 2]), 2.0)))) / + 2.0) + + qd[0 * 3 + 2] * + ((cos(q[0 * 3 + 1]) * + (RL * phi[2 * 10 + 7] * sin(q[0 * 3 + 2]) - + RL * phi[2 * 10 + 8] * cos(q[0 * 3 + 2]) + + RL * l[1 * 3 + 1] * phi[2 * 10 + 3] * cos(q[0 * 3 + 2]) - + RL * l[1 * 3 + 1] * phi[2 * 10 + 1] * sin(q[0 * 3 + 2]))) / + 2.0 + + (sin(q[0 * 3 + 1]) * + (RL * phi[2 * 10 + 8] * sin(q[0 * 3 + 2]) + + RL * phi[2 * 10 + 7] * cos(q[0 * 3 + 2]) - + RL * l[1 * 3 + 1] * phi[2 * 10 + 1] * cos(q[0 * 3 + 2]) - + RL * l[1 * 3 + 1] * phi[2 * 10 + 3] * sin(q[0 * 3 + 2]))) / + 2.0 + + (RL * l[0 * 3 + 1] * cos(q[0 * 3 + 1]) * + (phi[2 * 10 + 3] * cos(q[0 * 3 + 2]) - + phi[2 * 10 + 1] * sin(q[0 * 3 + 2]))) / + 2.0 - + (RL * l[0 * 3 + 1] * sin(q[0 * 3 + 1]) * + (phi[2 * 10 + 1] * cos(q[0 * 3 + 2]) + + phi[2 * 10 + 3] * sin(q[0 * 3 + 2]))) / + 2.0) * + 2.0 + + G[0 * 3 + 0] * + (qd[0 * 3 + 0] * + (sin(G[0 * 3 + 1] * q[0 * 3 + 1]) * + (cos(G[0 * 3 + 1] * q[0 * 3 + 1]) * + (phi[4 * 10 + 6] - + phi[4 * 10 + 0] * + (1.0 / + (phi[4 * 10 + 0] * phi[4 * 10 + 0]) * + (phi[4 * 10 + 1] * phi[4 * 10 + 1]) + + (RL * RL) * 1.0 / + (phi[4 * 10 + 0] * phi[4 * 10 + 0]) * + (phi[4 * 10 + 2] * phi[4 * 10 + 2])) + + (phi[4 * 10 + 1] * phi[4 * 10 + 1]) / + phi[4 * 10 + 0] + + ((RL * RL) * + (phi[4 * 10 + 2] * phi[4 * 10 + 2])) / + phi[4 * 10 + 0]) - + phi[4 * 10 + 9] * sin(G[0 * 3 + 1] * q[0 * 3 + 1]) + + (RL * RL) * l[3 * 3 + 1] * phi[4 * 10 + 2] * + cos(G[0 * 3 + 1] * q[0 * 3 + 1])) * + (-1.0 / 2.0) + + (cos(G[0 * 3 + 1] * q[0 * 3 + 1]) * + (sin(G[0 * 3 + 1] * q[0 * 3 + 1]) * + (phi[4 * 10 + 4] - + phi[4 * 10 + 0] * + (1.0 / (phi[4 * 10 + 0] * phi[4 * 10 + 0]) * + (phi[4 * 10 + 3] * phi[4 * 10 + 3]) + + (RL * RL) * 1.0 / + (phi[4 * 10 + 0] * phi[4 * 10 + 0]) * + (phi[4 * 10 + 2] * phi[4 * 10 + 2])) + + (phi[4 * 10 + 3] * phi[4 * 10 + 3]) / + phi[4 * 10 + 0] + + ((RL * RL) * + (phi[4 * 10 + 2] * phi[4 * 10 + 2])) / + phi[4 * 10 + 0]) - + phi[4 * 10 + 9] * cos(G[0 * 3 + 1] * q[0 * 3 + 1]) + + (RL * RL) * l[3 * 3 + 1] * phi[4 * 10 + 2] * + sin(G[0 * 3 + 1] * q[0 * 3 + 1]))) / + 2.0 - + (cos(G[0 * 3 + 1] * q[0 * 3 + 1]) * + (-l[3 * 3 + 2] * phi[4 * 10 + 1] + + sin(G[0 * 3 + 1] * q[0 * 3 + 1]) * + (phi[4 * 10 + 6] - + phi[4 * 10 + 0] * + (1.0 / (phi[4 * 10 + 0] * phi[4 * 10 + 0]) * + (phi[4 * 10 + 1] * phi[4 * 10 + 1]) + + (RL * RL) * 1.0 / + (phi[4 * 10 + 0] * phi[4 * 10 + 0]) * + (phi[4 * 10 + 2] * phi[4 * 10 + 2])) + + (phi[4 * 10 + 1] * phi[4 * 10 + 1]) / + phi[4 * 10 + 0] + + ((RL * RL) * + (phi[4 * 10 + 2] * phi[4 * 10 + 2])) / + phi[4 * 10 + 0]) + + phi[4 * 10 + 9] * cos(G[0 * 3 + 1] * q[0 * 3 + 1]) + + (RL * RL) * l[3 * 3 + 1] * phi[4 * 10 + 2] * + sin(G[0 * 3 + 1] * q[0 * 3 + 1]))) / + 2.0 + + (sin(G[0 * 3 + 1] * q[0 * 3 + 1]) * + (cos(G[0 * 3 + 1] * q[0 * 3 + 1]) * + (phi[4 * 10 + 4] - + phi[4 * 10 + 0] * + (1.0 / (phi[4 * 10 + 0] * phi[4 * 10 + 0]) * + (phi[4 * 10 + 3] * phi[4 * 10 + 3]) + + (RL * RL) * 1.0 / + (phi[4 * 10 + 0] * phi[4 * 10 + 0]) * + (phi[4 * 10 + 2] * phi[4 * 10 + 2])) + + (phi[4 * 10 + 3] * phi[4 * 10 + 3]) / + phi[4 * 10 + 0] + + ((RL * RL) * + (phi[4 * 10 + 2] * phi[4 * 10 + 2])) / + phi[4 * 10 + 0]) + + l[3 * 3 + 2] * phi[4 * 10 + 3] + + phi[4 * 10 + 9] * sin(G[0 * 3 + 1] * q[0 * 3 + 1]) + + (RL * RL) * l[3 * 3 + 1] * phi[4 * 10 + 2] * + cos(G[0 * 3 + 1] * q[0 * 3 + 1]))) / + 2.0 + + (l[3 * 3 + 2] * + (phi[4 * 10 + 1] * cos(G[0 * 3 + 1] * q[0 * 3 + 1]) + + phi[4 * 10 + 3] * sin(G[0 * 3 + 1] * q[0 * 3 + 1]))) / + 2.0) - + G[0 * 3 + 1] * qd[0 * 3 + 1] * + ((RL * phi[4 * 10 + 8] * + cos(G[0 * 3 + 1] * q[0 * 3 + 1])) / + 2.0 - + (RL * phi[4 * 10 + 7] * + sin(G[0 * 3 + 1] * q[0 * 3 + 1])) / + 2.0 - + (RL * l[3 * 3 + 1] * phi[4 * 10 + 3] * + cos(G[0 * 3 + 1] * q[0 * 3 + 1])) / + 2.0 + + (RL * l[3 * 3 + 1] * phi[4 * 10 + 1] * + sin(G[0 * 3 + 1] * q[0 * 3 + 1])) / + 2.0) * + 2.0) - + qd[0 * 3 + 1] * + ((cos(q[0 * 3 + 1]) * + (RL * phi[1 * 10 + 8] - + (RL * phi[5 * 10 + 2] * cos(G[0 * 3 + 2] * q[0 * 3 + 2]) + + RL * l[4 * 3 + 1] * phi[5 * 10 + 0] * + cos(G[0 * 3 + 2] * q[0 * 3 + 2])) * + (l[4 * 3 + 2] * cos(G[0 * 3 + 2] * q[0 * 3 + 2]) + + l[4 * 3 + 0] * sin(G[0 * 3 + 2] * q[0 * 3 + 2])) + + (RL * phi[5 * 10 + 2] * sin(G[0 * 3 + 2] * q[0 * 3 + 2]) + + RL * l[4 * 3 + 1] * phi[5 * 10 + 0] * + sin(G[0 * 3 + 2] * q[0 * 3 + 2])) * + (l[4 * 3 + 0] * cos(G[0 * 3 + 2] * q[0 * 3 + 2]) - + l[4 * 3 + 2] * sin(G[0 * 3 + 2] * q[0 * 3 + 2])) - + (l[1 * 3 + 2] * cos(q[0 * 3 + 2]) + + l[1 * 3 + 0] * sin(q[0 * 3 + 2])) * + (RL * phi[2 * 10 + 2] * cos(q[0 * 3 + 2]) + + RL * l[1 * 3 + 1] * phi[2 * 10 + 0] * + cos(q[0 * 3 + 2])) + + (l[1 * 3 + 0] * cos(q[0 * 3 + 2]) - + l[1 * 3 + 2] * sin(q[0 * 3 + 2])) * + (RL * phi[2 * 10 + 2] * sin(q[0 * 3 + 2]) + + RL * l[1 * 3 + 1] * phi[2 * 10 + 0] * + sin(q[0 * 3 + 2])) - + RL * phi[2 * 10 + 7] * sin(q[0 * 3 + 2]) + + RL * phi[5 * 10 + 8] * cos(G[0 * 3 + 2] * q[0 * 3 + 2]) - + RL * phi[5 * 10 + 7] * sin(G[0 * 3 + 2] * q[0 * 3 + 2]) + + RL * phi[2 * 10 + 8] * cos(q[0 * 3 + 2]) - + RL * l[4 * 3 + 1] * phi[5 * 10 + 3] * + cos(G[0 * 3 + 2] * q[0 * 3 + 2]) + + RL * l[4 * 3 + 1] * phi[5 * 10 + 1] * + sin(G[0 * 3 + 2] * q[0 * 3 + 2]) - + RL * l[1 * 3 + 1] * phi[2 * 10 + 3] * cos(q[0 * 3 + 2]) + + RL * l[1 * 3 + 1] * phi[2 * 10 + 1] * sin(q[0 * 3 + 2]))) / + 2.0 + + (sin(q[0 * 3 + 1]) * + (-RL * phi[1 * 10 + 7] + + (RL * phi[5 * 10 + 2] * cos(G[0 * 3 + 2] * q[0 * 3 + 2]) + + RL * l[4 * 3 + 1] * phi[5 * 10 + 0] * + cos(G[0 * 3 + 2] * q[0 * 3 + 2])) * + (l[4 * 3 + 0] * cos(G[0 * 3 + 2] * q[0 * 3 + 2]) - + l[4 * 3 + 2] * sin(G[0 * 3 + 2] * q[0 * 3 + 2])) + + (RL * phi[5 * 10 + 2] * sin(G[0 * 3 + 2] * q[0 * 3 + 2]) + + RL * l[4 * 3 + 1] * phi[5 * 10 + 0] * + sin(G[0 * 3 + 2] * q[0 * 3 + 2])) * + (l[4 * 3 + 2] * cos(G[0 * 3 + 2] * q[0 * 3 + 2]) + + l[4 * 3 + 0] * sin(G[0 * 3 + 2] * q[0 * 3 + 2])) + + (l[1 * 3 + 0] * cos(q[0 * 3 + 2]) - + l[1 * 3 + 2] * sin(q[0 * 3 + 2])) * + (RL * phi[2 * 10 + 2] * cos(q[0 * 3 + 2]) + + RL * l[1 * 3 + 1] * phi[2 * 10 + 0] * + cos(q[0 * 3 + 2])) + + (l[1 * 3 + 2] * cos(q[0 * 3 + 2]) + + l[1 * 3 + 0] * sin(q[0 * 3 + 2])) * + (RL * phi[2 * 10 + 2] * sin(q[0 * 3 + 2]) + + RL * l[1 * 3 + 1] * phi[2 * 10 + 0] * + sin(q[0 * 3 + 2])) - + RL * phi[2 * 10 + 8] * sin(q[0 * 3 + 2]) - + RL * phi[5 * 10 + 7] * cos(G[0 * 3 + 2] * q[0 * 3 + 2]) - + RL * phi[5 * 10 + 8] * sin(G[0 * 3 + 2] * q[0 * 3 + 2]) - + RL * phi[2 * 10 + 7] * cos(q[0 * 3 + 2]) + + RL * l[4 * 3 + 1] * phi[5 * 10 + 1] * + cos(G[0 * 3 + 2] * q[0 * 3 + 2]) + + RL * l[4 * 3 + 1] * phi[5 * 10 + 3] * + sin(G[0 * 3 + 2] * q[0 * 3 + 2]) + + RL * l[1 * 3 + 1] * phi[2 * 10 + 1] * cos(q[0 * 3 + 2]) + + RL * l[1 * 3 + 1] * phi[2 * 10 + 3] * sin(q[0 * 3 + 2]))) / + 2.0 - + (RL * l[0 * 3 + 1] * cos(q[0 * 3 + 1]) * + (phi[1 * 10 + 3] + + phi[5 * 10 + 3] * cos(G[0 * 3 + 2] * q[0 * 3 + 2]) - + phi[5 * 10 + 1] * sin(G[0 * 3 + 2] * q[0 * 3 + 2]) + + phi[2 * 10 + 3] * cos(q[0 * 3 + 2]) - + phi[2 * 10 + 1] * sin(q[0 * 3 + 2]) + + phi[5 * 10 + 0] * cos(G[0 * 3 + 2] * q[0 * 3 + 2]) * + (l[4 * 3 + 2] * cos(G[0 * 3 + 2] * q[0 * 3 + 2]) + + l[4 * 3 + 0] * sin(G[0 * 3 + 2] * q[0 * 3 + 2])) + + phi[2 * 10 + 0] * cos(q[0 * 3 + 2]) * + (l[1 * 3 + 2] * cos(q[0 * 3 + 2]) + + l[1 * 3 + 0] * sin(q[0 * 3 + 2])) - + phi[5 * 10 + 0] * sin(G[0 * 3 + 2] * q[0 * 3 + 2]) * + (l[4 * 3 + 0] * cos(G[0 * 3 + 2] * q[0 * 3 + 2]) - + l[4 * 3 + 2] * sin(G[0 * 3 + 2] * q[0 * 3 + 2])) - + phi[2 * 10 + 0] * sin(q[0 * 3 + 2]) * + (l[1 * 3 + 0] * cos(q[0 * 3 + 2]) - + l[1 * 3 + 2] * sin(q[0 * 3 + 2])))) / + 2.0 + + (RL * l[0 * 3 + 1] * sin(q[0 * 3 + 1]) * + (phi[1 * 10 + 1] + + phi[5 * 10 + 1] * cos(G[0 * 3 + 2] * q[0 * 3 + 2]) + + phi[5 * 10 + 3] * sin(G[0 * 3 + 2] * q[0 * 3 + 2]) + + phi[2 * 10 + 1] * cos(q[0 * 3 + 2]) + + phi[2 * 10 + 3] * sin(q[0 * 3 + 2]) + + phi[5 * 10 + 0] * cos(G[0 * 3 + 2] * q[0 * 3 + 2]) * + (l[4 * 3 + 0] * cos(G[0 * 3 + 2] * q[0 * 3 + 2]) - + l[4 * 3 + 2] * sin(G[0 * 3 + 2] * q[0 * 3 + 2])) + + phi[2 * 10 + 0] * cos(q[0 * 3 + 2]) * + (l[1 * 3 + 0] * cos(q[0 * 3 + 2]) - + l[1 * 3 + 2] * sin(q[0 * 3 + 2])) + + phi[5 * 10 + 0] * sin(G[0 * 3 + 2] * q[0 * 3 + 2]) * + (l[4 * 3 + 2] * cos(G[0 * 3 + 2] * q[0 * 3 + 2]) + + l[4 * 3 + 0] * sin(G[0 * 3 + 2] * q[0 * 3 + 2])) + + phi[2 * 10 + 0] * sin(q[0 * 3 + 2]) * + (l[1 * 3 + 2] * cos(q[0 * 3 + 2]) + + l[1 * 3 + 0] * sin(q[0 * 3 + 2])))) / + 2.0) * + 2.0 + + G[0 * 3 + 2] * qd[0 * 3 + 2] * + ((sin(q[0 * 3 + 1]) * + (RL * phi[5 * 10 + 7] * cos(G[0 * 3 + 2] * q[0 * 3 + 2]) + + RL * phi[5 * 10 + 8] * sin(G[0 * 3 + 2] * q[0 * 3 + 2]) - + RL * l[4 * 3 + 1] * phi[5 * 10 + 1] * + cos(G[0 * 3 + 2] * q[0 * 3 + 2]) - + RL * l[4 * 3 + 1] * phi[5 * 10 + 3] * + sin(G[0 * 3 + 2] * q[0 * 3 + 2]))) / + 2.0 - + (cos(q[0 * 3 + 1]) * + (RL * phi[5 * 10 + 8] * cos(G[0 * 3 + 2] * q[0 * 3 + 2]) - + RL * phi[5 * 10 + 7] * sin(G[0 * 3 + 2] * q[0 * 3 + 2]) - + RL * l[4 * 3 + 1] * phi[5 * 10 + 3] * + cos(G[0 * 3 + 2] * q[0 * 3 + 2]) + + RL * l[4 * 3 + 1] * phi[5 * 10 + 1] * + sin(G[0 * 3 + 2] * q[0 * 3 + 2]))) / + 2.0 - + (RL * l[0 * 3 + 1] * sin(q[0 * 3 + 1]) * + (phi[5 * 10 + 1] * cos(G[0 * 3 + 2] * q[0 * 3 + 2]) + + phi[5 * 10 + 3] * sin(G[0 * 3 + 2] * q[0 * 3 + 2]))) / + 2.0 + + (RL * l[0 * 3 + 1] * cos(q[0 * 3 + 1]) * + (phi[5 * 10 + 3] * cos(G[0 * 3 + 2] * q[0 * 3 + 2]) - + phi[5 * 10 + 1] * sin(G[0 * 3 + 2] * q[0 * 3 + 2]))) / + 2.0) * + 2.0) - + cos(q[0 * 3 + 0]) * + (phi[5 * 10 + 1] * cos(q[0 * 3 + 1] + G[0 * 3 + 2] * q[0 * 3 + 2]) + + phi[5 * 10 + 3] * sin(q[0 * 3 + 1] + G[0 * 3 + 2] * q[0 * 3 + 2]) + + phi[2 * 10 + 1] * cos(q[0 * 3 + 1] + q[0 * 3 + 2]) + + phi[2 * 10 + 3] * sin(q[0 * 3 + 1] + q[0 * 3 + 2]) + + phi[1 * 10 + 1] * cos(q[0 * 3 + 1]) + + phi[1 * 10 + 3] * sin(q[0 * 3 + 1]) + + l[1 * 3 + 0] * phi[2 * 10 + 0] * cos(q[0 * 3 + 1]) + + l[4 * 3 + 0] * phi[5 * 10 + 0] * cos(q[0 * 3 + 1]) + + l[1 * 3 + 2] * phi[2 * 10 + 0] * sin(q[0 * 3 + 1]) + + l[4 * 3 + 2] * phi[5 * 10 + 0] * sin(q[0 * 3 + 1])) * + (9.81E+2 / 1.0E+2) - + G[0 * 3 + 1] * cos(q[0 * 3 + 0]) * + (phi[4 * 10 + 1] * cos(G[0 * 3 + 1] * q[0 * 3 + 1]) + + phi[4 * 10 + 3] * sin(G[0 * 3 + 1] * q[0 * 3 + 1])) * + (9.81E+2 / 1.0E+2); + F(0 * 3 + 2) = + qd[0 * 3 + 1] * + (qd[0 * 3 + 2] * + ((phi[2 * 10 + 1] * (l[1 * 3 + 2] * cos(q[0 * 3 + 2]) + + l[1 * 3 + 0] * sin(q[0 * 3 + 2]))) / + 2.0 - + (phi[2 * 10 + 3] * (l[1 * 3 + 0] * cos(q[0 * 3 + 2]) - + l[1 * 3 + 2] * sin(q[0 * 3 + 2]))) / + 2.0) * + 2.0 + + G[0 * 3 + 1] * + (qd[0 * 3 + 1] * + ((phi[5 * 10 + 1] * + (l[4 * 3 + 2] * cos(G[0 * 3 + 2] * q[0 * 3 + 2]) + + l[4 * 3 + 0] * sin(G[0 * 3 + 2] * q[0 * 3 + 2]))) / + 2.0 - + (phi[5 * 10 + 3] * + (l[4 * 3 + 0] * cos(G[0 * 3 + 2] * q[0 * 3 + 2]) - + l[4 * 3 + 2] * sin(G[0 * 3 + 2] * q[0 * 3 + 2]))) / + 2.0 + + ((phi[5 * 10 + 1] + + phi[5 * 10 + 0] * + (l[4 * 3 + 0] * cos(G[0 * 3 + 2] * q[0 * 3 + 2]) - + l[4 * 3 + 2] * + sin(G[0 * 3 + 2] * q[0 * 3 + 2]))) * + (l[4 * 3 + 2] * cos(G[0 * 3 + 2] * q[0 * 3 + 2]) + + l[4 * 3 + 0] * sin(G[0 * 3 + 2] * q[0 * 3 + 2]))) / + 2.0 - + ((phi[5 * 10 + 3] + + phi[5 * 10 + 0] * + (l[4 * 3 + 2] * cos(G[0 * 3 + 2] * q[0 * 3 + 2]) + + l[4 * 3 + 0] * + sin(G[0 * 3 + 2] * q[0 * 3 + 2]))) * + (l[4 * 3 + 0] * cos(G[0 * 3 + 2] * q[0 * 3 + 2]) - + l[4 * 3 + 2] * sin(G[0 * 3 + 2] * q[0 * 3 + 2]))) / + 2.0) + + G[0 * 3 + 2] * qd[0 * 3 + 2] * + ((phi[5 * 10 + 1] * + (l[4 * 3 + 2] * cos(G[0 * 3 + 2] * q[0 * 3 + 2]) + + l[4 * 3 + 0] * sin(G[0 * 3 + 2] * q[0 * 3 + 2]))) / + 2.0 - + (phi[5 * 10 + 3] * + (l[4 * 3 + 0] * cos(G[0 * 3 + 2] * q[0 * 3 + 2]) - + l[4 * 3 + 2] * sin(G[0 * 3 + 2] * q[0 * 3 + 2]))) / + 2.0) * + 2.0) + + qd[0 * 3 + 1] * + (((phi[2 * 10 + 1] + + phi[2 * 10 + 0] * (l[1 * 3 + 0] * cos(q[0 * 3 + 2]) - + l[1 * 3 + 2] * sin(q[0 * 3 + 2]))) * + (l[1 * 3 + 2] * cos(q[0 * 3 + 2]) + + l[1 * 3 + 0] * sin(q[0 * 3 + 2]))) / + 2.0 - + ((phi[2 * 10 + 3] + + phi[2 * 10 + 0] * (l[1 * 3 + 2] * cos(q[0 * 3 + 2]) + + l[1 * 3 + 0] * sin(q[0 * 3 + 2]))) * + (l[1 * 3 + 0] * cos(q[0 * 3 + 2]) - + l[1 * 3 + 2] * sin(q[0 * 3 + 2]))) / + 2.0 + + (phi[2 * 10 + 1] * (l[1 * 3 + 2] * cos(q[0 * 3 + 2]) + + l[1 * 3 + 0] * sin(q[0 * 3 + 2]))) / + 2.0 - + (phi[2 * 10 + 3] * (l[1 * 3 + 0] * cos(q[0 * 3 + 2]) - + l[1 * 3 + 2] * sin(q[0 * 3 + 2]))) / + 2.0)) + + qd[0 * 3 + 0] * + (G[0 * 3 + 0] * + (qd[0 * 3 + 1] * + ((sin(q[0 * 3 + 1]) * + (RL * phi[5 * 10 + 7] * + cos(G[0 * 3 + 2] * q[0 * 3 + 2]) + + RL * phi[5 * 10 + 8] * + sin(G[0 * 3 + 2] * q[0 * 3 + 2]) - + RL * l[4 * 3 + 1] * phi[5 * 10 + 1] * + cos(G[0 * 3 + 2] * q[0 * 3 + 2]) - + RL * l[4 * 3 + 1] * phi[5 * 10 + 3] * + sin(G[0 * 3 + 2] * q[0 * 3 + 2]))) / + 2.0 - + (cos(q[0 * 3 + 1]) * + (RL * phi[5 * 10 + 8] * + cos(G[0 * 3 + 2] * q[0 * 3 + 2]) - + RL * phi[5 * 10 + 7] * + sin(G[0 * 3 + 2] * q[0 * 3 + 2]) - + RL * l[4 * 3 + 1] * phi[5 * 10 + 3] * + cos(G[0 * 3 + 2] * q[0 * 3 + 2]) + + RL * l[4 * 3 + 1] * phi[5 * 10 + 1] * + sin(G[0 * 3 + 2] * q[0 * 3 + 2]))) / + 2.0 - + (RL * l[0 * 3 + 1] * sin(q[0 * 3 + 1]) * + (phi[5 * 10 + 1] * cos(G[0 * 3 + 2] * q[0 * 3 + 2]) + + phi[5 * 10 + 3] * sin(G[0 * 3 + 2] * q[0 * 3 + 2]))) / + 2.0 + + (RL * l[0 * 3 + 1] * cos(q[0 * 3 + 1]) * + (phi[5 * 10 + 3] * cos(G[0 * 3 + 2] * q[0 * 3 + 2]) - + phi[5 * 10 + 1] * sin(G[0 * 3 + 2] * q[0 * 3 + 2]))) / + 2.0) * + 2.0 + + qd[0 * 3 + 0] * + ((cos(q[0 * 3 + 1]) * + (sin(q[0 * 3 + 1]) * + (cos(G[0 * 3 + 2] * q[0 * 3 + 2]) * + (cos(G[0 * 3 + 2] * q[0 * 3 + 2]) * + (phi[5 * 10 + 4] - + phi[5 * 10 + 0] * + (1.0 / + (phi[5 * 10 + 0] * + phi[5 * 10 + 0]) * + (phi[5 * 10 + 3] * + phi[5 * 10 + 3]) + + (RL * RL) * 1.0 / + (phi[5 * 10 + 0] * + phi[5 * 10 + 0]) * + (phi[5 * 10 + 2] * + phi[5 * 10 + 2])) + + (phi[5 * 10 + 3] * phi[5 * 10 + 3]) / + phi[5 * 10 + 0] + + ((RL * RL) * + (phi[5 * 10 + 2] * phi[5 * 10 + 2])) / + phi[5 * 10 + 0]) + + phi[5 * 10 + 9] * + sin(G[0 * 3 + 2] * q[0 * 3 + 2]) + + (RL * RL) * l[4 * 3 + 1] * phi[5 * 10 + 2] * + cos(G[0 * 3 + 2] * q[0 * 3 + 2])) + + sin(G[0 * 3 + 2] * q[0 * 3 + 2]) * + (sin(G[0 * 3 + 2] * q[0 * 3 + 2]) * + (phi[5 * 10 + 6] - + phi[5 * 10 + 0] * + (1.0 / + (phi[5 * 10 + 0] * + phi[5 * 10 + 0]) * + (phi[5 * 10 + 1] * + phi[5 * 10 + 1]) + + (RL * RL) * 1.0 / + (phi[5 * 10 + 0] * + phi[5 * 10 + 0]) * + (phi[5 * 10 + 2] * + phi[5 * 10 + 2])) + + (phi[5 * 10 + 1] * phi[5 * 10 + 1]) / + phi[5 * 10 + 0] + + ((RL * RL) * + (phi[5 * 10 + 2] * phi[5 * 10 + 2])) / + phi[5 * 10 + 0]) + + phi[5 * 10 + 9] * + cos(G[0 * 3 + 2] * q[0 * 3 + 2]) + + (RL * RL) * l[4 * 3 + 1] * phi[5 * 10 + 2] * + sin(G[0 * 3 + 2] * q[0 * 3 + 2])) - + cos(G[0 * 3 + 2] * q[0 * 3 + 2]) * + (cos(G[0 * 3 + 2] * q[0 * 3 + 2]) * + (phi[5 * 10 + 6] - + phi[5 * 10 + 0] * + (1.0 / + (phi[5 * 10 + 0] * + phi[5 * 10 + 0]) * + (phi[5 * 10 + 1] * + phi[5 * 10 + 1]) + + (RL * RL) * 1.0 / + (phi[5 * 10 + 0] * + phi[5 * 10 + 0]) * + (phi[5 * 10 + 2] * + phi[5 * 10 + 2])) + + (phi[5 * 10 + 1] * phi[5 * 10 + 1]) / + phi[5 * 10 + 0] + + ((RL * RL) * + (phi[5 * 10 + 2] * phi[5 * 10 + 2])) / + phi[5 * 10 + 0]) + + l[4 * 3 + 0] * phi[5 * 10 + 1] - + phi[5 * 10 + 9] * + sin(G[0 * 3 + 2] * q[0 * 3 + 2]) + + (RL * RL) * l[4 * 3 + 1] * phi[5 * 10 + 2] * + cos(G[0 * 3 + 2] * q[0 * 3 + 2])) - + sin(G[0 * 3 + 2] * q[0 * 3 + 2]) * + (l[4 * 3 + 0] * phi[5 * 10 + 3] + + sin(G[0 * 3 + 2] * q[0 * 3 + 2]) * + (phi[5 * 10 + 4] - + phi[5 * 10 + 0] * + (1.0 / + (phi[5 * 10 + 0] * + phi[5 * 10 + 0]) * + (phi[5 * 10 + 3] * + phi[5 * 10 + 3]) + + (RL * RL) * 1.0 / + (phi[5 * 10 + 0] * + phi[5 * 10 + 0]) * + (phi[5 * 10 + 2] * + phi[5 * 10 + 2])) + + (phi[5 * 10 + 3] * phi[5 * 10 + 3]) / + phi[5 * 10 + 0] + + ((RL * RL) * + (phi[5 * 10 + 2] * phi[5 * 10 + 2])) / + phi[5 * 10 + 0]) - + phi[5 * 10 + 9] * + cos(G[0 * 3 + 2] * q[0 * 3 + 2]) + + (RL * RL) * l[4 * 3 + 1] * phi[5 * 10 + 2] * + sin(G[0 * 3 + 2] * q[0 * 3 + 2])) + + l[4 * 3 + 2] * + (phi[5 * 10 + 3] * + cos(G[0 * 3 + 2] * q[0 * 3 + 2]) - + phi[5 * 10 + 1] * + sin(G[0 * 3 + 2] * q[0 * 3 + 2]))) + + l[0 * 3 + 2] * (phi[5 * 10 + 1] * + cos(G[0 * 3 + 2] * q[0 * 3 + 2]) + + phi[5 * 10 + 3] * + sin(G[0 * 3 + 2] * q[0 * 3 + 2])) + + cos(q[0 * 3 + 1]) * + (-sin(G[0 * 3 + 2] * q[0 * 3 + 2]) * + (cos(G[0 * 3 + 2] * q[0 * 3 + 2]) * + (phi[5 * 10 + 6] - + phi[5 * 10 + 0] * + (1.0 / + (phi[5 * 10 + 0] * + phi[5 * 10 + 0]) * + (phi[5 * 10 + 1] * + phi[5 * 10 + 1]) + + (RL * RL) * 1.0 / + (phi[5 * 10 + 0] * + phi[5 * 10 + 0]) * + (phi[5 * 10 + 2] * + phi[5 * 10 + 2])) + + (phi[5 * 10 + 1] * phi[5 * 10 + 1]) / + phi[5 * 10 + 0] + + ((RL * RL) * + (phi[5 * 10 + 2] * phi[5 * 10 + 2])) / + phi[5 * 10 + 0]) - + phi[5 * 10 + 9] * + sin(G[0 * 3 + 2] * q[0 * 3 + 2]) + + (RL * RL) * l[4 * 3 + 1] * phi[5 * 10 + 2] * + cos(G[0 * 3 + 2] * q[0 * 3 + 2])) + + cos(G[0 * 3 + 2] * q[0 * 3 + 2]) * + (sin(G[0 * 3 + 2] * q[0 * 3 + 2]) * + (phi[5 * 10 + 4] - + phi[5 * 10 + 0] * + (1.0 / + (phi[5 * 10 + 0] * + phi[5 * 10 + 0]) * + (phi[5 * 10 + 3] * + phi[5 * 10 + 3]) + + (RL * RL) * 1.0 / + (phi[5 * 10 + 0] * + phi[5 * 10 + 0]) * + (phi[5 * 10 + 2] * + phi[5 * 10 + 2])) + + (phi[5 * 10 + 3] * phi[5 * 10 + 3]) / + phi[5 * 10 + 0] + + ((RL * RL) * + (phi[5 * 10 + 2] * phi[5 * 10 + 2])) / + phi[5 * 10 + 0]) - + phi[5 * 10 + 9] * + cos(G[0 * 3 + 2] * q[0 * 3 + 2]) + + (RL * RL) * l[4 * 3 + 1] * phi[5 * 10 + 2] * + sin(G[0 * 3 + 2] * q[0 * 3 + 2])) - + cos(G[0 * 3 + 2] * q[0 * 3 + 2]) * + (-l[4 * 3 + 2] * phi[5 * 10 + 1] + + sin(G[0 * 3 + 2] * q[0 * 3 + 2]) * + (phi[5 * 10 + 6] - + phi[5 * 10 + 0] * + (1.0 / + (phi[5 * 10 + 0] * + phi[5 * 10 + 0]) * + (phi[5 * 10 + 1] * + phi[5 * 10 + 1]) + + (RL * RL) * 1.0 / + (phi[5 * 10 + 0] * + phi[5 * 10 + 0]) * + (phi[5 * 10 + 2] * + phi[5 * 10 + 2])) + + (phi[5 * 10 + 1] * phi[5 * 10 + 1]) / + phi[5 * 10 + 0] + + ((RL * RL) * + (phi[5 * 10 + 2] * phi[5 * 10 + 2])) / + phi[5 * 10 + 0]) + + phi[5 * 10 + 9] * + cos(G[0 * 3 + 2] * q[0 * 3 + 2]) + + (RL * RL) * l[4 * 3 + 1] * phi[5 * 10 + 2] * + sin(G[0 * 3 + 2] * q[0 * 3 + 2])) + + sin(G[0 * 3 + 2] * q[0 * 3 + 2]) * + (cos(G[0 * 3 + 2] * q[0 * 3 + 2]) * + (phi[5 * 10 + 4] - + phi[5 * 10 + 0] * + (1.0 / + (phi[5 * 10 + 0] * + phi[5 * 10 + 0]) * + (phi[5 * 10 + 3] * + phi[5 * 10 + 3]) + + (RL * RL) * 1.0 / + (phi[5 * 10 + 0] * + phi[5 * 10 + 0]) * + (phi[5 * 10 + 2] * + phi[5 * 10 + 2])) + + (phi[5 * 10 + 3] * phi[5 * 10 + 3]) / + phi[5 * 10 + 0] + + ((RL * RL) * + (phi[5 * 10 + 2] * phi[5 * 10 + 2])) / + phi[5 * 10 + 0]) + + l[4 * 3 + 2] * phi[5 * 10 + 3] + + phi[5 * 10 + 9] * + sin(G[0 * 3 + 2] * q[0 * 3 + 2]) + + (RL * RL) * l[4 * 3 + 1] * phi[5 * 10 + 2] * + cos(G[0 * 3 + 2] * q[0 * 3 + 2])) + + l[4 * 3 + 2] * + (phi[5 * 10 + 1] * + cos(G[0 * 3 + 2] * q[0 * 3 + 2]) + + phi[5 * 10 + 3] * + sin(G[0 * 3 + 2] * q[0 * 3 + 2]))))) / + 2.0 - + (sin(q[0 * 3 + 1]) * + (sin(q[0 * 3 + 1]) * + (-cos(G[0 * 3 + 2] * q[0 * 3 + 2]) * + (sin(G[0 * 3 + 2] * q[0 * 3 + 2]) * + (phi[5 * 10 + 6] - + phi[5 * 10 + 0] * + (1.0 / + (phi[5 * 10 + 0] * + phi[5 * 10 + 0]) * + (phi[5 * 10 + 1] * + phi[5 * 10 + 1]) + + (RL * RL) * 1.0 / + (phi[5 * 10 + 0] * + phi[5 * 10 + 0]) * + (phi[5 * 10 + 2] * + phi[5 * 10 + 2])) + + (phi[5 * 10 + 1] * phi[5 * 10 + 1]) / + phi[5 * 10 + 0] + + ((RL * RL) * + (phi[5 * 10 + 2] * phi[5 * 10 + 2])) / + phi[5 * 10 + 0]) + + phi[5 * 10 + 9] * + cos(G[0 * 3 + 2] * q[0 * 3 + 2]) + + (RL * RL) * l[4 * 3 + 1] * phi[5 * 10 + 2] * + sin(G[0 * 3 + 2] * q[0 * 3 + 2])) + + sin(G[0 * 3 + 2] * q[0 * 3 + 2]) * + (cos(G[0 * 3 + 2] * q[0 * 3 + 2]) * + (phi[5 * 10 + 4] - + phi[5 * 10 + 0] * + (1.0 / + (phi[5 * 10 + 0] * + phi[5 * 10 + 0]) * + (phi[5 * 10 + 3] * + phi[5 * 10 + 3]) + + (RL * RL) * 1.0 / + (phi[5 * 10 + 0] * + phi[5 * 10 + 0]) * + (phi[5 * 10 + 2] * + phi[5 * 10 + 2])) + + (phi[5 * 10 + 3] * phi[5 * 10 + 3]) / + phi[5 * 10 + 0] + + ((RL * RL) * + (phi[5 * 10 + 2] * phi[5 * 10 + 2])) / + phi[5 * 10 + 0]) + + phi[5 * 10 + 9] * + sin(G[0 * 3 + 2] * q[0 * 3 + 2]) + + (RL * RL) * l[4 * 3 + 1] * phi[5 * 10 + 2] * + cos(G[0 * 3 + 2] * q[0 * 3 + 2])) - + sin(G[0 * 3 + 2] * q[0 * 3 + 2]) * + (cos(G[0 * 3 + 2] * q[0 * 3 + 2]) * + (phi[5 * 10 + 6] - + phi[5 * 10 + 0] * + (1.0 / + (phi[5 * 10 + 0] * + phi[5 * 10 + 0]) * + (phi[5 * 10 + 1] * + phi[5 * 10 + 1]) + + (RL * RL) * 1.0 / + (phi[5 * 10 + 0] * + phi[5 * 10 + 0]) * + (phi[5 * 10 + 2] * + phi[5 * 10 + 2])) + + (phi[5 * 10 + 1] * phi[5 * 10 + 1]) / + phi[5 * 10 + 0] + + ((RL * RL) * + (phi[5 * 10 + 2] * phi[5 * 10 + 2])) / + phi[5 * 10 + 0]) + + l[4 * 3 + 0] * phi[5 * 10 + 1] - + phi[5 * 10 + 9] * + sin(G[0 * 3 + 2] * q[0 * 3 + 2]) + + (RL * RL) * l[4 * 3 + 1] * phi[5 * 10 + 2] * + cos(G[0 * 3 + 2] * q[0 * 3 + 2])) + + cos(G[0 * 3 + 2] * q[0 * 3 + 2]) * + (l[4 * 3 + 0] * phi[5 * 10 + 3] + + sin(G[0 * 3 + 2] * q[0 * 3 + 2]) * + (phi[5 * 10 + 4] - + phi[5 * 10 + 0] * + (1.0 / + (phi[5 * 10 + 0] * + phi[5 * 10 + 0]) * + (phi[5 * 10 + 3] * + phi[5 * 10 + 3]) + + (RL * RL) * 1.0 / + (phi[5 * 10 + 0] * + phi[5 * 10 + 0]) * + (phi[5 * 10 + 2] * + phi[5 * 10 + 2])) + + (phi[5 * 10 + 3] * phi[5 * 10 + 3]) / + phi[5 * 10 + 0] + + ((RL * RL) * + (phi[5 * 10 + 2] * phi[5 * 10 + 2])) / + phi[5 * 10 + 0]) - + phi[5 * 10 + 9] * + cos(G[0 * 3 + 2] * q[0 * 3 + 2]) + + (RL * RL) * l[4 * 3 + 1] * phi[5 * 10 + 2] * + sin(G[0 * 3 + 2] * q[0 * 3 + 2])) + + l[4 * 3 + 0] * + (phi[5 * 10 + 3] * + cos(G[0 * 3 + 2] * q[0 * 3 + 2]) - + phi[5 * 10 + 1] * + sin(G[0 * 3 + 2] * q[0 * 3 + 2]))) - + l[0 * 3 + 2] * (phi[5 * 10 + 3] * + cos(G[0 * 3 + 2] * q[0 * 3 + 2]) - + phi[5 * 10 + 1] * + sin(G[0 * 3 + 2] * q[0 * 3 + 2])) + + cos(q[0 * 3 + 1]) * + (cos(G[0 * 3 + 2] * q[0 * 3 + 2]) * + (cos(G[0 * 3 + 2] * q[0 * 3 + 2]) * + (phi[5 * 10 + 6] - + phi[5 * 10 + 0] * + (1.0 / + (phi[5 * 10 + 0] * + phi[5 * 10 + 0]) * + (phi[5 * 10 + 1] * + phi[5 * 10 + 1]) + + (RL * RL) * 1.0 / + (phi[5 * 10 + 0] * + phi[5 * 10 + 0]) * + (phi[5 * 10 + 2] * + phi[5 * 10 + 2])) + + (phi[5 * 10 + 1] * phi[5 * 10 + 1]) / + phi[5 * 10 + 0] + + ((RL * RL) * + (phi[5 * 10 + 2] * phi[5 * 10 + 2])) / + phi[5 * 10 + 0]) - + phi[5 * 10 + 9] * + sin(G[0 * 3 + 2] * q[0 * 3 + 2]) + + (RL * RL) * l[4 * 3 + 1] * phi[5 * 10 + 2] * + cos(G[0 * 3 + 2] * q[0 * 3 + 2])) + + sin(G[0 * 3 + 2] * q[0 * 3 + 2]) * + (sin(G[0 * 3 + 2] * q[0 * 3 + 2]) * + (phi[5 * 10 + 4] - + phi[5 * 10 + 0] * + (1.0 / + (phi[5 * 10 + 0] * + phi[5 * 10 + 0]) * + (phi[5 * 10 + 3] * + phi[5 * 10 + 3]) + + (RL * RL) * 1.0 / + (phi[5 * 10 + 0] * + phi[5 * 10 + 0]) * + (phi[5 * 10 + 2] * + phi[5 * 10 + 2])) + + (phi[5 * 10 + 3] * phi[5 * 10 + 3]) / + phi[5 * 10 + 0] + + ((RL * RL) * + (phi[5 * 10 + 2] * phi[5 * 10 + 2])) / + phi[5 * 10 + 0]) - + phi[5 * 10 + 9] * + cos(G[0 * 3 + 2] * q[0 * 3 + 2]) + + (RL * RL) * l[4 * 3 + 1] * phi[5 * 10 + 2] * + sin(G[0 * 3 + 2] * q[0 * 3 + 2])) - + cos(G[0 * 3 + 2] * q[0 * 3 + 2]) * + (cos(G[0 * 3 + 2] * q[0 * 3 + 2]) * + (phi[5 * 10 + 4] - + phi[5 * 10 + 0] * + (1.0 / + (phi[5 * 10 + 0] * + phi[5 * 10 + 0]) * + (phi[5 * 10 + 3] * + phi[5 * 10 + 3]) + + (RL * RL) * 1.0 / + (phi[5 * 10 + 0] * + phi[5 * 10 + 0]) * + (phi[5 * 10 + 2] * + phi[5 * 10 + 2])) + + (phi[5 * 10 + 3] * phi[5 * 10 + 3]) / + phi[5 * 10 + 0] + + ((RL * RL) * + (phi[5 * 10 + 2] * phi[5 * 10 + 2])) / + phi[5 * 10 + 0]) + + l[4 * 3 + 2] * phi[5 * 10 + 3] + + phi[5 * 10 + 9] * + sin(G[0 * 3 + 2] * q[0 * 3 + 2]) + + (RL * RL) * l[4 * 3 + 1] * phi[5 * 10 + 2] * + cos(G[0 * 3 + 2] * q[0 * 3 + 2])) - + sin(G[0 * 3 + 2] * q[0 * 3 + 2]) * + (-l[4 * 3 + 2] * phi[5 * 10 + 1] + + sin(G[0 * 3 + 2] * q[0 * 3 + 2]) * + (phi[5 * 10 + 6] - + phi[5 * 10 + 0] * + (1.0 / + (phi[5 * 10 + 0] * + phi[5 * 10 + 0]) * + (phi[5 * 10 + 1] * + phi[5 * 10 + 1]) + + (RL * RL) * 1.0 / + (phi[5 * 10 + 0] * + phi[5 * 10 + 0]) * + (phi[5 * 10 + 2] * + phi[5 * 10 + 2])) + + (phi[5 * 10 + 1] * phi[5 * 10 + 1]) / + phi[5 * 10 + 0] + + ((RL * RL) * + (phi[5 * 10 + 2] * phi[5 * 10 + 2])) / + phi[5 * 10 + 0]) + + phi[5 * 10 + 9] * + cos(G[0 * 3 + 2] * q[0 * 3 + 2]) + + (RL * RL) * l[4 * 3 + 1] * phi[5 * 10 + 2] * + sin(G[0 * 3 + 2] * q[0 * 3 + 2])) + + l[4 * 3 + 0] * + (phi[5 * 10 + 1] * + cos(G[0 * 3 + 2] * q[0 * 3 + 2]) + + phi[5 * 10 + 3] * + sin(G[0 * 3 + 2] * q[0 * 3 + 2]))))) / + 2.0 + + (l[0 * 3 + 2] * + (cos(q[0 * 3 + 1]) * + (phi[5 * 10 + 1] * + cos(G[0 * 3 + 2] * q[0 * 3 + 2]) + + phi[5 * 10 + 3] * + sin(G[0 * 3 + 2] * q[0 * 3 + 2])) + + sin(q[0 * 3 + 1]) * + (phi[5 * 10 + 3] * + cos(G[0 * 3 + 2] * q[0 * 3 + 2]) - + phi[5 * 10 + 1] * + sin(G[0 * 3 + 2] * q[0 * 3 + 2])))) / + 2.0) + + G[0 * 3 + 2] * qd[0 * 3 + 2] * + ((sin(q[0 * 3 + 1]) * + (RL * phi[5 * 10 + 7] * + cos(G[0 * 3 + 2] * q[0 * 3 + 2]) + + RL * phi[5 * 10 + 8] * + sin(G[0 * 3 + 2] * q[0 * 3 + 2]) - + RL * l[4 * 3 + 1] * phi[5 * 10 + 1] * + cos(G[0 * 3 + 2] * q[0 * 3 + 2]) - + RL * l[4 * 3 + 1] * phi[5 * 10 + 3] * + sin(G[0 * 3 + 2] * q[0 * 3 + 2]))) / + 2.0 - + (cos(q[0 * 3 + 1]) * + (RL * phi[5 * 10 + 8] * + cos(G[0 * 3 + 2] * q[0 * 3 + 2]) - + RL * phi[5 * 10 + 7] * + sin(G[0 * 3 + 2] * q[0 * 3 + 2]) - + RL * l[4 * 3 + 1] * phi[5 * 10 + 3] * + cos(G[0 * 3 + 2] * q[0 * 3 + 2]) + + RL * l[4 * 3 + 1] * phi[5 * 10 + 1] * + sin(G[0 * 3 + 2] * q[0 * 3 + 2]))) / + 2.0 - + (RL * l[0 * 3 + 1] * sin(q[0 * 3 + 1]) * + (phi[5 * 10 + 1] * cos(G[0 * 3 + 2] * q[0 * 3 + 2]) + + phi[5 * 10 + 3] * sin(G[0 * 3 + 2] * q[0 * 3 + 2]))) / + 2.0 + + (RL * l[0 * 3 + 1] * cos(q[0 * 3 + 1]) * + (phi[5 * 10 + 3] * cos(G[0 * 3 + 2] * q[0 * 3 + 2]) - + phi[5 * 10 + 1] * sin(G[0 * 3 + 2] * q[0 * 3 + 2]))) / + 2.0) * + 2.0) + + qd[0 * 3 + 1] * + ((cos(q[0 * 3 + 1]) * + (RL * phi[2 * 10 + 7] * sin(q[0 * 3 + 2]) - + RL * phi[2 * 10 + 8] * cos(q[0 * 3 + 2]) + + RL * l[1 * 3 + 1] * phi[2 * 10 + 3] * cos(q[0 * 3 + 2]) - + RL * l[1 * 3 + 1] * phi[2 * 10 + 1] * sin(q[0 * 3 + 2]))) / + 2.0 + + (sin(q[0 * 3 + 1]) * + (RL * phi[2 * 10 + 8] * sin(q[0 * 3 + 2]) + + RL * phi[2 * 10 + 7] * cos(q[0 * 3 + 2]) - + RL * l[1 * 3 + 1] * phi[2 * 10 + 1] * cos(q[0 * 3 + 2]) - + RL * l[1 * 3 + 1] * phi[2 * 10 + 3] * sin(q[0 * 3 + 2]))) / + 2.0 + + (RL * l[0 * 3 + 1] * cos(q[0 * 3 + 1]) * + (phi[2 * 10 + 3] * cos(q[0 * 3 + 2]) - + phi[2 * 10 + 1] * sin(q[0 * 3 + 2]))) / + 2.0 - + (RL * l[0 * 3 + 1] * sin(q[0 * 3 + 1]) * + (phi[2 * 10 + 1] * cos(q[0 * 3 + 2]) + + phi[2 * 10 + 3] * sin(q[0 * 3 + 2]))) / + 2.0) * + 2.0 + + qd[0 * 3 + 2] * + ((cos(q[0 * 3 + 1]) * + (RL * phi[2 * 10 + 7] * sin(q[0 * 3 + 2]) - + RL * phi[2 * 10 + 8] * cos(q[0 * 3 + 2]) + + RL * l[1 * 3 + 1] * phi[2 * 10 + 3] * cos(q[0 * 3 + 2]) - + RL * l[1 * 3 + 1] * phi[2 * 10 + 1] * sin(q[0 * 3 + 2]))) / + 2.0 + + (sin(q[0 * 3 + 1]) * + (RL * phi[2 * 10 + 8] * sin(q[0 * 3 + 2]) + + RL * phi[2 * 10 + 7] * cos(q[0 * 3 + 2]) - + RL * l[1 * 3 + 1] * phi[2 * 10 + 1] * cos(q[0 * 3 + 2]) - + RL * l[1 * 3 + 1] * phi[2 * 10 + 3] * sin(q[0 * 3 + 2]))) / + 2.0 + + (RL * l[0 * 3 + 1] * cos(q[0 * 3 + 1]) * + (phi[2 * 10 + 3] * cos(q[0 * 3 + 2]) - + phi[2 * 10 + 1] * sin(q[0 * 3 + 2]))) / + 2.0 - + (RL * l[0 * 3 + 1] * sin(q[0 * 3 + 1]) * + (phi[2 * 10 + 1] * cos(q[0 * 3 + 2]) + + phi[2 * 10 + 3] * sin(q[0 * 3 + 2]))) / + 2.0) * + 2.0 + + qd[0 * 3 + 0] * + ((l[0 * 3 + 2] * + (cos(q[0 * 3 + 1]) * (phi[2 * 10 + 1] * cos(q[0 * 3 + 2]) + + phi[2 * 10 + 3] * sin(q[0 * 3 + 2])) + + sin(q[0 * 3 + 1]) * + (phi[2 * 10 + 3] * cos(q[0 * 3 + 2]) - + phi[2 * 10 + 1] * sin(q[0 * 3 + 2])))) / + 2.0 + + (cos(q[0 * 3 + 1]) * + (cos(q[0 * 3 + 1]) * + (l[1 * 3 + 2] * (phi[2 * 10 + 1] * cos(q[0 * 3 + 2]) + + phi[2 * 10 + 3] * sin(q[0 * 3 + 2])) - + sin(q[0 * 3 + 2]) * + (cos(q[0 * 3 + 2]) * + (phi[2 * 10 + 6] - + phi[2 * 10 + 0] * + (1.0 / + (phi[2 * 10 + 0] * + phi[2 * 10 + 0]) * + (phi[2 * 10 + 1] * + phi[2 * 10 + 1]) + + (RL * RL) * 1.0 / + (phi[2 * 10 + 0] * + phi[2 * 10 + 0]) * + (phi[2 * 10 + 2] * + phi[2 * 10 + 2])) + + (phi[2 * 10 + 1] * phi[2 * 10 + 1]) / + phi[2 * 10 + 0] + + ((RL * RL) * + (phi[2 * 10 + 2] * phi[2 * 10 + 2])) / + phi[2 * 10 + 0]) - + phi[2 * 10 + 9] * sin(q[0 * 3 + 2]) + + (RL * RL) * l[1 * 3 + 1] * phi[2 * 10 + 2] * + cos(q[0 * 3 + 2])) + + cos(q[0 * 3 + 2]) * + (sin(q[0 * 3 + 2]) * + (phi[2 * 10 + 4] - + phi[2 * 10 + 0] * + (1.0 / + (phi[2 * 10 + 0] * + phi[2 * 10 + 0]) * + (phi[2 * 10 + 3] * + phi[2 * 10 + 3]) + + (RL * RL) * 1.0 / + (phi[2 * 10 + 0] * + phi[2 * 10 + 0]) * + (phi[2 * 10 + 2] * + phi[2 * 10 + 2])) + + (phi[2 * 10 + 3] * phi[2 * 10 + 3]) / + phi[2 * 10 + 0] + + ((RL * RL) * + (phi[2 * 10 + 2] * phi[2 * 10 + 2])) / + phi[2 * 10 + 0]) - + phi[2 * 10 + 9] * cos(q[0 * 3 + 2]) + + (RL * RL) * l[1 * 3 + 1] * phi[2 * 10 + 2] * + sin(q[0 * 3 + 2])) - + cos(q[0 * 3 + 2]) * + (-l[1 * 3 + 2] * phi[2 * 10 + 1] + + sin(q[0 * 3 + 2]) * + (phi[2 * 10 + 6] - + phi[2 * 10 + 0] * + (1.0 / + (phi[2 * 10 + 0] * + phi[2 * 10 + 0]) * + (phi[2 * 10 + 1] * + phi[2 * 10 + 1]) + + (RL * RL) * 1.0 / + (phi[2 * 10 + 0] * + phi[2 * 10 + 0]) * + (phi[2 * 10 + 2] * + phi[2 * 10 + 2])) + + (phi[2 * 10 + 1] * phi[2 * 10 + 1]) / + phi[2 * 10 + 0] + + ((RL * RL) * + (phi[2 * 10 + 2] * phi[2 * 10 + 2])) / + phi[2 * 10 + 0]) + + phi[2 * 10 + 9] * cos(q[0 * 3 + 2]) + + (RL * RL) * l[1 * 3 + 1] * phi[2 * 10 + 2] * + sin(q[0 * 3 + 2])) + + sin(q[0 * 3 + 2]) * + (l[1 * 3 + 2] * phi[2 * 10 + 3] + + cos(q[0 * 3 + 2]) * + (phi[2 * 10 + 4] - + phi[2 * 10 + 0] * + (1.0 / + (phi[2 * 10 + 0] * + phi[2 * 10 + 0]) * + (phi[2 * 10 + 3] * + phi[2 * 10 + 3]) + + (RL * RL) * 1.0 / + (phi[2 * 10 + 0] * + phi[2 * 10 + 0]) * + (phi[2 * 10 + 2] * + phi[2 * 10 + 2])) + + (phi[2 * 10 + 3] * phi[2 * 10 + 3]) / + phi[2 * 10 + 0] + + ((RL * RL) * + (phi[2 * 10 + 2] * phi[2 * 10 + 2])) / + phi[2 * 10 + 0]) + + phi[2 * 10 + 9] * sin(q[0 * 3 + 2]) + + (RL * RL) * l[1 * 3 + 1] * phi[2 * 10 + 2] * + cos(q[0 * 3 + 2]))) + + sin(q[0 * 3 + 1]) * + (l[1 * 3 + 2] * (phi[2 * 10 + 3] * cos(q[0 * 3 + 2]) - + phi[2 * 10 + 1] * sin(q[0 * 3 + 2])) + + cos(q[0 * 3 + 2]) * + (cos(q[0 * 3 + 2]) * + (phi[2 * 10 + 4] - + phi[2 * 10 + 0] * + (1.0 / + (phi[2 * 10 + 0] * + phi[2 * 10 + 0]) * + (phi[2 * 10 + 3] * + phi[2 * 10 + 3]) + + (RL * RL) * 1.0 / + (phi[2 * 10 + 0] * + phi[2 * 10 + 0]) * + (phi[2 * 10 + 2] * + phi[2 * 10 + 2])) + + (phi[2 * 10 + 3] * phi[2 * 10 + 3]) / + phi[2 * 10 + 0] + + ((RL * RL) * + (phi[2 * 10 + 2] * phi[2 * 10 + 2])) / + phi[2 * 10 + 0]) + + phi[2 * 10 + 9] * sin(q[0 * 3 + 2]) + + (RL * RL) * l[1 * 3 + 1] * phi[2 * 10 + 2] * + cos(q[0 * 3 + 2])) + + sin(q[0 * 3 + 2]) * + (sin(q[0 * 3 + 2]) * + (phi[2 * 10 + 6] - + phi[2 * 10 + 0] * + (1.0 / + (phi[2 * 10 + 0] * + phi[2 * 10 + 0]) * + (phi[2 * 10 + 1] * + phi[2 * 10 + 1]) + + (RL * RL) * 1.0 / + (phi[2 * 10 + 0] * + phi[2 * 10 + 0]) * + (phi[2 * 10 + 2] * + phi[2 * 10 + 2])) + + (phi[2 * 10 + 1] * phi[2 * 10 + 1]) / + phi[2 * 10 + 0] + + ((RL * RL) * + (phi[2 * 10 + 2] * phi[2 * 10 + 2])) / + phi[2 * 10 + 0]) + + phi[2 * 10 + 9] * cos(q[0 * 3 + 2]) + + (RL * RL) * l[1 * 3 + 1] * phi[2 * 10 + 2] * + sin(q[0 * 3 + 2])) - + cos(q[0 * 3 + 2]) * + (l[1 * 3 + 0] * phi[2 * 10 + 1] + + cos(q[0 * 3 + 2]) * + (phi[2 * 10 + 6] - + phi[2 * 10 + 0] * + (1.0 / + (phi[2 * 10 + 0] * + phi[2 * 10 + 0]) * + (phi[2 * 10 + 1] * + phi[2 * 10 + 1]) + + (RL * RL) * 1.0 / + (phi[2 * 10 + 0] * + phi[2 * 10 + 0]) * + (phi[2 * 10 + 2] * + phi[2 * 10 + 2])) + + (phi[2 * 10 + 1] * phi[2 * 10 + 1]) / + phi[2 * 10 + 0] + + ((RL * RL) * + (phi[2 * 10 + 2] * phi[2 * 10 + 2])) / + phi[2 * 10 + 0]) - + phi[2 * 10 + 9] * sin(q[0 * 3 + 2]) + + (RL * RL) * l[1 * 3 + 1] * phi[2 * 10 + 2] * + cos(q[0 * 3 + 2])) - + sin(q[0 * 3 + 2]) * + (l[1 * 3 + 0] * phi[2 * 10 + 3] + + sin(q[0 * 3 + 2]) * + (phi[2 * 10 + 4] - + phi[2 * 10 + 0] * + (1.0 / + (phi[2 * 10 + 0] * + phi[2 * 10 + 0]) * + (phi[2 * 10 + 3] * + phi[2 * 10 + 3]) + + (RL * RL) * 1.0 / + (phi[2 * 10 + 0] * + phi[2 * 10 + 0]) * + (phi[2 * 10 + 2] * + phi[2 * 10 + 2])) + + (phi[2 * 10 + 3] * phi[2 * 10 + 3]) / + phi[2 * 10 + 0] + + ((RL * RL) * + (phi[2 * 10 + 2] * phi[2 * 10 + 2])) / + phi[2 * 10 + 0]) - + phi[2 * 10 + 9] * cos(q[0 * 3 + 2]) + + (RL * RL) * l[1 * 3 + 1] * phi[2 * 10 + 2] * + sin(q[0 * 3 + 2]))) + + l[0 * 3 + 2] * (phi[2 * 10 + 1] * cos(q[0 * 3 + 2]) + + phi[2 * 10 + 3] * sin(q[0 * 3 + 2])))) / + 2.0 - + (sin(q[0 * 3 + 1]) * + (cos(q[0 * 3 + 1]) * + (l[1 * 3 + 0] * (phi[2 * 10 + 1] * cos(q[0 * 3 + 2]) + + phi[2 * 10 + 3] * sin(q[0 * 3 + 2])) + + cos(q[0 * 3 + 2]) * + (cos(q[0 * 3 + 2]) * + (phi[2 * 10 + 6] - + phi[2 * 10 + 0] * + (1.0 / + (phi[2 * 10 + 0] * + phi[2 * 10 + 0]) * + (phi[2 * 10 + 1] * + phi[2 * 10 + 1]) + + (RL * RL) * 1.0 / + (phi[2 * 10 + 0] * + phi[2 * 10 + 0]) * + (phi[2 * 10 + 2] * + phi[2 * 10 + 2])) + + (phi[2 * 10 + 1] * phi[2 * 10 + 1]) / + phi[2 * 10 + 0] + + ((RL * RL) * + (phi[2 * 10 + 2] * phi[2 * 10 + 2])) / + phi[2 * 10 + 0]) - + phi[2 * 10 + 9] * sin(q[0 * 3 + 2]) + + (RL * RL) * l[1 * 3 + 1] * phi[2 * 10 + 2] * + cos(q[0 * 3 + 2])) + + sin(q[0 * 3 + 2]) * + (sin(q[0 * 3 + 2]) * + (phi[2 * 10 + 4] - + phi[2 * 10 + 0] * + (1.0 / + (phi[2 * 10 + 0] * + phi[2 * 10 + 0]) * + (phi[2 * 10 + 3] * + phi[2 * 10 + 3]) + + (RL * RL) * 1.0 / + (phi[2 * 10 + 0] * + phi[2 * 10 + 0]) * + (phi[2 * 10 + 2] * + phi[2 * 10 + 2])) + + (phi[2 * 10 + 3] * phi[2 * 10 + 3]) / + phi[2 * 10 + 0] + + ((RL * RL) * + (phi[2 * 10 + 2] * phi[2 * 10 + 2])) / + phi[2 * 10 + 0]) - + phi[2 * 10 + 9] * cos(q[0 * 3 + 2]) + + (RL * RL) * l[1 * 3 + 1] * phi[2 * 10 + 2] * + sin(q[0 * 3 + 2])) - + cos(q[0 * 3 + 2]) * + (l[1 * 3 + 2] * phi[2 * 10 + 3] + + cos(q[0 * 3 + 2]) * + (phi[2 * 10 + 4] - + phi[2 * 10 + 0] * + (1.0 / + (phi[2 * 10 + 0] * + phi[2 * 10 + 0]) * + (phi[2 * 10 + 3] * + phi[2 * 10 + 3]) + + (RL * RL) * 1.0 / + (phi[2 * 10 + 0] * + phi[2 * 10 + 0]) * + (phi[2 * 10 + 2] * + phi[2 * 10 + 2])) + + (phi[2 * 10 + 3] * phi[2 * 10 + 3]) / + phi[2 * 10 + 0] + + ((RL * RL) * + (phi[2 * 10 + 2] * phi[2 * 10 + 2])) / + phi[2 * 10 + 0]) + + phi[2 * 10 + 9] * sin(q[0 * 3 + 2]) + + (RL * RL) * l[1 * 3 + 1] * phi[2 * 10 + 2] * + cos(q[0 * 3 + 2])) - + sin(q[0 * 3 + 2]) * + (-l[1 * 3 + 2] * phi[2 * 10 + 1] + + sin(q[0 * 3 + 2]) * + (phi[2 * 10 + 6] - + phi[2 * 10 + 0] * + (1.0 / + (phi[2 * 10 + 0] * + phi[2 * 10 + 0]) * + (phi[2 * 10 + 1] * + phi[2 * 10 + 1]) + + (RL * RL) * 1.0 / + (phi[2 * 10 + 0] * + phi[2 * 10 + 0]) * + (phi[2 * 10 + 2] * + phi[2 * 10 + 2])) + + (phi[2 * 10 + 1] * phi[2 * 10 + 1]) / + phi[2 * 10 + 0] + + ((RL * RL) * + (phi[2 * 10 + 2] * phi[2 * 10 + 2])) / + phi[2 * 10 + 0]) + + phi[2 * 10 + 9] * cos(q[0 * 3 + 2]) + + (RL * RL) * l[1 * 3 + 1] * phi[2 * 10 + 2] * + sin(q[0 * 3 + 2]))) + + sin(q[0 * 3 + 1]) * + (l[1 * 3 + 0] * (phi[2 * 10 + 3] * cos(q[0 * 3 + 2]) - + phi[2 * 10 + 1] * sin(q[0 * 3 + 2])) - + cos(q[0 * 3 + 2]) * + (sin(q[0 * 3 + 2]) * + (phi[2 * 10 + 6] - + phi[2 * 10 + 0] * + (1.0 / + (phi[2 * 10 + 0] * + phi[2 * 10 + 0]) * + (phi[2 * 10 + 1] * + phi[2 * 10 + 1]) + + (RL * RL) * 1.0 / + (phi[2 * 10 + 0] * + phi[2 * 10 + 0]) * + (phi[2 * 10 + 2] * + phi[2 * 10 + 2])) + + (phi[2 * 10 + 1] * phi[2 * 10 + 1]) / + phi[2 * 10 + 0] + + ((RL * RL) * + (phi[2 * 10 + 2] * phi[2 * 10 + 2])) / + phi[2 * 10 + 0]) + + phi[2 * 10 + 9] * cos(q[0 * 3 + 2]) + + (RL * RL) * l[1 * 3 + 1] * phi[2 * 10 + 2] * + sin(q[0 * 3 + 2])) + + sin(q[0 * 3 + 2]) * + (cos(q[0 * 3 + 2]) * + (phi[2 * 10 + 4] - + phi[2 * 10 + 0] * + (1.0 / + (phi[2 * 10 + 0] * + phi[2 * 10 + 0]) * + (phi[2 * 10 + 3] * + phi[2 * 10 + 3]) + + (RL * RL) * 1.0 / + (phi[2 * 10 + 0] * + phi[2 * 10 + 0]) * + (phi[2 * 10 + 2] * + phi[2 * 10 + 2])) + + (phi[2 * 10 + 3] * phi[2 * 10 + 3]) / + phi[2 * 10 + 0] + + ((RL * RL) * + (phi[2 * 10 + 2] * phi[2 * 10 + 2])) / + phi[2 * 10 + 0]) + + phi[2 * 10 + 9] * sin(q[0 * 3 + 2]) + + (RL * RL) * l[1 * 3 + 1] * phi[2 * 10 + 2] * + cos(q[0 * 3 + 2])) - + sin(q[0 * 3 + 2]) * + (l[1 * 3 + 0] * phi[2 * 10 + 1] + + cos(q[0 * 3 + 2]) * + (phi[2 * 10 + 6] - + phi[2 * 10 + 0] * + (1.0 / + (phi[2 * 10 + 0] * + phi[2 * 10 + 0]) * + (phi[2 * 10 + 1] * + phi[2 * 10 + 1]) + + (RL * RL) * 1.0 / + (phi[2 * 10 + 0] * + phi[2 * 10 + 0]) * + (phi[2 * 10 + 2] * + phi[2 * 10 + 2])) + + (phi[2 * 10 + 1] * phi[2 * 10 + 1]) / + phi[2 * 10 + 0] + + ((RL * RL) * + (phi[2 * 10 + 2] * phi[2 * 10 + 2])) / + phi[2 * 10 + 0]) - + phi[2 * 10 + 9] * sin(q[0 * 3 + 2]) + + (RL * RL) * l[1 * 3 + 1] * phi[2 * 10 + 2] * + cos(q[0 * 3 + 2])) + + cos(q[0 * 3 + 2]) * + (l[1 * 3 + 0] * phi[2 * 10 + 3] + + sin(q[0 * 3 + 2]) * + (phi[2 * 10 + 4] - + phi[2 * 10 + 0] * + (1.0 / + (phi[2 * 10 + 0] * + phi[2 * 10 + 0]) * + (phi[2 * 10 + 3] * + phi[2 * 10 + 3]) + + (RL * RL) * 1.0 / + (phi[2 * 10 + 0] * + phi[2 * 10 + 0]) * + (phi[2 * 10 + 2] * + phi[2 * 10 + 2])) + + (phi[2 * 10 + 3] * phi[2 * 10 + 3]) / + phi[2 * 10 + 0] + + ((RL * RL) * + (phi[2 * 10 + 2] * phi[2 * 10 + 2])) / + phi[2 * 10 + 0]) - + phi[2 * 10 + 9] * cos(q[0 * 3 + 2]) + + (RL * RL) * l[1 * 3 + 1] * phi[2 * 10 + 2] * + sin(q[0 * 3 + 2]))) - + l[0 * 3 + 2] * (phi[2 * 10 + 3] * cos(q[0 * 3 + 2]) - + phi[2 * 10 + 1] * sin(q[0 * 3 + 2])))) / + 2.0)) - + cos(q[0 * 3 + 0]) * + (phi[2 * 10 + 1] * cos(q[0 * 3 + 1] + q[0 * 3 + 2]) + + phi[2 * 10 + 3] * sin(q[0 * 3 + 1] + q[0 * 3 + 2])) * + (9.81E+2 / 1.0E+2) - + G[0 * 3 + 2] * cos(q[0 * 3 + 0]) * + (phi[5 * 10 + 1] * cos(q[0 * 3 + 1] + G[0 * 3 + 2] * q[0 * 3 + 2]) + + phi[5 * 10 + 3] * + sin(q[0 * 3 + 1] + G[0 * 3 + 2] * q[0 * 3 + 2])) * + (9.81E+2 / 1.0E+2); +} + +void f_J_MO(Eigen::Vector3d q, int RL, Eigen::Matrix3d& F) { + F(0 * 3 + 0) = + ((cos(q[0 * 3 + 1]) * cos(q[0 * 3 + 2]) - + sin(q[0 * 3 + 1]) * sin(q[0 * 3 + 2])) * + (RL * l[0 * 3 + 1] * cos(q[0 * 3 + 1]) * sin(q[0 * 3 + 2]) + + RL * l[0 * 3 + 1] * cos(q[0 * 3 + 2]) * sin(q[0 * 3 + 1]) + + RL * l[1 * 3 + 1] * cos(q[0 * 3 + 1]) * sin(q[0 * 3 + 2]) + + RL * l[1 * 3 + 1] * cos(q[0 * 3 + 2]) * sin(q[0 * 3 + 1])) * + -2.0) / + (pow(cos(q[0 * 3 + 1]), 2.0) * pow(cos(q[0 * 3 + 2]), 2.0) + + pow(cos(q[0 * 3 + 1]), 2.0) * pow(sin(q[0 * 3 + 2]), 2.0) + + pow(cos(q[0 * 3 + 2]), 2.0) * pow(sin(q[0 * 3 + 1]), 2.0) + + pow(sin(q[0 * 3 + 1]), 2.0) * pow(sin(q[0 * 3 + 2]), 2.0)) + + ((cos(q[0 * 3 + 1]) * sin(q[0 * 3 + 2]) + + cos(q[0 * 3 + 2]) * sin(q[0 * 3 + 1])) * + (RL * l[0 * 3 + 1] * cos(q[0 * 3 + 1]) * cos(q[0 * 3 + 2]) + + RL * l[1 * 3 + 1] * cos(q[0 * 3 + 1]) * cos(q[0 * 3 + 2]) - + RL * l[0 * 3 + 1] * sin(q[0 * 3 + 1]) * sin(q[0 * 3 + 2]) - + RL * l[1 * 3 + 1] * sin(q[0 * 3 + 1]) * sin(q[0 * 3 + 2])) * + 2.0) / + (pow(cos(q[0 * 3 + 1]), 2.0) * pow(cos(q[0 * 3 + 2]), 2.0) + + pow(cos(q[0 * 3 + 1]), 2.0) * pow(sin(q[0 * 3 + 2]), 2.0) + + pow(cos(q[0 * 3 + 2]), 2.0) * pow(sin(q[0 * 3 + 1]), 2.0) + + pow(sin(q[0 * 3 + 1]), 2.0) * pow(sin(q[0 * 3 + 2]), 2.0)); + F(1 * 3 + 0) = + -(l[0 * 3 + 2] * pow(sin(q[0 * 3 + 1]), 2.0) + + l[1 * 3 + 2] * cos(q[0 * 3 + 1]) - l[1 * 3 + 0] * sin(q[0 * 3 + 1]) + + l[0 * 3 + 2] * pow(cos(q[0 * 3 + 1]), 2.0)) / + (pow(cos(q[0 * 3 + 1]), 2.0) + pow(sin(q[0 * 3 + 1]), 2.0)) + + (cos(q[0 * 3 + 0]) * + ((sin(q[0 * 3 + 0]) * + (RL * l[2 * 3 + 1] + + ((cos(q[0 * 3 + 1]) * sin(q[0 * 3 + 2]) + + cos(q[0 * 3 + 2]) * sin(q[0 * 3 + 1])) * + (RL * l[0 * 3 + 1] * cos(q[0 * 3 + 1]) * sin(q[0 * 3 + 2]) + + RL * l[0 * 3 + 1] * cos(q[0 * 3 + 2]) * sin(q[0 * 3 + 1]) + + RL * l[1 * 3 + 1] * cos(q[0 * 3 + 1]) * sin(q[0 * 3 + 2]) + + RL * l[1 * 3 + 1] * cos(q[0 * 3 + 2]) * sin(q[0 * 3 + 1])) * + 1.0 / + pow(pow(cos(q[0 * 3 + 1]), 2.0) * pow(cos(q[0 * 3 + 2]), 2.0) + + pow(cos(q[0 * 3 + 1]), 2.0) * pow(sin(q[0 * 3 + 2]), 2.0) + + pow(cos(q[0 * 3 + 2]), 2.0) * pow(sin(q[0 * 3 + 1]), 2.0) + + pow(sin(q[0 * 3 + 1]), 2.0) * pow(sin(q[0 * 3 + 2]), 2.0), + 2.0)) / + 2.0 + + ((cos(q[0 * 3 + 1]) * cos(q[0 * 3 + 2]) - + sin(q[0 * 3 + 1]) * sin(q[0 * 3 + 2])) * + (RL * l[0 * 3 + 1] * cos(q[0 * 3 + 1]) * cos(q[0 * 3 + 2]) + + RL * l[1 * 3 + 1] * cos(q[0 * 3 + 1]) * cos(q[0 * 3 + 2]) - + RL * l[0 * 3 + 1] * sin(q[0 * 3 + 1]) * sin(q[0 * 3 + 2]) - + RL * l[1 * 3 + 1] * sin(q[0 * 3 + 1]) * sin(q[0 * 3 + 2])) * + 1.0 / + pow(pow(cos(q[0 * 3 + 1]), 2.0) * pow(cos(q[0 * 3 + 2]), 2.0) + + pow(cos(q[0 * 3 + 1]), 2.0) * pow(sin(q[0 * 3 + 2]), 2.0) + + pow(cos(q[0 * 3 + 2]), 2.0) * pow(sin(q[0 * 3 + 1]), 2.0) + + pow(sin(q[0 * 3 + 1]), 2.0) * pow(sin(q[0 * 3 + 2]), 2.0), + 2.0)) / + 2.0 + + (sin(q[0 * 3 + 0]) * + (cos(q[0 * 3 + 1]) * sin(q[0 * 3 + 2]) + + cos(q[0 * 3 + 2]) * sin(q[0 * 3 + 1])) * + 1.0 / + pow(pow(cos(q[0 * 3 + 0]), 2.0) * pow(cos(q[0 * 3 + 1]), 2.0) * + pow(sin(q[0 * 3 + 2]), 2.0) + + pow(cos(q[0 * 3 + 0]), 2.0) * pow(cos(q[0 * 3 + 2]), 2.0) * + pow(sin(q[0 * 3 + 1]), 2.0) + + pow(cos(q[0 * 3 + 1]), 2.0) * pow(cos(q[0 * 3 + 2]), 2.0) * + pow(sin(q[0 * 3 + 0]), 2.0) + + pow(cos(q[0 * 3 + 0]), 2.0) * pow(sin(q[0 * 3 + 1]), 2.0) * + pow(sin(q[0 * 3 + 2]), 2.0) + + pow(cos(q[0 * 3 + 1]), 2.0) * pow(sin(q[0 * 3 + 0]), 2.0) * + pow(sin(q[0 * 3 + 2]), 2.0) + + pow(cos(q[0 * 3 + 2]), 2.0) * pow(sin(q[0 * 3 + 0]), 2.0) * + pow(sin(q[0 * 3 + 1]), 2.0) + + pow(sin(q[0 * 3 + 0]), 2.0) * pow(sin(q[0 * 3 + 1]), 2.0) * + pow(sin(q[0 * 3 + 2]), 2.0) + + pow(cos(q[0 * 3 + 0]), 2.0) * pow(cos(q[0 * 3 + 1]), 2.0) * + pow(cos(q[0 * 3 + 2]), 2.0), + 2.0) * + (-l[0 * 3 + 0] * cos(q[0 * 3 + 0]) * cos(q[0 * 3 + 1]) * + cos(q[0 * 3 + 2]) + + l[0 * 3 + 2] * cos(q[0 * 3 + 0]) * cos(q[0 * 3 + 1]) * + sin(q[0 * 3 + 2]) + + l[0 * 3 + 2] * cos(q[0 * 3 + 0]) * cos(q[0 * 3 + 2]) * + sin(q[0 * 3 + 1]) + + l[0 * 3 + 0] * cos(q[0 * 3 + 0]) * sin(q[0 * 3 + 1]) * + sin(q[0 * 3 + 2]) - + l[1 * 3 + 0] * cos(q[0 * 3 + 0]) * pow(cos(q[0 * 3 + 1]), 2.0) * + cos(q[0 * 3 + 2]) - + l[1 * 3 + 0] * cos(q[0 * 3 + 0]) * cos(q[0 * 3 + 2]) * + pow(sin(q[0 * 3 + 1]), 2.0) + + l[1 * 3 + 2] * cos(q[0 * 3 + 0]) * pow(cos(q[0 * 3 + 1]), 2.0) * + sin(q[0 * 3 + 2]) + + l[1 * 3 + 2] * cos(q[0 * 3 + 0]) * pow(sin(q[0 * 3 + 1]), 2.0) * + sin(q[0 * 3 + 2]) + + RL * l[0 * 3 + 1] * cos(q[0 * 3 + 1]) * sin(q[0 * 3 + 0]) * + sin(q[0 * 3 + 2]) + + RL * l[0 * 3 + 1] * cos(q[0 * 3 + 2]) * sin(q[0 * 3 + 0]) * + sin(q[0 * 3 + 1]) + + RL * l[1 * 3 + 1] * cos(q[0 * 3 + 1]) * sin(q[0 * 3 + 0]) * + sin(q[0 * 3 + 2]) + + RL * l[1 * 3 + 1] * cos(q[0 * 3 + 2]) * sin(q[0 * 3 + 0]) * + sin(q[0 * 3 + 1]))) / + 2.0 + + (sin(q[0 * 3 + 0]) * + (cos(q[0 * 3 + 1]) * cos(q[0 * 3 + 2]) - + sin(q[0 * 3 + 1]) * sin(q[0 * 3 + 2])) * + 1.0 / + pow(pow(cos(q[0 * 3 + 0]), 2.0) * pow(cos(q[0 * 3 + 1]), 2.0) * + pow(sin(q[0 * 3 + 2]), 2.0) + + pow(cos(q[0 * 3 + 0]), 2.0) * pow(cos(q[0 * 3 + 2]), 2.0) * + pow(sin(q[0 * 3 + 1]), 2.0) + + pow(cos(q[0 * 3 + 1]), 2.0) * pow(cos(q[0 * 3 + 2]), 2.0) * + pow(sin(q[0 * 3 + 0]), 2.0) + + pow(cos(q[0 * 3 + 0]), 2.0) * pow(sin(q[0 * 3 + 1]), 2.0) * + pow(sin(q[0 * 3 + 2]), 2.0) + + pow(cos(q[0 * 3 + 1]), 2.0) * pow(sin(q[0 * 3 + 0]), 2.0) * + pow(sin(q[0 * 3 + 2]), 2.0) + + pow(cos(q[0 * 3 + 2]), 2.0) * pow(sin(q[0 * 3 + 0]), 2.0) * + pow(sin(q[0 * 3 + 1]), 2.0) + + pow(sin(q[0 * 3 + 0]), 2.0) * pow(sin(q[0 * 3 + 1]), 2.0) * + pow(sin(q[0 * 3 + 2]), 2.0) + + pow(cos(q[0 * 3 + 0]), 2.0) * pow(cos(q[0 * 3 + 1]), 2.0) * + pow(cos(q[0 * 3 + 2]), 2.0), + 2.0) * + (l[0 * 3 + 2] * cos(q[0 * 3 + 0]) * cos(q[0 * 3 + 1]) * + cos(q[0 * 3 + 2]) + + l[0 * 3 + 0] * cos(q[0 * 3 + 0]) * cos(q[0 * 3 + 1]) * + sin(q[0 * 3 + 2]) + + l[0 * 3 + 0] * cos(q[0 * 3 + 0]) * cos(q[0 * 3 + 2]) * + sin(q[0 * 3 + 1]) - + l[0 * 3 + 2] * cos(q[0 * 3 + 0]) * sin(q[0 * 3 + 1]) * + sin(q[0 * 3 + 2]) + + l[1 * 3 + 2] * cos(q[0 * 3 + 0]) * pow(cos(q[0 * 3 + 1]), 2.0) * + cos(q[0 * 3 + 2]) + + l[1 * 3 + 0] * cos(q[0 * 3 + 0]) * pow(cos(q[0 * 3 + 1]), 2.0) * + sin(q[0 * 3 + 2]) + + l[1 * 3 + 2] * cos(q[0 * 3 + 0]) * cos(q[0 * 3 + 2]) * + pow(sin(q[0 * 3 + 1]), 2.0) + + l[1 * 3 + 0] * cos(q[0 * 3 + 0]) * pow(sin(q[0 * 3 + 1]), 2.0) * + sin(q[0 * 3 + 2]) + + RL * l[0 * 3 + 1] * cos(q[0 * 3 + 1]) * cos(q[0 * 3 + 2]) * + sin(q[0 * 3 + 0]) + + RL * l[1 * 3 + 1] * cos(q[0 * 3 + 1]) * cos(q[0 * 3 + 2]) * + sin(q[0 * 3 + 0]) - + RL * l[0 * 3 + 1] * sin(q[0 * 3 + 0]) * sin(q[0 * 3 + 1]) * + sin(q[0 * 3 + 2]) - + RL * l[1 * 3 + 1] * sin(q[0 * 3 + 0]) * sin(q[0 * 3 + 1]) * + sin(q[0 * 3 + 2]))) / + 2.0 - + (cos(q[0 * 3 + 0]) * + (cos(q[0 * 3 + 1]) * cos(q[0 * 3 + 2]) - + sin(q[0 * 3 + 1]) * sin(q[0 * 3 + 2])) * + 1.0 / + pow(pow(cos(q[0 * 3 + 0]), 2.0) * pow(cos(q[0 * 3 + 1]), 2.0) * + pow(sin(q[0 * 3 + 2]), 2.0) + + pow(cos(q[0 * 3 + 0]), 2.0) * pow(cos(q[0 * 3 + 2]), 2.0) * + pow(sin(q[0 * 3 + 1]), 2.0) + + pow(cos(q[0 * 3 + 1]), 2.0) * pow(cos(q[0 * 3 + 2]), 2.0) * + pow(sin(q[0 * 3 + 0]), 2.0) + + pow(cos(q[0 * 3 + 0]), 2.0) * pow(sin(q[0 * 3 + 1]), 2.0) * + pow(sin(q[0 * 3 + 2]), 2.0) + + pow(cos(q[0 * 3 + 1]), 2.0) * pow(sin(q[0 * 3 + 0]), 2.0) * + pow(sin(q[0 * 3 + 2]), 2.0) + + pow(cos(q[0 * 3 + 2]), 2.0) * pow(sin(q[0 * 3 + 0]), 2.0) * + pow(sin(q[0 * 3 + 1]), 2.0) + + pow(sin(q[0 * 3 + 0]), 2.0) * pow(sin(q[0 * 3 + 1]), 2.0) * + pow(sin(q[0 * 3 + 2]), 2.0) + + pow(cos(q[0 * 3 + 0]), 2.0) * pow(cos(q[0 * 3 + 1]), 2.0) * + pow(cos(q[0 * 3 + 2]), 2.0), + 2.0) * + (l[0 * 3 + 2] * cos(q[0 * 3 + 1]) * cos(q[0 * 3 + 2]) * + sin(q[0 * 3 + 0]) + + l[0 * 3 + 0] * cos(q[0 * 3 + 1]) * sin(q[0 * 3 + 0]) * + sin(q[0 * 3 + 2]) + + l[0 * 3 + 0] * cos(q[0 * 3 + 2]) * sin(q[0 * 3 + 0]) * + sin(q[0 * 3 + 1]) - + l[0 * 3 + 2] * sin(q[0 * 3 + 0]) * sin(q[0 * 3 + 1]) * + sin(q[0 * 3 + 2]) + + l[1 * 3 + 2] * pow(cos(q[0 * 3 + 1]), 2.0) * cos(q[0 * 3 + 2]) * + sin(q[0 * 3 + 0]) + + l[1 * 3 + 0] * pow(cos(q[0 * 3 + 1]), 2.0) * sin(q[0 * 3 + 0]) * + sin(q[0 * 3 + 2]) + + l[1 * 3 + 2] * cos(q[0 * 3 + 2]) * sin(q[0 * 3 + 0]) * + pow(sin(q[0 * 3 + 1]), 2.0) + + l[1 * 3 + 0] * sin(q[0 * 3 + 0]) * pow(sin(q[0 * 3 + 1]), 2.0) * + sin(q[0 * 3 + 2]) - + RL * l[0 * 3 + 1] * cos(q[0 * 3 + 0]) * cos(q[0 * 3 + 1]) * + cos(q[0 * 3 + 2]) - + RL * l[1 * 3 + 1] * cos(q[0 * 3 + 0]) * cos(q[0 * 3 + 1]) * + cos(q[0 * 3 + 2]) + + RL * l[0 * 3 + 1] * cos(q[0 * 3 + 0]) * sin(q[0 * 3 + 1]) * + sin(q[0 * 3 + 2]) + + RL * l[1 * 3 + 1] * cos(q[0 * 3 + 0]) * sin(q[0 * 3 + 1]) * + sin(q[0 * 3 + 2]))) / + 2.0 + + (cos(q[0 * 3 + 0]) * + (cos(q[0 * 3 + 1]) * sin(q[0 * 3 + 2]) + + cos(q[0 * 3 + 2]) * sin(q[0 * 3 + 1])) * + 1.0 / + pow(pow(cos(q[0 * 3 + 0]), 2.0) * pow(cos(q[0 * 3 + 1]), 2.0) * + pow(sin(q[0 * 3 + 2]), 2.0) + + pow(cos(q[0 * 3 + 0]), 2.0) * pow(cos(q[0 * 3 + 2]), 2.0) * + pow(sin(q[0 * 3 + 1]), 2.0) + + pow(cos(q[0 * 3 + 1]), 2.0) * pow(cos(q[0 * 3 + 2]), 2.0) * + pow(sin(q[0 * 3 + 0]), 2.0) + + pow(cos(q[0 * 3 + 0]), 2.0) * pow(sin(q[0 * 3 + 1]), 2.0) * + pow(sin(q[0 * 3 + 2]), 2.0) + + pow(cos(q[0 * 3 + 1]), 2.0) * pow(sin(q[0 * 3 + 0]), 2.0) * + pow(sin(q[0 * 3 + 2]), 2.0) + + pow(cos(q[0 * 3 + 2]), 2.0) * pow(sin(q[0 * 3 + 0]), 2.0) * + pow(sin(q[0 * 3 + 1]), 2.0) + + pow(sin(q[0 * 3 + 0]), 2.0) * pow(sin(q[0 * 3 + 1]), 2.0) * + pow(sin(q[0 * 3 + 2]), 2.0) + + pow(cos(q[0 * 3 + 0]), 2.0) * pow(cos(q[0 * 3 + 1]), 2.0) * + pow(cos(q[0 * 3 + 2]), 2.0), + 2.0) * + (l[0 * 3 + 0] * cos(q[0 * 3 + 1]) * cos(q[0 * 3 + 2]) * + sin(q[0 * 3 + 0]) - + l[0 * 3 + 2] * cos(q[0 * 3 + 1]) * sin(q[0 * 3 + 0]) * + sin(q[0 * 3 + 2]) - + l[0 * 3 + 2] * cos(q[0 * 3 + 2]) * sin(q[0 * 3 + 0]) * + sin(q[0 * 3 + 1]) - + l[0 * 3 + 0] * sin(q[0 * 3 + 0]) * sin(q[0 * 3 + 1]) * + sin(q[0 * 3 + 2]) + + l[1 * 3 + 0] * pow(cos(q[0 * 3 + 1]), 2.0) * cos(q[0 * 3 + 2]) * + sin(q[0 * 3 + 0]) + + l[1 * 3 + 0] * cos(q[0 * 3 + 2]) * sin(q[0 * 3 + 0]) * + pow(sin(q[0 * 3 + 1]), 2.0) - + l[1 * 3 + 2] * pow(cos(q[0 * 3 + 1]), 2.0) * sin(q[0 * 3 + 0]) * + sin(q[0 * 3 + 2]) - + l[1 * 3 + 2] * sin(q[0 * 3 + 0]) * pow(sin(q[0 * 3 + 1]), 2.0) * + sin(q[0 * 3 + 2]) + + RL * l[0 * 3 + 1] * cos(q[0 * 3 + 0]) * cos(q[0 * 3 + 1]) * + sin(q[0 * 3 + 2]) + + RL * l[0 * 3 + 1] * cos(q[0 * 3 + 0]) * cos(q[0 * 3 + 2]) * + sin(q[0 * 3 + 1]) + + RL * l[1 * 3 + 1] * cos(q[0 * 3 + 0]) * cos(q[0 * 3 + 1]) * + sin(q[0 * 3 + 2]) + + RL * l[1 * 3 + 1] * cos(q[0 * 3 + 0]) * cos(q[0 * 3 + 2]) * + sin(q[0 * 3 + 1]))) / + 2.0)) / + (pow(cos(q[0 * 3 + 0]), 2.0) + pow(sin(q[0 * 3 + 0]), 2.0)) - + (cos(q[0 * 3 + 0]) * + (cos(q[0 * 3 + 1]) * sin(q[0 * 3 + 2]) + + cos(q[0 * 3 + 2]) * sin(q[0 * 3 + 1])) * + (l[2 * 3 + 0] - + (cos(q[0 * 3 + 0]) * + (-l[0 * 3 + 0] * cos(q[0 * 3 + 0]) * cos(q[0 * 3 + 1]) * + cos(q[0 * 3 + 2]) + + l[0 * 3 + 2] * cos(q[0 * 3 + 0]) * cos(q[0 * 3 + 1]) * + sin(q[0 * 3 + 2]) + + l[0 * 3 + 2] * cos(q[0 * 3 + 0]) * cos(q[0 * 3 + 2]) * + sin(q[0 * 3 + 1]) + + l[0 * 3 + 0] * cos(q[0 * 3 + 0]) * sin(q[0 * 3 + 1]) * + sin(q[0 * 3 + 2]) - + l[1 * 3 + 0] * cos(q[0 * 3 + 0]) * pow(cos(q[0 * 3 + 1]), 2.0) * + cos(q[0 * 3 + 2]) - + l[1 * 3 + 0] * cos(q[0 * 3 + 0]) * cos(q[0 * 3 + 2]) * + pow(sin(q[0 * 3 + 1]), 2.0) + + l[1 * 3 + 2] * cos(q[0 * 3 + 0]) * pow(cos(q[0 * 3 + 1]), 2.0) * + sin(q[0 * 3 + 2]) + + l[1 * 3 + 2] * cos(q[0 * 3 + 0]) * pow(sin(q[0 * 3 + 1]), 2.0) * + sin(q[0 * 3 + 2]) + + RL * l[0 * 3 + 1] * cos(q[0 * 3 + 1]) * sin(q[0 * 3 + 0]) * + sin(q[0 * 3 + 2]) + + RL * l[0 * 3 + 1] * cos(q[0 * 3 + 2]) * sin(q[0 * 3 + 0]) * + sin(q[0 * 3 + 1]) + + RL * l[1 * 3 + 1] * cos(q[0 * 3 + 1]) * sin(q[0 * 3 + 0]) * + sin(q[0 * 3 + 2]) + + RL * l[1 * 3 + 1] * cos(q[0 * 3 + 2]) * sin(q[0 * 3 + 0]) * + sin(q[0 * 3 + 1]))) / + ((pow(cos(q[0 * 3 + 0]), 2.0) + pow(sin(q[0 * 3 + 0]), 2.0)) * + (pow(cos(q[0 * 3 + 0]), 2.0) * pow(cos(q[0 * 3 + 1]), 2.0) * + pow(sin(q[0 * 3 + 2]), 2.0) + + pow(cos(q[0 * 3 + 0]), 2.0) * pow(cos(q[0 * 3 + 2]), 2.0) * + pow(sin(q[0 * 3 + 1]), 2.0) + + pow(cos(q[0 * 3 + 1]), 2.0) * pow(cos(q[0 * 3 + 2]), 2.0) * + pow(sin(q[0 * 3 + 0]), 2.0) + + pow(cos(q[0 * 3 + 0]), 2.0) * pow(sin(q[0 * 3 + 1]), 2.0) * + pow(sin(q[0 * 3 + 2]), 2.0) + + pow(cos(q[0 * 3 + 1]), 2.0) * pow(sin(q[0 * 3 + 0]), 2.0) * + pow(sin(q[0 * 3 + 2]), 2.0) + + pow(cos(q[0 * 3 + 2]), 2.0) * pow(sin(q[0 * 3 + 0]), 2.0) * + pow(sin(q[0 * 3 + 1]), 2.0) + + pow(sin(q[0 * 3 + 0]), 2.0) * pow(sin(q[0 * 3 + 1]), 2.0) * + pow(sin(q[0 * 3 + 2]), 2.0) + + pow(cos(q[0 * 3 + 0]), 2.0) * pow(cos(q[0 * 3 + 1]), 2.0) * + pow(cos(q[0 * 3 + 2]), 2.0)) * + 2.0) + + (sin(q[0 * 3 + 0]) * + (l[0 * 3 + 0] * cos(q[0 * 3 + 1]) * cos(q[0 * 3 + 2]) * + sin(q[0 * 3 + 0]) - + l[0 * 3 + 2] * cos(q[0 * 3 + 1]) * sin(q[0 * 3 + 0]) * + sin(q[0 * 3 + 2]) - + l[0 * 3 + 2] * cos(q[0 * 3 + 2]) * sin(q[0 * 3 + 0]) * + sin(q[0 * 3 + 1]) - + l[0 * 3 + 0] * sin(q[0 * 3 + 0]) * sin(q[0 * 3 + 1]) * + sin(q[0 * 3 + 2]) + + l[1 * 3 + 0] * pow(cos(q[0 * 3 + 1]), 2.0) * cos(q[0 * 3 + 2]) * + sin(q[0 * 3 + 0]) + + l[1 * 3 + 0] * cos(q[0 * 3 + 2]) * sin(q[0 * 3 + 0]) * + pow(sin(q[0 * 3 + 1]), 2.0) - + l[1 * 3 + 2] * pow(cos(q[0 * 3 + 1]), 2.0) * sin(q[0 * 3 + 0]) * + sin(q[0 * 3 + 2]) - + l[1 * 3 + 2] * sin(q[0 * 3 + 0]) * pow(sin(q[0 * 3 + 1]), 2.0) * + sin(q[0 * 3 + 2]) + + RL * l[0 * 3 + 1] * cos(q[0 * 3 + 0]) * cos(q[0 * 3 + 1]) * + sin(q[0 * 3 + 2]) + + RL * l[0 * 3 + 1] * cos(q[0 * 3 + 0]) * cos(q[0 * 3 + 2]) * + sin(q[0 * 3 + 1]) + + RL * l[1 * 3 + 1] * cos(q[0 * 3 + 0]) * cos(q[0 * 3 + 1]) * + sin(q[0 * 3 + 2]) + + RL * l[1 * 3 + 1] * cos(q[0 * 3 + 0]) * cos(q[0 * 3 + 2]) * + sin(q[0 * 3 + 1]))) / + ((pow(cos(q[0 * 3 + 0]), 2.0) + pow(sin(q[0 * 3 + 0]), 2.0)) * + (pow(cos(q[0 * 3 + 0]), 2.0) * pow(cos(q[0 * 3 + 1]), 2.0) * + pow(sin(q[0 * 3 + 2]), 2.0) + + pow(cos(q[0 * 3 + 0]), 2.0) * pow(cos(q[0 * 3 + 2]), 2.0) * + pow(sin(q[0 * 3 + 1]), 2.0) + + pow(cos(q[0 * 3 + 1]), 2.0) * pow(cos(q[0 * 3 + 2]), 2.0) * + pow(sin(q[0 * 3 + 0]), 2.0) + + pow(cos(q[0 * 3 + 0]), 2.0) * pow(sin(q[0 * 3 + 1]), 2.0) * + pow(sin(q[0 * 3 + 2]), 2.0) + + pow(cos(q[0 * 3 + 1]), 2.0) * pow(sin(q[0 * 3 + 0]), 2.0) * + pow(sin(q[0 * 3 + 2]), 2.0) + + pow(cos(q[0 * 3 + 2]), 2.0) * pow(sin(q[0 * 3 + 0]), 2.0) * + pow(sin(q[0 * 3 + 1]), 2.0) + + pow(sin(q[0 * 3 + 0]), 2.0) * pow(sin(q[0 * 3 + 1]), 2.0) * + pow(sin(q[0 * 3 + 2]), 2.0) + + pow(cos(q[0 * 3 + 0]), 2.0) * pow(cos(q[0 * 3 + 1]), 2.0) * + pow(cos(q[0 * 3 + 2]), 2.0)) * + 2.0) - + ((cos(q[0 * 3 + 1]) * sin(q[0 * 3 + 2]) + + cos(q[0 * 3 + 2]) * sin(q[0 * 3 + 1])) * + (l[0 * 3 + 2] * pow(sin(q[0 * 3 + 1]), 2.0) + + l[1 * 3 + 2] * cos(q[0 * 3 + 1]) - + l[1 * 3 + 0] * sin(q[0 * 3 + 1]) + + l[0 * 3 + 2] * pow(cos(q[0 * 3 + 1]), 2.0))) / + ((pow(cos(q[0 * 3 + 1]), 2.0) + pow(sin(q[0 * 3 + 1]), 2.0)) * + (pow(cos(q[0 * 3 + 1]), 2.0) * pow(cos(q[0 * 3 + 2]), 2.0) + + pow(cos(q[0 * 3 + 1]), 2.0) * pow(sin(q[0 * 3 + 2]), 2.0) + + pow(cos(q[0 * 3 + 2]), 2.0) * pow(sin(q[0 * 3 + 1]), 2.0) + + pow(sin(q[0 * 3 + 1]), 2.0) * pow(sin(q[0 * 3 + 2]), 2.0)) * + 2.0) + + (pow(cos(q[0 * 3 + 0]), 2.0) * + (cos(q[0 * 3 + 1]) * cos(q[0 * 3 + 2]) - + sin(q[0 * 3 + 1]) * sin(q[0 * 3 + 2])) * + (l[0 * 3 + 0] * pow(sin(q[0 * 3 + 1]), 2.0) + + l[1 * 3 + 0] * cos(q[0 * 3 + 1]) + + l[1 * 3 + 2] * sin(q[0 * 3 + 1]) + + l[0 * 3 + 0] * pow(cos(q[0 * 3 + 1]), 2.0))) / + ((pow(cos(q[0 * 3 + 0]), 2.0) * pow(cos(q[0 * 3 + 1]), 2.0) + + pow(cos(q[0 * 3 + 0]), 2.0) * pow(sin(q[0 * 3 + 1]), 2.0) + + pow(cos(q[0 * 3 + 1]), 2.0) * pow(sin(q[0 * 3 + 0]), 2.0) + + pow(sin(q[0 * 3 + 0]), 2.0) * pow(sin(q[0 * 3 + 1]), 2.0)) * + (pow(cos(q[0 * 3 + 0]), 2.0) * pow(cos(q[0 * 3 + 1]), 2.0) * + pow(sin(q[0 * 3 + 2]), 2.0) + + pow(cos(q[0 * 3 + 0]), 2.0) * pow(cos(q[0 * 3 + 2]), 2.0) * + pow(sin(q[0 * 3 + 1]), 2.0) + + pow(cos(q[0 * 3 + 1]), 2.0) * pow(cos(q[0 * 3 + 2]), 2.0) * + pow(sin(q[0 * 3 + 0]), 2.0) + + pow(cos(q[0 * 3 + 0]), 2.0) * pow(sin(q[0 * 3 + 1]), 2.0) * + pow(sin(q[0 * 3 + 2]), 2.0) + + pow(cos(q[0 * 3 + 1]), 2.0) * pow(sin(q[0 * 3 + 0]), 2.0) * + pow(sin(q[0 * 3 + 2]), 2.0) + + pow(cos(q[0 * 3 + 2]), 2.0) * pow(sin(q[0 * 3 + 0]), 2.0) * + pow(sin(q[0 * 3 + 1]), 2.0) + + pow(sin(q[0 * 3 + 0]), 2.0) * pow(sin(q[0 * 3 + 1]), 2.0) * + pow(sin(q[0 * 3 + 2]), 2.0) + + pow(cos(q[0 * 3 + 0]), 2.0) * pow(cos(q[0 * 3 + 1]), 2.0) * + pow(cos(q[0 * 3 + 2]), 2.0)) * + 2.0) + + (pow(sin(q[0 * 3 + 0]), 2.0) * + (cos(q[0 * 3 + 1]) * cos(q[0 * 3 + 2]) - + sin(q[0 * 3 + 1]) * sin(q[0 * 3 + 2])) * + (l[0 * 3 + 0] * pow(sin(q[0 * 3 + 1]), 2.0) + + l[1 * 3 + 0] * cos(q[0 * 3 + 1]) + + l[1 * 3 + 2] * sin(q[0 * 3 + 1]) + + l[0 * 3 + 0] * pow(cos(q[0 * 3 + 1]), 2.0))) / + ((pow(cos(q[0 * 3 + 0]), 2.0) * pow(cos(q[0 * 3 + 1]), 2.0) + + pow(cos(q[0 * 3 + 0]), 2.0) * pow(sin(q[0 * 3 + 1]), 2.0) + + pow(cos(q[0 * 3 + 1]), 2.0) * pow(sin(q[0 * 3 + 0]), 2.0) + + pow(sin(q[0 * 3 + 0]), 2.0) * pow(sin(q[0 * 3 + 1]), 2.0)) * + (pow(cos(q[0 * 3 + 0]), 2.0) * pow(cos(q[0 * 3 + 1]), 2.0) * + pow(sin(q[0 * 3 + 2]), 2.0) + + pow(cos(q[0 * 3 + 0]), 2.0) * pow(cos(q[0 * 3 + 2]), 2.0) * + pow(sin(q[0 * 3 + 1]), 2.0) + + pow(cos(q[0 * 3 + 1]), 2.0) * pow(cos(q[0 * 3 + 2]), 2.0) * + pow(sin(q[0 * 3 + 0]), 2.0) + + pow(cos(q[0 * 3 + 0]), 2.0) * pow(sin(q[0 * 3 + 1]), 2.0) * + pow(sin(q[0 * 3 + 2]), 2.0) + + pow(cos(q[0 * 3 + 1]), 2.0) * pow(sin(q[0 * 3 + 0]), 2.0) * + pow(sin(q[0 * 3 + 2]), 2.0) + + pow(cos(q[0 * 3 + 2]), 2.0) * pow(sin(q[0 * 3 + 0]), 2.0) * + pow(sin(q[0 * 3 + 1]), 2.0) + + pow(sin(q[0 * 3 + 0]), 2.0) * pow(sin(q[0 * 3 + 1]), 2.0) * + pow(sin(q[0 * 3 + 2]), 2.0) + + pow(cos(q[0 * 3 + 0]), 2.0) * pow(cos(q[0 * 3 + 1]), 2.0) * + pow(cos(q[0 * 3 + 2]), 2.0)) * + 2.0))) / + (pow(cos(q[0 * 3 + 0]), 2.0) * pow(cos(q[0 * 3 + 1]), 2.0) * + pow(sin(q[0 * 3 + 2]), 2.0) + + pow(cos(q[0 * 3 + 0]), 2.0) * pow(cos(q[0 * 3 + 2]), 2.0) * + pow(sin(q[0 * 3 + 1]), 2.0) + + pow(cos(q[0 * 3 + 1]), 2.0) * pow(cos(q[0 * 3 + 2]), 2.0) * + pow(sin(q[0 * 3 + 0]), 2.0) + + pow(cos(q[0 * 3 + 0]), 2.0) * pow(sin(q[0 * 3 + 1]), 2.0) * + pow(sin(q[0 * 3 + 2]), 2.0) + + pow(cos(q[0 * 3 + 1]), 2.0) * pow(sin(q[0 * 3 + 0]), 2.0) * + pow(sin(q[0 * 3 + 2]), 2.0) + + pow(cos(q[0 * 3 + 2]), 2.0) * pow(sin(q[0 * 3 + 0]), 2.0) * + pow(sin(q[0 * 3 + 1]), 2.0) + + pow(sin(q[0 * 3 + 0]), 2.0) * pow(sin(q[0 * 3 + 1]), 2.0) * + pow(sin(q[0 * 3 + 2]), 2.0) + + pow(cos(q[0 * 3 + 0]), 2.0) * pow(cos(q[0 * 3 + 1]), 2.0) * + pow(cos(q[0 * 3 + 2]), 2.0)) + + (cos(q[0 * 3 + 0]) * + (cos(q[0 * 3 + 1]) * cos(q[0 * 3 + 2]) - + sin(q[0 * 3 + 1]) * sin(q[0 * 3 + 2])) * + (l[2 * 3 + 2] + + (cos(q[0 * 3 + 0]) * + (l[0 * 3 + 2] * cos(q[0 * 3 + 0]) * cos(q[0 * 3 + 1]) * + cos(q[0 * 3 + 2]) + + l[0 * 3 + 0] * cos(q[0 * 3 + 0]) * cos(q[0 * 3 + 1]) * + sin(q[0 * 3 + 2]) + + l[0 * 3 + 0] * cos(q[0 * 3 + 0]) * cos(q[0 * 3 + 2]) * + sin(q[0 * 3 + 1]) - + l[0 * 3 + 2] * cos(q[0 * 3 + 0]) * sin(q[0 * 3 + 1]) * + sin(q[0 * 3 + 2]) + + l[1 * 3 + 2] * cos(q[0 * 3 + 0]) * pow(cos(q[0 * 3 + 1]), 2.0) * + cos(q[0 * 3 + 2]) + + l[1 * 3 + 0] * cos(q[0 * 3 + 0]) * pow(cos(q[0 * 3 + 1]), 2.0) * + sin(q[0 * 3 + 2]) + + l[1 * 3 + 2] * cos(q[0 * 3 + 0]) * cos(q[0 * 3 + 2]) * + pow(sin(q[0 * 3 + 1]), 2.0) + + l[1 * 3 + 0] * cos(q[0 * 3 + 0]) * pow(sin(q[0 * 3 + 1]), 2.0) * + sin(q[0 * 3 + 2]) + + RL * l[0 * 3 + 1] * cos(q[0 * 3 + 1]) * cos(q[0 * 3 + 2]) * + sin(q[0 * 3 + 0]) + + RL * l[1 * 3 + 1] * cos(q[0 * 3 + 1]) * cos(q[0 * 3 + 2]) * + sin(q[0 * 3 + 0]) - + RL * l[0 * 3 + 1] * sin(q[0 * 3 + 0]) * sin(q[0 * 3 + 1]) * + sin(q[0 * 3 + 2]) - + RL * l[1 * 3 + 1] * sin(q[0 * 3 + 0]) * sin(q[0 * 3 + 1]) * + sin(q[0 * 3 + 2]))) / + ((pow(cos(q[0 * 3 + 0]), 2.0) + pow(sin(q[0 * 3 + 0]), 2.0)) * + (pow(cos(q[0 * 3 + 0]), 2.0) * pow(cos(q[0 * 3 + 1]), 2.0) * + pow(sin(q[0 * 3 + 2]), 2.0) + + pow(cos(q[0 * 3 + 0]), 2.0) * pow(cos(q[0 * 3 + 2]), 2.0) * + pow(sin(q[0 * 3 + 1]), 2.0) + + pow(cos(q[0 * 3 + 1]), 2.0) * pow(cos(q[0 * 3 + 2]), 2.0) * + pow(sin(q[0 * 3 + 0]), 2.0) + + pow(cos(q[0 * 3 + 0]), 2.0) * pow(sin(q[0 * 3 + 1]), 2.0) * + pow(sin(q[0 * 3 + 2]), 2.0) + + pow(cos(q[0 * 3 + 1]), 2.0) * pow(sin(q[0 * 3 + 0]), 2.0) * + pow(sin(q[0 * 3 + 2]), 2.0) + + pow(cos(q[0 * 3 + 2]), 2.0) * pow(sin(q[0 * 3 + 0]), 2.0) * + pow(sin(q[0 * 3 + 1]), 2.0) + + pow(sin(q[0 * 3 + 0]), 2.0) * pow(sin(q[0 * 3 + 1]), 2.0) * + pow(sin(q[0 * 3 + 2]), 2.0) + + pow(cos(q[0 * 3 + 0]), 2.0) * pow(cos(q[0 * 3 + 1]), 2.0) * + pow(cos(q[0 * 3 + 2]), 2.0)) * + 2.0) + + (sin(q[0 * 3 + 0]) * + (l[0 * 3 + 2] * cos(q[0 * 3 + 1]) * cos(q[0 * 3 + 2]) * + sin(q[0 * 3 + 0]) + + l[0 * 3 + 0] * cos(q[0 * 3 + 1]) * sin(q[0 * 3 + 0]) * + sin(q[0 * 3 + 2]) + + l[0 * 3 + 0] * cos(q[0 * 3 + 2]) * sin(q[0 * 3 + 0]) * + sin(q[0 * 3 + 1]) - + l[0 * 3 + 2] * sin(q[0 * 3 + 0]) * sin(q[0 * 3 + 1]) * + sin(q[0 * 3 + 2]) + + l[1 * 3 + 2] * pow(cos(q[0 * 3 + 1]), 2.0) * cos(q[0 * 3 + 2]) * + sin(q[0 * 3 + 0]) + + l[1 * 3 + 0] * pow(cos(q[0 * 3 + 1]), 2.0) * sin(q[0 * 3 + 0]) * + sin(q[0 * 3 + 2]) + + l[1 * 3 + 2] * cos(q[0 * 3 + 2]) * sin(q[0 * 3 + 0]) * + pow(sin(q[0 * 3 + 1]), 2.0) + + l[1 * 3 + 0] * sin(q[0 * 3 + 0]) * pow(sin(q[0 * 3 + 1]), 2.0) * + sin(q[0 * 3 + 2]) - + RL * l[0 * 3 + 1] * cos(q[0 * 3 + 0]) * cos(q[0 * 3 + 1]) * + cos(q[0 * 3 + 2]) - + RL * l[1 * 3 + 1] * cos(q[0 * 3 + 0]) * cos(q[0 * 3 + 1]) * + cos(q[0 * 3 + 2]) + + RL * l[0 * 3 + 1] * cos(q[0 * 3 + 0]) * sin(q[0 * 3 + 1]) * + sin(q[0 * 3 + 2]) + + RL * l[1 * 3 + 1] * cos(q[0 * 3 + 0]) * sin(q[0 * 3 + 1]) * + sin(q[0 * 3 + 2]))) / + ((pow(cos(q[0 * 3 + 0]), 2.0) + pow(sin(q[0 * 3 + 0]), 2.0)) * + (pow(cos(q[0 * 3 + 0]), 2.0) * pow(cos(q[0 * 3 + 1]), 2.0) * + pow(sin(q[0 * 3 + 2]), 2.0) + + pow(cos(q[0 * 3 + 0]), 2.0) * pow(cos(q[0 * 3 + 2]), 2.0) * + pow(sin(q[0 * 3 + 1]), 2.0) + + pow(cos(q[0 * 3 + 1]), 2.0) * pow(cos(q[0 * 3 + 2]), 2.0) * + pow(sin(q[0 * 3 + 0]), 2.0) + + pow(cos(q[0 * 3 + 0]), 2.0) * pow(sin(q[0 * 3 + 1]), 2.0) * + pow(sin(q[0 * 3 + 2]), 2.0) + + pow(cos(q[0 * 3 + 1]), 2.0) * pow(sin(q[0 * 3 + 0]), 2.0) * + pow(sin(q[0 * 3 + 2]), 2.0) + + pow(cos(q[0 * 3 + 2]), 2.0) * pow(sin(q[0 * 3 + 0]), 2.0) * + pow(sin(q[0 * 3 + 1]), 2.0) + + pow(sin(q[0 * 3 + 0]), 2.0) * pow(sin(q[0 * 3 + 1]), 2.0) * + pow(sin(q[0 * 3 + 2]), 2.0) + + pow(cos(q[0 * 3 + 0]), 2.0) * pow(cos(q[0 * 3 + 1]), 2.0) * + pow(cos(q[0 * 3 + 2]), 2.0)) * + 2.0) + + ((cos(q[0 * 3 + 1]) * cos(q[0 * 3 + 2]) - + sin(q[0 * 3 + 1]) * sin(q[0 * 3 + 2])) * + (l[0 * 3 + 2] * pow(sin(q[0 * 3 + 1]), 2.0) + + l[1 * 3 + 2] * cos(q[0 * 3 + 1]) - + l[1 * 3 + 0] * sin(q[0 * 3 + 1]) + + l[0 * 3 + 2] * pow(cos(q[0 * 3 + 1]), 2.0))) / + ((pow(cos(q[0 * 3 + 1]), 2.0) + pow(sin(q[0 * 3 + 1]), 2.0)) * + (pow(cos(q[0 * 3 + 1]), 2.0) * pow(cos(q[0 * 3 + 2]), 2.0) + + pow(cos(q[0 * 3 + 1]), 2.0) * pow(sin(q[0 * 3 + 2]), 2.0) + + pow(cos(q[0 * 3 + 2]), 2.0) * pow(sin(q[0 * 3 + 1]), 2.0) + + pow(sin(q[0 * 3 + 1]), 2.0) * pow(sin(q[0 * 3 + 2]), 2.0)) * + 2.0) + + (pow(cos(q[0 * 3 + 0]), 2.0) * + (cos(q[0 * 3 + 1]) * sin(q[0 * 3 + 2]) + + cos(q[0 * 3 + 2]) * sin(q[0 * 3 + 1])) * + (l[0 * 3 + 0] * pow(sin(q[0 * 3 + 1]), 2.0) + + l[1 * 3 + 0] * cos(q[0 * 3 + 1]) + + l[1 * 3 + 2] * sin(q[0 * 3 + 1]) + + l[0 * 3 + 0] * pow(cos(q[0 * 3 + 1]), 2.0))) / + ((pow(cos(q[0 * 3 + 0]), 2.0) * pow(cos(q[0 * 3 + 1]), 2.0) + + pow(cos(q[0 * 3 + 0]), 2.0) * pow(sin(q[0 * 3 + 1]), 2.0) + + pow(cos(q[0 * 3 + 1]), 2.0) * pow(sin(q[0 * 3 + 0]), 2.0) + + pow(sin(q[0 * 3 + 0]), 2.0) * pow(sin(q[0 * 3 + 1]), 2.0)) * + (pow(cos(q[0 * 3 + 0]), 2.0) * pow(cos(q[0 * 3 + 1]), 2.0) * + pow(sin(q[0 * 3 + 2]), 2.0) + + pow(cos(q[0 * 3 + 0]), 2.0) * pow(cos(q[0 * 3 + 2]), 2.0) * + pow(sin(q[0 * 3 + 1]), 2.0) + + pow(cos(q[0 * 3 + 1]), 2.0) * pow(cos(q[0 * 3 + 2]), 2.0) * + pow(sin(q[0 * 3 + 0]), 2.0) + + pow(cos(q[0 * 3 + 0]), 2.0) * pow(sin(q[0 * 3 + 1]), 2.0) * + pow(sin(q[0 * 3 + 2]), 2.0) + + pow(cos(q[0 * 3 + 1]), 2.0) * pow(sin(q[0 * 3 + 0]), 2.0) * + pow(sin(q[0 * 3 + 2]), 2.0) + + pow(cos(q[0 * 3 + 2]), 2.0) * pow(sin(q[0 * 3 + 0]), 2.0) * + pow(sin(q[0 * 3 + 1]), 2.0) + + pow(sin(q[0 * 3 + 0]), 2.0) * pow(sin(q[0 * 3 + 1]), 2.0) * + pow(sin(q[0 * 3 + 2]), 2.0) + + pow(cos(q[0 * 3 + 0]), 2.0) * pow(cos(q[0 * 3 + 1]), 2.0) * + pow(cos(q[0 * 3 + 2]), 2.0)) * + 2.0) + + (pow(sin(q[0 * 3 + 0]), 2.0) * + (cos(q[0 * 3 + 1]) * sin(q[0 * 3 + 2]) + + cos(q[0 * 3 + 2]) * sin(q[0 * 3 + 1])) * + (l[0 * 3 + 0] * pow(sin(q[0 * 3 + 1]), 2.0) + + l[1 * 3 + 0] * cos(q[0 * 3 + 1]) + + l[1 * 3 + 2] * sin(q[0 * 3 + 1]) + + l[0 * 3 + 0] * pow(cos(q[0 * 3 + 1]), 2.0))) / + ((pow(cos(q[0 * 3 + 0]), 2.0) * pow(cos(q[0 * 3 + 1]), 2.0) + + pow(cos(q[0 * 3 + 0]), 2.0) * pow(sin(q[0 * 3 + 1]), 2.0) + + pow(cos(q[0 * 3 + 1]), 2.0) * pow(sin(q[0 * 3 + 0]), 2.0) + + pow(sin(q[0 * 3 + 0]), 2.0) * pow(sin(q[0 * 3 + 1]), 2.0)) * + (pow(cos(q[0 * 3 + 0]), 2.0) * pow(cos(q[0 * 3 + 1]), 2.0) * + pow(sin(q[0 * 3 + 2]), 2.0) + + pow(cos(q[0 * 3 + 0]), 2.0) * pow(cos(q[0 * 3 + 2]), 2.0) * + pow(sin(q[0 * 3 + 1]), 2.0) + + pow(cos(q[0 * 3 + 1]), 2.0) * pow(cos(q[0 * 3 + 2]), 2.0) * + pow(sin(q[0 * 3 + 0]), 2.0) + + pow(cos(q[0 * 3 + 0]), 2.0) * pow(sin(q[0 * 3 + 1]), 2.0) * + pow(sin(q[0 * 3 + 2]), 2.0) + + pow(cos(q[0 * 3 + 1]), 2.0) * pow(sin(q[0 * 3 + 0]), 2.0) * + pow(sin(q[0 * 3 + 2]), 2.0) + + pow(cos(q[0 * 3 + 2]), 2.0) * pow(sin(q[0 * 3 + 0]), 2.0) * + pow(sin(q[0 * 3 + 1]), 2.0) + + pow(sin(q[0 * 3 + 0]), 2.0) * pow(sin(q[0 * 3 + 1]), 2.0) * + pow(sin(q[0 * 3 + 2]), 2.0) + + pow(cos(q[0 * 3 + 0]), 2.0) * pow(cos(q[0 * 3 + 1]), 2.0) * + pow(cos(q[0 * 3 + 2]), 2.0)) * + 2.0))) / + (pow(cos(q[0 * 3 + 0]), 2.0) * pow(cos(q[0 * 3 + 1]), 2.0) * + pow(sin(q[0 * 3 + 2]), 2.0) + + pow(cos(q[0 * 3 + 0]), 2.0) * pow(cos(q[0 * 3 + 2]), 2.0) * + pow(sin(q[0 * 3 + 1]), 2.0) + + pow(cos(q[0 * 3 + 1]), 2.0) * pow(cos(q[0 * 3 + 2]), 2.0) * + pow(sin(q[0 * 3 + 0]), 2.0) + + pow(cos(q[0 * 3 + 0]), 2.0) * pow(sin(q[0 * 3 + 1]), 2.0) * + pow(sin(q[0 * 3 + 2]), 2.0) + + pow(cos(q[0 * 3 + 1]), 2.0) * pow(sin(q[0 * 3 + 0]), 2.0) * + pow(sin(q[0 * 3 + 2]), 2.0) + + pow(cos(q[0 * 3 + 2]), 2.0) * pow(sin(q[0 * 3 + 0]), 2.0) * + pow(sin(q[0 * 3 + 1]), 2.0) + + pow(sin(q[0 * 3 + 0]), 2.0) * pow(sin(q[0 * 3 + 1]), 2.0) * + pow(sin(q[0 * 3 + 2]), 2.0) + + pow(cos(q[0 * 3 + 0]), 2.0) * pow(cos(q[0 * 3 + 1]), 2.0) * + pow(cos(q[0 * 3 + 2]), 2.0)))) / + (pow(cos(q[0 * 3 + 0]), 2.0) + pow(sin(q[0 * 3 + 0]), 2.0)) - + ((cos(q[0 * 3 + 1]) * sin(q[0 * 3 + 2]) + + cos(q[0 * 3 + 2]) * sin(q[0 * 3 + 1])) * + (l[1 * 3 + 0] * cos(q[0 * 3 + 2]) - + l[1 * 3 + 2] * sin(q[0 * 3 + 2]))) / + (pow(cos(q[0 * 3 + 1]), 2.0) * pow(cos(q[0 * 3 + 2]), 2.0) + + pow(cos(q[0 * 3 + 1]), 2.0) * pow(sin(q[0 * 3 + 2]), 2.0) + + pow(cos(q[0 * 3 + 2]), 2.0) * pow(sin(q[0 * 3 + 1]), 2.0) + + pow(sin(q[0 * 3 + 1]), 2.0) * pow(sin(q[0 * 3 + 2]), 2.0)) + + ((cos(q[0 * 3 + 1]) * cos(q[0 * 3 + 2]) - + sin(q[0 * 3 + 1]) * sin(q[0 * 3 + 2])) * + (l[1 * 3 + 2] * cos(q[0 * 3 + 2]) + + l[1 * 3 + 0] * sin(q[0 * 3 + 2]))) / + (pow(cos(q[0 * 3 + 1]), 2.0) * pow(cos(q[0 * 3 + 2]), 2.0) + + pow(cos(q[0 * 3 + 1]), 2.0) * pow(sin(q[0 * 3 + 2]), 2.0) + + pow(cos(q[0 * 3 + 2]), 2.0) * pow(sin(q[0 * 3 + 1]), 2.0) + + pow(sin(q[0 * 3 + 1]), 2.0) * pow(sin(q[0 * 3 + 2]), 2.0)) - + (sin(q[0 * 3 + 0]) * + ((cos(q[0 * 3 + 0]) * + (RL * l[2 * 3 + 1] + + ((cos(q[0 * 3 + 1]) * sin(q[0 * 3 + 2]) + + cos(q[0 * 3 + 2]) * sin(q[0 * 3 + 1])) * + (RL * l[0 * 3 + 1] * cos(q[0 * 3 + 1]) * sin(q[0 * 3 + 2]) + + RL * l[0 * 3 + 1] * cos(q[0 * 3 + 2]) * sin(q[0 * 3 + 1]) + + RL * l[1 * 3 + 1] * cos(q[0 * 3 + 1]) * sin(q[0 * 3 + 2]) + + RL * l[1 * 3 + 1] * cos(q[0 * 3 + 2]) * sin(q[0 * 3 + 1])) * + 1.0 / + pow(pow(cos(q[0 * 3 + 1]), 2.0) * pow(cos(q[0 * 3 + 2]), 2.0) + + pow(cos(q[0 * 3 + 1]), 2.0) * pow(sin(q[0 * 3 + 2]), 2.0) + + pow(cos(q[0 * 3 + 2]), 2.0) * pow(sin(q[0 * 3 + 1]), 2.0) + + pow(sin(q[0 * 3 + 1]), 2.0) * pow(sin(q[0 * 3 + 2]), 2.0), + 2.0)) / + 2.0 + + ((cos(q[0 * 3 + 1]) * cos(q[0 * 3 + 2]) - + sin(q[0 * 3 + 1]) * sin(q[0 * 3 + 2])) * + (RL * l[0 * 3 + 1] * cos(q[0 * 3 + 1]) * cos(q[0 * 3 + 2]) + + RL * l[1 * 3 + 1] * cos(q[0 * 3 + 1]) * cos(q[0 * 3 + 2]) - + RL * l[0 * 3 + 1] * sin(q[0 * 3 + 1]) * sin(q[0 * 3 + 2]) - + RL * l[1 * 3 + 1] * sin(q[0 * 3 + 1]) * sin(q[0 * 3 + 2])) * + 1.0 / + pow(pow(cos(q[0 * 3 + 1]), 2.0) * pow(cos(q[0 * 3 + 2]), 2.0) + + pow(cos(q[0 * 3 + 1]), 2.0) * pow(sin(q[0 * 3 + 2]), 2.0) + + pow(cos(q[0 * 3 + 2]), 2.0) * pow(sin(q[0 * 3 + 1]), 2.0) + + pow(sin(q[0 * 3 + 1]), 2.0) * pow(sin(q[0 * 3 + 2]), 2.0), + 2.0)) / + 2.0 + + (sin(q[0 * 3 + 0]) * + (cos(q[0 * 3 + 1]) * sin(q[0 * 3 + 2]) + + cos(q[0 * 3 + 2]) * sin(q[0 * 3 + 1])) * + 1.0 / + pow(pow(cos(q[0 * 3 + 0]), 2.0) * pow(cos(q[0 * 3 + 1]), 2.0) * + pow(sin(q[0 * 3 + 2]), 2.0) + + pow(cos(q[0 * 3 + 0]), 2.0) * pow(cos(q[0 * 3 + 2]), 2.0) * + pow(sin(q[0 * 3 + 1]), 2.0) + + pow(cos(q[0 * 3 + 1]), 2.0) * pow(cos(q[0 * 3 + 2]), 2.0) * + pow(sin(q[0 * 3 + 0]), 2.0) + + pow(cos(q[0 * 3 + 0]), 2.0) * pow(sin(q[0 * 3 + 1]), 2.0) * + pow(sin(q[0 * 3 + 2]), 2.0) + + pow(cos(q[0 * 3 + 1]), 2.0) * pow(sin(q[0 * 3 + 0]), 2.0) * + pow(sin(q[0 * 3 + 2]), 2.0) + + pow(cos(q[0 * 3 + 2]), 2.0) * pow(sin(q[0 * 3 + 0]), 2.0) * + pow(sin(q[0 * 3 + 1]), 2.0) + + pow(sin(q[0 * 3 + 0]), 2.0) * pow(sin(q[0 * 3 + 1]), 2.0) * + pow(sin(q[0 * 3 + 2]), 2.0) + + pow(cos(q[0 * 3 + 0]), 2.0) * pow(cos(q[0 * 3 + 1]), 2.0) * + pow(cos(q[0 * 3 + 2]), 2.0), + 2.0) * + (-l[0 * 3 + 0] * cos(q[0 * 3 + 0]) * cos(q[0 * 3 + 1]) * + cos(q[0 * 3 + 2]) + + l[0 * 3 + 2] * cos(q[0 * 3 + 0]) * cos(q[0 * 3 + 1]) * + sin(q[0 * 3 + 2]) + + l[0 * 3 + 2] * cos(q[0 * 3 + 0]) * cos(q[0 * 3 + 2]) * + sin(q[0 * 3 + 1]) + + l[0 * 3 + 0] * cos(q[0 * 3 + 0]) * sin(q[0 * 3 + 1]) * + sin(q[0 * 3 + 2]) - + l[1 * 3 + 0] * cos(q[0 * 3 + 0]) * pow(cos(q[0 * 3 + 1]), 2.0) * + cos(q[0 * 3 + 2]) - + l[1 * 3 + 0] * cos(q[0 * 3 + 0]) * cos(q[0 * 3 + 2]) * + pow(sin(q[0 * 3 + 1]), 2.0) + + l[1 * 3 + 2] * cos(q[0 * 3 + 0]) * pow(cos(q[0 * 3 + 1]), 2.0) * + sin(q[0 * 3 + 2]) + + l[1 * 3 + 2] * cos(q[0 * 3 + 0]) * pow(sin(q[0 * 3 + 1]), 2.0) * + sin(q[0 * 3 + 2]) + + RL * l[0 * 3 + 1] * cos(q[0 * 3 + 1]) * sin(q[0 * 3 + 0]) * + sin(q[0 * 3 + 2]) + + RL * l[0 * 3 + 1] * cos(q[0 * 3 + 2]) * sin(q[0 * 3 + 0]) * + sin(q[0 * 3 + 1]) + + RL * l[1 * 3 + 1] * cos(q[0 * 3 + 1]) * sin(q[0 * 3 + 0]) * + sin(q[0 * 3 + 2]) + + RL * l[1 * 3 + 1] * cos(q[0 * 3 + 2]) * sin(q[0 * 3 + 0]) * + sin(q[0 * 3 + 1]))) / + 2.0 + + (sin(q[0 * 3 + 0]) * + (cos(q[0 * 3 + 1]) * cos(q[0 * 3 + 2]) - + sin(q[0 * 3 + 1]) * sin(q[0 * 3 + 2])) * + 1.0 / + pow(pow(cos(q[0 * 3 + 0]), 2.0) * pow(cos(q[0 * 3 + 1]), 2.0) * + pow(sin(q[0 * 3 + 2]), 2.0) + + pow(cos(q[0 * 3 + 0]), 2.0) * pow(cos(q[0 * 3 + 2]), 2.0) * + pow(sin(q[0 * 3 + 1]), 2.0) + + pow(cos(q[0 * 3 + 1]), 2.0) * pow(cos(q[0 * 3 + 2]), 2.0) * + pow(sin(q[0 * 3 + 0]), 2.0) + + pow(cos(q[0 * 3 + 0]), 2.0) * pow(sin(q[0 * 3 + 1]), 2.0) * + pow(sin(q[0 * 3 + 2]), 2.0) + + pow(cos(q[0 * 3 + 1]), 2.0) * pow(sin(q[0 * 3 + 0]), 2.0) * + pow(sin(q[0 * 3 + 2]), 2.0) + + pow(cos(q[0 * 3 + 2]), 2.0) * pow(sin(q[0 * 3 + 0]), 2.0) * + pow(sin(q[0 * 3 + 1]), 2.0) + + pow(sin(q[0 * 3 + 0]), 2.0) * pow(sin(q[0 * 3 + 1]), 2.0) * + pow(sin(q[0 * 3 + 2]), 2.0) + + pow(cos(q[0 * 3 + 0]), 2.0) * pow(cos(q[0 * 3 + 1]), 2.0) * + pow(cos(q[0 * 3 + 2]), 2.0), + 2.0) * + (l[0 * 3 + 2] * cos(q[0 * 3 + 0]) * cos(q[0 * 3 + 1]) * + cos(q[0 * 3 + 2]) + + l[0 * 3 + 0] * cos(q[0 * 3 + 0]) * cos(q[0 * 3 + 1]) * + sin(q[0 * 3 + 2]) + + l[0 * 3 + 0] * cos(q[0 * 3 + 0]) * cos(q[0 * 3 + 2]) * + sin(q[0 * 3 + 1]) - + l[0 * 3 + 2] * cos(q[0 * 3 + 0]) * sin(q[0 * 3 + 1]) * + sin(q[0 * 3 + 2]) + + l[1 * 3 + 2] * cos(q[0 * 3 + 0]) * pow(cos(q[0 * 3 + 1]), 2.0) * + cos(q[0 * 3 + 2]) + + l[1 * 3 + 0] * cos(q[0 * 3 + 0]) * pow(cos(q[0 * 3 + 1]), 2.0) * + sin(q[0 * 3 + 2]) + + l[1 * 3 + 2] * cos(q[0 * 3 + 0]) * cos(q[0 * 3 + 2]) * + pow(sin(q[0 * 3 + 1]), 2.0) + + l[1 * 3 + 0] * cos(q[0 * 3 + 0]) * pow(sin(q[0 * 3 + 1]), 2.0) * + sin(q[0 * 3 + 2]) + + RL * l[0 * 3 + 1] * cos(q[0 * 3 + 1]) * cos(q[0 * 3 + 2]) * + sin(q[0 * 3 + 0]) + + RL * l[1 * 3 + 1] * cos(q[0 * 3 + 1]) * cos(q[0 * 3 + 2]) * + sin(q[0 * 3 + 0]) - + RL * l[0 * 3 + 1] * sin(q[0 * 3 + 0]) * sin(q[0 * 3 + 1]) * + sin(q[0 * 3 + 2]) - + RL * l[1 * 3 + 1] * sin(q[0 * 3 + 0]) * sin(q[0 * 3 + 1]) * + sin(q[0 * 3 + 2]))) / + 2.0 - + (cos(q[0 * 3 + 0]) * + (cos(q[0 * 3 + 1]) * cos(q[0 * 3 + 2]) - + sin(q[0 * 3 + 1]) * sin(q[0 * 3 + 2])) * + 1.0 / + pow(pow(cos(q[0 * 3 + 0]), 2.0) * pow(cos(q[0 * 3 + 1]), 2.0) * + pow(sin(q[0 * 3 + 2]), 2.0) + + pow(cos(q[0 * 3 + 0]), 2.0) * pow(cos(q[0 * 3 + 2]), 2.0) * + pow(sin(q[0 * 3 + 1]), 2.0) + + pow(cos(q[0 * 3 + 1]), 2.0) * pow(cos(q[0 * 3 + 2]), 2.0) * + pow(sin(q[0 * 3 + 0]), 2.0) + + pow(cos(q[0 * 3 + 0]), 2.0) * pow(sin(q[0 * 3 + 1]), 2.0) * + pow(sin(q[0 * 3 + 2]), 2.0) + + pow(cos(q[0 * 3 + 1]), 2.0) * pow(sin(q[0 * 3 + 0]), 2.0) * + pow(sin(q[0 * 3 + 2]), 2.0) + + pow(cos(q[0 * 3 + 2]), 2.0) * pow(sin(q[0 * 3 + 0]), 2.0) * + pow(sin(q[0 * 3 + 1]), 2.0) + + pow(sin(q[0 * 3 + 0]), 2.0) * pow(sin(q[0 * 3 + 1]), 2.0) * + pow(sin(q[0 * 3 + 2]), 2.0) + + pow(cos(q[0 * 3 + 0]), 2.0) * pow(cos(q[0 * 3 + 1]), 2.0) * + pow(cos(q[0 * 3 + 2]), 2.0), + 2.0) * + (l[0 * 3 + 2] * cos(q[0 * 3 + 1]) * cos(q[0 * 3 + 2]) * + sin(q[0 * 3 + 0]) + + l[0 * 3 + 0] * cos(q[0 * 3 + 1]) * sin(q[0 * 3 + 0]) * + sin(q[0 * 3 + 2]) + + l[0 * 3 + 0] * cos(q[0 * 3 + 2]) * sin(q[0 * 3 + 0]) * + sin(q[0 * 3 + 1]) - + l[0 * 3 + 2] * sin(q[0 * 3 + 0]) * sin(q[0 * 3 + 1]) * + sin(q[0 * 3 + 2]) + + l[1 * 3 + 2] * pow(cos(q[0 * 3 + 1]), 2.0) * cos(q[0 * 3 + 2]) * + sin(q[0 * 3 + 0]) + + l[1 * 3 + 0] * pow(cos(q[0 * 3 + 1]), 2.0) * sin(q[0 * 3 + 0]) * + sin(q[0 * 3 + 2]) + + l[1 * 3 + 2] * cos(q[0 * 3 + 2]) * sin(q[0 * 3 + 0]) * + pow(sin(q[0 * 3 + 1]), 2.0) + + l[1 * 3 + 0] * sin(q[0 * 3 + 0]) * pow(sin(q[0 * 3 + 1]), 2.0) * + sin(q[0 * 3 + 2]) - + RL * l[0 * 3 + 1] * cos(q[0 * 3 + 0]) * cos(q[0 * 3 + 1]) * + cos(q[0 * 3 + 2]) - + RL * l[1 * 3 + 1] * cos(q[0 * 3 + 0]) * cos(q[0 * 3 + 1]) * + cos(q[0 * 3 + 2]) + + RL * l[0 * 3 + 1] * cos(q[0 * 3 + 0]) * sin(q[0 * 3 + 1]) * + sin(q[0 * 3 + 2]) + + RL * l[1 * 3 + 1] * cos(q[0 * 3 + 0]) * sin(q[0 * 3 + 1]) * + sin(q[0 * 3 + 2]))) / + 2.0 + + (cos(q[0 * 3 + 0]) * + (cos(q[0 * 3 + 1]) * sin(q[0 * 3 + 2]) + + cos(q[0 * 3 + 2]) * sin(q[0 * 3 + 1])) * + 1.0 / + pow(pow(cos(q[0 * 3 + 0]), 2.0) * pow(cos(q[0 * 3 + 1]), 2.0) * + pow(sin(q[0 * 3 + 2]), 2.0) + + pow(cos(q[0 * 3 + 0]), 2.0) * pow(cos(q[0 * 3 + 2]), 2.0) * + pow(sin(q[0 * 3 + 1]), 2.0) + + pow(cos(q[0 * 3 + 1]), 2.0) * pow(cos(q[0 * 3 + 2]), 2.0) * + pow(sin(q[0 * 3 + 0]), 2.0) + + pow(cos(q[0 * 3 + 0]), 2.0) * pow(sin(q[0 * 3 + 1]), 2.0) * + pow(sin(q[0 * 3 + 2]), 2.0) + + pow(cos(q[0 * 3 + 1]), 2.0) * pow(sin(q[0 * 3 + 0]), 2.0) * + pow(sin(q[0 * 3 + 2]), 2.0) + + pow(cos(q[0 * 3 + 2]), 2.0) * pow(sin(q[0 * 3 + 0]), 2.0) * + pow(sin(q[0 * 3 + 1]), 2.0) + + pow(sin(q[0 * 3 + 0]), 2.0) * pow(sin(q[0 * 3 + 1]), 2.0) * + pow(sin(q[0 * 3 + 2]), 2.0) + + pow(cos(q[0 * 3 + 0]), 2.0) * pow(cos(q[0 * 3 + 1]), 2.0) * + pow(cos(q[0 * 3 + 2]), 2.0), + 2.0) * + (l[0 * 3 + 0] * cos(q[0 * 3 + 1]) * cos(q[0 * 3 + 2]) * + sin(q[0 * 3 + 0]) - + l[0 * 3 + 2] * cos(q[0 * 3 + 1]) * sin(q[0 * 3 + 0]) * + sin(q[0 * 3 + 2]) - + l[0 * 3 + 2] * cos(q[0 * 3 + 2]) * sin(q[0 * 3 + 0]) * + sin(q[0 * 3 + 1]) - + l[0 * 3 + 0] * sin(q[0 * 3 + 0]) * sin(q[0 * 3 + 1]) * + sin(q[0 * 3 + 2]) + + l[1 * 3 + 0] * pow(cos(q[0 * 3 + 1]), 2.0) * cos(q[0 * 3 + 2]) * + sin(q[0 * 3 + 0]) + + l[1 * 3 + 0] * cos(q[0 * 3 + 2]) * sin(q[0 * 3 + 0]) * + pow(sin(q[0 * 3 + 1]), 2.0) - + l[1 * 3 + 2] * pow(cos(q[0 * 3 + 1]), 2.0) * sin(q[0 * 3 + 0]) * + sin(q[0 * 3 + 2]) - + l[1 * 3 + 2] * sin(q[0 * 3 + 0]) * pow(sin(q[0 * 3 + 1]), 2.0) * + sin(q[0 * 3 + 2]) + + RL * l[0 * 3 + 1] * cos(q[0 * 3 + 0]) * cos(q[0 * 3 + 1]) * + sin(q[0 * 3 + 2]) + + RL * l[0 * 3 + 1] * cos(q[0 * 3 + 0]) * cos(q[0 * 3 + 2]) * + sin(q[0 * 3 + 1]) + + RL * l[1 * 3 + 1] * cos(q[0 * 3 + 0]) * cos(q[0 * 3 + 1]) * + sin(q[0 * 3 + 2]) + + RL * l[1 * 3 + 1] * cos(q[0 * 3 + 0]) * cos(q[0 * 3 + 2]) * + sin(q[0 * 3 + 1]))) / + 2.0)) / + (pow(cos(q[0 * 3 + 0]), 2.0) + pow(sin(q[0 * 3 + 0]), 2.0)) + + (sin(q[0 * 3 + 0]) * + (cos(q[0 * 3 + 1]) * sin(q[0 * 3 + 2]) + + cos(q[0 * 3 + 2]) * sin(q[0 * 3 + 1])) * + (l[2 * 3 + 0] - + (cos(q[0 * 3 + 0]) * + (-l[0 * 3 + 0] * cos(q[0 * 3 + 0]) * cos(q[0 * 3 + 1]) * + cos(q[0 * 3 + 2]) + + l[0 * 3 + 2] * cos(q[0 * 3 + 0]) * cos(q[0 * 3 + 1]) * + sin(q[0 * 3 + 2]) + + l[0 * 3 + 2] * cos(q[0 * 3 + 0]) * cos(q[0 * 3 + 2]) * + sin(q[0 * 3 + 1]) + + l[0 * 3 + 0] * cos(q[0 * 3 + 0]) * sin(q[0 * 3 + 1]) * + sin(q[0 * 3 + 2]) - + l[1 * 3 + 0] * cos(q[0 * 3 + 0]) * pow(cos(q[0 * 3 + 1]), 2.0) * + cos(q[0 * 3 + 2]) - + l[1 * 3 + 0] * cos(q[0 * 3 + 0]) * cos(q[0 * 3 + 2]) * + pow(sin(q[0 * 3 + 1]), 2.0) + + l[1 * 3 + 2] * cos(q[0 * 3 + 0]) * pow(cos(q[0 * 3 + 1]), 2.0) * + sin(q[0 * 3 + 2]) + + l[1 * 3 + 2] * cos(q[0 * 3 + 0]) * pow(sin(q[0 * 3 + 1]), 2.0) * + sin(q[0 * 3 + 2]) + + RL * l[0 * 3 + 1] * cos(q[0 * 3 + 1]) * sin(q[0 * 3 + 0]) * + sin(q[0 * 3 + 2]) + + RL * l[0 * 3 + 1] * cos(q[0 * 3 + 2]) * sin(q[0 * 3 + 0]) * + sin(q[0 * 3 + 1]) + + RL * l[1 * 3 + 1] * cos(q[0 * 3 + 1]) * sin(q[0 * 3 + 0]) * + sin(q[0 * 3 + 2]) + + RL * l[1 * 3 + 1] * cos(q[0 * 3 + 2]) * sin(q[0 * 3 + 0]) * + sin(q[0 * 3 + 1]))) / + ((pow(cos(q[0 * 3 + 0]), 2.0) + pow(sin(q[0 * 3 + 0]), 2.0)) * + (pow(cos(q[0 * 3 + 0]), 2.0) * pow(cos(q[0 * 3 + 1]), 2.0) * + pow(sin(q[0 * 3 + 2]), 2.0) + + pow(cos(q[0 * 3 + 0]), 2.0) * pow(cos(q[0 * 3 + 2]), 2.0) * + pow(sin(q[0 * 3 + 1]), 2.0) + + pow(cos(q[0 * 3 + 1]), 2.0) * pow(cos(q[0 * 3 + 2]), 2.0) * + pow(sin(q[0 * 3 + 0]), 2.0) + + pow(cos(q[0 * 3 + 0]), 2.0) * pow(sin(q[0 * 3 + 1]), 2.0) * + pow(sin(q[0 * 3 + 2]), 2.0) + + pow(cos(q[0 * 3 + 1]), 2.0) * pow(sin(q[0 * 3 + 0]), 2.0) * + pow(sin(q[0 * 3 + 2]), 2.0) + + pow(cos(q[0 * 3 + 2]), 2.0) * pow(sin(q[0 * 3 + 0]), 2.0) * + pow(sin(q[0 * 3 + 1]), 2.0) + + pow(sin(q[0 * 3 + 0]), 2.0) * pow(sin(q[0 * 3 + 1]), 2.0) * + pow(sin(q[0 * 3 + 2]), 2.0) + + pow(cos(q[0 * 3 + 0]), 2.0) * pow(cos(q[0 * 3 + 1]), 2.0) * + pow(cos(q[0 * 3 + 2]), 2.0)) * + 2.0) + + (sin(q[0 * 3 + 0]) * + (l[0 * 3 + 0] * cos(q[0 * 3 + 1]) * cos(q[0 * 3 + 2]) * + sin(q[0 * 3 + 0]) - + l[0 * 3 + 2] * cos(q[0 * 3 + 1]) * sin(q[0 * 3 + 0]) * + sin(q[0 * 3 + 2]) - + l[0 * 3 + 2] * cos(q[0 * 3 + 2]) * sin(q[0 * 3 + 0]) * + sin(q[0 * 3 + 1]) - + l[0 * 3 + 0] * sin(q[0 * 3 + 0]) * sin(q[0 * 3 + 1]) * + sin(q[0 * 3 + 2]) + + l[1 * 3 + 0] * pow(cos(q[0 * 3 + 1]), 2.0) * cos(q[0 * 3 + 2]) * + sin(q[0 * 3 + 0]) + + l[1 * 3 + 0] * cos(q[0 * 3 + 2]) * sin(q[0 * 3 + 0]) * + pow(sin(q[0 * 3 + 1]), 2.0) - + l[1 * 3 + 2] * pow(cos(q[0 * 3 + 1]), 2.0) * sin(q[0 * 3 + 0]) * + sin(q[0 * 3 + 2]) - + l[1 * 3 + 2] * sin(q[0 * 3 + 0]) * pow(sin(q[0 * 3 + 1]), 2.0) * + sin(q[0 * 3 + 2]) + + RL * l[0 * 3 + 1] * cos(q[0 * 3 + 0]) * cos(q[0 * 3 + 1]) * + sin(q[0 * 3 + 2]) + + RL * l[0 * 3 + 1] * cos(q[0 * 3 + 0]) * cos(q[0 * 3 + 2]) * + sin(q[0 * 3 + 1]) + + RL * l[1 * 3 + 1] * cos(q[0 * 3 + 0]) * cos(q[0 * 3 + 1]) * + sin(q[0 * 3 + 2]) + + RL * l[1 * 3 + 1] * cos(q[0 * 3 + 0]) * cos(q[0 * 3 + 2]) * + sin(q[0 * 3 + 1]))) / + ((pow(cos(q[0 * 3 + 0]), 2.0) + pow(sin(q[0 * 3 + 0]), 2.0)) * + (pow(cos(q[0 * 3 + 0]), 2.0) * pow(cos(q[0 * 3 + 1]), 2.0) * + pow(sin(q[0 * 3 + 2]), 2.0) + + pow(cos(q[0 * 3 + 0]), 2.0) * pow(cos(q[0 * 3 + 2]), 2.0) * + pow(sin(q[0 * 3 + 1]), 2.0) + + pow(cos(q[0 * 3 + 1]), 2.0) * pow(cos(q[0 * 3 + 2]), 2.0) * + pow(sin(q[0 * 3 + 0]), 2.0) + + pow(cos(q[0 * 3 + 0]), 2.0) * pow(sin(q[0 * 3 + 1]), 2.0) * + pow(sin(q[0 * 3 + 2]), 2.0) + + pow(cos(q[0 * 3 + 1]), 2.0) * pow(sin(q[0 * 3 + 0]), 2.0) * + pow(sin(q[0 * 3 + 2]), 2.0) + + pow(cos(q[0 * 3 + 2]), 2.0) * pow(sin(q[0 * 3 + 0]), 2.0) * + pow(sin(q[0 * 3 + 1]), 2.0) + + pow(sin(q[0 * 3 + 0]), 2.0) * pow(sin(q[0 * 3 + 1]), 2.0) * + pow(sin(q[0 * 3 + 2]), 2.0) + + pow(cos(q[0 * 3 + 0]), 2.0) * pow(cos(q[0 * 3 + 1]), 2.0) * + pow(cos(q[0 * 3 + 2]), 2.0)) * + 2.0) - + ((cos(q[0 * 3 + 1]) * sin(q[0 * 3 + 2]) + + cos(q[0 * 3 + 2]) * sin(q[0 * 3 + 1])) * + (l[0 * 3 + 2] * pow(sin(q[0 * 3 + 1]), 2.0) + + l[1 * 3 + 2] * cos(q[0 * 3 + 1]) - + l[1 * 3 + 0] * sin(q[0 * 3 + 1]) + + l[0 * 3 + 2] * pow(cos(q[0 * 3 + 1]), 2.0))) / + ((pow(cos(q[0 * 3 + 1]), 2.0) + pow(sin(q[0 * 3 + 1]), 2.0)) * + (pow(cos(q[0 * 3 + 1]), 2.0) * pow(cos(q[0 * 3 + 2]), 2.0) + + pow(cos(q[0 * 3 + 1]), 2.0) * pow(sin(q[0 * 3 + 2]), 2.0) + + pow(cos(q[0 * 3 + 2]), 2.0) * pow(sin(q[0 * 3 + 1]), 2.0) + + pow(sin(q[0 * 3 + 1]), 2.0) * pow(sin(q[0 * 3 + 2]), 2.0)) * + 2.0) + + (pow(cos(q[0 * 3 + 0]), 2.0) * + (cos(q[0 * 3 + 1]) * cos(q[0 * 3 + 2]) - + sin(q[0 * 3 + 1]) * sin(q[0 * 3 + 2])) * + (l[0 * 3 + 0] * pow(sin(q[0 * 3 + 1]), 2.0) + + l[1 * 3 + 0] * cos(q[0 * 3 + 1]) + + l[1 * 3 + 2] * sin(q[0 * 3 + 1]) + + l[0 * 3 + 0] * pow(cos(q[0 * 3 + 1]), 2.0))) / + ((pow(cos(q[0 * 3 + 0]), 2.0) * pow(cos(q[0 * 3 + 1]), 2.0) + + pow(cos(q[0 * 3 + 0]), 2.0) * pow(sin(q[0 * 3 + 1]), 2.0) + + pow(cos(q[0 * 3 + 1]), 2.0) * pow(sin(q[0 * 3 + 0]), 2.0) + + pow(sin(q[0 * 3 + 0]), 2.0) * pow(sin(q[0 * 3 + 1]), 2.0)) * + (pow(cos(q[0 * 3 + 0]), 2.0) * pow(cos(q[0 * 3 + 1]), 2.0) * + pow(sin(q[0 * 3 + 2]), 2.0) + + pow(cos(q[0 * 3 + 0]), 2.0) * pow(cos(q[0 * 3 + 2]), 2.0) * + pow(sin(q[0 * 3 + 1]), 2.0) + + pow(cos(q[0 * 3 + 1]), 2.0) * pow(cos(q[0 * 3 + 2]), 2.0) * + pow(sin(q[0 * 3 + 0]), 2.0) + + pow(cos(q[0 * 3 + 0]), 2.0) * pow(sin(q[0 * 3 + 1]), 2.0) * + pow(sin(q[0 * 3 + 2]), 2.0) + + pow(cos(q[0 * 3 + 1]), 2.0) * pow(sin(q[0 * 3 + 0]), 2.0) * + pow(sin(q[0 * 3 + 2]), 2.0) + + pow(cos(q[0 * 3 + 2]), 2.0) * pow(sin(q[0 * 3 + 0]), 2.0) * + pow(sin(q[0 * 3 + 1]), 2.0) + + pow(sin(q[0 * 3 + 0]), 2.0) * pow(sin(q[0 * 3 + 1]), 2.0) * + pow(sin(q[0 * 3 + 2]), 2.0) + + pow(cos(q[0 * 3 + 0]), 2.0) * pow(cos(q[0 * 3 + 1]), 2.0) * + pow(cos(q[0 * 3 + 2]), 2.0)) * + 2.0) + + (pow(sin(q[0 * 3 + 0]), 2.0) * + (cos(q[0 * 3 + 1]) * cos(q[0 * 3 + 2]) - + sin(q[0 * 3 + 1]) * sin(q[0 * 3 + 2])) * + (l[0 * 3 + 0] * pow(sin(q[0 * 3 + 1]), 2.0) + + l[1 * 3 + 0] * cos(q[0 * 3 + 1]) + + l[1 * 3 + 2] * sin(q[0 * 3 + 1]) + + l[0 * 3 + 0] * pow(cos(q[0 * 3 + 1]), 2.0))) / + ((pow(cos(q[0 * 3 + 0]), 2.0) * pow(cos(q[0 * 3 + 1]), 2.0) + + pow(cos(q[0 * 3 + 0]), 2.0) * pow(sin(q[0 * 3 + 1]), 2.0) + + pow(cos(q[0 * 3 + 1]), 2.0) * pow(sin(q[0 * 3 + 0]), 2.0) + + pow(sin(q[0 * 3 + 0]), 2.0) * pow(sin(q[0 * 3 + 1]), 2.0)) * + (pow(cos(q[0 * 3 + 0]), 2.0) * pow(cos(q[0 * 3 + 1]), 2.0) * + pow(sin(q[0 * 3 + 2]), 2.0) + + pow(cos(q[0 * 3 + 0]), 2.0) * pow(cos(q[0 * 3 + 2]), 2.0) * + pow(sin(q[0 * 3 + 1]), 2.0) + + pow(cos(q[0 * 3 + 1]), 2.0) * pow(cos(q[0 * 3 + 2]), 2.0) * + pow(sin(q[0 * 3 + 0]), 2.0) + + pow(cos(q[0 * 3 + 0]), 2.0) * pow(sin(q[0 * 3 + 1]), 2.0) * + pow(sin(q[0 * 3 + 2]), 2.0) + + pow(cos(q[0 * 3 + 1]), 2.0) * pow(sin(q[0 * 3 + 0]), 2.0) * + pow(sin(q[0 * 3 + 2]), 2.0) + + pow(cos(q[0 * 3 + 2]), 2.0) * pow(sin(q[0 * 3 + 0]), 2.0) * + pow(sin(q[0 * 3 + 1]), 2.0) + + pow(sin(q[0 * 3 + 0]), 2.0) * pow(sin(q[0 * 3 + 1]), 2.0) * + pow(sin(q[0 * 3 + 2]), 2.0) + + pow(cos(q[0 * 3 + 0]), 2.0) * pow(cos(q[0 * 3 + 1]), 2.0) * + pow(cos(q[0 * 3 + 2]), 2.0)) * + 2.0))) / + (pow(cos(q[0 * 3 + 0]), 2.0) * pow(cos(q[0 * 3 + 1]), 2.0) * + pow(sin(q[0 * 3 + 2]), 2.0) + + pow(cos(q[0 * 3 + 0]), 2.0) * pow(cos(q[0 * 3 + 2]), 2.0) * + pow(sin(q[0 * 3 + 1]), 2.0) + + pow(cos(q[0 * 3 + 1]), 2.0) * pow(cos(q[0 * 3 + 2]), 2.0) * + pow(sin(q[0 * 3 + 0]), 2.0) + + pow(cos(q[0 * 3 + 0]), 2.0) * pow(sin(q[0 * 3 + 1]), 2.0) * + pow(sin(q[0 * 3 + 2]), 2.0) + + pow(cos(q[0 * 3 + 1]), 2.0) * pow(sin(q[0 * 3 + 0]), 2.0) * + pow(sin(q[0 * 3 + 2]), 2.0) + + pow(cos(q[0 * 3 + 2]), 2.0) * pow(sin(q[0 * 3 + 0]), 2.0) * + pow(sin(q[0 * 3 + 1]), 2.0) + + pow(sin(q[0 * 3 + 0]), 2.0) * pow(sin(q[0 * 3 + 1]), 2.0) * + pow(sin(q[0 * 3 + 2]), 2.0) + + pow(cos(q[0 * 3 + 0]), 2.0) * pow(cos(q[0 * 3 + 1]), 2.0) * + pow(cos(q[0 * 3 + 2]), 2.0)) - + (sin(q[0 * 3 + 0]) * + (cos(q[0 * 3 + 1]) * cos(q[0 * 3 + 2]) - + sin(q[0 * 3 + 1]) * sin(q[0 * 3 + 2])) * + (l[2 * 3 + 2] + + (cos(q[0 * 3 + 0]) * + (l[0 * 3 + 2] * cos(q[0 * 3 + 0]) * cos(q[0 * 3 + 1]) * + cos(q[0 * 3 + 2]) + + l[0 * 3 + 0] * cos(q[0 * 3 + 0]) * cos(q[0 * 3 + 1]) * + sin(q[0 * 3 + 2]) + + l[0 * 3 + 0] * cos(q[0 * 3 + 0]) * cos(q[0 * 3 + 2]) * + sin(q[0 * 3 + 1]) - + l[0 * 3 + 2] * cos(q[0 * 3 + 0]) * sin(q[0 * 3 + 1]) * + sin(q[0 * 3 + 2]) + + l[1 * 3 + 2] * cos(q[0 * 3 + 0]) * pow(cos(q[0 * 3 + 1]), 2.0) * + cos(q[0 * 3 + 2]) + + l[1 * 3 + 0] * cos(q[0 * 3 + 0]) * pow(cos(q[0 * 3 + 1]), 2.0) * + sin(q[0 * 3 + 2]) + + l[1 * 3 + 2] * cos(q[0 * 3 + 0]) * cos(q[0 * 3 + 2]) * + pow(sin(q[0 * 3 + 1]), 2.0) + + l[1 * 3 + 0] * cos(q[0 * 3 + 0]) * pow(sin(q[0 * 3 + 1]), 2.0) * + sin(q[0 * 3 + 2]) + + RL * l[0 * 3 + 1] * cos(q[0 * 3 + 1]) * cos(q[0 * 3 + 2]) * + sin(q[0 * 3 + 0]) + + RL * l[1 * 3 + 1] * cos(q[0 * 3 + 1]) * cos(q[0 * 3 + 2]) * + sin(q[0 * 3 + 0]) - + RL * l[0 * 3 + 1] * sin(q[0 * 3 + 0]) * sin(q[0 * 3 + 1]) * + sin(q[0 * 3 + 2]) - + RL * l[1 * 3 + 1] * sin(q[0 * 3 + 0]) * sin(q[0 * 3 + 1]) * + sin(q[0 * 3 + 2]))) / + ((pow(cos(q[0 * 3 + 0]), 2.0) + pow(sin(q[0 * 3 + 0]), 2.0)) * + (pow(cos(q[0 * 3 + 0]), 2.0) * pow(cos(q[0 * 3 + 1]), 2.0) * + pow(sin(q[0 * 3 + 2]), 2.0) + + pow(cos(q[0 * 3 + 0]), 2.0) * pow(cos(q[0 * 3 + 2]), 2.0) * + pow(sin(q[0 * 3 + 1]), 2.0) + + pow(cos(q[0 * 3 + 1]), 2.0) * pow(cos(q[0 * 3 + 2]), 2.0) * + pow(sin(q[0 * 3 + 0]), 2.0) + + pow(cos(q[0 * 3 + 0]), 2.0) * pow(sin(q[0 * 3 + 1]), 2.0) * + pow(sin(q[0 * 3 + 2]), 2.0) + + pow(cos(q[0 * 3 + 1]), 2.0) * pow(sin(q[0 * 3 + 0]), 2.0) * + pow(sin(q[0 * 3 + 2]), 2.0) + + pow(cos(q[0 * 3 + 2]), 2.0) * pow(sin(q[0 * 3 + 0]), 2.0) * + pow(sin(q[0 * 3 + 1]), 2.0) + + pow(sin(q[0 * 3 + 0]), 2.0) * pow(sin(q[0 * 3 + 1]), 2.0) * + pow(sin(q[0 * 3 + 2]), 2.0) + + pow(cos(q[0 * 3 + 0]), 2.0) * pow(cos(q[0 * 3 + 1]), 2.0) * + pow(cos(q[0 * 3 + 2]), 2.0)) * + 2.0) + + (sin(q[0 * 3 + 0]) * + (l[0 * 3 + 2] * cos(q[0 * 3 + 1]) * cos(q[0 * 3 + 2]) * + sin(q[0 * 3 + 0]) + + l[0 * 3 + 0] * cos(q[0 * 3 + 1]) * sin(q[0 * 3 + 0]) * + sin(q[0 * 3 + 2]) + + l[0 * 3 + 0] * cos(q[0 * 3 + 2]) * sin(q[0 * 3 + 0]) * + sin(q[0 * 3 + 1]) - + l[0 * 3 + 2] * sin(q[0 * 3 + 0]) * sin(q[0 * 3 + 1]) * + sin(q[0 * 3 + 2]) + + l[1 * 3 + 2] * pow(cos(q[0 * 3 + 1]), 2.0) * cos(q[0 * 3 + 2]) * + sin(q[0 * 3 + 0]) + + l[1 * 3 + 0] * pow(cos(q[0 * 3 + 1]), 2.0) * sin(q[0 * 3 + 0]) * + sin(q[0 * 3 + 2]) + + l[1 * 3 + 2] * cos(q[0 * 3 + 2]) * sin(q[0 * 3 + 0]) * + pow(sin(q[0 * 3 + 1]), 2.0) + + l[1 * 3 + 0] * sin(q[0 * 3 + 0]) * pow(sin(q[0 * 3 + 1]), 2.0) * + sin(q[0 * 3 + 2]) - + RL * l[0 * 3 + 1] * cos(q[0 * 3 + 0]) * cos(q[0 * 3 + 1]) * + cos(q[0 * 3 + 2]) - + RL * l[1 * 3 + 1] * cos(q[0 * 3 + 0]) * cos(q[0 * 3 + 1]) * + cos(q[0 * 3 + 2]) + + RL * l[0 * 3 + 1] * cos(q[0 * 3 + 0]) * sin(q[0 * 3 + 1]) * + sin(q[0 * 3 + 2]) + + RL * l[1 * 3 + 1] * cos(q[0 * 3 + 0]) * sin(q[0 * 3 + 1]) * + sin(q[0 * 3 + 2]))) / + ((pow(cos(q[0 * 3 + 0]), 2.0) + pow(sin(q[0 * 3 + 0]), 2.0)) * + (pow(cos(q[0 * 3 + 0]), 2.0) * pow(cos(q[0 * 3 + 1]), 2.0) * + pow(sin(q[0 * 3 + 2]), 2.0) + + pow(cos(q[0 * 3 + 0]), 2.0) * pow(cos(q[0 * 3 + 2]), 2.0) * + pow(sin(q[0 * 3 + 1]), 2.0) + + pow(cos(q[0 * 3 + 1]), 2.0) * pow(cos(q[0 * 3 + 2]), 2.0) * + pow(sin(q[0 * 3 + 0]), 2.0) + + pow(cos(q[0 * 3 + 0]), 2.0) * pow(sin(q[0 * 3 + 1]), 2.0) * + pow(sin(q[0 * 3 + 2]), 2.0) + + pow(cos(q[0 * 3 + 1]), 2.0) * pow(sin(q[0 * 3 + 0]), 2.0) * + pow(sin(q[0 * 3 + 2]), 2.0) + + pow(cos(q[0 * 3 + 2]), 2.0) * pow(sin(q[0 * 3 + 0]), 2.0) * + pow(sin(q[0 * 3 + 1]), 2.0) + + pow(sin(q[0 * 3 + 0]), 2.0) * pow(sin(q[0 * 3 + 1]), 2.0) * + pow(sin(q[0 * 3 + 2]), 2.0) + + pow(cos(q[0 * 3 + 0]), 2.0) * pow(cos(q[0 * 3 + 1]), 2.0) * + pow(cos(q[0 * 3 + 2]), 2.0)) * + 2.0) + + ((cos(q[0 * 3 + 1]) * cos(q[0 * 3 + 2]) - + sin(q[0 * 3 + 1]) * sin(q[0 * 3 + 2])) * + (l[0 * 3 + 2] * pow(sin(q[0 * 3 + 1]), 2.0) + + l[1 * 3 + 2] * cos(q[0 * 3 + 1]) - + l[1 * 3 + 0] * sin(q[0 * 3 + 1]) + + l[0 * 3 + 2] * pow(cos(q[0 * 3 + 1]), 2.0))) / + ((pow(cos(q[0 * 3 + 1]), 2.0) + pow(sin(q[0 * 3 + 1]), 2.0)) * + (pow(cos(q[0 * 3 + 1]), 2.0) * pow(cos(q[0 * 3 + 2]), 2.0) + + pow(cos(q[0 * 3 + 1]), 2.0) * pow(sin(q[0 * 3 + 2]), 2.0) + + pow(cos(q[0 * 3 + 2]), 2.0) * pow(sin(q[0 * 3 + 1]), 2.0) + + pow(sin(q[0 * 3 + 1]), 2.0) * pow(sin(q[0 * 3 + 2]), 2.0)) * + 2.0) + + (pow(cos(q[0 * 3 + 0]), 2.0) * + (cos(q[0 * 3 + 1]) * sin(q[0 * 3 + 2]) + + cos(q[0 * 3 + 2]) * sin(q[0 * 3 + 1])) * + (l[0 * 3 + 0] * pow(sin(q[0 * 3 + 1]), 2.0) + + l[1 * 3 + 0] * cos(q[0 * 3 + 1]) + + l[1 * 3 + 2] * sin(q[0 * 3 + 1]) + + l[0 * 3 + 0] * pow(cos(q[0 * 3 + 1]), 2.0))) / + ((pow(cos(q[0 * 3 + 0]), 2.0) * pow(cos(q[0 * 3 + 1]), 2.0) + + pow(cos(q[0 * 3 + 0]), 2.0) * pow(sin(q[0 * 3 + 1]), 2.0) + + pow(cos(q[0 * 3 + 1]), 2.0) * pow(sin(q[0 * 3 + 0]), 2.0) + + pow(sin(q[0 * 3 + 0]), 2.0) * pow(sin(q[0 * 3 + 1]), 2.0)) * + (pow(cos(q[0 * 3 + 0]), 2.0) * pow(cos(q[0 * 3 + 1]), 2.0) * + pow(sin(q[0 * 3 + 2]), 2.0) + + pow(cos(q[0 * 3 + 0]), 2.0) * pow(cos(q[0 * 3 + 2]), 2.0) * + pow(sin(q[0 * 3 + 1]), 2.0) + + pow(cos(q[0 * 3 + 1]), 2.0) * pow(cos(q[0 * 3 + 2]), 2.0) * + pow(sin(q[0 * 3 + 0]), 2.0) + + pow(cos(q[0 * 3 + 0]), 2.0) * pow(sin(q[0 * 3 + 1]), 2.0) * + pow(sin(q[0 * 3 + 2]), 2.0) + + pow(cos(q[0 * 3 + 1]), 2.0) * pow(sin(q[0 * 3 + 0]), 2.0) * + pow(sin(q[0 * 3 + 2]), 2.0) + + pow(cos(q[0 * 3 + 2]), 2.0) * pow(sin(q[0 * 3 + 0]), 2.0) * + pow(sin(q[0 * 3 + 1]), 2.0) + + pow(sin(q[0 * 3 + 0]), 2.0) * pow(sin(q[0 * 3 + 1]), 2.0) * + pow(sin(q[0 * 3 + 2]), 2.0) + + pow(cos(q[0 * 3 + 0]), 2.0) * pow(cos(q[0 * 3 + 1]), 2.0) * + pow(cos(q[0 * 3 + 2]), 2.0)) * + 2.0) + + (pow(sin(q[0 * 3 + 0]), 2.0) * + (cos(q[0 * 3 + 1]) * sin(q[0 * 3 + 2]) + + cos(q[0 * 3 + 2]) * sin(q[0 * 3 + 1])) * + (l[0 * 3 + 0] * pow(sin(q[0 * 3 + 1]), 2.0) + + l[1 * 3 + 0] * cos(q[0 * 3 + 1]) + + l[1 * 3 + 2] * sin(q[0 * 3 + 1]) + + l[0 * 3 + 0] * pow(cos(q[0 * 3 + 1]), 2.0))) / + ((pow(cos(q[0 * 3 + 0]), 2.0) * pow(cos(q[0 * 3 + 1]), 2.0) + + pow(cos(q[0 * 3 + 0]), 2.0) * pow(sin(q[0 * 3 + 1]), 2.0) + + pow(cos(q[0 * 3 + 1]), 2.0) * pow(sin(q[0 * 3 + 0]), 2.0) + + pow(sin(q[0 * 3 + 0]), 2.0) * pow(sin(q[0 * 3 + 1]), 2.0)) * + (pow(cos(q[0 * 3 + 0]), 2.0) * pow(cos(q[0 * 3 + 1]), 2.0) * + pow(sin(q[0 * 3 + 2]), 2.0) + + pow(cos(q[0 * 3 + 0]), 2.0) * pow(cos(q[0 * 3 + 2]), 2.0) * + pow(sin(q[0 * 3 + 1]), 2.0) + + pow(cos(q[0 * 3 + 1]), 2.0) * pow(cos(q[0 * 3 + 2]), 2.0) * + pow(sin(q[0 * 3 + 0]), 2.0) + + pow(cos(q[0 * 3 + 0]), 2.0) * pow(sin(q[0 * 3 + 1]), 2.0) * + pow(sin(q[0 * 3 + 2]), 2.0) + + pow(cos(q[0 * 3 + 1]), 2.0) * pow(sin(q[0 * 3 + 0]), 2.0) * + pow(sin(q[0 * 3 + 2]), 2.0) + + pow(cos(q[0 * 3 + 2]), 2.0) * pow(sin(q[0 * 3 + 0]), 2.0) * + pow(sin(q[0 * 3 + 1]), 2.0) + + pow(sin(q[0 * 3 + 0]), 2.0) * pow(sin(q[0 * 3 + 1]), 2.0) * + pow(sin(q[0 * 3 + 2]), 2.0) + + pow(cos(q[0 * 3 + 0]), 2.0) * pow(cos(q[0 * 3 + 1]), 2.0) * + pow(cos(q[0 * 3 + 2]), 2.0)) * + 2.0))) / + (pow(cos(q[0 * 3 + 0]), 2.0) * pow(cos(q[0 * 3 + 1]), 2.0) * + pow(sin(q[0 * 3 + 2]), 2.0) + + pow(cos(q[0 * 3 + 0]), 2.0) * pow(cos(q[0 * 3 + 2]), 2.0) * + pow(sin(q[0 * 3 + 1]), 2.0) + + pow(cos(q[0 * 3 + 1]), 2.0) * pow(cos(q[0 * 3 + 2]), 2.0) * + pow(sin(q[0 * 3 + 0]), 2.0) + + pow(cos(q[0 * 3 + 0]), 2.0) * pow(sin(q[0 * 3 + 1]), 2.0) * + pow(sin(q[0 * 3 + 2]), 2.0) + + pow(cos(q[0 * 3 + 1]), 2.0) * pow(sin(q[0 * 3 + 0]), 2.0) * + pow(sin(q[0 * 3 + 2]), 2.0) + + pow(cos(q[0 * 3 + 2]), 2.0) * pow(sin(q[0 * 3 + 0]), 2.0) * + pow(sin(q[0 * 3 + 1]), 2.0) + + pow(sin(q[0 * 3 + 0]), 2.0) * pow(sin(q[0 * 3 + 1]), 2.0) * + pow(sin(q[0 * 3 + 2]), 2.0) + + pow(cos(q[0 * 3 + 0]), 2.0) * pow(cos(q[0 * 3 + 1]), 2.0) * + pow(cos(q[0 * 3 + 2]), 2.0)))) / + (pow(cos(q[0 * 3 + 0]), 2.0) + pow(sin(q[0 * 3 + 0]), 2.0)); + F(2 * 3 + 0) = + -(l[0 * 3 + 2] * pow(sin(q[0 * 3 + 1]), 2.0) + + l[1 * 3 + 2] * cos(q[0 * 3 + 1]) - l[1 * 3 + 0] * sin(q[0 * 3 + 1]) + + l[0 * 3 + 2] * pow(cos(q[0 * 3 + 1]), 2.0)) / + (pow(cos(q[0 * 3 + 1]), 2.0) + pow(sin(q[0 * 3 + 1]), 2.0)) + + (cos(q[0 * 3 + 0]) * + ((sin(q[0 * 3 + 0]) * + (RL * l[2 * 3 + 1] + + ((cos(q[0 * 3 + 1]) * sin(q[0 * 3 + 2]) + + cos(q[0 * 3 + 2]) * sin(q[0 * 3 + 1])) * + (RL * l[0 * 3 + 1] * cos(q[0 * 3 + 1]) * sin(q[0 * 3 + 2]) + + RL * l[0 * 3 + 1] * cos(q[0 * 3 + 2]) * sin(q[0 * 3 + 1]) + + RL * l[1 * 3 + 1] * cos(q[0 * 3 + 1]) * sin(q[0 * 3 + 2]) + + RL * l[1 * 3 + 1] * cos(q[0 * 3 + 2]) * sin(q[0 * 3 + 1])) * + 1.0 / + pow(pow(cos(q[0 * 3 + 1]), 2.0) * pow(cos(q[0 * 3 + 2]), 2.0) + + pow(cos(q[0 * 3 + 1]), 2.0) * pow(sin(q[0 * 3 + 2]), 2.0) + + pow(cos(q[0 * 3 + 2]), 2.0) * pow(sin(q[0 * 3 + 1]), 2.0) + + pow(sin(q[0 * 3 + 1]), 2.0) * pow(sin(q[0 * 3 + 2]), 2.0), + 2.0)) / + 2.0 + + ((cos(q[0 * 3 + 1]) * cos(q[0 * 3 + 2]) - + sin(q[0 * 3 + 1]) * sin(q[0 * 3 + 2])) * + (RL * l[0 * 3 + 1] * cos(q[0 * 3 + 1]) * cos(q[0 * 3 + 2]) + + RL * l[1 * 3 + 1] * cos(q[0 * 3 + 1]) * cos(q[0 * 3 + 2]) - + RL * l[0 * 3 + 1] * sin(q[0 * 3 + 1]) * sin(q[0 * 3 + 2]) - + RL * l[1 * 3 + 1] * sin(q[0 * 3 + 1]) * sin(q[0 * 3 + 2])) * + 1.0 / + pow(pow(cos(q[0 * 3 + 1]), 2.0) * pow(cos(q[0 * 3 + 2]), 2.0) + + pow(cos(q[0 * 3 + 1]), 2.0) * pow(sin(q[0 * 3 + 2]), 2.0) + + pow(cos(q[0 * 3 + 2]), 2.0) * pow(sin(q[0 * 3 + 1]), 2.0) + + pow(sin(q[0 * 3 + 1]), 2.0) * pow(sin(q[0 * 3 + 2]), 2.0), + 2.0)) / + 2.0 + + (sin(q[0 * 3 + 0]) * + (cos(q[0 * 3 + 1]) * sin(q[0 * 3 + 2]) + + cos(q[0 * 3 + 2]) * sin(q[0 * 3 + 1])) * + 1.0 / + pow(pow(cos(q[0 * 3 + 0]), 2.0) * pow(cos(q[0 * 3 + 1]), 2.0) * + pow(sin(q[0 * 3 + 2]), 2.0) + + pow(cos(q[0 * 3 + 0]), 2.0) * pow(cos(q[0 * 3 + 2]), 2.0) * + pow(sin(q[0 * 3 + 1]), 2.0) + + pow(cos(q[0 * 3 + 1]), 2.0) * pow(cos(q[0 * 3 + 2]), 2.0) * + pow(sin(q[0 * 3 + 0]), 2.0) + + pow(cos(q[0 * 3 + 0]), 2.0) * pow(sin(q[0 * 3 + 1]), 2.0) * + pow(sin(q[0 * 3 + 2]), 2.0) + + pow(cos(q[0 * 3 + 1]), 2.0) * pow(sin(q[0 * 3 + 0]), 2.0) * + pow(sin(q[0 * 3 + 2]), 2.0) + + pow(cos(q[0 * 3 + 2]), 2.0) * pow(sin(q[0 * 3 + 0]), 2.0) * + pow(sin(q[0 * 3 + 1]), 2.0) + + pow(sin(q[0 * 3 + 0]), 2.0) * pow(sin(q[0 * 3 + 1]), 2.0) * + pow(sin(q[0 * 3 + 2]), 2.0) + + pow(cos(q[0 * 3 + 0]), 2.0) * pow(cos(q[0 * 3 + 1]), 2.0) * + pow(cos(q[0 * 3 + 2]), 2.0), + 2.0) * + (-l[0 * 3 + 0] * cos(q[0 * 3 + 0]) * cos(q[0 * 3 + 1]) * + cos(q[0 * 3 + 2]) + + l[0 * 3 + 2] * cos(q[0 * 3 + 0]) * cos(q[0 * 3 + 1]) * + sin(q[0 * 3 + 2]) + + l[0 * 3 + 2] * cos(q[0 * 3 + 0]) * cos(q[0 * 3 + 2]) * + sin(q[0 * 3 + 1]) + + l[0 * 3 + 0] * cos(q[0 * 3 + 0]) * sin(q[0 * 3 + 1]) * + sin(q[0 * 3 + 2]) - + l[1 * 3 + 0] * cos(q[0 * 3 + 0]) * pow(cos(q[0 * 3 + 1]), 2.0) * + cos(q[0 * 3 + 2]) - + l[1 * 3 + 0] * cos(q[0 * 3 + 0]) * cos(q[0 * 3 + 2]) * + pow(sin(q[0 * 3 + 1]), 2.0) + + l[1 * 3 + 2] * cos(q[0 * 3 + 0]) * pow(cos(q[0 * 3 + 1]), 2.0) * + sin(q[0 * 3 + 2]) + + l[1 * 3 + 2] * cos(q[0 * 3 + 0]) * pow(sin(q[0 * 3 + 1]), 2.0) * + sin(q[0 * 3 + 2]) + + RL * l[0 * 3 + 1] * cos(q[0 * 3 + 1]) * sin(q[0 * 3 + 0]) * + sin(q[0 * 3 + 2]) + + RL * l[0 * 3 + 1] * cos(q[0 * 3 + 2]) * sin(q[0 * 3 + 0]) * + sin(q[0 * 3 + 1]) + + RL * l[1 * 3 + 1] * cos(q[0 * 3 + 1]) * sin(q[0 * 3 + 0]) * + sin(q[0 * 3 + 2]) + + RL * l[1 * 3 + 1] * cos(q[0 * 3 + 2]) * sin(q[0 * 3 + 0]) * + sin(q[0 * 3 + 1]))) / + 2.0 + + (sin(q[0 * 3 + 0]) * + (cos(q[0 * 3 + 1]) * cos(q[0 * 3 + 2]) - + sin(q[0 * 3 + 1]) * sin(q[0 * 3 + 2])) * + 1.0 / + pow(pow(cos(q[0 * 3 + 0]), 2.0) * pow(cos(q[0 * 3 + 1]), 2.0) * + pow(sin(q[0 * 3 + 2]), 2.0) + + pow(cos(q[0 * 3 + 0]), 2.0) * pow(cos(q[0 * 3 + 2]), 2.0) * + pow(sin(q[0 * 3 + 1]), 2.0) + + pow(cos(q[0 * 3 + 1]), 2.0) * pow(cos(q[0 * 3 + 2]), 2.0) * + pow(sin(q[0 * 3 + 0]), 2.0) + + pow(cos(q[0 * 3 + 0]), 2.0) * pow(sin(q[0 * 3 + 1]), 2.0) * + pow(sin(q[0 * 3 + 2]), 2.0) + + pow(cos(q[0 * 3 + 1]), 2.0) * pow(sin(q[0 * 3 + 0]), 2.0) * + pow(sin(q[0 * 3 + 2]), 2.0) + + pow(cos(q[0 * 3 + 2]), 2.0) * pow(sin(q[0 * 3 + 0]), 2.0) * + pow(sin(q[0 * 3 + 1]), 2.0) + + pow(sin(q[0 * 3 + 0]), 2.0) * pow(sin(q[0 * 3 + 1]), 2.0) * + pow(sin(q[0 * 3 + 2]), 2.0) + + pow(cos(q[0 * 3 + 0]), 2.0) * pow(cos(q[0 * 3 + 1]), 2.0) * + pow(cos(q[0 * 3 + 2]), 2.0), + 2.0) * + (l[0 * 3 + 2] * cos(q[0 * 3 + 0]) * cos(q[0 * 3 + 1]) * + cos(q[0 * 3 + 2]) + + l[0 * 3 + 0] * cos(q[0 * 3 + 0]) * cos(q[0 * 3 + 1]) * + sin(q[0 * 3 + 2]) + + l[0 * 3 + 0] * cos(q[0 * 3 + 0]) * cos(q[0 * 3 + 2]) * + sin(q[0 * 3 + 1]) - + l[0 * 3 + 2] * cos(q[0 * 3 + 0]) * sin(q[0 * 3 + 1]) * + sin(q[0 * 3 + 2]) + + l[1 * 3 + 2] * cos(q[0 * 3 + 0]) * pow(cos(q[0 * 3 + 1]), 2.0) * + cos(q[0 * 3 + 2]) + + l[1 * 3 + 0] * cos(q[0 * 3 + 0]) * pow(cos(q[0 * 3 + 1]), 2.0) * + sin(q[0 * 3 + 2]) + + l[1 * 3 + 2] * cos(q[0 * 3 + 0]) * cos(q[0 * 3 + 2]) * + pow(sin(q[0 * 3 + 1]), 2.0) + + l[1 * 3 + 0] * cos(q[0 * 3 + 0]) * pow(sin(q[0 * 3 + 1]), 2.0) * + sin(q[0 * 3 + 2]) + + RL * l[0 * 3 + 1] * cos(q[0 * 3 + 1]) * cos(q[0 * 3 + 2]) * + sin(q[0 * 3 + 0]) + + RL * l[1 * 3 + 1] * cos(q[0 * 3 + 1]) * cos(q[0 * 3 + 2]) * + sin(q[0 * 3 + 0]) - + RL * l[0 * 3 + 1] * sin(q[0 * 3 + 0]) * sin(q[0 * 3 + 1]) * + sin(q[0 * 3 + 2]) - + RL * l[1 * 3 + 1] * sin(q[0 * 3 + 0]) * sin(q[0 * 3 + 1]) * + sin(q[0 * 3 + 2]))) / + 2.0 - + (cos(q[0 * 3 + 0]) * + (cos(q[0 * 3 + 1]) * cos(q[0 * 3 + 2]) - + sin(q[0 * 3 + 1]) * sin(q[0 * 3 + 2])) * + 1.0 / + pow(pow(cos(q[0 * 3 + 0]), 2.0) * pow(cos(q[0 * 3 + 1]), 2.0) * + pow(sin(q[0 * 3 + 2]), 2.0) + + pow(cos(q[0 * 3 + 0]), 2.0) * pow(cos(q[0 * 3 + 2]), 2.0) * + pow(sin(q[0 * 3 + 1]), 2.0) + + pow(cos(q[0 * 3 + 1]), 2.0) * pow(cos(q[0 * 3 + 2]), 2.0) * + pow(sin(q[0 * 3 + 0]), 2.0) + + pow(cos(q[0 * 3 + 0]), 2.0) * pow(sin(q[0 * 3 + 1]), 2.0) * + pow(sin(q[0 * 3 + 2]), 2.0) + + pow(cos(q[0 * 3 + 1]), 2.0) * pow(sin(q[0 * 3 + 0]), 2.0) * + pow(sin(q[0 * 3 + 2]), 2.0) + + pow(cos(q[0 * 3 + 2]), 2.0) * pow(sin(q[0 * 3 + 0]), 2.0) * + pow(sin(q[0 * 3 + 1]), 2.0) + + pow(sin(q[0 * 3 + 0]), 2.0) * pow(sin(q[0 * 3 + 1]), 2.0) * + pow(sin(q[0 * 3 + 2]), 2.0) + + pow(cos(q[0 * 3 + 0]), 2.0) * pow(cos(q[0 * 3 + 1]), 2.0) * + pow(cos(q[0 * 3 + 2]), 2.0), + 2.0) * + (l[0 * 3 + 2] * cos(q[0 * 3 + 1]) * cos(q[0 * 3 + 2]) * + sin(q[0 * 3 + 0]) + + l[0 * 3 + 0] * cos(q[0 * 3 + 1]) * sin(q[0 * 3 + 0]) * + sin(q[0 * 3 + 2]) + + l[0 * 3 + 0] * cos(q[0 * 3 + 2]) * sin(q[0 * 3 + 0]) * + sin(q[0 * 3 + 1]) - + l[0 * 3 + 2] * sin(q[0 * 3 + 0]) * sin(q[0 * 3 + 1]) * + sin(q[0 * 3 + 2]) + + l[1 * 3 + 2] * pow(cos(q[0 * 3 + 1]), 2.0) * cos(q[0 * 3 + 2]) * + sin(q[0 * 3 + 0]) + + l[1 * 3 + 0] * pow(cos(q[0 * 3 + 1]), 2.0) * sin(q[0 * 3 + 0]) * + sin(q[0 * 3 + 2]) + + l[1 * 3 + 2] * cos(q[0 * 3 + 2]) * sin(q[0 * 3 + 0]) * + pow(sin(q[0 * 3 + 1]), 2.0) + + l[1 * 3 + 0] * sin(q[0 * 3 + 0]) * pow(sin(q[0 * 3 + 1]), 2.0) * + sin(q[0 * 3 + 2]) - + RL * l[0 * 3 + 1] * cos(q[0 * 3 + 0]) * cos(q[0 * 3 + 1]) * + cos(q[0 * 3 + 2]) - + RL * l[1 * 3 + 1] * cos(q[0 * 3 + 0]) * cos(q[0 * 3 + 1]) * + cos(q[0 * 3 + 2]) + + RL * l[0 * 3 + 1] * cos(q[0 * 3 + 0]) * sin(q[0 * 3 + 1]) * + sin(q[0 * 3 + 2]) + + RL * l[1 * 3 + 1] * cos(q[0 * 3 + 0]) * sin(q[0 * 3 + 1]) * + sin(q[0 * 3 + 2]))) / + 2.0 + + (cos(q[0 * 3 + 0]) * + (cos(q[0 * 3 + 1]) * sin(q[0 * 3 + 2]) + + cos(q[0 * 3 + 2]) * sin(q[0 * 3 + 1])) * + 1.0 / + pow(pow(cos(q[0 * 3 + 0]), 2.0) * pow(cos(q[0 * 3 + 1]), 2.0) * + pow(sin(q[0 * 3 + 2]), 2.0) + + pow(cos(q[0 * 3 + 0]), 2.0) * pow(cos(q[0 * 3 + 2]), 2.0) * + pow(sin(q[0 * 3 + 1]), 2.0) + + pow(cos(q[0 * 3 + 1]), 2.0) * pow(cos(q[0 * 3 + 2]), 2.0) * + pow(sin(q[0 * 3 + 0]), 2.0) + + pow(cos(q[0 * 3 + 0]), 2.0) * pow(sin(q[0 * 3 + 1]), 2.0) * + pow(sin(q[0 * 3 + 2]), 2.0) + + pow(cos(q[0 * 3 + 1]), 2.0) * pow(sin(q[0 * 3 + 0]), 2.0) * + pow(sin(q[0 * 3 + 2]), 2.0) + + pow(cos(q[0 * 3 + 2]), 2.0) * pow(sin(q[0 * 3 + 0]), 2.0) * + pow(sin(q[0 * 3 + 1]), 2.0) + + pow(sin(q[0 * 3 + 0]), 2.0) * pow(sin(q[0 * 3 + 1]), 2.0) * + pow(sin(q[0 * 3 + 2]), 2.0) + + pow(cos(q[0 * 3 + 0]), 2.0) * pow(cos(q[0 * 3 + 1]), 2.0) * + pow(cos(q[0 * 3 + 2]), 2.0), + 2.0) * + (l[0 * 3 + 0] * cos(q[0 * 3 + 1]) * cos(q[0 * 3 + 2]) * + sin(q[0 * 3 + 0]) - + l[0 * 3 + 2] * cos(q[0 * 3 + 1]) * sin(q[0 * 3 + 0]) * + sin(q[0 * 3 + 2]) - + l[0 * 3 + 2] * cos(q[0 * 3 + 2]) * sin(q[0 * 3 + 0]) * + sin(q[0 * 3 + 1]) - + l[0 * 3 + 0] * sin(q[0 * 3 + 0]) * sin(q[0 * 3 + 1]) * + sin(q[0 * 3 + 2]) + + l[1 * 3 + 0] * pow(cos(q[0 * 3 + 1]), 2.0) * cos(q[0 * 3 + 2]) * + sin(q[0 * 3 + 0]) + + l[1 * 3 + 0] * cos(q[0 * 3 + 2]) * sin(q[0 * 3 + 0]) * + pow(sin(q[0 * 3 + 1]), 2.0) - + l[1 * 3 + 2] * pow(cos(q[0 * 3 + 1]), 2.0) * sin(q[0 * 3 + 0]) * + sin(q[0 * 3 + 2]) - + l[1 * 3 + 2] * sin(q[0 * 3 + 0]) * pow(sin(q[0 * 3 + 1]), 2.0) * + sin(q[0 * 3 + 2]) + + RL * l[0 * 3 + 1] * cos(q[0 * 3 + 0]) * cos(q[0 * 3 + 1]) * + sin(q[0 * 3 + 2]) + + RL * l[0 * 3 + 1] * cos(q[0 * 3 + 0]) * cos(q[0 * 3 + 2]) * + sin(q[0 * 3 + 1]) + + RL * l[1 * 3 + 1] * cos(q[0 * 3 + 0]) * cos(q[0 * 3 + 1]) * + sin(q[0 * 3 + 2]) + + RL * l[1 * 3 + 1] * cos(q[0 * 3 + 0]) * cos(q[0 * 3 + 2]) * + sin(q[0 * 3 + 1]))) / + 2.0)) / + (pow(cos(q[0 * 3 + 0]), 2.0) + pow(sin(q[0 * 3 + 0]), 2.0)) - + (cos(q[0 * 3 + 0]) * + (cos(q[0 * 3 + 1]) * sin(q[0 * 3 + 2]) + + cos(q[0 * 3 + 2]) * sin(q[0 * 3 + 1])) * + (l[2 * 3 + 0] - + (cos(q[0 * 3 + 0]) * + (-l[0 * 3 + 0] * cos(q[0 * 3 + 0]) * cos(q[0 * 3 + 1]) * + cos(q[0 * 3 + 2]) + + l[0 * 3 + 2] * cos(q[0 * 3 + 0]) * cos(q[0 * 3 + 1]) * + sin(q[0 * 3 + 2]) + + l[0 * 3 + 2] * cos(q[0 * 3 + 0]) * cos(q[0 * 3 + 2]) * + sin(q[0 * 3 + 1]) + + l[0 * 3 + 0] * cos(q[0 * 3 + 0]) * sin(q[0 * 3 + 1]) * + sin(q[0 * 3 + 2]) - + l[1 * 3 + 0] * cos(q[0 * 3 + 0]) * pow(cos(q[0 * 3 + 1]), 2.0) * + cos(q[0 * 3 + 2]) - + l[1 * 3 + 0] * cos(q[0 * 3 + 0]) * cos(q[0 * 3 + 2]) * + pow(sin(q[0 * 3 + 1]), 2.0) + + l[1 * 3 + 2] * cos(q[0 * 3 + 0]) * pow(cos(q[0 * 3 + 1]), 2.0) * + sin(q[0 * 3 + 2]) + + l[1 * 3 + 2] * cos(q[0 * 3 + 0]) * pow(sin(q[0 * 3 + 1]), 2.0) * + sin(q[0 * 3 + 2]) + + RL * l[0 * 3 + 1] * cos(q[0 * 3 + 1]) * sin(q[0 * 3 + 0]) * + sin(q[0 * 3 + 2]) + + RL * l[0 * 3 + 1] * cos(q[0 * 3 + 2]) * sin(q[0 * 3 + 0]) * + sin(q[0 * 3 + 1]) + + RL * l[1 * 3 + 1] * cos(q[0 * 3 + 1]) * sin(q[0 * 3 + 0]) * + sin(q[0 * 3 + 2]) + + RL * l[1 * 3 + 1] * cos(q[0 * 3 + 2]) * sin(q[0 * 3 + 0]) * + sin(q[0 * 3 + 1]))) / + ((pow(cos(q[0 * 3 + 0]), 2.0) + pow(sin(q[0 * 3 + 0]), 2.0)) * + (pow(cos(q[0 * 3 + 0]), 2.0) * pow(cos(q[0 * 3 + 1]), 2.0) * + pow(sin(q[0 * 3 + 2]), 2.0) + + pow(cos(q[0 * 3 + 0]), 2.0) * pow(cos(q[0 * 3 + 2]), 2.0) * + pow(sin(q[0 * 3 + 1]), 2.0) + + pow(cos(q[0 * 3 + 1]), 2.0) * pow(cos(q[0 * 3 + 2]), 2.0) * + pow(sin(q[0 * 3 + 0]), 2.0) + + pow(cos(q[0 * 3 + 0]), 2.0) * pow(sin(q[0 * 3 + 1]), 2.0) * + pow(sin(q[0 * 3 + 2]), 2.0) + + pow(cos(q[0 * 3 + 1]), 2.0) * pow(sin(q[0 * 3 + 0]), 2.0) * + pow(sin(q[0 * 3 + 2]), 2.0) + + pow(cos(q[0 * 3 + 2]), 2.0) * pow(sin(q[0 * 3 + 0]), 2.0) * + pow(sin(q[0 * 3 + 1]), 2.0) + + pow(sin(q[0 * 3 + 0]), 2.0) * pow(sin(q[0 * 3 + 1]), 2.0) * + pow(sin(q[0 * 3 + 2]), 2.0) + + pow(cos(q[0 * 3 + 0]), 2.0) * pow(cos(q[0 * 3 + 1]), 2.0) * + pow(cos(q[0 * 3 + 2]), 2.0)) * + 2.0) + + (sin(q[0 * 3 + 0]) * + (l[0 * 3 + 0] * cos(q[0 * 3 + 1]) * cos(q[0 * 3 + 2]) * + sin(q[0 * 3 + 0]) - + l[0 * 3 + 2] * cos(q[0 * 3 + 1]) * sin(q[0 * 3 + 0]) * + sin(q[0 * 3 + 2]) - + l[0 * 3 + 2] * cos(q[0 * 3 + 2]) * sin(q[0 * 3 + 0]) * + sin(q[0 * 3 + 1]) - + l[0 * 3 + 0] * sin(q[0 * 3 + 0]) * sin(q[0 * 3 + 1]) * + sin(q[0 * 3 + 2]) + + l[1 * 3 + 0] * pow(cos(q[0 * 3 + 1]), 2.0) * cos(q[0 * 3 + 2]) * + sin(q[0 * 3 + 0]) + + l[1 * 3 + 0] * cos(q[0 * 3 + 2]) * sin(q[0 * 3 + 0]) * + pow(sin(q[0 * 3 + 1]), 2.0) - + l[1 * 3 + 2] * pow(cos(q[0 * 3 + 1]), 2.0) * sin(q[0 * 3 + 0]) * + sin(q[0 * 3 + 2]) - + l[1 * 3 + 2] * sin(q[0 * 3 + 0]) * pow(sin(q[0 * 3 + 1]), 2.0) * + sin(q[0 * 3 + 2]) + + RL * l[0 * 3 + 1] * cos(q[0 * 3 + 0]) * cos(q[0 * 3 + 1]) * + sin(q[0 * 3 + 2]) + + RL * l[0 * 3 + 1] * cos(q[0 * 3 + 0]) * cos(q[0 * 3 + 2]) * + sin(q[0 * 3 + 1]) + + RL * l[1 * 3 + 1] * cos(q[0 * 3 + 0]) * cos(q[0 * 3 + 1]) * + sin(q[0 * 3 + 2]) + + RL * l[1 * 3 + 1] * cos(q[0 * 3 + 0]) * cos(q[0 * 3 + 2]) * + sin(q[0 * 3 + 1]))) / + ((pow(cos(q[0 * 3 + 0]), 2.0) + pow(sin(q[0 * 3 + 0]), 2.0)) * + (pow(cos(q[0 * 3 + 0]), 2.0) * pow(cos(q[0 * 3 + 1]), 2.0) * + pow(sin(q[0 * 3 + 2]), 2.0) + + pow(cos(q[0 * 3 + 0]), 2.0) * pow(cos(q[0 * 3 + 2]), 2.0) * + pow(sin(q[0 * 3 + 1]), 2.0) + + pow(cos(q[0 * 3 + 1]), 2.0) * pow(cos(q[0 * 3 + 2]), 2.0) * + pow(sin(q[0 * 3 + 0]), 2.0) + + pow(cos(q[0 * 3 + 0]), 2.0) * pow(sin(q[0 * 3 + 1]), 2.0) * + pow(sin(q[0 * 3 + 2]), 2.0) + + pow(cos(q[0 * 3 + 1]), 2.0) * pow(sin(q[0 * 3 + 0]), 2.0) * + pow(sin(q[0 * 3 + 2]), 2.0) + + pow(cos(q[0 * 3 + 2]), 2.0) * pow(sin(q[0 * 3 + 0]), 2.0) * + pow(sin(q[0 * 3 + 1]), 2.0) + + pow(sin(q[0 * 3 + 0]), 2.0) * pow(sin(q[0 * 3 + 1]), 2.0) * + pow(sin(q[0 * 3 + 2]), 2.0) + + pow(cos(q[0 * 3 + 0]), 2.0) * pow(cos(q[0 * 3 + 1]), 2.0) * + pow(cos(q[0 * 3 + 2]), 2.0)) * + 2.0) - + ((cos(q[0 * 3 + 1]) * sin(q[0 * 3 + 2]) + + cos(q[0 * 3 + 2]) * sin(q[0 * 3 + 1])) * + (l[0 * 3 + 2] * pow(sin(q[0 * 3 + 1]), 2.0) + + l[1 * 3 + 2] * cos(q[0 * 3 + 1]) - + l[1 * 3 + 0] * sin(q[0 * 3 + 1]) + + l[0 * 3 + 2] * pow(cos(q[0 * 3 + 1]), 2.0))) / + ((pow(cos(q[0 * 3 + 1]), 2.0) + pow(sin(q[0 * 3 + 1]), 2.0)) * + (pow(cos(q[0 * 3 + 1]), 2.0) * pow(cos(q[0 * 3 + 2]), 2.0) + + pow(cos(q[0 * 3 + 1]), 2.0) * pow(sin(q[0 * 3 + 2]), 2.0) + + pow(cos(q[0 * 3 + 2]), 2.0) * pow(sin(q[0 * 3 + 1]), 2.0) + + pow(sin(q[0 * 3 + 1]), 2.0) * pow(sin(q[0 * 3 + 2]), 2.0)) * + 2.0) + + (pow(cos(q[0 * 3 + 0]), 2.0) * + (cos(q[0 * 3 + 1]) * cos(q[0 * 3 + 2]) - + sin(q[0 * 3 + 1]) * sin(q[0 * 3 + 2])) * + (l[0 * 3 + 0] * pow(sin(q[0 * 3 + 1]), 2.0) + + l[1 * 3 + 0] * cos(q[0 * 3 + 1]) + + l[1 * 3 + 2] * sin(q[0 * 3 + 1]) + + l[0 * 3 + 0] * pow(cos(q[0 * 3 + 1]), 2.0))) / + ((pow(cos(q[0 * 3 + 0]), 2.0) * pow(cos(q[0 * 3 + 1]), 2.0) + + pow(cos(q[0 * 3 + 0]), 2.0) * pow(sin(q[0 * 3 + 1]), 2.0) + + pow(cos(q[0 * 3 + 1]), 2.0) * pow(sin(q[0 * 3 + 0]), 2.0) + + pow(sin(q[0 * 3 + 0]), 2.0) * pow(sin(q[0 * 3 + 1]), 2.0)) * + (pow(cos(q[0 * 3 + 0]), 2.0) * pow(cos(q[0 * 3 + 1]), 2.0) * + pow(sin(q[0 * 3 + 2]), 2.0) + + pow(cos(q[0 * 3 + 0]), 2.0) * pow(cos(q[0 * 3 + 2]), 2.0) * + pow(sin(q[0 * 3 + 1]), 2.0) + + pow(cos(q[0 * 3 + 1]), 2.0) * pow(cos(q[0 * 3 + 2]), 2.0) * + pow(sin(q[0 * 3 + 0]), 2.0) + + pow(cos(q[0 * 3 + 0]), 2.0) * pow(sin(q[0 * 3 + 1]), 2.0) * + pow(sin(q[0 * 3 + 2]), 2.0) + + pow(cos(q[0 * 3 + 1]), 2.0) * pow(sin(q[0 * 3 + 0]), 2.0) * + pow(sin(q[0 * 3 + 2]), 2.0) + + pow(cos(q[0 * 3 + 2]), 2.0) * pow(sin(q[0 * 3 + 0]), 2.0) * + pow(sin(q[0 * 3 + 1]), 2.0) + + pow(sin(q[0 * 3 + 0]), 2.0) * pow(sin(q[0 * 3 + 1]), 2.0) * + pow(sin(q[0 * 3 + 2]), 2.0) + + pow(cos(q[0 * 3 + 0]), 2.0) * pow(cos(q[0 * 3 + 1]), 2.0) * + pow(cos(q[0 * 3 + 2]), 2.0)) * + 2.0) + + (pow(sin(q[0 * 3 + 0]), 2.0) * + (cos(q[0 * 3 + 1]) * cos(q[0 * 3 + 2]) - + sin(q[0 * 3 + 1]) * sin(q[0 * 3 + 2])) * + (l[0 * 3 + 0] * pow(sin(q[0 * 3 + 1]), 2.0) + + l[1 * 3 + 0] * cos(q[0 * 3 + 1]) + + l[1 * 3 + 2] * sin(q[0 * 3 + 1]) + + l[0 * 3 + 0] * pow(cos(q[0 * 3 + 1]), 2.0))) / + ((pow(cos(q[0 * 3 + 0]), 2.0) * pow(cos(q[0 * 3 + 1]), 2.0) + + pow(cos(q[0 * 3 + 0]), 2.0) * pow(sin(q[0 * 3 + 1]), 2.0) + + pow(cos(q[0 * 3 + 1]), 2.0) * pow(sin(q[0 * 3 + 0]), 2.0) + + pow(sin(q[0 * 3 + 0]), 2.0) * pow(sin(q[0 * 3 + 1]), 2.0)) * + (pow(cos(q[0 * 3 + 0]), 2.0) * pow(cos(q[0 * 3 + 1]), 2.0) * + pow(sin(q[0 * 3 + 2]), 2.0) + + pow(cos(q[0 * 3 + 0]), 2.0) * pow(cos(q[0 * 3 + 2]), 2.0) * + pow(sin(q[0 * 3 + 1]), 2.0) + + pow(cos(q[0 * 3 + 1]), 2.0) * pow(cos(q[0 * 3 + 2]), 2.0) * + pow(sin(q[0 * 3 + 0]), 2.0) + + pow(cos(q[0 * 3 + 0]), 2.0) * pow(sin(q[0 * 3 + 1]), 2.0) * + pow(sin(q[0 * 3 + 2]), 2.0) + + pow(cos(q[0 * 3 + 1]), 2.0) * pow(sin(q[0 * 3 + 0]), 2.0) * + pow(sin(q[0 * 3 + 2]), 2.0) + + pow(cos(q[0 * 3 + 2]), 2.0) * pow(sin(q[0 * 3 + 0]), 2.0) * + pow(sin(q[0 * 3 + 1]), 2.0) + + pow(sin(q[0 * 3 + 0]), 2.0) * pow(sin(q[0 * 3 + 1]), 2.0) * + pow(sin(q[0 * 3 + 2]), 2.0) + + pow(cos(q[0 * 3 + 0]), 2.0) * pow(cos(q[0 * 3 + 1]), 2.0) * + pow(cos(q[0 * 3 + 2]), 2.0)) * + 2.0))) / + (pow(cos(q[0 * 3 + 0]), 2.0) * pow(cos(q[0 * 3 + 1]), 2.0) * + pow(sin(q[0 * 3 + 2]), 2.0) + + pow(cos(q[0 * 3 + 0]), 2.0) * pow(cos(q[0 * 3 + 2]), 2.0) * + pow(sin(q[0 * 3 + 1]), 2.0) + + pow(cos(q[0 * 3 + 1]), 2.0) * pow(cos(q[0 * 3 + 2]), 2.0) * + pow(sin(q[0 * 3 + 0]), 2.0) + + pow(cos(q[0 * 3 + 0]), 2.0) * pow(sin(q[0 * 3 + 1]), 2.0) * + pow(sin(q[0 * 3 + 2]), 2.0) + + pow(cos(q[0 * 3 + 1]), 2.0) * pow(sin(q[0 * 3 + 0]), 2.0) * + pow(sin(q[0 * 3 + 2]), 2.0) + + pow(cos(q[0 * 3 + 2]), 2.0) * pow(sin(q[0 * 3 + 0]), 2.0) * + pow(sin(q[0 * 3 + 1]), 2.0) + + pow(sin(q[0 * 3 + 0]), 2.0) * pow(sin(q[0 * 3 + 1]), 2.0) * + pow(sin(q[0 * 3 + 2]), 2.0) + + pow(cos(q[0 * 3 + 0]), 2.0) * pow(cos(q[0 * 3 + 1]), 2.0) * + pow(cos(q[0 * 3 + 2]), 2.0)) + + (cos(q[0 * 3 + 0]) * + (cos(q[0 * 3 + 1]) * cos(q[0 * 3 + 2]) - + sin(q[0 * 3 + 1]) * sin(q[0 * 3 + 2])) * + (l[2 * 3 + 2] + + (cos(q[0 * 3 + 0]) * + (l[0 * 3 + 2] * cos(q[0 * 3 + 0]) * cos(q[0 * 3 + 1]) * + cos(q[0 * 3 + 2]) + + l[0 * 3 + 0] * cos(q[0 * 3 + 0]) * cos(q[0 * 3 + 1]) * + sin(q[0 * 3 + 2]) + + l[0 * 3 + 0] * cos(q[0 * 3 + 0]) * cos(q[0 * 3 + 2]) * + sin(q[0 * 3 + 1]) - + l[0 * 3 + 2] * cos(q[0 * 3 + 0]) * sin(q[0 * 3 + 1]) * + sin(q[0 * 3 + 2]) + + l[1 * 3 + 2] * cos(q[0 * 3 + 0]) * pow(cos(q[0 * 3 + 1]), 2.0) * + cos(q[0 * 3 + 2]) + + l[1 * 3 + 0] * cos(q[0 * 3 + 0]) * pow(cos(q[0 * 3 + 1]), 2.0) * + sin(q[0 * 3 + 2]) + + l[1 * 3 + 2] * cos(q[0 * 3 + 0]) * cos(q[0 * 3 + 2]) * + pow(sin(q[0 * 3 + 1]), 2.0) + + l[1 * 3 + 0] * cos(q[0 * 3 + 0]) * pow(sin(q[0 * 3 + 1]), 2.0) * + sin(q[0 * 3 + 2]) + + RL * l[0 * 3 + 1] * cos(q[0 * 3 + 1]) * cos(q[0 * 3 + 2]) * + sin(q[0 * 3 + 0]) + + RL * l[1 * 3 + 1] * cos(q[0 * 3 + 1]) * cos(q[0 * 3 + 2]) * + sin(q[0 * 3 + 0]) - + RL * l[0 * 3 + 1] * sin(q[0 * 3 + 0]) * sin(q[0 * 3 + 1]) * + sin(q[0 * 3 + 2]) - + RL * l[1 * 3 + 1] * sin(q[0 * 3 + 0]) * sin(q[0 * 3 + 1]) * + sin(q[0 * 3 + 2]))) / + ((pow(cos(q[0 * 3 + 0]), 2.0) + pow(sin(q[0 * 3 + 0]), 2.0)) * + (pow(cos(q[0 * 3 + 0]), 2.0) * pow(cos(q[0 * 3 + 1]), 2.0) * + pow(sin(q[0 * 3 + 2]), 2.0) + + pow(cos(q[0 * 3 + 0]), 2.0) * pow(cos(q[0 * 3 + 2]), 2.0) * + pow(sin(q[0 * 3 + 1]), 2.0) + + pow(cos(q[0 * 3 + 1]), 2.0) * pow(cos(q[0 * 3 + 2]), 2.0) * + pow(sin(q[0 * 3 + 0]), 2.0) + + pow(cos(q[0 * 3 + 0]), 2.0) * pow(sin(q[0 * 3 + 1]), 2.0) * + pow(sin(q[0 * 3 + 2]), 2.0) + + pow(cos(q[0 * 3 + 1]), 2.0) * pow(sin(q[0 * 3 + 0]), 2.0) * + pow(sin(q[0 * 3 + 2]), 2.0) + + pow(cos(q[0 * 3 + 2]), 2.0) * pow(sin(q[0 * 3 + 0]), 2.0) * + pow(sin(q[0 * 3 + 1]), 2.0) + + pow(sin(q[0 * 3 + 0]), 2.0) * pow(sin(q[0 * 3 + 1]), 2.0) * + pow(sin(q[0 * 3 + 2]), 2.0) + + pow(cos(q[0 * 3 + 0]), 2.0) * pow(cos(q[0 * 3 + 1]), 2.0) * + pow(cos(q[0 * 3 + 2]), 2.0)) * + 2.0) + + (sin(q[0 * 3 + 0]) * + (l[0 * 3 + 2] * cos(q[0 * 3 + 1]) * cos(q[0 * 3 + 2]) * + sin(q[0 * 3 + 0]) + + l[0 * 3 + 0] * cos(q[0 * 3 + 1]) * sin(q[0 * 3 + 0]) * + sin(q[0 * 3 + 2]) + + l[0 * 3 + 0] * cos(q[0 * 3 + 2]) * sin(q[0 * 3 + 0]) * + sin(q[0 * 3 + 1]) - + l[0 * 3 + 2] * sin(q[0 * 3 + 0]) * sin(q[0 * 3 + 1]) * + sin(q[0 * 3 + 2]) + + l[1 * 3 + 2] * pow(cos(q[0 * 3 + 1]), 2.0) * cos(q[0 * 3 + 2]) * + sin(q[0 * 3 + 0]) + + l[1 * 3 + 0] * pow(cos(q[0 * 3 + 1]), 2.0) * sin(q[0 * 3 + 0]) * + sin(q[0 * 3 + 2]) + + l[1 * 3 + 2] * cos(q[0 * 3 + 2]) * sin(q[0 * 3 + 0]) * + pow(sin(q[0 * 3 + 1]), 2.0) + + l[1 * 3 + 0] * sin(q[0 * 3 + 0]) * pow(sin(q[0 * 3 + 1]), 2.0) * + sin(q[0 * 3 + 2]) - + RL * l[0 * 3 + 1] * cos(q[0 * 3 + 0]) * cos(q[0 * 3 + 1]) * + cos(q[0 * 3 + 2]) - + RL * l[1 * 3 + 1] * cos(q[0 * 3 + 0]) * cos(q[0 * 3 + 1]) * + cos(q[0 * 3 + 2]) + + RL * l[0 * 3 + 1] * cos(q[0 * 3 + 0]) * sin(q[0 * 3 + 1]) * + sin(q[0 * 3 + 2]) + + RL * l[1 * 3 + 1] * cos(q[0 * 3 + 0]) * sin(q[0 * 3 + 1]) * + sin(q[0 * 3 + 2]))) / + ((pow(cos(q[0 * 3 + 0]), 2.0) + pow(sin(q[0 * 3 + 0]), 2.0)) * + (pow(cos(q[0 * 3 + 0]), 2.0) * pow(cos(q[0 * 3 + 1]), 2.0) * + pow(sin(q[0 * 3 + 2]), 2.0) + + pow(cos(q[0 * 3 + 0]), 2.0) * pow(cos(q[0 * 3 + 2]), 2.0) * + pow(sin(q[0 * 3 + 1]), 2.0) + + pow(cos(q[0 * 3 + 1]), 2.0) * pow(cos(q[0 * 3 + 2]), 2.0) * + pow(sin(q[0 * 3 + 0]), 2.0) + + pow(cos(q[0 * 3 + 0]), 2.0) * pow(sin(q[0 * 3 + 1]), 2.0) * + pow(sin(q[0 * 3 + 2]), 2.0) + + pow(cos(q[0 * 3 + 1]), 2.0) * pow(sin(q[0 * 3 + 0]), 2.0) * + pow(sin(q[0 * 3 + 2]), 2.0) + + pow(cos(q[0 * 3 + 2]), 2.0) * pow(sin(q[0 * 3 + 0]), 2.0) * + pow(sin(q[0 * 3 + 1]), 2.0) + + pow(sin(q[0 * 3 + 0]), 2.0) * pow(sin(q[0 * 3 + 1]), 2.0) * + pow(sin(q[0 * 3 + 2]), 2.0) + + pow(cos(q[0 * 3 + 0]), 2.0) * pow(cos(q[0 * 3 + 1]), 2.0) * + pow(cos(q[0 * 3 + 2]), 2.0)) * + 2.0) + + ((cos(q[0 * 3 + 1]) * cos(q[0 * 3 + 2]) - + sin(q[0 * 3 + 1]) * sin(q[0 * 3 + 2])) * + (l[0 * 3 + 2] * pow(sin(q[0 * 3 + 1]), 2.0) + + l[1 * 3 + 2] * cos(q[0 * 3 + 1]) - + l[1 * 3 + 0] * sin(q[0 * 3 + 1]) + + l[0 * 3 + 2] * pow(cos(q[0 * 3 + 1]), 2.0))) / + ((pow(cos(q[0 * 3 + 1]), 2.0) + pow(sin(q[0 * 3 + 1]), 2.0)) * + (pow(cos(q[0 * 3 + 1]), 2.0) * pow(cos(q[0 * 3 + 2]), 2.0) + + pow(cos(q[0 * 3 + 1]), 2.0) * pow(sin(q[0 * 3 + 2]), 2.0) + + pow(cos(q[0 * 3 + 2]), 2.0) * pow(sin(q[0 * 3 + 1]), 2.0) + + pow(sin(q[0 * 3 + 1]), 2.0) * pow(sin(q[0 * 3 + 2]), 2.0)) * + 2.0) + + (pow(cos(q[0 * 3 + 0]), 2.0) * + (cos(q[0 * 3 + 1]) * sin(q[0 * 3 + 2]) + + cos(q[0 * 3 + 2]) * sin(q[0 * 3 + 1])) * + (l[0 * 3 + 0] * pow(sin(q[0 * 3 + 1]), 2.0) + + l[1 * 3 + 0] * cos(q[0 * 3 + 1]) + + l[1 * 3 + 2] * sin(q[0 * 3 + 1]) + + l[0 * 3 + 0] * pow(cos(q[0 * 3 + 1]), 2.0))) / + ((pow(cos(q[0 * 3 + 0]), 2.0) * pow(cos(q[0 * 3 + 1]), 2.0) + + pow(cos(q[0 * 3 + 0]), 2.0) * pow(sin(q[0 * 3 + 1]), 2.0) + + pow(cos(q[0 * 3 + 1]), 2.0) * pow(sin(q[0 * 3 + 0]), 2.0) + + pow(sin(q[0 * 3 + 0]), 2.0) * pow(sin(q[0 * 3 + 1]), 2.0)) * + (pow(cos(q[0 * 3 + 0]), 2.0) * pow(cos(q[0 * 3 + 1]), 2.0) * + pow(sin(q[0 * 3 + 2]), 2.0) + + pow(cos(q[0 * 3 + 0]), 2.0) * pow(cos(q[0 * 3 + 2]), 2.0) * + pow(sin(q[0 * 3 + 1]), 2.0) + + pow(cos(q[0 * 3 + 1]), 2.0) * pow(cos(q[0 * 3 + 2]), 2.0) * + pow(sin(q[0 * 3 + 0]), 2.0) + + pow(cos(q[0 * 3 + 0]), 2.0) * pow(sin(q[0 * 3 + 1]), 2.0) * + pow(sin(q[0 * 3 + 2]), 2.0) + + pow(cos(q[0 * 3 + 1]), 2.0) * pow(sin(q[0 * 3 + 0]), 2.0) * + pow(sin(q[0 * 3 + 2]), 2.0) + + pow(cos(q[0 * 3 + 2]), 2.0) * pow(sin(q[0 * 3 + 0]), 2.0) * + pow(sin(q[0 * 3 + 1]), 2.0) + + pow(sin(q[0 * 3 + 0]), 2.0) * pow(sin(q[0 * 3 + 1]), 2.0) * + pow(sin(q[0 * 3 + 2]), 2.0) + + pow(cos(q[0 * 3 + 0]), 2.0) * pow(cos(q[0 * 3 + 1]), 2.0) * + pow(cos(q[0 * 3 + 2]), 2.0)) * + 2.0) + + (pow(sin(q[0 * 3 + 0]), 2.0) * + (cos(q[0 * 3 + 1]) * sin(q[0 * 3 + 2]) + + cos(q[0 * 3 + 2]) * sin(q[0 * 3 + 1])) * + (l[0 * 3 + 0] * pow(sin(q[0 * 3 + 1]), 2.0) + + l[1 * 3 + 0] * cos(q[0 * 3 + 1]) + + l[1 * 3 + 2] * sin(q[0 * 3 + 1]) + + l[0 * 3 + 0] * pow(cos(q[0 * 3 + 1]), 2.0))) / + ((pow(cos(q[0 * 3 + 0]), 2.0) * pow(cos(q[0 * 3 + 1]), 2.0) + + pow(cos(q[0 * 3 + 0]), 2.0) * pow(sin(q[0 * 3 + 1]), 2.0) + + pow(cos(q[0 * 3 + 1]), 2.0) * pow(sin(q[0 * 3 + 0]), 2.0) + + pow(sin(q[0 * 3 + 0]), 2.0) * pow(sin(q[0 * 3 + 1]), 2.0)) * + (pow(cos(q[0 * 3 + 0]), 2.0) * pow(cos(q[0 * 3 + 1]), 2.0) * + pow(sin(q[0 * 3 + 2]), 2.0) + + pow(cos(q[0 * 3 + 0]), 2.0) * pow(cos(q[0 * 3 + 2]), 2.0) * + pow(sin(q[0 * 3 + 1]), 2.0) + + pow(cos(q[0 * 3 + 1]), 2.0) * pow(cos(q[0 * 3 + 2]), 2.0) * + pow(sin(q[0 * 3 + 0]), 2.0) + + pow(cos(q[0 * 3 + 0]), 2.0) * pow(sin(q[0 * 3 + 1]), 2.0) * + pow(sin(q[0 * 3 + 2]), 2.0) + + pow(cos(q[0 * 3 + 1]), 2.0) * pow(sin(q[0 * 3 + 0]), 2.0) * + pow(sin(q[0 * 3 + 2]), 2.0) + + pow(cos(q[0 * 3 + 2]), 2.0) * pow(sin(q[0 * 3 + 0]), 2.0) * + pow(sin(q[0 * 3 + 1]), 2.0) + + pow(sin(q[0 * 3 + 0]), 2.0) * pow(sin(q[0 * 3 + 1]), 2.0) * + pow(sin(q[0 * 3 + 2]), 2.0) + + pow(cos(q[0 * 3 + 0]), 2.0) * pow(cos(q[0 * 3 + 1]), 2.0) * + pow(cos(q[0 * 3 + 2]), 2.0)) * + 2.0))) / + (pow(cos(q[0 * 3 + 0]), 2.0) * pow(cos(q[0 * 3 + 1]), 2.0) * + pow(sin(q[0 * 3 + 2]), 2.0) + + pow(cos(q[0 * 3 + 0]), 2.0) * pow(cos(q[0 * 3 + 2]), 2.0) * + pow(sin(q[0 * 3 + 1]), 2.0) + + pow(cos(q[0 * 3 + 1]), 2.0) * pow(cos(q[0 * 3 + 2]), 2.0) * + pow(sin(q[0 * 3 + 0]), 2.0) + + pow(cos(q[0 * 3 + 0]), 2.0) * pow(sin(q[0 * 3 + 1]), 2.0) * + pow(sin(q[0 * 3 + 2]), 2.0) + + pow(cos(q[0 * 3 + 1]), 2.0) * pow(sin(q[0 * 3 + 0]), 2.0) * + pow(sin(q[0 * 3 + 2]), 2.0) + + pow(cos(q[0 * 3 + 2]), 2.0) * pow(sin(q[0 * 3 + 0]), 2.0) * + pow(sin(q[0 * 3 + 1]), 2.0) + + pow(sin(q[0 * 3 + 0]), 2.0) * pow(sin(q[0 * 3 + 1]), 2.0) * + pow(sin(q[0 * 3 + 2]), 2.0) + + pow(cos(q[0 * 3 + 0]), 2.0) * pow(cos(q[0 * 3 + 1]), 2.0) * + pow(cos(q[0 * 3 + 2]), 2.0)))) / + (pow(cos(q[0 * 3 + 0]), 2.0) + pow(sin(q[0 * 3 + 0]), 2.0)) - + (sin(q[0 * 3 + 0]) * + ((cos(q[0 * 3 + 0]) * + (RL * l[2 * 3 + 1] + + ((cos(q[0 * 3 + 1]) * sin(q[0 * 3 + 2]) + + cos(q[0 * 3 + 2]) * sin(q[0 * 3 + 1])) * + (RL * l[0 * 3 + 1] * cos(q[0 * 3 + 1]) * sin(q[0 * 3 + 2]) + + RL * l[0 * 3 + 1] * cos(q[0 * 3 + 2]) * sin(q[0 * 3 + 1]) + + RL * l[1 * 3 + 1] * cos(q[0 * 3 + 1]) * sin(q[0 * 3 + 2]) + + RL * l[1 * 3 + 1] * cos(q[0 * 3 + 2]) * sin(q[0 * 3 + 1])) * + 1.0 / + pow(pow(cos(q[0 * 3 + 1]), 2.0) * pow(cos(q[0 * 3 + 2]), 2.0) + + pow(cos(q[0 * 3 + 1]), 2.0) * pow(sin(q[0 * 3 + 2]), 2.0) + + pow(cos(q[0 * 3 + 2]), 2.0) * pow(sin(q[0 * 3 + 1]), 2.0) + + pow(sin(q[0 * 3 + 1]), 2.0) * pow(sin(q[0 * 3 + 2]), 2.0), + 2.0)) / + 2.0 + + ((cos(q[0 * 3 + 1]) * cos(q[0 * 3 + 2]) - + sin(q[0 * 3 + 1]) * sin(q[0 * 3 + 2])) * + (RL * l[0 * 3 + 1] * cos(q[0 * 3 + 1]) * cos(q[0 * 3 + 2]) + + RL * l[1 * 3 + 1] * cos(q[0 * 3 + 1]) * cos(q[0 * 3 + 2]) - + RL * l[0 * 3 + 1] * sin(q[0 * 3 + 1]) * sin(q[0 * 3 + 2]) - + RL * l[1 * 3 + 1] * sin(q[0 * 3 + 1]) * sin(q[0 * 3 + 2])) * + 1.0 / + pow(pow(cos(q[0 * 3 + 1]), 2.0) * pow(cos(q[0 * 3 + 2]), 2.0) + + pow(cos(q[0 * 3 + 1]), 2.0) * pow(sin(q[0 * 3 + 2]), 2.0) + + pow(cos(q[0 * 3 + 2]), 2.0) * pow(sin(q[0 * 3 + 1]), 2.0) + + pow(sin(q[0 * 3 + 1]), 2.0) * pow(sin(q[0 * 3 + 2]), 2.0), + 2.0)) / + 2.0 + + (sin(q[0 * 3 + 0]) * + (cos(q[0 * 3 + 1]) * sin(q[0 * 3 + 2]) + + cos(q[0 * 3 + 2]) * sin(q[0 * 3 + 1])) * + 1.0 / + pow(pow(cos(q[0 * 3 + 0]), 2.0) * pow(cos(q[0 * 3 + 1]), 2.0) * + pow(sin(q[0 * 3 + 2]), 2.0) + + pow(cos(q[0 * 3 + 0]), 2.0) * pow(cos(q[0 * 3 + 2]), 2.0) * + pow(sin(q[0 * 3 + 1]), 2.0) + + pow(cos(q[0 * 3 + 1]), 2.0) * pow(cos(q[0 * 3 + 2]), 2.0) * + pow(sin(q[0 * 3 + 0]), 2.0) + + pow(cos(q[0 * 3 + 0]), 2.0) * pow(sin(q[0 * 3 + 1]), 2.0) * + pow(sin(q[0 * 3 + 2]), 2.0) + + pow(cos(q[0 * 3 + 1]), 2.0) * pow(sin(q[0 * 3 + 0]), 2.0) * + pow(sin(q[0 * 3 + 2]), 2.0) + + pow(cos(q[0 * 3 + 2]), 2.0) * pow(sin(q[0 * 3 + 0]), 2.0) * + pow(sin(q[0 * 3 + 1]), 2.0) + + pow(sin(q[0 * 3 + 0]), 2.0) * pow(sin(q[0 * 3 + 1]), 2.0) * + pow(sin(q[0 * 3 + 2]), 2.0) + + pow(cos(q[0 * 3 + 0]), 2.0) * pow(cos(q[0 * 3 + 1]), 2.0) * + pow(cos(q[0 * 3 + 2]), 2.0), + 2.0) * + (-l[0 * 3 + 0] * cos(q[0 * 3 + 0]) * cos(q[0 * 3 + 1]) * + cos(q[0 * 3 + 2]) + + l[0 * 3 + 2] * cos(q[0 * 3 + 0]) * cos(q[0 * 3 + 1]) * + sin(q[0 * 3 + 2]) + + l[0 * 3 + 2] * cos(q[0 * 3 + 0]) * cos(q[0 * 3 + 2]) * + sin(q[0 * 3 + 1]) + + l[0 * 3 + 0] * cos(q[0 * 3 + 0]) * sin(q[0 * 3 + 1]) * + sin(q[0 * 3 + 2]) - + l[1 * 3 + 0] * cos(q[0 * 3 + 0]) * pow(cos(q[0 * 3 + 1]), 2.0) * + cos(q[0 * 3 + 2]) - + l[1 * 3 + 0] * cos(q[0 * 3 + 0]) * cos(q[0 * 3 + 2]) * + pow(sin(q[0 * 3 + 1]), 2.0) + + l[1 * 3 + 2] * cos(q[0 * 3 + 0]) * pow(cos(q[0 * 3 + 1]), 2.0) * + sin(q[0 * 3 + 2]) + + l[1 * 3 + 2] * cos(q[0 * 3 + 0]) * pow(sin(q[0 * 3 + 1]), 2.0) * + sin(q[0 * 3 + 2]) + + RL * l[0 * 3 + 1] * cos(q[0 * 3 + 1]) * sin(q[0 * 3 + 0]) * + sin(q[0 * 3 + 2]) + + RL * l[0 * 3 + 1] * cos(q[0 * 3 + 2]) * sin(q[0 * 3 + 0]) * + sin(q[0 * 3 + 1]) + + RL * l[1 * 3 + 1] * cos(q[0 * 3 + 1]) * sin(q[0 * 3 + 0]) * + sin(q[0 * 3 + 2]) + + RL * l[1 * 3 + 1] * cos(q[0 * 3 + 2]) * sin(q[0 * 3 + 0]) * + sin(q[0 * 3 + 1]))) / + 2.0 + + (sin(q[0 * 3 + 0]) * + (cos(q[0 * 3 + 1]) * cos(q[0 * 3 + 2]) - + sin(q[0 * 3 + 1]) * sin(q[0 * 3 + 2])) * + 1.0 / + pow(pow(cos(q[0 * 3 + 0]), 2.0) * pow(cos(q[0 * 3 + 1]), 2.0) * + pow(sin(q[0 * 3 + 2]), 2.0) + + pow(cos(q[0 * 3 + 0]), 2.0) * pow(cos(q[0 * 3 + 2]), 2.0) * + pow(sin(q[0 * 3 + 1]), 2.0) + + pow(cos(q[0 * 3 + 1]), 2.0) * pow(cos(q[0 * 3 + 2]), 2.0) * + pow(sin(q[0 * 3 + 0]), 2.0) + + pow(cos(q[0 * 3 + 0]), 2.0) * pow(sin(q[0 * 3 + 1]), 2.0) * + pow(sin(q[0 * 3 + 2]), 2.0) + + pow(cos(q[0 * 3 + 1]), 2.0) * pow(sin(q[0 * 3 + 0]), 2.0) * + pow(sin(q[0 * 3 + 2]), 2.0) + + pow(cos(q[0 * 3 + 2]), 2.0) * pow(sin(q[0 * 3 + 0]), 2.0) * + pow(sin(q[0 * 3 + 1]), 2.0) + + pow(sin(q[0 * 3 + 0]), 2.0) * pow(sin(q[0 * 3 + 1]), 2.0) * + pow(sin(q[0 * 3 + 2]), 2.0) + + pow(cos(q[0 * 3 + 0]), 2.0) * pow(cos(q[0 * 3 + 1]), 2.0) * + pow(cos(q[0 * 3 + 2]), 2.0), + 2.0) * + (l[0 * 3 + 2] * cos(q[0 * 3 + 0]) * cos(q[0 * 3 + 1]) * + cos(q[0 * 3 + 2]) + + l[0 * 3 + 0] * cos(q[0 * 3 + 0]) * cos(q[0 * 3 + 1]) * + sin(q[0 * 3 + 2]) + + l[0 * 3 + 0] * cos(q[0 * 3 + 0]) * cos(q[0 * 3 + 2]) * + sin(q[0 * 3 + 1]) - + l[0 * 3 + 2] * cos(q[0 * 3 + 0]) * sin(q[0 * 3 + 1]) * + sin(q[0 * 3 + 2]) + + l[1 * 3 + 2] * cos(q[0 * 3 + 0]) * pow(cos(q[0 * 3 + 1]), 2.0) * + cos(q[0 * 3 + 2]) + + l[1 * 3 + 0] * cos(q[0 * 3 + 0]) * pow(cos(q[0 * 3 + 1]), 2.0) * + sin(q[0 * 3 + 2]) + + l[1 * 3 + 2] * cos(q[0 * 3 + 0]) * cos(q[0 * 3 + 2]) * + pow(sin(q[0 * 3 + 1]), 2.0) + + l[1 * 3 + 0] * cos(q[0 * 3 + 0]) * pow(sin(q[0 * 3 + 1]), 2.0) * + sin(q[0 * 3 + 2]) + + RL * l[0 * 3 + 1] * cos(q[0 * 3 + 1]) * cos(q[0 * 3 + 2]) * + sin(q[0 * 3 + 0]) + + RL * l[1 * 3 + 1] * cos(q[0 * 3 + 1]) * cos(q[0 * 3 + 2]) * + sin(q[0 * 3 + 0]) - + RL * l[0 * 3 + 1] * sin(q[0 * 3 + 0]) * sin(q[0 * 3 + 1]) * + sin(q[0 * 3 + 2]) - + RL * l[1 * 3 + 1] * sin(q[0 * 3 + 0]) * sin(q[0 * 3 + 1]) * + sin(q[0 * 3 + 2]))) / + 2.0 - + (cos(q[0 * 3 + 0]) * + (cos(q[0 * 3 + 1]) * cos(q[0 * 3 + 2]) - + sin(q[0 * 3 + 1]) * sin(q[0 * 3 + 2])) * + 1.0 / + pow(pow(cos(q[0 * 3 + 0]), 2.0) * pow(cos(q[0 * 3 + 1]), 2.0) * + pow(sin(q[0 * 3 + 2]), 2.0) + + pow(cos(q[0 * 3 + 0]), 2.0) * pow(cos(q[0 * 3 + 2]), 2.0) * + pow(sin(q[0 * 3 + 1]), 2.0) + + pow(cos(q[0 * 3 + 1]), 2.0) * pow(cos(q[0 * 3 + 2]), 2.0) * + pow(sin(q[0 * 3 + 0]), 2.0) + + pow(cos(q[0 * 3 + 0]), 2.0) * pow(sin(q[0 * 3 + 1]), 2.0) * + pow(sin(q[0 * 3 + 2]), 2.0) + + pow(cos(q[0 * 3 + 1]), 2.0) * pow(sin(q[0 * 3 + 0]), 2.0) * + pow(sin(q[0 * 3 + 2]), 2.0) + + pow(cos(q[0 * 3 + 2]), 2.0) * pow(sin(q[0 * 3 + 0]), 2.0) * + pow(sin(q[0 * 3 + 1]), 2.0) + + pow(sin(q[0 * 3 + 0]), 2.0) * pow(sin(q[0 * 3 + 1]), 2.0) * + pow(sin(q[0 * 3 + 2]), 2.0) + + pow(cos(q[0 * 3 + 0]), 2.0) * pow(cos(q[0 * 3 + 1]), 2.0) * + pow(cos(q[0 * 3 + 2]), 2.0), + 2.0) * + (l[0 * 3 + 2] * cos(q[0 * 3 + 1]) * cos(q[0 * 3 + 2]) * + sin(q[0 * 3 + 0]) + + l[0 * 3 + 0] * cos(q[0 * 3 + 1]) * sin(q[0 * 3 + 0]) * + sin(q[0 * 3 + 2]) + + l[0 * 3 + 0] * cos(q[0 * 3 + 2]) * sin(q[0 * 3 + 0]) * + sin(q[0 * 3 + 1]) - + l[0 * 3 + 2] * sin(q[0 * 3 + 0]) * sin(q[0 * 3 + 1]) * + sin(q[0 * 3 + 2]) + + l[1 * 3 + 2] * pow(cos(q[0 * 3 + 1]), 2.0) * cos(q[0 * 3 + 2]) * + sin(q[0 * 3 + 0]) + + l[1 * 3 + 0] * pow(cos(q[0 * 3 + 1]), 2.0) * sin(q[0 * 3 + 0]) * + sin(q[0 * 3 + 2]) + + l[1 * 3 + 2] * cos(q[0 * 3 + 2]) * sin(q[0 * 3 + 0]) * + pow(sin(q[0 * 3 + 1]), 2.0) + + l[1 * 3 + 0] * sin(q[0 * 3 + 0]) * pow(sin(q[0 * 3 + 1]), 2.0) * + sin(q[0 * 3 + 2]) - + RL * l[0 * 3 + 1] * cos(q[0 * 3 + 0]) * cos(q[0 * 3 + 1]) * + cos(q[0 * 3 + 2]) - + RL * l[1 * 3 + 1] * cos(q[0 * 3 + 0]) * cos(q[0 * 3 + 1]) * + cos(q[0 * 3 + 2]) + + RL * l[0 * 3 + 1] * cos(q[0 * 3 + 0]) * sin(q[0 * 3 + 1]) * + sin(q[0 * 3 + 2]) + + RL * l[1 * 3 + 1] * cos(q[0 * 3 + 0]) * sin(q[0 * 3 + 1]) * + sin(q[0 * 3 + 2]))) / + 2.0 + + (cos(q[0 * 3 + 0]) * + (cos(q[0 * 3 + 1]) * sin(q[0 * 3 + 2]) + + cos(q[0 * 3 + 2]) * sin(q[0 * 3 + 1])) * + 1.0 / + pow(pow(cos(q[0 * 3 + 0]), 2.0) * pow(cos(q[0 * 3 + 1]), 2.0) * + pow(sin(q[0 * 3 + 2]), 2.0) + + pow(cos(q[0 * 3 + 0]), 2.0) * pow(cos(q[0 * 3 + 2]), 2.0) * + pow(sin(q[0 * 3 + 1]), 2.0) + + pow(cos(q[0 * 3 + 1]), 2.0) * pow(cos(q[0 * 3 + 2]), 2.0) * + pow(sin(q[0 * 3 + 0]), 2.0) + + pow(cos(q[0 * 3 + 0]), 2.0) * pow(sin(q[0 * 3 + 1]), 2.0) * + pow(sin(q[0 * 3 + 2]), 2.0) + + pow(cos(q[0 * 3 + 1]), 2.0) * pow(sin(q[0 * 3 + 0]), 2.0) * + pow(sin(q[0 * 3 + 2]), 2.0) + + pow(cos(q[0 * 3 + 2]), 2.0) * pow(sin(q[0 * 3 + 0]), 2.0) * + pow(sin(q[0 * 3 + 1]), 2.0) + + pow(sin(q[0 * 3 + 0]), 2.0) * pow(sin(q[0 * 3 + 1]), 2.0) * + pow(sin(q[0 * 3 + 2]), 2.0) + + pow(cos(q[0 * 3 + 0]), 2.0) * pow(cos(q[0 * 3 + 1]), 2.0) * + pow(cos(q[0 * 3 + 2]), 2.0), + 2.0) * + (l[0 * 3 + 0] * cos(q[0 * 3 + 1]) * cos(q[0 * 3 + 2]) * + sin(q[0 * 3 + 0]) - + l[0 * 3 + 2] * cos(q[0 * 3 + 1]) * sin(q[0 * 3 + 0]) * + sin(q[0 * 3 + 2]) - + l[0 * 3 + 2] * cos(q[0 * 3 + 2]) * sin(q[0 * 3 + 0]) * + sin(q[0 * 3 + 1]) - + l[0 * 3 + 0] * sin(q[0 * 3 + 0]) * sin(q[0 * 3 + 1]) * + sin(q[0 * 3 + 2]) + + l[1 * 3 + 0] * pow(cos(q[0 * 3 + 1]), 2.0) * cos(q[0 * 3 + 2]) * + sin(q[0 * 3 + 0]) + + l[1 * 3 + 0] * cos(q[0 * 3 + 2]) * sin(q[0 * 3 + 0]) * + pow(sin(q[0 * 3 + 1]), 2.0) - + l[1 * 3 + 2] * pow(cos(q[0 * 3 + 1]), 2.0) * sin(q[0 * 3 + 0]) * + sin(q[0 * 3 + 2]) - + l[1 * 3 + 2] * sin(q[0 * 3 + 0]) * pow(sin(q[0 * 3 + 1]), 2.0) * + sin(q[0 * 3 + 2]) + + RL * l[0 * 3 + 1] * cos(q[0 * 3 + 0]) * cos(q[0 * 3 + 1]) * + sin(q[0 * 3 + 2]) + + RL * l[0 * 3 + 1] * cos(q[0 * 3 + 0]) * cos(q[0 * 3 + 2]) * + sin(q[0 * 3 + 1]) + + RL * l[1 * 3 + 1] * cos(q[0 * 3 + 0]) * cos(q[0 * 3 + 1]) * + sin(q[0 * 3 + 2]) + + RL * l[1 * 3 + 1] * cos(q[0 * 3 + 0]) * cos(q[0 * 3 + 2]) * + sin(q[0 * 3 + 1]))) / + 2.0)) / + (pow(cos(q[0 * 3 + 0]), 2.0) + pow(sin(q[0 * 3 + 0]), 2.0)) + + (sin(q[0 * 3 + 0]) * + (cos(q[0 * 3 + 1]) * sin(q[0 * 3 + 2]) + + cos(q[0 * 3 + 2]) * sin(q[0 * 3 + 1])) * + (l[2 * 3 + 0] - + (cos(q[0 * 3 + 0]) * + (-l[0 * 3 + 0] * cos(q[0 * 3 + 0]) * cos(q[0 * 3 + 1]) * + cos(q[0 * 3 + 2]) + + l[0 * 3 + 2] * cos(q[0 * 3 + 0]) * cos(q[0 * 3 + 1]) * + sin(q[0 * 3 + 2]) + + l[0 * 3 + 2] * cos(q[0 * 3 + 0]) * cos(q[0 * 3 + 2]) * + sin(q[0 * 3 + 1]) + + l[0 * 3 + 0] * cos(q[0 * 3 + 0]) * sin(q[0 * 3 + 1]) * + sin(q[0 * 3 + 2]) - + l[1 * 3 + 0] * cos(q[0 * 3 + 0]) * pow(cos(q[0 * 3 + 1]), 2.0) * + cos(q[0 * 3 + 2]) - + l[1 * 3 + 0] * cos(q[0 * 3 + 0]) * cos(q[0 * 3 + 2]) * + pow(sin(q[0 * 3 + 1]), 2.0) + + l[1 * 3 + 2] * cos(q[0 * 3 + 0]) * pow(cos(q[0 * 3 + 1]), 2.0) * + sin(q[0 * 3 + 2]) + + l[1 * 3 + 2] * cos(q[0 * 3 + 0]) * pow(sin(q[0 * 3 + 1]), 2.0) * + sin(q[0 * 3 + 2]) + + RL * l[0 * 3 + 1] * cos(q[0 * 3 + 1]) * sin(q[0 * 3 + 0]) * + sin(q[0 * 3 + 2]) + + RL * l[0 * 3 + 1] * cos(q[0 * 3 + 2]) * sin(q[0 * 3 + 0]) * + sin(q[0 * 3 + 1]) + + RL * l[1 * 3 + 1] * cos(q[0 * 3 + 1]) * sin(q[0 * 3 + 0]) * + sin(q[0 * 3 + 2]) + + RL * l[1 * 3 + 1] * cos(q[0 * 3 + 2]) * sin(q[0 * 3 + 0]) * + sin(q[0 * 3 + 1]))) / + ((pow(cos(q[0 * 3 + 0]), 2.0) + pow(sin(q[0 * 3 + 0]), 2.0)) * + (pow(cos(q[0 * 3 + 0]), 2.0) * pow(cos(q[0 * 3 + 1]), 2.0) * + pow(sin(q[0 * 3 + 2]), 2.0) + + pow(cos(q[0 * 3 + 0]), 2.0) * pow(cos(q[0 * 3 + 2]), 2.0) * + pow(sin(q[0 * 3 + 1]), 2.0) + + pow(cos(q[0 * 3 + 1]), 2.0) * pow(cos(q[0 * 3 + 2]), 2.0) * + pow(sin(q[0 * 3 + 0]), 2.0) + + pow(cos(q[0 * 3 + 0]), 2.0) * pow(sin(q[0 * 3 + 1]), 2.0) * + pow(sin(q[0 * 3 + 2]), 2.0) + + pow(cos(q[0 * 3 + 1]), 2.0) * pow(sin(q[0 * 3 + 0]), 2.0) * + pow(sin(q[0 * 3 + 2]), 2.0) + + pow(cos(q[0 * 3 + 2]), 2.0) * pow(sin(q[0 * 3 + 0]), 2.0) * + pow(sin(q[0 * 3 + 1]), 2.0) + + pow(sin(q[0 * 3 + 0]), 2.0) * pow(sin(q[0 * 3 + 1]), 2.0) * + pow(sin(q[0 * 3 + 2]), 2.0) + + pow(cos(q[0 * 3 + 0]), 2.0) * pow(cos(q[0 * 3 + 1]), 2.0) * + pow(cos(q[0 * 3 + 2]), 2.0)) * + 2.0) + + (sin(q[0 * 3 + 0]) * + (l[0 * 3 + 0] * cos(q[0 * 3 + 1]) * cos(q[0 * 3 + 2]) * + sin(q[0 * 3 + 0]) - + l[0 * 3 + 2] * cos(q[0 * 3 + 1]) * sin(q[0 * 3 + 0]) * + sin(q[0 * 3 + 2]) - + l[0 * 3 + 2] * cos(q[0 * 3 + 2]) * sin(q[0 * 3 + 0]) * + sin(q[0 * 3 + 1]) - + l[0 * 3 + 0] * sin(q[0 * 3 + 0]) * sin(q[0 * 3 + 1]) * + sin(q[0 * 3 + 2]) + + l[1 * 3 + 0] * pow(cos(q[0 * 3 + 1]), 2.0) * cos(q[0 * 3 + 2]) * + sin(q[0 * 3 + 0]) + + l[1 * 3 + 0] * cos(q[0 * 3 + 2]) * sin(q[0 * 3 + 0]) * + pow(sin(q[0 * 3 + 1]), 2.0) - + l[1 * 3 + 2] * pow(cos(q[0 * 3 + 1]), 2.0) * sin(q[0 * 3 + 0]) * + sin(q[0 * 3 + 2]) - + l[1 * 3 + 2] * sin(q[0 * 3 + 0]) * pow(sin(q[0 * 3 + 1]), 2.0) * + sin(q[0 * 3 + 2]) + + RL * l[0 * 3 + 1] * cos(q[0 * 3 + 0]) * cos(q[0 * 3 + 1]) * + sin(q[0 * 3 + 2]) + + RL * l[0 * 3 + 1] * cos(q[0 * 3 + 0]) * cos(q[0 * 3 + 2]) * + sin(q[0 * 3 + 1]) + + RL * l[1 * 3 + 1] * cos(q[0 * 3 + 0]) * cos(q[0 * 3 + 1]) * + sin(q[0 * 3 + 2]) + + RL * l[1 * 3 + 1] * cos(q[0 * 3 + 0]) * cos(q[0 * 3 + 2]) * + sin(q[0 * 3 + 1]))) / + ((pow(cos(q[0 * 3 + 0]), 2.0) + pow(sin(q[0 * 3 + 0]), 2.0)) * + (pow(cos(q[0 * 3 + 0]), 2.0) * pow(cos(q[0 * 3 + 1]), 2.0) * + pow(sin(q[0 * 3 + 2]), 2.0) + + pow(cos(q[0 * 3 + 0]), 2.0) * pow(cos(q[0 * 3 + 2]), 2.0) * + pow(sin(q[0 * 3 + 1]), 2.0) + + pow(cos(q[0 * 3 + 1]), 2.0) * pow(cos(q[0 * 3 + 2]), 2.0) * + pow(sin(q[0 * 3 + 0]), 2.0) + + pow(cos(q[0 * 3 + 0]), 2.0) * pow(sin(q[0 * 3 + 1]), 2.0) * + pow(sin(q[0 * 3 + 2]), 2.0) + + pow(cos(q[0 * 3 + 1]), 2.0) * pow(sin(q[0 * 3 + 0]), 2.0) * + pow(sin(q[0 * 3 + 2]), 2.0) + + pow(cos(q[0 * 3 + 2]), 2.0) * pow(sin(q[0 * 3 + 0]), 2.0) * + pow(sin(q[0 * 3 + 1]), 2.0) + + pow(sin(q[0 * 3 + 0]), 2.0) * pow(sin(q[0 * 3 + 1]), 2.0) * + pow(sin(q[0 * 3 + 2]), 2.0) + + pow(cos(q[0 * 3 + 0]), 2.0) * pow(cos(q[0 * 3 + 1]), 2.0) * + pow(cos(q[0 * 3 + 2]), 2.0)) * + 2.0) - + ((cos(q[0 * 3 + 1]) * sin(q[0 * 3 + 2]) + + cos(q[0 * 3 + 2]) * sin(q[0 * 3 + 1])) * + (l[0 * 3 + 2] * pow(sin(q[0 * 3 + 1]), 2.0) + + l[1 * 3 + 2] * cos(q[0 * 3 + 1]) - + l[1 * 3 + 0] * sin(q[0 * 3 + 1]) + + l[0 * 3 + 2] * pow(cos(q[0 * 3 + 1]), 2.0))) / + ((pow(cos(q[0 * 3 + 1]), 2.0) + pow(sin(q[0 * 3 + 1]), 2.0)) * + (pow(cos(q[0 * 3 + 1]), 2.0) * pow(cos(q[0 * 3 + 2]), 2.0) + + pow(cos(q[0 * 3 + 1]), 2.0) * pow(sin(q[0 * 3 + 2]), 2.0) + + pow(cos(q[0 * 3 + 2]), 2.0) * pow(sin(q[0 * 3 + 1]), 2.0) + + pow(sin(q[0 * 3 + 1]), 2.0) * pow(sin(q[0 * 3 + 2]), 2.0)) * + 2.0) + + (pow(cos(q[0 * 3 + 0]), 2.0) * + (cos(q[0 * 3 + 1]) * cos(q[0 * 3 + 2]) - + sin(q[0 * 3 + 1]) * sin(q[0 * 3 + 2])) * + (l[0 * 3 + 0] * pow(sin(q[0 * 3 + 1]), 2.0) + + l[1 * 3 + 0] * cos(q[0 * 3 + 1]) + + l[1 * 3 + 2] * sin(q[0 * 3 + 1]) + + l[0 * 3 + 0] * pow(cos(q[0 * 3 + 1]), 2.0))) / + ((pow(cos(q[0 * 3 + 0]), 2.0) * pow(cos(q[0 * 3 + 1]), 2.0) + + pow(cos(q[0 * 3 + 0]), 2.0) * pow(sin(q[0 * 3 + 1]), 2.0) + + pow(cos(q[0 * 3 + 1]), 2.0) * pow(sin(q[0 * 3 + 0]), 2.0) + + pow(sin(q[0 * 3 + 0]), 2.0) * pow(sin(q[0 * 3 + 1]), 2.0)) * + (pow(cos(q[0 * 3 + 0]), 2.0) * pow(cos(q[0 * 3 + 1]), 2.0) * + pow(sin(q[0 * 3 + 2]), 2.0) + + pow(cos(q[0 * 3 + 0]), 2.0) * pow(cos(q[0 * 3 + 2]), 2.0) * + pow(sin(q[0 * 3 + 1]), 2.0) + + pow(cos(q[0 * 3 + 1]), 2.0) * pow(cos(q[0 * 3 + 2]), 2.0) * + pow(sin(q[0 * 3 + 0]), 2.0) + + pow(cos(q[0 * 3 + 0]), 2.0) * pow(sin(q[0 * 3 + 1]), 2.0) * + pow(sin(q[0 * 3 + 2]), 2.0) + + pow(cos(q[0 * 3 + 1]), 2.0) * pow(sin(q[0 * 3 + 0]), 2.0) * + pow(sin(q[0 * 3 + 2]), 2.0) + + pow(cos(q[0 * 3 + 2]), 2.0) * pow(sin(q[0 * 3 + 0]), 2.0) * + pow(sin(q[0 * 3 + 1]), 2.0) + + pow(sin(q[0 * 3 + 0]), 2.0) * pow(sin(q[0 * 3 + 1]), 2.0) * + pow(sin(q[0 * 3 + 2]), 2.0) + + pow(cos(q[0 * 3 + 0]), 2.0) * pow(cos(q[0 * 3 + 1]), 2.0) * + pow(cos(q[0 * 3 + 2]), 2.0)) * + 2.0) + + (pow(sin(q[0 * 3 + 0]), 2.0) * + (cos(q[0 * 3 + 1]) * cos(q[0 * 3 + 2]) - + sin(q[0 * 3 + 1]) * sin(q[0 * 3 + 2])) * + (l[0 * 3 + 0] * pow(sin(q[0 * 3 + 1]), 2.0) + + l[1 * 3 + 0] * cos(q[0 * 3 + 1]) + + l[1 * 3 + 2] * sin(q[0 * 3 + 1]) + + l[0 * 3 + 0] * pow(cos(q[0 * 3 + 1]), 2.0))) / + ((pow(cos(q[0 * 3 + 0]), 2.0) * pow(cos(q[0 * 3 + 1]), 2.0) + + pow(cos(q[0 * 3 + 0]), 2.0) * pow(sin(q[0 * 3 + 1]), 2.0) + + pow(cos(q[0 * 3 + 1]), 2.0) * pow(sin(q[0 * 3 + 0]), 2.0) + + pow(sin(q[0 * 3 + 0]), 2.0) * pow(sin(q[0 * 3 + 1]), 2.0)) * + (pow(cos(q[0 * 3 + 0]), 2.0) * pow(cos(q[0 * 3 + 1]), 2.0) * + pow(sin(q[0 * 3 + 2]), 2.0) + + pow(cos(q[0 * 3 + 0]), 2.0) * pow(cos(q[0 * 3 + 2]), 2.0) * + pow(sin(q[0 * 3 + 1]), 2.0) + + pow(cos(q[0 * 3 + 1]), 2.0) * pow(cos(q[0 * 3 + 2]), 2.0) * + pow(sin(q[0 * 3 + 0]), 2.0) + + pow(cos(q[0 * 3 + 0]), 2.0) * pow(sin(q[0 * 3 + 1]), 2.0) * + pow(sin(q[0 * 3 + 2]), 2.0) + + pow(cos(q[0 * 3 + 1]), 2.0) * pow(sin(q[0 * 3 + 0]), 2.0) * + pow(sin(q[0 * 3 + 2]), 2.0) + + pow(cos(q[0 * 3 + 2]), 2.0) * pow(sin(q[0 * 3 + 0]), 2.0) * + pow(sin(q[0 * 3 + 1]), 2.0) + + pow(sin(q[0 * 3 + 0]), 2.0) * pow(sin(q[0 * 3 + 1]), 2.0) * + pow(sin(q[0 * 3 + 2]), 2.0) + + pow(cos(q[0 * 3 + 0]), 2.0) * pow(cos(q[0 * 3 + 1]), 2.0) * + pow(cos(q[0 * 3 + 2]), 2.0)) * + 2.0))) / + (pow(cos(q[0 * 3 + 0]), 2.0) * pow(cos(q[0 * 3 + 1]), 2.0) * + pow(sin(q[0 * 3 + 2]), 2.0) + + pow(cos(q[0 * 3 + 0]), 2.0) * pow(cos(q[0 * 3 + 2]), 2.0) * + pow(sin(q[0 * 3 + 1]), 2.0) + + pow(cos(q[0 * 3 + 1]), 2.0) * pow(cos(q[0 * 3 + 2]), 2.0) * + pow(sin(q[0 * 3 + 0]), 2.0) + + pow(cos(q[0 * 3 + 0]), 2.0) * pow(sin(q[0 * 3 + 1]), 2.0) * + pow(sin(q[0 * 3 + 2]), 2.0) + + pow(cos(q[0 * 3 + 1]), 2.0) * pow(sin(q[0 * 3 + 0]), 2.0) * + pow(sin(q[0 * 3 + 2]), 2.0) + + pow(cos(q[0 * 3 + 2]), 2.0) * pow(sin(q[0 * 3 + 0]), 2.0) * + pow(sin(q[0 * 3 + 1]), 2.0) + + pow(sin(q[0 * 3 + 0]), 2.0) * pow(sin(q[0 * 3 + 1]), 2.0) * + pow(sin(q[0 * 3 + 2]), 2.0) + + pow(cos(q[0 * 3 + 0]), 2.0) * pow(cos(q[0 * 3 + 1]), 2.0) * + pow(cos(q[0 * 3 + 2]), 2.0)) - + (sin(q[0 * 3 + 0]) * + (cos(q[0 * 3 + 1]) * cos(q[0 * 3 + 2]) - + sin(q[0 * 3 + 1]) * sin(q[0 * 3 + 2])) * + (l[2 * 3 + 2] + + (cos(q[0 * 3 + 0]) * + (l[0 * 3 + 2] * cos(q[0 * 3 + 0]) * cos(q[0 * 3 + 1]) * + cos(q[0 * 3 + 2]) + + l[0 * 3 + 0] * cos(q[0 * 3 + 0]) * cos(q[0 * 3 + 1]) * + sin(q[0 * 3 + 2]) + + l[0 * 3 + 0] * cos(q[0 * 3 + 0]) * cos(q[0 * 3 + 2]) * + sin(q[0 * 3 + 1]) - + l[0 * 3 + 2] * cos(q[0 * 3 + 0]) * sin(q[0 * 3 + 1]) * + sin(q[0 * 3 + 2]) + + l[1 * 3 + 2] * cos(q[0 * 3 + 0]) * pow(cos(q[0 * 3 + 1]), 2.0) * + cos(q[0 * 3 + 2]) + + l[1 * 3 + 0] * cos(q[0 * 3 + 0]) * pow(cos(q[0 * 3 + 1]), 2.0) * + sin(q[0 * 3 + 2]) + + l[1 * 3 + 2] * cos(q[0 * 3 + 0]) * cos(q[0 * 3 + 2]) * + pow(sin(q[0 * 3 + 1]), 2.0) + + l[1 * 3 + 0] * cos(q[0 * 3 + 0]) * pow(sin(q[0 * 3 + 1]), 2.0) * + sin(q[0 * 3 + 2]) + + RL * l[0 * 3 + 1] * cos(q[0 * 3 + 1]) * cos(q[0 * 3 + 2]) * + sin(q[0 * 3 + 0]) + + RL * l[1 * 3 + 1] * cos(q[0 * 3 + 1]) * cos(q[0 * 3 + 2]) * + sin(q[0 * 3 + 0]) - + RL * l[0 * 3 + 1] * sin(q[0 * 3 + 0]) * sin(q[0 * 3 + 1]) * + sin(q[0 * 3 + 2]) - + RL * l[1 * 3 + 1] * sin(q[0 * 3 + 0]) * sin(q[0 * 3 + 1]) * + sin(q[0 * 3 + 2]))) / + ((pow(cos(q[0 * 3 + 0]), 2.0) + pow(sin(q[0 * 3 + 0]), 2.0)) * + (pow(cos(q[0 * 3 + 0]), 2.0) * pow(cos(q[0 * 3 + 1]), 2.0) * + pow(sin(q[0 * 3 + 2]), 2.0) + + pow(cos(q[0 * 3 + 0]), 2.0) * pow(cos(q[0 * 3 + 2]), 2.0) * + pow(sin(q[0 * 3 + 1]), 2.0) + + pow(cos(q[0 * 3 + 1]), 2.0) * pow(cos(q[0 * 3 + 2]), 2.0) * + pow(sin(q[0 * 3 + 0]), 2.0) + + pow(cos(q[0 * 3 + 0]), 2.0) * pow(sin(q[0 * 3 + 1]), 2.0) * + pow(sin(q[0 * 3 + 2]), 2.0) + + pow(cos(q[0 * 3 + 1]), 2.0) * pow(sin(q[0 * 3 + 0]), 2.0) * + pow(sin(q[0 * 3 + 2]), 2.0) + + pow(cos(q[0 * 3 + 2]), 2.0) * pow(sin(q[0 * 3 + 0]), 2.0) * + pow(sin(q[0 * 3 + 1]), 2.0) + + pow(sin(q[0 * 3 + 0]), 2.0) * pow(sin(q[0 * 3 + 1]), 2.0) * + pow(sin(q[0 * 3 + 2]), 2.0) + + pow(cos(q[0 * 3 + 0]), 2.0) * pow(cos(q[0 * 3 + 1]), 2.0) * + pow(cos(q[0 * 3 + 2]), 2.0)) * + 2.0) + + (sin(q[0 * 3 + 0]) * + (l[0 * 3 + 2] * cos(q[0 * 3 + 1]) * cos(q[0 * 3 + 2]) * + sin(q[0 * 3 + 0]) + + l[0 * 3 + 0] * cos(q[0 * 3 + 1]) * sin(q[0 * 3 + 0]) * + sin(q[0 * 3 + 2]) + + l[0 * 3 + 0] * cos(q[0 * 3 + 2]) * sin(q[0 * 3 + 0]) * + sin(q[0 * 3 + 1]) - + l[0 * 3 + 2] * sin(q[0 * 3 + 0]) * sin(q[0 * 3 + 1]) * + sin(q[0 * 3 + 2]) + + l[1 * 3 + 2] * pow(cos(q[0 * 3 + 1]), 2.0) * cos(q[0 * 3 + 2]) * + sin(q[0 * 3 + 0]) + + l[1 * 3 + 0] * pow(cos(q[0 * 3 + 1]), 2.0) * sin(q[0 * 3 + 0]) * + sin(q[0 * 3 + 2]) + + l[1 * 3 + 2] * cos(q[0 * 3 + 2]) * sin(q[0 * 3 + 0]) * + pow(sin(q[0 * 3 + 1]), 2.0) + + l[1 * 3 + 0] * sin(q[0 * 3 + 0]) * pow(sin(q[0 * 3 + 1]), 2.0) * + sin(q[0 * 3 + 2]) - + RL * l[0 * 3 + 1] * cos(q[0 * 3 + 0]) * cos(q[0 * 3 + 1]) * + cos(q[0 * 3 + 2]) - + RL * l[1 * 3 + 1] * cos(q[0 * 3 + 0]) * cos(q[0 * 3 + 1]) * + cos(q[0 * 3 + 2]) + + RL * l[0 * 3 + 1] * cos(q[0 * 3 + 0]) * sin(q[0 * 3 + 1]) * + sin(q[0 * 3 + 2]) + + RL * l[1 * 3 + 1] * cos(q[0 * 3 + 0]) * sin(q[0 * 3 + 1]) * + sin(q[0 * 3 + 2]))) / + ((pow(cos(q[0 * 3 + 0]), 2.0) + pow(sin(q[0 * 3 + 0]), 2.0)) * + (pow(cos(q[0 * 3 + 0]), 2.0) * pow(cos(q[0 * 3 + 1]), 2.0) * + pow(sin(q[0 * 3 + 2]), 2.0) + + pow(cos(q[0 * 3 + 0]), 2.0) * pow(cos(q[0 * 3 + 2]), 2.0) * + pow(sin(q[0 * 3 + 1]), 2.0) + + pow(cos(q[0 * 3 + 1]), 2.0) * pow(cos(q[0 * 3 + 2]), 2.0) * + pow(sin(q[0 * 3 + 0]), 2.0) + + pow(cos(q[0 * 3 + 0]), 2.0) * pow(sin(q[0 * 3 + 1]), 2.0) * + pow(sin(q[0 * 3 + 2]), 2.0) + + pow(cos(q[0 * 3 + 1]), 2.0) * pow(sin(q[0 * 3 + 0]), 2.0) * + pow(sin(q[0 * 3 + 2]), 2.0) + + pow(cos(q[0 * 3 + 2]), 2.0) * pow(sin(q[0 * 3 + 0]), 2.0) * + pow(sin(q[0 * 3 + 1]), 2.0) + + pow(sin(q[0 * 3 + 0]), 2.0) * pow(sin(q[0 * 3 + 1]), 2.0) * + pow(sin(q[0 * 3 + 2]), 2.0) + + pow(cos(q[0 * 3 + 0]), 2.0) * pow(cos(q[0 * 3 + 1]), 2.0) * + pow(cos(q[0 * 3 + 2]), 2.0)) * + 2.0) + + ((cos(q[0 * 3 + 1]) * cos(q[0 * 3 + 2]) - + sin(q[0 * 3 + 1]) * sin(q[0 * 3 + 2])) * + (l[0 * 3 + 2] * pow(sin(q[0 * 3 + 1]), 2.0) + + l[1 * 3 + 2] * cos(q[0 * 3 + 1]) - + l[1 * 3 + 0] * sin(q[0 * 3 + 1]) + + l[0 * 3 + 2] * pow(cos(q[0 * 3 + 1]), 2.0))) / + ((pow(cos(q[0 * 3 + 1]), 2.0) + pow(sin(q[0 * 3 + 1]), 2.0)) * + (pow(cos(q[0 * 3 + 1]), 2.0) * pow(cos(q[0 * 3 + 2]), 2.0) + + pow(cos(q[0 * 3 + 1]), 2.0) * pow(sin(q[0 * 3 + 2]), 2.0) + + pow(cos(q[0 * 3 + 2]), 2.0) * pow(sin(q[0 * 3 + 1]), 2.0) + + pow(sin(q[0 * 3 + 1]), 2.0) * pow(sin(q[0 * 3 + 2]), 2.0)) * + 2.0) + + (pow(cos(q[0 * 3 + 0]), 2.0) * + (cos(q[0 * 3 + 1]) * sin(q[0 * 3 + 2]) + + cos(q[0 * 3 + 2]) * sin(q[0 * 3 + 1])) * + (l[0 * 3 + 0] * pow(sin(q[0 * 3 + 1]), 2.0) + + l[1 * 3 + 0] * cos(q[0 * 3 + 1]) + + l[1 * 3 + 2] * sin(q[0 * 3 + 1]) + + l[0 * 3 + 0] * pow(cos(q[0 * 3 + 1]), 2.0))) / + ((pow(cos(q[0 * 3 + 0]), 2.0) * pow(cos(q[0 * 3 + 1]), 2.0) + + pow(cos(q[0 * 3 + 0]), 2.0) * pow(sin(q[0 * 3 + 1]), 2.0) + + pow(cos(q[0 * 3 + 1]), 2.0) * pow(sin(q[0 * 3 + 0]), 2.0) + + pow(sin(q[0 * 3 + 0]), 2.0) * pow(sin(q[0 * 3 + 1]), 2.0)) * + (pow(cos(q[0 * 3 + 0]), 2.0) * pow(cos(q[0 * 3 + 1]), 2.0) * + pow(sin(q[0 * 3 + 2]), 2.0) + + pow(cos(q[0 * 3 + 0]), 2.0) * pow(cos(q[0 * 3 + 2]), 2.0) * + pow(sin(q[0 * 3 + 1]), 2.0) + + pow(cos(q[0 * 3 + 1]), 2.0) * pow(cos(q[0 * 3 + 2]), 2.0) * + pow(sin(q[0 * 3 + 0]), 2.0) + + pow(cos(q[0 * 3 + 0]), 2.0) * pow(sin(q[0 * 3 + 1]), 2.0) * + pow(sin(q[0 * 3 + 2]), 2.0) + + pow(cos(q[0 * 3 + 1]), 2.0) * pow(sin(q[0 * 3 + 0]), 2.0) * + pow(sin(q[0 * 3 + 2]), 2.0) + + pow(cos(q[0 * 3 + 2]), 2.0) * pow(sin(q[0 * 3 + 0]), 2.0) * + pow(sin(q[0 * 3 + 1]), 2.0) + + pow(sin(q[0 * 3 + 0]), 2.0) * pow(sin(q[0 * 3 + 1]), 2.0) * + pow(sin(q[0 * 3 + 2]), 2.0) + + pow(cos(q[0 * 3 + 0]), 2.0) * pow(cos(q[0 * 3 + 1]), 2.0) * + pow(cos(q[0 * 3 + 2]), 2.0)) * + 2.0) + + (pow(sin(q[0 * 3 + 0]), 2.0) * + (cos(q[0 * 3 + 1]) * sin(q[0 * 3 + 2]) + + cos(q[0 * 3 + 2]) * sin(q[0 * 3 + 1])) * + (l[0 * 3 + 0] * pow(sin(q[0 * 3 + 1]), 2.0) + + l[1 * 3 + 0] * cos(q[0 * 3 + 1]) + + l[1 * 3 + 2] * sin(q[0 * 3 + 1]) + + l[0 * 3 + 0] * pow(cos(q[0 * 3 + 1]), 2.0))) / + ((pow(cos(q[0 * 3 + 0]), 2.0) * pow(cos(q[0 * 3 + 1]), 2.0) + + pow(cos(q[0 * 3 + 0]), 2.0) * pow(sin(q[0 * 3 + 1]), 2.0) + + pow(cos(q[0 * 3 + 1]), 2.0) * pow(sin(q[0 * 3 + 0]), 2.0) + + pow(sin(q[0 * 3 + 0]), 2.0) * pow(sin(q[0 * 3 + 1]), 2.0)) * + (pow(cos(q[0 * 3 + 0]), 2.0) * pow(cos(q[0 * 3 + 1]), 2.0) * + pow(sin(q[0 * 3 + 2]), 2.0) + + pow(cos(q[0 * 3 + 0]), 2.0) * pow(cos(q[0 * 3 + 2]), 2.0) * + pow(sin(q[0 * 3 + 1]), 2.0) + + pow(cos(q[0 * 3 + 1]), 2.0) * pow(cos(q[0 * 3 + 2]), 2.0) * + pow(sin(q[0 * 3 + 0]), 2.0) + + pow(cos(q[0 * 3 + 0]), 2.0) * pow(sin(q[0 * 3 + 1]), 2.0) * + pow(sin(q[0 * 3 + 2]), 2.0) + + pow(cos(q[0 * 3 + 1]), 2.0) * pow(sin(q[0 * 3 + 0]), 2.0) * + pow(sin(q[0 * 3 + 2]), 2.0) + + pow(cos(q[0 * 3 + 2]), 2.0) * pow(sin(q[0 * 3 + 0]), 2.0) * + pow(sin(q[0 * 3 + 1]), 2.0) + + pow(sin(q[0 * 3 + 0]), 2.0) * pow(sin(q[0 * 3 + 1]), 2.0) * + pow(sin(q[0 * 3 + 2]), 2.0) + + pow(cos(q[0 * 3 + 0]), 2.0) * pow(cos(q[0 * 3 + 1]), 2.0) * + pow(cos(q[0 * 3 + 2]), 2.0)) * + 2.0))) / + (pow(cos(q[0 * 3 + 0]), 2.0) * pow(cos(q[0 * 3 + 1]), 2.0) * + pow(sin(q[0 * 3 + 2]), 2.0) + + pow(cos(q[0 * 3 + 0]), 2.0) * pow(cos(q[0 * 3 + 2]), 2.0) * + pow(sin(q[0 * 3 + 1]), 2.0) + + pow(cos(q[0 * 3 + 1]), 2.0) * pow(cos(q[0 * 3 + 2]), 2.0) * + pow(sin(q[0 * 3 + 0]), 2.0) + + pow(cos(q[0 * 3 + 0]), 2.0) * pow(sin(q[0 * 3 + 1]), 2.0) * + pow(sin(q[0 * 3 + 2]), 2.0) + + pow(cos(q[0 * 3 + 1]), 2.0) * pow(sin(q[0 * 3 + 0]), 2.0) * + pow(sin(q[0 * 3 + 2]), 2.0) + + pow(cos(q[0 * 3 + 2]), 2.0) * pow(sin(q[0 * 3 + 0]), 2.0) * + pow(sin(q[0 * 3 + 1]), 2.0) + + pow(sin(q[0 * 3 + 0]), 2.0) * pow(sin(q[0 * 3 + 1]), 2.0) * + pow(sin(q[0 * 3 + 2]), 2.0) + + pow(cos(q[0 * 3 + 0]), 2.0) * pow(cos(q[0 * 3 + 1]), 2.0) * + pow(cos(q[0 * 3 + 2]), 2.0)))) / + (pow(cos(q[0 * 3 + 0]), 2.0) + pow(sin(q[0 * 3 + 0]), 2.0)); + F(0 * 3 + 1) = + -(pow(cos(q[0 * 3 + 1]) * sin(q[0 * 3 + 2]) + + cos(q[0 * 3 + 2]) * sin(q[0 * 3 + 1]), + 2.0) / + (pow(cos(q[0 * 3 + 1]), 2.0) * pow(cos(q[0 * 3 + 2]), 2.0) + + pow(cos(q[0 * 3 + 1]), 2.0) * pow(sin(q[0 * 3 + 2]), 2.0) + + pow(cos(q[0 * 3 + 2]), 2.0) * pow(sin(q[0 * 3 + 1]), 2.0) + + pow(sin(q[0 * 3 + 1]), 2.0) * pow(sin(q[0 * 3 + 2]), 2.0)) + + pow(cos(q[0 * 3 + 1]) * cos(q[0 * 3 + 2]) - + sin(q[0 * 3 + 1]) * sin(q[0 * 3 + 2]), + 2.0) / + (pow(cos(q[0 * 3 + 1]), 2.0) * pow(cos(q[0 * 3 + 2]), 2.0) + + pow(cos(q[0 * 3 + 1]), 2.0) * pow(sin(q[0 * 3 + 2]), 2.0) + + pow(cos(q[0 * 3 + 2]), 2.0) * pow(sin(q[0 * 3 + 1]), 2.0) + + pow(sin(q[0 * 3 + 1]), 2.0) * pow(sin(q[0 * 3 + 2]), 2.0))) * + ((sin(q[0 * 3 + 0]) * + (RL * l[2 * 3 + 1] + + ((cos(q[0 * 3 + 1]) * sin(q[0 * 3 + 2]) + + cos(q[0 * 3 + 2]) * sin(q[0 * 3 + 1])) * + (RL * l[0 * 3 + 1] * cos(q[0 * 3 + 1]) * sin(q[0 * 3 + 2]) + + RL * l[0 * 3 + 1] * cos(q[0 * 3 + 2]) * sin(q[0 * 3 + 1]) + + RL * l[1 * 3 + 1] * cos(q[0 * 3 + 1]) * sin(q[0 * 3 + 2]) + + RL * l[1 * 3 + 1] * cos(q[0 * 3 + 2]) * sin(q[0 * 3 + 1])) * + 1.0 / + pow(pow(cos(q[0 * 3 + 1]), 2.0) * pow(cos(q[0 * 3 + 2]), 2.0) + + pow(cos(q[0 * 3 + 1]), 2.0) * + pow(sin(q[0 * 3 + 2]), 2.0) + + pow(cos(q[0 * 3 + 2]), 2.0) * + pow(sin(q[0 * 3 + 1]), 2.0) + + pow(sin(q[0 * 3 + 1]), 2.0) * + pow(sin(q[0 * 3 + 2]), 2.0), + 2.0)) / + 2.0 + + ((cos(q[0 * 3 + 1]) * cos(q[0 * 3 + 2]) - + sin(q[0 * 3 + 1]) * sin(q[0 * 3 + 2])) * + (RL * l[0 * 3 + 1] * cos(q[0 * 3 + 1]) * cos(q[0 * 3 + 2]) + + RL * l[1 * 3 + 1] * cos(q[0 * 3 + 1]) * cos(q[0 * 3 + 2]) - + RL * l[0 * 3 + 1] * sin(q[0 * 3 + 1]) * sin(q[0 * 3 + 2]) - + RL * l[1 * 3 + 1] * sin(q[0 * 3 + 1]) * sin(q[0 * 3 + 2])) * + 1.0 / + pow(pow(cos(q[0 * 3 + 1]), 2.0) * pow(cos(q[0 * 3 + 2]), 2.0) + + pow(cos(q[0 * 3 + 1]), 2.0) * + pow(sin(q[0 * 3 + 2]), 2.0) + + pow(cos(q[0 * 3 + 2]), 2.0) * + pow(sin(q[0 * 3 + 1]), 2.0) + + pow(sin(q[0 * 3 + 1]), 2.0) * + pow(sin(q[0 * 3 + 2]), 2.0), + 2.0)) / + 2.0 + + (sin(q[0 * 3 + 0]) * + (cos(q[0 * 3 + 1]) * sin(q[0 * 3 + 2]) + + cos(q[0 * 3 + 2]) * sin(q[0 * 3 + 1])) * + 1.0 / + pow(pow(cos(q[0 * 3 + 0]), 2.0) * pow(cos(q[0 * 3 + 1]), 2.0) * + pow(sin(q[0 * 3 + 2]), 2.0) + + pow(cos(q[0 * 3 + 0]), 2.0) * + pow(cos(q[0 * 3 + 2]), 2.0) * + pow(sin(q[0 * 3 + 1]), 2.0) + + pow(cos(q[0 * 3 + 1]), 2.0) * + pow(cos(q[0 * 3 + 2]), 2.0) * + pow(sin(q[0 * 3 + 0]), 2.0) + + pow(cos(q[0 * 3 + 0]), 2.0) * + pow(sin(q[0 * 3 + 1]), 2.0) * + pow(sin(q[0 * 3 + 2]), 2.0) + + pow(cos(q[0 * 3 + 1]), 2.0) * + pow(sin(q[0 * 3 + 0]), 2.0) * + pow(sin(q[0 * 3 + 2]), 2.0) + + pow(cos(q[0 * 3 + 2]), 2.0) * + pow(sin(q[0 * 3 + 0]), 2.0) * + pow(sin(q[0 * 3 + 1]), 2.0) + + pow(sin(q[0 * 3 + 0]), 2.0) * + pow(sin(q[0 * 3 + 1]), 2.0) * + pow(sin(q[0 * 3 + 2]), 2.0) + + pow(cos(q[0 * 3 + 0]), 2.0) * + pow(cos(q[0 * 3 + 1]), 2.0) * + pow(cos(q[0 * 3 + 2]), 2.0), + 2.0) * + (-l[0 * 3 + 0] * cos(q[0 * 3 + 0]) * cos(q[0 * 3 + 1]) * + cos(q[0 * 3 + 2]) + + l[0 * 3 + 2] * cos(q[0 * 3 + 0]) * cos(q[0 * 3 + 1]) * + sin(q[0 * 3 + 2]) + + l[0 * 3 + 2] * cos(q[0 * 3 + 0]) * cos(q[0 * 3 + 2]) * + sin(q[0 * 3 + 1]) + + l[0 * 3 + 0] * cos(q[0 * 3 + 0]) * sin(q[0 * 3 + 1]) * + sin(q[0 * 3 + 2]) - + l[1 * 3 + 0] * cos(q[0 * 3 + 0]) * + pow(cos(q[0 * 3 + 1]), 2.0) * cos(q[0 * 3 + 2]) - + l[1 * 3 + 0] * cos(q[0 * 3 + 0]) * cos(q[0 * 3 + 2]) * + pow(sin(q[0 * 3 + 1]), 2.0) + + l[1 * 3 + 2] * cos(q[0 * 3 + 0]) * + pow(cos(q[0 * 3 + 1]), 2.0) * sin(q[0 * 3 + 2]) + + l[1 * 3 + 2] * cos(q[0 * 3 + 0]) * + pow(sin(q[0 * 3 + 1]), 2.0) * sin(q[0 * 3 + 2]) + + RL * l[0 * 3 + 1] * cos(q[0 * 3 + 1]) * sin(q[0 * 3 + 0]) * + sin(q[0 * 3 + 2]) + + RL * l[0 * 3 + 1] * cos(q[0 * 3 + 2]) * sin(q[0 * 3 + 0]) * + sin(q[0 * 3 + 1]) + + RL * l[1 * 3 + 1] * cos(q[0 * 3 + 1]) * sin(q[0 * 3 + 0]) * + sin(q[0 * 3 + 2]) + + RL * l[1 * 3 + 1] * cos(q[0 * 3 + 2]) * sin(q[0 * 3 + 0]) * + sin(q[0 * 3 + 1]))) / + 2.0 + + (sin(q[0 * 3 + 0]) * + (cos(q[0 * 3 + 1]) * cos(q[0 * 3 + 2]) - + sin(q[0 * 3 + 1]) * sin(q[0 * 3 + 2])) * + 1.0 / + pow(pow(cos(q[0 * 3 + 0]), 2.0) * pow(cos(q[0 * 3 + 1]), 2.0) * + pow(sin(q[0 * 3 + 2]), 2.0) + + pow(cos(q[0 * 3 + 0]), 2.0) * + pow(cos(q[0 * 3 + 2]), 2.0) * + pow(sin(q[0 * 3 + 1]), 2.0) + + pow(cos(q[0 * 3 + 1]), 2.0) * + pow(cos(q[0 * 3 + 2]), 2.0) * + pow(sin(q[0 * 3 + 0]), 2.0) + + pow(cos(q[0 * 3 + 0]), 2.0) * + pow(sin(q[0 * 3 + 1]), 2.0) * + pow(sin(q[0 * 3 + 2]), 2.0) + + pow(cos(q[0 * 3 + 1]), 2.0) * + pow(sin(q[0 * 3 + 0]), 2.0) * + pow(sin(q[0 * 3 + 2]), 2.0) + + pow(cos(q[0 * 3 + 2]), 2.0) * + pow(sin(q[0 * 3 + 0]), 2.0) * + pow(sin(q[0 * 3 + 1]), 2.0) + + pow(sin(q[0 * 3 + 0]), 2.0) * + pow(sin(q[0 * 3 + 1]), 2.0) * + pow(sin(q[0 * 3 + 2]), 2.0) + + pow(cos(q[0 * 3 + 0]), 2.0) * + pow(cos(q[0 * 3 + 1]), 2.0) * + pow(cos(q[0 * 3 + 2]), 2.0), + 2.0) * + (l[0 * 3 + 2] * cos(q[0 * 3 + 0]) * cos(q[0 * 3 + 1]) * + cos(q[0 * 3 + 2]) + + l[0 * 3 + 0] * cos(q[0 * 3 + 0]) * cos(q[0 * 3 + 1]) * + sin(q[0 * 3 + 2]) + + l[0 * 3 + 0] * cos(q[0 * 3 + 0]) * cos(q[0 * 3 + 2]) * + sin(q[0 * 3 + 1]) - + l[0 * 3 + 2] * cos(q[0 * 3 + 0]) * sin(q[0 * 3 + 1]) * + sin(q[0 * 3 + 2]) + + l[1 * 3 + 2] * cos(q[0 * 3 + 0]) * + pow(cos(q[0 * 3 + 1]), 2.0) * cos(q[0 * 3 + 2]) + + l[1 * 3 + 0] * cos(q[0 * 3 + 0]) * + pow(cos(q[0 * 3 + 1]), 2.0) * sin(q[0 * 3 + 2]) + + l[1 * 3 + 2] * cos(q[0 * 3 + 0]) * cos(q[0 * 3 + 2]) * + pow(sin(q[0 * 3 + 1]), 2.0) + + l[1 * 3 + 0] * cos(q[0 * 3 + 0]) * + pow(sin(q[0 * 3 + 1]), 2.0) * sin(q[0 * 3 + 2]) + + RL * l[0 * 3 + 1] * cos(q[0 * 3 + 1]) * cos(q[0 * 3 + 2]) * + sin(q[0 * 3 + 0]) + + RL * l[1 * 3 + 1] * cos(q[0 * 3 + 1]) * cos(q[0 * 3 + 2]) * + sin(q[0 * 3 + 0]) - + RL * l[0 * 3 + 1] * sin(q[0 * 3 + 0]) * sin(q[0 * 3 + 1]) * + sin(q[0 * 3 + 2]) - + RL * l[1 * 3 + 1] * sin(q[0 * 3 + 0]) * sin(q[0 * 3 + 1]) * + sin(q[0 * 3 + 2]))) / + 2.0 - + (cos(q[0 * 3 + 0]) * + (cos(q[0 * 3 + 1]) * cos(q[0 * 3 + 2]) - + sin(q[0 * 3 + 1]) * sin(q[0 * 3 + 2])) * + 1.0 / + pow(pow(cos(q[0 * 3 + 0]), 2.0) * pow(cos(q[0 * 3 + 1]), 2.0) * + pow(sin(q[0 * 3 + 2]), 2.0) + + pow(cos(q[0 * 3 + 0]), 2.0) * + pow(cos(q[0 * 3 + 2]), 2.0) * + pow(sin(q[0 * 3 + 1]), 2.0) + + pow(cos(q[0 * 3 + 1]), 2.0) * + pow(cos(q[0 * 3 + 2]), 2.0) * + pow(sin(q[0 * 3 + 0]), 2.0) + + pow(cos(q[0 * 3 + 0]), 2.0) * + pow(sin(q[0 * 3 + 1]), 2.0) * + pow(sin(q[0 * 3 + 2]), 2.0) + + pow(cos(q[0 * 3 + 1]), 2.0) * + pow(sin(q[0 * 3 + 0]), 2.0) * + pow(sin(q[0 * 3 + 2]), 2.0) + + pow(cos(q[0 * 3 + 2]), 2.0) * + pow(sin(q[0 * 3 + 0]), 2.0) * + pow(sin(q[0 * 3 + 1]), 2.0) + + pow(sin(q[0 * 3 + 0]), 2.0) * + pow(sin(q[0 * 3 + 1]), 2.0) * + pow(sin(q[0 * 3 + 2]), 2.0) + + pow(cos(q[0 * 3 + 0]), 2.0) * + pow(cos(q[0 * 3 + 1]), 2.0) * + pow(cos(q[0 * 3 + 2]), 2.0), + 2.0) * + (l[0 * 3 + 2] * cos(q[0 * 3 + 1]) * cos(q[0 * 3 + 2]) * + sin(q[0 * 3 + 0]) + + l[0 * 3 + 0] * cos(q[0 * 3 + 1]) * sin(q[0 * 3 + 0]) * + sin(q[0 * 3 + 2]) + + l[0 * 3 + 0] * cos(q[0 * 3 + 2]) * sin(q[0 * 3 + 0]) * + sin(q[0 * 3 + 1]) - + l[0 * 3 + 2] * sin(q[0 * 3 + 0]) * sin(q[0 * 3 + 1]) * + sin(q[0 * 3 + 2]) + + l[1 * 3 + 2] * pow(cos(q[0 * 3 + 1]), 2.0) * + cos(q[0 * 3 + 2]) * sin(q[0 * 3 + 0]) + + l[1 * 3 + 0] * pow(cos(q[0 * 3 + 1]), 2.0) * + sin(q[0 * 3 + 0]) * sin(q[0 * 3 + 2]) + + l[1 * 3 + 2] * cos(q[0 * 3 + 2]) * sin(q[0 * 3 + 0]) * + pow(sin(q[0 * 3 + 1]), 2.0) + + l[1 * 3 + 0] * sin(q[0 * 3 + 0]) * + pow(sin(q[0 * 3 + 1]), 2.0) * sin(q[0 * 3 + 2]) - + RL * l[0 * 3 + 1] * cos(q[0 * 3 + 0]) * cos(q[0 * 3 + 1]) * + cos(q[0 * 3 + 2]) - + RL * l[1 * 3 + 1] * cos(q[0 * 3 + 0]) * cos(q[0 * 3 + 1]) * + cos(q[0 * 3 + 2]) + + RL * l[0 * 3 + 1] * cos(q[0 * 3 + 0]) * sin(q[0 * 3 + 1]) * + sin(q[0 * 3 + 2]) + + RL * l[1 * 3 + 1] * cos(q[0 * 3 + 0]) * sin(q[0 * 3 + 1]) * + sin(q[0 * 3 + 2]))) / + 2.0 + + (cos(q[0 * 3 + 0]) * + (cos(q[0 * 3 + 1]) * sin(q[0 * 3 + 2]) + + cos(q[0 * 3 + 2]) * sin(q[0 * 3 + 1])) * + 1.0 / + pow(pow(cos(q[0 * 3 + 0]), 2.0) * pow(cos(q[0 * 3 + 1]), 2.0) * + pow(sin(q[0 * 3 + 2]), 2.0) + + pow(cos(q[0 * 3 + 0]), 2.0) * + pow(cos(q[0 * 3 + 2]), 2.0) * + pow(sin(q[0 * 3 + 1]), 2.0) + + pow(cos(q[0 * 3 + 1]), 2.0) * + pow(cos(q[0 * 3 + 2]), 2.0) * + pow(sin(q[0 * 3 + 0]), 2.0) + + pow(cos(q[0 * 3 + 0]), 2.0) * + pow(sin(q[0 * 3 + 1]), 2.0) * + pow(sin(q[0 * 3 + 2]), 2.0) + + pow(cos(q[0 * 3 + 1]), 2.0) * + pow(sin(q[0 * 3 + 0]), 2.0) * + pow(sin(q[0 * 3 + 2]), 2.0) + + pow(cos(q[0 * 3 + 2]), 2.0) * + pow(sin(q[0 * 3 + 0]), 2.0) * + pow(sin(q[0 * 3 + 1]), 2.0) + + pow(sin(q[0 * 3 + 0]), 2.0) * + pow(sin(q[0 * 3 + 1]), 2.0) * + pow(sin(q[0 * 3 + 2]), 2.0) + + pow(cos(q[0 * 3 + 0]), 2.0) * + pow(cos(q[0 * 3 + 1]), 2.0) * + pow(cos(q[0 * 3 + 2]), 2.0), + 2.0) * + (l[0 * 3 + 0] * cos(q[0 * 3 + 1]) * cos(q[0 * 3 + 2]) * + sin(q[0 * 3 + 0]) - + l[0 * 3 + 2] * cos(q[0 * 3 + 1]) * sin(q[0 * 3 + 0]) * + sin(q[0 * 3 + 2]) - + l[0 * 3 + 2] * cos(q[0 * 3 + 2]) * sin(q[0 * 3 + 0]) * + sin(q[0 * 3 + 1]) - + l[0 * 3 + 0] * sin(q[0 * 3 + 0]) * sin(q[0 * 3 + 1]) * + sin(q[0 * 3 + 2]) + + l[1 * 3 + 0] * pow(cos(q[0 * 3 + 1]), 2.0) * + cos(q[0 * 3 + 2]) * sin(q[0 * 3 + 0]) + + l[1 * 3 + 0] * cos(q[0 * 3 + 2]) * sin(q[0 * 3 + 0]) * + pow(sin(q[0 * 3 + 1]), 2.0) - + l[1 * 3 + 2] * pow(cos(q[0 * 3 + 1]), 2.0) * + sin(q[0 * 3 + 0]) * sin(q[0 * 3 + 2]) - + l[1 * 3 + 2] * sin(q[0 * 3 + 0]) * + pow(sin(q[0 * 3 + 1]), 2.0) * sin(q[0 * 3 + 2]) + + RL * l[0 * 3 + 1] * cos(q[0 * 3 + 0]) * cos(q[0 * 3 + 1]) * + sin(q[0 * 3 + 2]) + + RL * l[0 * 3 + 1] * cos(q[0 * 3 + 0]) * cos(q[0 * 3 + 2]) * + sin(q[0 * 3 + 1]) + + RL * l[1 * 3 + 1] * cos(q[0 * 3 + 0]) * cos(q[0 * 3 + 1]) * + sin(q[0 * 3 + 2]) + + RL * l[1 * 3 + 1] * cos(q[0 * 3 + 0]) * cos(q[0 * 3 + 2]) * + sin(q[0 * 3 + 1]))) / + 2.0)) / + (pow(cos(q[0 * 3 + 0]), 2.0) + pow(sin(q[0 * 3 + 0]), 2.0)) - + (cos(q[0 * 3 + 0]) * + (cos(q[0 * 3 + 1]) * sin(q[0 * 3 + 2]) + + cos(q[0 * 3 + 2]) * sin(q[0 * 3 + 1])) * + (l[2 * 3 + 0] - + (cos(q[0 * 3 + 0]) * + (-l[0 * 3 + 0] * cos(q[0 * 3 + 0]) * cos(q[0 * 3 + 1]) * + cos(q[0 * 3 + 2]) + + l[0 * 3 + 2] * cos(q[0 * 3 + 0]) * cos(q[0 * 3 + 1]) * + sin(q[0 * 3 + 2]) + + l[0 * 3 + 2] * cos(q[0 * 3 + 0]) * cos(q[0 * 3 + 2]) * + sin(q[0 * 3 + 1]) + + l[0 * 3 + 0] * cos(q[0 * 3 + 0]) * sin(q[0 * 3 + 1]) * + sin(q[0 * 3 + 2]) - + l[1 * 3 + 0] * cos(q[0 * 3 + 0]) * + pow(cos(q[0 * 3 + 1]), 2.0) * cos(q[0 * 3 + 2]) - + l[1 * 3 + 0] * cos(q[0 * 3 + 0]) * cos(q[0 * 3 + 2]) * + pow(sin(q[0 * 3 + 1]), 2.0) + + l[1 * 3 + 2] * cos(q[0 * 3 + 0]) * + pow(cos(q[0 * 3 + 1]), 2.0) * sin(q[0 * 3 + 2]) + + l[1 * 3 + 2] * cos(q[0 * 3 + 0]) * + pow(sin(q[0 * 3 + 1]), 2.0) * sin(q[0 * 3 + 2]) + + RL * l[0 * 3 + 1] * cos(q[0 * 3 + 1]) * sin(q[0 * 3 + 0]) * + sin(q[0 * 3 + 2]) + + RL * l[0 * 3 + 1] * cos(q[0 * 3 + 2]) * sin(q[0 * 3 + 0]) * + sin(q[0 * 3 + 1]) + + RL * l[1 * 3 + 1] * cos(q[0 * 3 + 1]) * sin(q[0 * 3 + 0]) * + sin(q[0 * 3 + 2]) + + RL * l[1 * 3 + 1] * cos(q[0 * 3 + 2]) * sin(q[0 * 3 + 0]) * + sin(q[0 * 3 + 1]))) / + ((pow(cos(q[0 * 3 + 0]), 2.0) + + pow(sin(q[0 * 3 + 0]), 2.0)) * + (pow(cos(q[0 * 3 + 0]), 2.0) * pow(cos(q[0 * 3 + 1]), 2.0) * + pow(sin(q[0 * 3 + 2]), 2.0) + + pow(cos(q[0 * 3 + 0]), 2.0) * pow(cos(q[0 * 3 + 2]), 2.0) * + pow(sin(q[0 * 3 + 1]), 2.0) + + pow(cos(q[0 * 3 + 1]), 2.0) * pow(cos(q[0 * 3 + 2]), 2.0) * + pow(sin(q[0 * 3 + 0]), 2.0) + + pow(cos(q[0 * 3 + 0]), 2.0) * pow(sin(q[0 * 3 + 1]), 2.0) * + pow(sin(q[0 * 3 + 2]), 2.0) + + pow(cos(q[0 * 3 + 1]), 2.0) * pow(sin(q[0 * 3 + 0]), 2.0) * + pow(sin(q[0 * 3 + 2]), 2.0) + + pow(cos(q[0 * 3 + 2]), 2.0) * pow(sin(q[0 * 3 + 0]), 2.0) * + pow(sin(q[0 * 3 + 1]), 2.0) + + pow(sin(q[0 * 3 + 0]), 2.0) * pow(sin(q[0 * 3 + 1]), 2.0) * + pow(sin(q[0 * 3 + 2]), 2.0) + + pow(cos(q[0 * 3 + 0]), 2.0) * pow(cos(q[0 * 3 + 1]), 2.0) * + pow(cos(q[0 * 3 + 2]), 2.0)) * + 2.0) + + (sin(q[0 * 3 + 0]) * + (l[0 * 3 + 0] * cos(q[0 * 3 + 1]) * cos(q[0 * 3 + 2]) * + sin(q[0 * 3 + 0]) - + l[0 * 3 + 2] * cos(q[0 * 3 + 1]) * sin(q[0 * 3 + 0]) * + sin(q[0 * 3 + 2]) - + l[0 * 3 + 2] * cos(q[0 * 3 + 2]) * sin(q[0 * 3 + 0]) * + sin(q[0 * 3 + 1]) - + l[0 * 3 + 0] * sin(q[0 * 3 + 0]) * sin(q[0 * 3 + 1]) * + sin(q[0 * 3 + 2]) + + l[1 * 3 + 0] * pow(cos(q[0 * 3 + 1]), 2.0) * + cos(q[0 * 3 + 2]) * sin(q[0 * 3 + 0]) + + l[1 * 3 + 0] * cos(q[0 * 3 + 2]) * sin(q[0 * 3 + 0]) * + pow(sin(q[0 * 3 + 1]), 2.0) - + l[1 * 3 + 2] * pow(cos(q[0 * 3 + 1]), 2.0) * + sin(q[0 * 3 + 0]) * sin(q[0 * 3 + 2]) - + l[1 * 3 + 2] * sin(q[0 * 3 + 0]) * + pow(sin(q[0 * 3 + 1]), 2.0) * sin(q[0 * 3 + 2]) + + RL * l[0 * 3 + 1] * cos(q[0 * 3 + 0]) * cos(q[0 * 3 + 1]) * + sin(q[0 * 3 + 2]) + + RL * l[0 * 3 + 1] * cos(q[0 * 3 + 0]) * cos(q[0 * 3 + 2]) * + sin(q[0 * 3 + 1]) + + RL * l[1 * 3 + 1] * cos(q[0 * 3 + 0]) * cos(q[0 * 3 + 1]) * + sin(q[0 * 3 + 2]) + + RL * l[1 * 3 + 1] * cos(q[0 * 3 + 0]) * cos(q[0 * 3 + 2]) * + sin(q[0 * 3 + 1]))) / + ((pow(cos(q[0 * 3 + 0]), 2.0) + + pow(sin(q[0 * 3 + 0]), 2.0)) * + (pow(cos(q[0 * 3 + 0]), 2.0) * pow(cos(q[0 * 3 + 1]), 2.0) * + pow(sin(q[0 * 3 + 2]), 2.0) + + pow(cos(q[0 * 3 + 0]), 2.0) * pow(cos(q[0 * 3 + 2]), 2.0) * + pow(sin(q[0 * 3 + 1]), 2.0) + + pow(cos(q[0 * 3 + 1]), 2.0) * pow(cos(q[0 * 3 + 2]), 2.0) * + pow(sin(q[0 * 3 + 0]), 2.0) + + pow(cos(q[0 * 3 + 0]), 2.0) * pow(sin(q[0 * 3 + 1]), 2.0) * + pow(sin(q[0 * 3 + 2]), 2.0) + + pow(cos(q[0 * 3 + 1]), 2.0) * pow(sin(q[0 * 3 + 0]), 2.0) * + pow(sin(q[0 * 3 + 2]), 2.0) + + pow(cos(q[0 * 3 + 2]), 2.0) * pow(sin(q[0 * 3 + 0]), 2.0) * + pow(sin(q[0 * 3 + 1]), 2.0) + + pow(sin(q[0 * 3 + 0]), 2.0) * pow(sin(q[0 * 3 + 1]), 2.0) * + pow(sin(q[0 * 3 + 2]), 2.0) + + pow(cos(q[0 * 3 + 0]), 2.0) * pow(cos(q[0 * 3 + 1]), 2.0) * + pow(cos(q[0 * 3 + 2]), 2.0)) * + 2.0) - + ((cos(q[0 * 3 + 1]) * sin(q[0 * 3 + 2]) + + cos(q[0 * 3 + 2]) * sin(q[0 * 3 + 1])) * + (l[0 * 3 + 2] * pow(sin(q[0 * 3 + 1]), 2.0) + + l[1 * 3 + 2] * cos(q[0 * 3 + 1]) - + l[1 * 3 + 0] * sin(q[0 * 3 + 1]) + + l[0 * 3 + 2] * pow(cos(q[0 * 3 + 1]), 2.0))) / + ((pow(cos(q[0 * 3 + 1]), 2.0) + + pow(sin(q[0 * 3 + 1]), 2.0)) * + (pow(cos(q[0 * 3 + 1]), 2.0) * pow(cos(q[0 * 3 + 2]), 2.0) + + pow(cos(q[0 * 3 + 1]), 2.0) * pow(sin(q[0 * 3 + 2]), 2.0) + + pow(cos(q[0 * 3 + 2]), 2.0) * pow(sin(q[0 * 3 + 1]), 2.0) + + pow(sin(q[0 * 3 + 1]), 2.0) * + pow(sin(q[0 * 3 + 2]), 2.0)) * + 2.0) + + (pow(cos(q[0 * 3 + 0]), 2.0) * + (cos(q[0 * 3 + 1]) * cos(q[0 * 3 + 2]) - + sin(q[0 * 3 + 1]) * sin(q[0 * 3 + 2])) * + (l[0 * 3 + 0] * pow(sin(q[0 * 3 + 1]), 2.0) + + l[1 * 3 + 0] * cos(q[0 * 3 + 1]) + + l[1 * 3 + 2] * sin(q[0 * 3 + 1]) + + l[0 * 3 + 0] * pow(cos(q[0 * 3 + 1]), 2.0))) / + ((pow(cos(q[0 * 3 + 0]), 2.0) * pow(cos(q[0 * 3 + 1]), 2.0) + + pow(cos(q[0 * 3 + 0]), 2.0) * pow(sin(q[0 * 3 + 1]), 2.0) + + pow(cos(q[0 * 3 + 1]), 2.0) * pow(sin(q[0 * 3 + 0]), 2.0) + + pow(sin(q[0 * 3 + 0]), 2.0) * + pow(sin(q[0 * 3 + 1]), 2.0)) * + (pow(cos(q[0 * 3 + 0]), 2.0) * pow(cos(q[0 * 3 + 1]), 2.0) * + pow(sin(q[0 * 3 + 2]), 2.0) + + pow(cos(q[0 * 3 + 0]), 2.0) * pow(cos(q[0 * 3 + 2]), 2.0) * + pow(sin(q[0 * 3 + 1]), 2.0) + + pow(cos(q[0 * 3 + 1]), 2.0) * pow(cos(q[0 * 3 + 2]), 2.0) * + pow(sin(q[0 * 3 + 0]), 2.0) + + pow(cos(q[0 * 3 + 0]), 2.0) * pow(sin(q[0 * 3 + 1]), 2.0) * + pow(sin(q[0 * 3 + 2]), 2.0) + + pow(cos(q[0 * 3 + 1]), 2.0) * pow(sin(q[0 * 3 + 0]), 2.0) * + pow(sin(q[0 * 3 + 2]), 2.0) + + pow(cos(q[0 * 3 + 2]), 2.0) * pow(sin(q[0 * 3 + 0]), 2.0) * + pow(sin(q[0 * 3 + 1]), 2.0) + + pow(sin(q[0 * 3 + 0]), 2.0) * pow(sin(q[0 * 3 + 1]), 2.0) * + pow(sin(q[0 * 3 + 2]), 2.0) + + pow(cos(q[0 * 3 + 0]), 2.0) * pow(cos(q[0 * 3 + 1]), 2.0) * + pow(cos(q[0 * 3 + 2]), 2.0)) * + 2.0) + + (pow(sin(q[0 * 3 + 0]), 2.0) * + (cos(q[0 * 3 + 1]) * cos(q[0 * 3 + 2]) - + sin(q[0 * 3 + 1]) * sin(q[0 * 3 + 2])) * + (l[0 * 3 + 0] * pow(sin(q[0 * 3 + 1]), 2.0) + + l[1 * 3 + 0] * cos(q[0 * 3 + 1]) + + l[1 * 3 + 2] * sin(q[0 * 3 + 1]) + + l[0 * 3 + 0] * pow(cos(q[0 * 3 + 1]), 2.0))) / + ((pow(cos(q[0 * 3 + 0]), 2.0) * pow(cos(q[0 * 3 + 1]), 2.0) + + pow(cos(q[0 * 3 + 0]), 2.0) * pow(sin(q[0 * 3 + 1]), 2.0) + + pow(cos(q[0 * 3 + 1]), 2.0) * pow(sin(q[0 * 3 + 0]), 2.0) + + pow(sin(q[0 * 3 + 0]), 2.0) * + pow(sin(q[0 * 3 + 1]), 2.0)) * + (pow(cos(q[0 * 3 + 0]), 2.0) * pow(cos(q[0 * 3 + 1]), 2.0) * + pow(sin(q[0 * 3 + 2]), 2.0) + + pow(cos(q[0 * 3 + 0]), 2.0) * pow(cos(q[0 * 3 + 2]), 2.0) * + pow(sin(q[0 * 3 + 1]), 2.0) + + pow(cos(q[0 * 3 + 1]), 2.0) * pow(cos(q[0 * 3 + 2]), 2.0) * + pow(sin(q[0 * 3 + 0]), 2.0) + + pow(cos(q[0 * 3 + 0]), 2.0) * pow(sin(q[0 * 3 + 1]), 2.0) * + pow(sin(q[0 * 3 + 2]), 2.0) + + pow(cos(q[0 * 3 + 1]), 2.0) * pow(sin(q[0 * 3 + 0]), 2.0) * + pow(sin(q[0 * 3 + 2]), 2.0) + + pow(cos(q[0 * 3 + 2]), 2.0) * pow(sin(q[0 * 3 + 0]), 2.0) * + pow(sin(q[0 * 3 + 1]), 2.0) + + pow(sin(q[0 * 3 + 0]), 2.0) * pow(sin(q[0 * 3 + 1]), 2.0) * + pow(sin(q[0 * 3 + 2]), 2.0) + + pow(cos(q[0 * 3 + 0]), 2.0) * pow(cos(q[0 * 3 + 1]), 2.0) * + pow(cos(q[0 * 3 + 2]), 2.0)) * + 2.0))) / + (pow(cos(q[0 * 3 + 0]), 2.0) * pow(cos(q[0 * 3 + 1]), 2.0) * + pow(sin(q[0 * 3 + 2]), 2.0) + + pow(cos(q[0 * 3 + 0]), 2.0) * pow(cos(q[0 * 3 + 2]), 2.0) * + pow(sin(q[0 * 3 + 1]), 2.0) + + pow(cos(q[0 * 3 + 1]), 2.0) * pow(cos(q[0 * 3 + 2]), 2.0) * + pow(sin(q[0 * 3 + 0]), 2.0) + + pow(cos(q[0 * 3 + 0]), 2.0) * pow(sin(q[0 * 3 + 1]), 2.0) * + pow(sin(q[0 * 3 + 2]), 2.0) + + pow(cos(q[0 * 3 + 1]), 2.0) * pow(sin(q[0 * 3 + 0]), 2.0) * + pow(sin(q[0 * 3 + 2]), 2.0) + + pow(cos(q[0 * 3 + 2]), 2.0) * pow(sin(q[0 * 3 + 0]), 2.0) * + pow(sin(q[0 * 3 + 1]), 2.0) + + pow(sin(q[0 * 3 + 0]), 2.0) * pow(sin(q[0 * 3 + 1]), 2.0) * + pow(sin(q[0 * 3 + 2]), 2.0) + + pow(cos(q[0 * 3 + 0]), 2.0) * pow(cos(q[0 * 3 + 1]), 2.0) * + pow(cos(q[0 * 3 + 2]), 2.0)) + + (cos(q[0 * 3 + 0]) * + (cos(q[0 * 3 + 1]) * cos(q[0 * 3 + 2]) - + sin(q[0 * 3 + 1]) * sin(q[0 * 3 + 2])) * + (l[2 * 3 + 2] + + (cos(q[0 * 3 + 0]) * + (l[0 * 3 + 2] * cos(q[0 * 3 + 0]) * cos(q[0 * 3 + 1]) * + cos(q[0 * 3 + 2]) + + l[0 * 3 + 0] * cos(q[0 * 3 + 0]) * cos(q[0 * 3 + 1]) * + sin(q[0 * 3 + 2]) + + l[0 * 3 + 0] * cos(q[0 * 3 + 0]) * cos(q[0 * 3 + 2]) * + sin(q[0 * 3 + 1]) - + l[0 * 3 + 2] * cos(q[0 * 3 + 0]) * sin(q[0 * 3 + 1]) * + sin(q[0 * 3 + 2]) + + l[1 * 3 + 2] * cos(q[0 * 3 + 0]) * + pow(cos(q[0 * 3 + 1]), 2.0) * cos(q[0 * 3 + 2]) + + l[1 * 3 + 0] * cos(q[0 * 3 + 0]) * + pow(cos(q[0 * 3 + 1]), 2.0) * sin(q[0 * 3 + 2]) + + l[1 * 3 + 2] * cos(q[0 * 3 + 0]) * cos(q[0 * 3 + 2]) * + pow(sin(q[0 * 3 + 1]), 2.0) + + l[1 * 3 + 0] * cos(q[0 * 3 + 0]) * + pow(sin(q[0 * 3 + 1]), 2.0) * sin(q[0 * 3 + 2]) + + RL * l[0 * 3 + 1] * cos(q[0 * 3 + 1]) * cos(q[0 * 3 + 2]) * + sin(q[0 * 3 + 0]) + + RL * l[1 * 3 + 1] * cos(q[0 * 3 + 1]) * cos(q[0 * 3 + 2]) * + sin(q[0 * 3 + 0]) - + RL * l[0 * 3 + 1] * sin(q[0 * 3 + 0]) * sin(q[0 * 3 + 1]) * + sin(q[0 * 3 + 2]) - + RL * l[1 * 3 + 1] * sin(q[0 * 3 + 0]) * sin(q[0 * 3 + 1]) * + sin(q[0 * 3 + 2]))) / + ((pow(cos(q[0 * 3 + 0]), 2.0) + + pow(sin(q[0 * 3 + 0]), 2.0)) * + (pow(cos(q[0 * 3 + 0]), 2.0) * pow(cos(q[0 * 3 + 1]), 2.0) * + pow(sin(q[0 * 3 + 2]), 2.0) + + pow(cos(q[0 * 3 + 0]), 2.0) * pow(cos(q[0 * 3 + 2]), 2.0) * + pow(sin(q[0 * 3 + 1]), 2.0) + + pow(cos(q[0 * 3 + 1]), 2.0) * pow(cos(q[0 * 3 + 2]), 2.0) * + pow(sin(q[0 * 3 + 0]), 2.0) + + pow(cos(q[0 * 3 + 0]), 2.0) * pow(sin(q[0 * 3 + 1]), 2.0) * + pow(sin(q[0 * 3 + 2]), 2.0) + + pow(cos(q[0 * 3 + 1]), 2.0) * pow(sin(q[0 * 3 + 0]), 2.0) * + pow(sin(q[0 * 3 + 2]), 2.0) + + pow(cos(q[0 * 3 + 2]), 2.0) * pow(sin(q[0 * 3 + 0]), 2.0) * + pow(sin(q[0 * 3 + 1]), 2.0) + + pow(sin(q[0 * 3 + 0]), 2.0) * pow(sin(q[0 * 3 + 1]), 2.0) * + pow(sin(q[0 * 3 + 2]), 2.0) + + pow(cos(q[0 * 3 + 0]), 2.0) * pow(cos(q[0 * 3 + 1]), 2.0) * + pow(cos(q[0 * 3 + 2]), 2.0)) * + 2.0) + + (sin(q[0 * 3 + 0]) * + (l[0 * 3 + 2] * cos(q[0 * 3 + 1]) * cos(q[0 * 3 + 2]) * + sin(q[0 * 3 + 0]) + + l[0 * 3 + 0] * cos(q[0 * 3 + 1]) * sin(q[0 * 3 + 0]) * + sin(q[0 * 3 + 2]) + + l[0 * 3 + 0] * cos(q[0 * 3 + 2]) * sin(q[0 * 3 + 0]) * + sin(q[0 * 3 + 1]) - + l[0 * 3 + 2] * sin(q[0 * 3 + 0]) * sin(q[0 * 3 + 1]) * + sin(q[0 * 3 + 2]) + + l[1 * 3 + 2] * pow(cos(q[0 * 3 + 1]), 2.0) * + cos(q[0 * 3 + 2]) * sin(q[0 * 3 + 0]) + + l[1 * 3 + 0] * pow(cos(q[0 * 3 + 1]), 2.0) * + sin(q[0 * 3 + 0]) * sin(q[0 * 3 + 2]) + + l[1 * 3 + 2] * cos(q[0 * 3 + 2]) * sin(q[0 * 3 + 0]) * + pow(sin(q[0 * 3 + 1]), 2.0) + + l[1 * 3 + 0] * sin(q[0 * 3 + 0]) * + pow(sin(q[0 * 3 + 1]), 2.0) * sin(q[0 * 3 + 2]) - + RL * l[0 * 3 + 1] * cos(q[0 * 3 + 0]) * cos(q[0 * 3 + 1]) * + cos(q[0 * 3 + 2]) - + RL * l[1 * 3 + 1] * cos(q[0 * 3 + 0]) * cos(q[0 * 3 + 1]) * + cos(q[0 * 3 + 2]) + + RL * l[0 * 3 + 1] * cos(q[0 * 3 + 0]) * sin(q[0 * 3 + 1]) * + sin(q[0 * 3 + 2]) + + RL * l[1 * 3 + 1] * cos(q[0 * 3 + 0]) * sin(q[0 * 3 + 1]) * + sin(q[0 * 3 + 2]))) / + ((pow(cos(q[0 * 3 + 0]), 2.0) + + pow(sin(q[0 * 3 + 0]), 2.0)) * + (pow(cos(q[0 * 3 + 0]), 2.0) * pow(cos(q[0 * 3 + 1]), 2.0) * + pow(sin(q[0 * 3 + 2]), 2.0) + + pow(cos(q[0 * 3 + 0]), 2.0) * pow(cos(q[0 * 3 + 2]), 2.0) * + pow(sin(q[0 * 3 + 1]), 2.0) + + pow(cos(q[0 * 3 + 1]), 2.0) * pow(cos(q[0 * 3 + 2]), 2.0) * + pow(sin(q[0 * 3 + 0]), 2.0) + + pow(cos(q[0 * 3 + 0]), 2.0) * pow(sin(q[0 * 3 + 1]), 2.0) * + pow(sin(q[0 * 3 + 2]), 2.0) + + pow(cos(q[0 * 3 + 1]), 2.0) * pow(sin(q[0 * 3 + 0]), 2.0) * + pow(sin(q[0 * 3 + 2]), 2.0) + + pow(cos(q[0 * 3 + 2]), 2.0) * pow(sin(q[0 * 3 + 0]), 2.0) * + pow(sin(q[0 * 3 + 1]), 2.0) + + pow(sin(q[0 * 3 + 0]), 2.0) * pow(sin(q[0 * 3 + 1]), 2.0) * + pow(sin(q[0 * 3 + 2]), 2.0) + + pow(cos(q[0 * 3 + 0]), 2.0) * pow(cos(q[0 * 3 + 1]), 2.0) * + pow(cos(q[0 * 3 + 2]), 2.0)) * + 2.0) + + ((cos(q[0 * 3 + 1]) * cos(q[0 * 3 + 2]) - + sin(q[0 * 3 + 1]) * sin(q[0 * 3 + 2])) * + (l[0 * 3 + 2] * pow(sin(q[0 * 3 + 1]), 2.0) + + l[1 * 3 + 2] * cos(q[0 * 3 + 1]) - + l[1 * 3 + 0] * sin(q[0 * 3 + 1]) + + l[0 * 3 + 2] * pow(cos(q[0 * 3 + 1]), 2.0))) / + ((pow(cos(q[0 * 3 + 1]), 2.0) + + pow(sin(q[0 * 3 + 1]), 2.0)) * + (pow(cos(q[0 * 3 + 1]), 2.0) * pow(cos(q[0 * 3 + 2]), 2.0) + + pow(cos(q[0 * 3 + 1]), 2.0) * pow(sin(q[0 * 3 + 2]), 2.0) + + pow(cos(q[0 * 3 + 2]), 2.0) * pow(sin(q[0 * 3 + 1]), 2.0) + + pow(sin(q[0 * 3 + 1]), 2.0) * + pow(sin(q[0 * 3 + 2]), 2.0)) * + 2.0) + + (pow(cos(q[0 * 3 + 0]), 2.0) * + (cos(q[0 * 3 + 1]) * sin(q[0 * 3 + 2]) + + cos(q[0 * 3 + 2]) * sin(q[0 * 3 + 1])) * + (l[0 * 3 + 0] * pow(sin(q[0 * 3 + 1]), 2.0) + + l[1 * 3 + 0] * cos(q[0 * 3 + 1]) + + l[1 * 3 + 2] * sin(q[0 * 3 + 1]) + + l[0 * 3 + 0] * pow(cos(q[0 * 3 + 1]), 2.0))) / + ((pow(cos(q[0 * 3 + 0]), 2.0) * pow(cos(q[0 * 3 + 1]), 2.0) + + pow(cos(q[0 * 3 + 0]), 2.0) * pow(sin(q[0 * 3 + 1]), 2.0) + + pow(cos(q[0 * 3 + 1]), 2.0) * pow(sin(q[0 * 3 + 0]), 2.0) + + pow(sin(q[0 * 3 + 0]), 2.0) * + pow(sin(q[0 * 3 + 1]), 2.0)) * + (pow(cos(q[0 * 3 + 0]), 2.0) * pow(cos(q[0 * 3 + 1]), 2.0) * + pow(sin(q[0 * 3 + 2]), 2.0) + + pow(cos(q[0 * 3 + 0]), 2.0) * pow(cos(q[0 * 3 + 2]), 2.0) * + pow(sin(q[0 * 3 + 1]), 2.0) + + pow(cos(q[0 * 3 + 1]), 2.0) * pow(cos(q[0 * 3 + 2]), 2.0) * + pow(sin(q[0 * 3 + 0]), 2.0) + + pow(cos(q[0 * 3 + 0]), 2.0) * pow(sin(q[0 * 3 + 1]), 2.0) * + pow(sin(q[0 * 3 + 2]), 2.0) + + pow(cos(q[0 * 3 + 1]), 2.0) * pow(sin(q[0 * 3 + 0]), 2.0) * + pow(sin(q[0 * 3 + 2]), 2.0) + + pow(cos(q[0 * 3 + 2]), 2.0) * pow(sin(q[0 * 3 + 0]), 2.0) * + pow(sin(q[0 * 3 + 1]), 2.0) + + pow(sin(q[0 * 3 + 0]), 2.0) * pow(sin(q[0 * 3 + 1]), 2.0) * + pow(sin(q[0 * 3 + 2]), 2.0) + + pow(cos(q[0 * 3 + 0]), 2.0) * pow(cos(q[0 * 3 + 1]), 2.0) * + pow(cos(q[0 * 3 + 2]), 2.0)) * + 2.0) + + (pow(sin(q[0 * 3 + 0]), 2.0) * + (cos(q[0 * 3 + 1]) * sin(q[0 * 3 + 2]) + + cos(q[0 * 3 + 2]) * sin(q[0 * 3 + 1])) * + (l[0 * 3 + 0] * pow(sin(q[0 * 3 + 1]), 2.0) + + l[1 * 3 + 0] * cos(q[0 * 3 + 1]) + + l[1 * 3 + 2] * sin(q[0 * 3 + 1]) + + l[0 * 3 + 0] * pow(cos(q[0 * 3 + 1]), 2.0))) / + ((pow(cos(q[0 * 3 + 0]), 2.0) * pow(cos(q[0 * 3 + 1]), 2.0) + + pow(cos(q[0 * 3 + 0]), 2.0) * pow(sin(q[0 * 3 + 1]), 2.0) + + pow(cos(q[0 * 3 + 1]), 2.0) * pow(sin(q[0 * 3 + 0]), 2.0) + + pow(sin(q[0 * 3 + 0]), 2.0) * + pow(sin(q[0 * 3 + 1]), 2.0)) * + (pow(cos(q[0 * 3 + 0]), 2.0) * pow(cos(q[0 * 3 + 1]), 2.0) * + pow(sin(q[0 * 3 + 2]), 2.0) + + pow(cos(q[0 * 3 + 0]), 2.0) * pow(cos(q[0 * 3 + 2]), 2.0) * + pow(sin(q[0 * 3 + 1]), 2.0) + + pow(cos(q[0 * 3 + 1]), 2.0) * pow(cos(q[0 * 3 + 2]), 2.0) * + pow(sin(q[0 * 3 + 0]), 2.0) + + pow(cos(q[0 * 3 + 0]), 2.0) * pow(sin(q[0 * 3 + 1]), 2.0) * + pow(sin(q[0 * 3 + 2]), 2.0) + + pow(cos(q[0 * 3 + 1]), 2.0) * pow(sin(q[0 * 3 + 0]), 2.0) * + pow(sin(q[0 * 3 + 2]), 2.0) + + pow(cos(q[0 * 3 + 2]), 2.0) * pow(sin(q[0 * 3 + 0]), 2.0) * + pow(sin(q[0 * 3 + 1]), 2.0) + + pow(sin(q[0 * 3 + 0]), 2.0) * pow(sin(q[0 * 3 + 1]), 2.0) * + pow(sin(q[0 * 3 + 2]), 2.0) + + pow(cos(q[0 * 3 + 0]), 2.0) * pow(cos(q[0 * 3 + 1]), 2.0) * + pow(cos(q[0 * 3 + 2]), 2.0)) * + 2.0))) / + (pow(cos(q[0 * 3 + 0]), 2.0) * pow(cos(q[0 * 3 + 1]), 2.0) * + pow(sin(q[0 * 3 + 2]), 2.0) + + pow(cos(q[0 * 3 + 0]), 2.0) * pow(cos(q[0 * 3 + 2]), 2.0) * + pow(sin(q[0 * 3 + 1]), 2.0) + + pow(cos(q[0 * 3 + 1]), 2.0) * pow(cos(q[0 * 3 + 2]), 2.0) * + pow(sin(q[0 * 3 + 0]), 2.0) + + pow(cos(q[0 * 3 + 0]), 2.0) * pow(sin(q[0 * 3 + 1]), 2.0) * + pow(sin(q[0 * 3 + 2]), 2.0) + + pow(cos(q[0 * 3 + 1]), 2.0) * pow(sin(q[0 * 3 + 0]), 2.0) * + pow(sin(q[0 * 3 + 2]), 2.0) + + pow(cos(q[0 * 3 + 2]), 2.0) * pow(sin(q[0 * 3 + 0]), 2.0) * + pow(sin(q[0 * 3 + 1]), 2.0) + + pow(sin(q[0 * 3 + 0]), 2.0) * pow(sin(q[0 * 3 + 1]), 2.0) * + pow(sin(q[0 * 3 + 2]), 2.0) + + pow(cos(q[0 * 3 + 0]), 2.0) * pow(cos(q[0 * 3 + 1]), 2.0) * + pow(cos(q[0 * 3 + 2]), 2.0))) - + (cos(q[0 * 3 + 0]) * (l[0 * 3 + 2] + l[1 * 3 + 2] * cos(q[0 * 3 + 1]) - + l[1 * 3 + 0] * sin(q[0 * 3 + 1]))) / + (pow(cos(q[0 * 3 + 0]), 2.0) + pow(sin(q[0 * 3 + 0]), 2.0)) + + ((cos(q[0 * 3 + 1]) * sin(q[0 * 3 + 2]) + + cos(q[0 * 3 + 2]) * sin(q[0 * 3 + 1])) * + (-l[0 * 3 + 0] * cos(q[0 * 3 + 0]) * cos(q[0 * 3 + 1]) * + cos(q[0 * 3 + 2]) + + l[0 * 3 + 2] * cos(q[0 * 3 + 0]) * cos(q[0 * 3 + 1]) * + sin(q[0 * 3 + 2]) + + l[0 * 3 + 2] * cos(q[0 * 3 + 0]) * cos(q[0 * 3 + 2]) * + sin(q[0 * 3 + 1]) + + l[0 * 3 + 0] * cos(q[0 * 3 + 0]) * sin(q[0 * 3 + 1]) * + sin(q[0 * 3 + 2]) - + l[1 * 3 + 0] * cos(q[0 * 3 + 0]) * pow(cos(q[0 * 3 + 1]), 2.0) * + cos(q[0 * 3 + 2]) - + l[1 * 3 + 0] * cos(q[0 * 3 + 0]) * cos(q[0 * 3 + 2]) * + pow(sin(q[0 * 3 + 1]), 2.0) + + l[1 * 3 + 2] * cos(q[0 * 3 + 0]) * pow(cos(q[0 * 3 + 1]), 2.0) * + sin(q[0 * 3 + 2]) + + l[1 * 3 + 2] * cos(q[0 * 3 + 0]) * pow(sin(q[0 * 3 + 1]), 2.0) * + sin(q[0 * 3 + 2]) + + RL * l[0 * 3 + 1] * cos(q[0 * 3 + 1]) * sin(q[0 * 3 + 0]) * + sin(q[0 * 3 + 2]) + + RL * l[0 * 3 + 1] * cos(q[0 * 3 + 2]) * sin(q[0 * 3 + 0]) * + sin(q[0 * 3 + 1]) + + RL * l[1 * 3 + 1] * cos(q[0 * 3 + 1]) * sin(q[0 * 3 + 0]) * + sin(q[0 * 3 + 2]) + + RL * l[1 * 3 + 1] * cos(q[0 * 3 + 2]) * sin(q[0 * 3 + 0]) * + sin(q[0 * 3 + 1]))) / + (pow(cos(q[0 * 3 + 0]), 2.0) * pow(cos(q[0 * 3 + 1]), 2.0) * + pow(sin(q[0 * 3 + 2]), 2.0) + + pow(cos(q[0 * 3 + 0]), 2.0) * pow(cos(q[0 * 3 + 2]), 2.0) * + pow(sin(q[0 * 3 + 1]), 2.0) + + pow(cos(q[0 * 3 + 1]), 2.0) * pow(cos(q[0 * 3 + 2]), 2.0) * + pow(sin(q[0 * 3 + 0]), 2.0) + + pow(cos(q[0 * 3 + 0]), 2.0) * pow(sin(q[0 * 3 + 1]), 2.0) * + pow(sin(q[0 * 3 + 2]), 2.0) + + pow(cos(q[0 * 3 + 1]), 2.0) * pow(sin(q[0 * 3 + 0]), 2.0) * + pow(sin(q[0 * 3 + 2]), 2.0) + + pow(cos(q[0 * 3 + 2]), 2.0) * pow(sin(q[0 * 3 + 0]), 2.0) * + pow(sin(q[0 * 3 + 1]), 2.0) + + pow(sin(q[0 * 3 + 0]), 2.0) * pow(sin(q[0 * 3 + 1]), 2.0) * + pow(sin(q[0 * 3 + 2]), 2.0) + + pow(cos(q[0 * 3 + 0]), 2.0) * pow(cos(q[0 * 3 + 1]), 2.0) * + pow(cos(q[0 * 3 + 2]), 2.0)) + + ((cos(q[0 * 3 + 1]) * cos(q[0 * 3 + 2]) - + sin(q[0 * 3 + 1]) * sin(q[0 * 3 + 2])) * + (l[0 * 3 + 2] * cos(q[0 * 3 + 0]) * cos(q[0 * 3 + 1]) * + cos(q[0 * 3 + 2]) + + l[0 * 3 + 0] * cos(q[0 * 3 + 0]) * cos(q[0 * 3 + 1]) * + sin(q[0 * 3 + 2]) + + l[0 * 3 + 0] * cos(q[0 * 3 + 0]) * cos(q[0 * 3 + 2]) * + sin(q[0 * 3 + 1]) - + l[0 * 3 + 2] * cos(q[0 * 3 + 0]) * sin(q[0 * 3 + 1]) * + sin(q[0 * 3 + 2]) + + l[1 * 3 + 2] * cos(q[0 * 3 + 0]) * pow(cos(q[0 * 3 + 1]), 2.0) * + cos(q[0 * 3 + 2]) + + l[1 * 3 + 0] * cos(q[0 * 3 + 0]) * pow(cos(q[0 * 3 + 1]), 2.0) * + sin(q[0 * 3 + 2]) + + l[1 * 3 + 2] * cos(q[0 * 3 + 0]) * cos(q[0 * 3 + 2]) * + pow(sin(q[0 * 3 + 1]), 2.0) + + l[1 * 3 + 0] * cos(q[0 * 3 + 0]) * pow(sin(q[0 * 3 + 1]), 2.0) * + sin(q[0 * 3 + 2]) + + RL * l[0 * 3 + 1] * cos(q[0 * 3 + 1]) * cos(q[0 * 3 + 2]) * + sin(q[0 * 3 + 0]) + + RL * l[1 * 3 + 1] * cos(q[0 * 3 + 1]) * cos(q[0 * 3 + 2]) * + sin(q[0 * 3 + 0]) - + RL * l[0 * 3 + 1] * sin(q[0 * 3 + 0]) * sin(q[0 * 3 + 1]) * + sin(q[0 * 3 + 2]) - + RL * l[1 * 3 + 1] * sin(q[0 * 3 + 0]) * sin(q[0 * 3 + 1]) * + sin(q[0 * 3 + 2]))) / + (pow(cos(q[0 * 3 + 0]), 2.0) * pow(cos(q[0 * 3 + 1]), 2.0) * + pow(sin(q[0 * 3 + 2]), 2.0) + + pow(cos(q[0 * 3 + 0]), 2.0) * pow(cos(q[0 * 3 + 2]), 2.0) * + pow(sin(q[0 * 3 + 1]), 2.0) + + pow(cos(q[0 * 3 + 1]), 2.0) * pow(cos(q[0 * 3 + 2]), 2.0) * + pow(sin(q[0 * 3 + 0]), 2.0) + + pow(cos(q[0 * 3 + 0]), 2.0) * pow(sin(q[0 * 3 + 1]), 2.0) * + pow(sin(q[0 * 3 + 2]), 2.0) + + pow(cos(q[0 * 3 + 1]), 2.0) * pow(sin(q[0 * 3 + 0]), 2.0) * + pow(sin(q[0 * 3 + 2]), 2.0) + + pow(cos(q[0 * 3 + 2]), 2.0) * pow(sin(q[0 * 3 + 0]), 2.0) * + pow(sin(q[0 * 3 + 1]), 2.0) + + pow(sin(q[0 * 3 + 0]), 2.0) * pow(sin(q[0 * 3 + 1]), 2.0) * + pow(sin(q[0 * 3 + 2]), 2.0) + + pow(cos(q[0 * 3 + 0]), 2.0) * pow(cos(q[0 * 3 + 1]), 2.0) * + pow(cos(q[0 * 3 + 2]), 2.0)) - + (sin(q[0 * 3 + 0]) * + (cos(q[0 * 3 + 1]) * sin(q[0 * 3 + 2]) + + cos(q[0 * 3 + 2]) * sin(q[0 * 3 + 1])) * + (RL * l[0 * 3 + 1] * cos(q[0 * 3 + 1]) * sin(q[0 * 3 + 2]) + + RL * l[0 * 3 + 1] * cos(q[0 * 3 + 2]) * sin(q[0 * 3 + 1]) + + RL * l[1 * 3 + 1] * cos(q[0 * 3 + 1]) * sin(q[0 * 3 + 2]) + + RL * l[1 * 3 + 1] * cos(q[0 * 3 + 2]) * sin(q[0 * 3 + 1]))) / + (pow(cos(q[0 * 3 + 0]), 2.0) * pow(cos(q[0 * 3 + 1]), 2.0) * + pow(sin(q[0 * 3 + 2]), 2.0) + + pow(cos(q[0 * 3 + 0]), 2.0) * pow(cos(q[0 * 3 + 2]), 2.0) * + pow(sin(q[0 * 3 + 1]), 2.0) + + pow(cos(q[0 * 3 + 1]), 2.0) * pow(cos(q[0 * 3 + 2]), 2.0) * + pow(sin(q[0 * 3 + 0]), 2.0) + + pow(cos(q[0 * 3 + 0]), 2.0) * pow(sin(q[0 * 3 + 1]), 2.0) * + pow(sin(q[0 * 3 + 2]), 2.0) + + pow(cos(q[0 * 3 + 1]), 2.0) * pow(sin(q[0 * 3 + 0]), 2.0) * + pow(sin(q[0 * 3 + 2]), 2.0) + + pow(cos(q[0 * 3 + 2]), 2.0) * pow(sin(q[0 * 3 + 0]), 2.0) * + pow(sin(q[0 * 3 + 1]), 2.0) + + pow(sin(q[0 * 3 + 0]), 2.0) * pow(sin(q[0 * 3 + 1]), 2.0) * + pow(sin(q[0 * 3 + 2]), 2.0) + + pow(cos(q[0 * 3 + 0]), 2.0) * pow(cos(q[0 * 3 + 1]), 2.0) * + pow(cos(q[0 * 3 + 2]), 2.0)) - + (sin(q[0 * 3 + 0]) * + (cos(q[0 * 3 + 1]) * cos(q[0 * 3 + 2]) - + sin(q[0 * 3 + 1]) * sin(q[0 * 3 + 2])) * + (RL * l[0 * 3 + 1] * cos(q[0 * 3 + 1]) * cos(q[0 * 3 + 2]) + + RL * l[1 * 3 + 1] * cos(q[0 * 3 + 1]) * cos(q[0 * 3 + 2]) - + RL * l[0 * 3 + 1] * sin(q[0 * 3 + 1]) * sin(q[0 * 3 + 2]) - + RL * l[1 * 3 + 1] * sin(q[0 * 3 + 1]) * sin(q[0 * 3 + 2]))) / + (pow(cos(q[0 * 3 + 0]), 2.0) * pow(cos(q[0 * 3 + 1]), 2.0) * + pow(sin(q[0 * 3 + 2]), 2.0) + + pow(cos(q[0 * 3 + 0]), 2.0) * pow(cos(q[0 * 3 + 2]), 2.0) * + pow(sin(q[0 * 3 + 1]), 2.0) + + pow(cos(q[0 * 3 + 1]), 2.0) * pow(cos(q[0 * 3 + 2]), 2.0) * + pow(sin(q[0 * 3 + 0]), 2.0) + + pow(cos(q[0 * 3 + 0]), 2.0) * pow(sin(q[0 * 3 + 1]), 2.0) * + pow(sin(q[0 * 3 + 2]), 2.0) + + pow(cos(q[0 * 3 + 1]), 2.0) * pow(sin(q[0 * 3 + 0]), 2.0) * + pow(sin(q[0 * 3 + 2]), 2.0) + + pow(cos(q[0 * 3 + 2]), 2.0) * pow(sin(q[0 * 3 + 0]), 2.0) * + pow(sin(q[0 * 3 + 1]), 2.0) + + pow(sin(q[0 * 3 + 0]), 2.0) * pow(sin(q[0 * 3 + 1]), 2.0) * + pow(sin(q[0 * 3 + 2]), 2.0) + + pow(cos(q[0 * 3 + 0]), 2.0) * pow(cos(q[0 * 3 + 1]), 2.0) * + pow(cos(q[0 * 3 + 2]), 2.0)); + F(1 * 3 + 1) = + -(sin(q[0 * 3 + 0]) * + (l[0 * 3 + 0] * pow(sin(q[0 * 3 + 1]), 2.0) + + l[1 * 3 + 0] * cos(q[0 * 3 + 1]) + l[1 * 3 + 2] * sin(q[0 * 3 + 1]) + + l[0 * 3 + 0] * pow(cos(q[0 * 3 + 1]), 2.0))) / + (pow(cos(q[0 * 3 + 0]), 2.0) * pow(cos(q[0 * 3 + 1]), 2.0) + + pow(cos(q[0 * 3 + 0]), 2.0) * pow(sin(q[0 * 3 + 1]), 2.0) + + pow(cos(q[0 * 3 + 1]), 2.0) * pow(sin(q[0 * 3 + 0]), 2.0) + + pow(sin(q[0 * 3 + 0]), 2.0) * pow(sin(q[0 * 3 + 1]), 2.0)) + + (sin(q[0 * 3 + 0]) * + (((cos(q[0 * 3 + 1]) * cos(q[0 * 3 + 2]) - + sin(q[0 * 3 + 1]) * sin(q[0 * 3 + 2])) * + (l[2 * 3 + 0] - + (cos(q[0 * 3 + 0]) * + (-l[0 * 3 + 0] * cos(q[0 * 3 + 0]) * cos(q[0 * 3 + 1]) * + cos(q[0 * 3 + 2]) + + l[0 * 3 + 2] * cos(q[0 * 3 + 0]) * cos(q[0 * 3 + 1]) * + sin(q[0 * 3 + 2]) + + l[0 * 3 + 2] * cos(q[0 * 3 + 0]) * cos(q[0 * 3 + 2]) * + sin(q[0 * 3 + 1]) + + l[0 * 3 + 0] * cos(q[0 * 3 + 0]) * sin(q[0 * 3 + 1]) * + sin(q[0 * 3 + 2]) - + l[1 * 3 + 0] * cos(q[0 * 3 + 0]) * pow(cos(q[0 * 3 + 1]), 2.0) * + cos(q[0 * 3 + 2]) - + l[1 * 3 + 0] * cos(q[0 * 3 + 0]) * cos(q[0 * 3 + 2]) * + pow(sin(q[0 * 3 + 1]), 2.0) + + l[1 * 3 + 2] * cos(q[0 * 3 + 0]) * pow(cos(q[0 * 3 + 1]), 2.0) * + sin(q[0 * 3 + 2]) + + l[1 * 3 + 2] * cos(q[0 * 3 + 0]) * pow(sin(q[0 * 3 + 1]), 2.0) * + sin(q[0 * 3 + 2]) + + RL * l[0 * 3 + 1] * cos(q[0 * 3 + 1]) * sin(q[0 * 3 + 0]) * + sin(q[0 * 3 + 2]) + + RL * l[0 * 3 + 1] * cos(q[0 * 3 + 2]) * sin(q[0 * 3 + 0]) * + sin(q[0 * 3 + 1]) + + RL * l[1 * 3 + 1] * cos(q[0 * 3 + 1]) * sin(q[0 * 3 + 0]) * + sin(q[0 * 3 + 2]) + + RL * l[1 * 3 + 1] * cos(q[0 * 3 + 2]) * sin(q[0 * 3 + 0]) * + sin(q[0 * 3 + 1]))) / + ((pow(cos(q[0 * 3 + 0]), 2.0) + pow(sin(q[0 * 3 + 0]), 2.0)) * + (pow(cos(q[0 * 3 + 0]), 2.0) * pow(cos(q[0 * 3 + 1]), 2.0) * + pow(sin(q[0 * 3 + 2]), 2.0) + + pow(cos(q[0 * 3 + 0]), 2.0) * pow(cos(q[0 * 3 + 2]), 2.0) * + pow(sin(q[0 * 3 + 1]), 2.0) + + pow(cos(q[0 * 3 + 1]), 2.0) * pow(cos(q[0 * 3 + 2]), 2.0) * + pow(sin(q[0 * 3 + 0]), 2.0) + + pow(cos(q[0 * 3 + 0]), 2.0) * pow(sin(q[0 * 3 + 1]), 2.0) * + pow(sin(q[0 * 3 + 2]), 2.0) + + pow(cos(q[0 * 3 + 1]), 2.0) * pow(sin(q[0 * 3 + 0]), 2.0) * + pow(sin(q[0 * 3 + 2]), 2.0) + + pow(cos(q[0 * 3 + 2]), 2.0) * pow(sin(q[0 * 3 + 0]), 2.0) * + pow(sin(q[0 * 3 + 1]), 2.0) + + pow(sin(q[0 * 3 + 0]), 2.0) * pow(sin(q[0 * 3 + 1]), 2.0) * + pow(sin(q[0 * 3 + 2]), 2.0) + + pow(cos(q[0 * 3 + 0]), 2.0) * pow(cos(q[0 * 3 + 1]), 2.0) * + pow(cos(q[0 * 3 + 2]), 2.0)) * + 2.0) + + (sin(q[0 * 3 + 0]) * + (l[0 * 3 + 0] * cos(q[0 * 3 + 1]) * cos(q[0 * 3 + 2]) * + sin(q[0 * 3 + 0]) - + l[0 * 3 + 2] * cos(q[0 * 3 + 1]) * sin(q[0 * 3 + 0]) * + sin(q[0 * 3 + 2]) - + l[0 * 3 + 2] * cos(q[0 * 3 + 2]) * sin(q[0 * 3 + 0]) * + sin(q[0 * 3 + 1]) - + l[0 * 3 + 0] * sin(q[0 * 3 + 0]) * sin(q[0 * 3 + 1]) * + sin(q[0 * 3 + 2]) + + l[1 * 3 + 0] * pow(cos(q[0 * 3 + 1]), 2.0) * cos(q[0 * 3 + 2]) * + sin(q[0 * 3 + 0]) + + l[1 * 3 + 0] * cos(q[0 * 3 + 2]) * sin(q[0 * 3 + 0]) * + pow(sin(q[0 * 3 + 1]), 2.0) - + l[1 * 3 + 2] * pow(cos(q[0 * 3 + 1]), 2.0) * sin(q[0 * 3 + 0]) * + sin(q[0 * 3 + 2]) - + l[1 * 3 + 2] * sin(q[0 * 3 + 0]) * pow(sin(q[0 * 3 + 1]), 2.0) * + sin(q[0 * 3 + 2]) + + RL * l[0 * 3 + 1] * cos(q[0 * 3 + 0]) * cos(q[0 * 3 + 1]) * + sin(q[0 * 3 + 2]) + + RL * l[0 * 3 + 1] * cos(q[0 * 3 + 0]) * cos(q[0 * 3 + 2]) * + sin(q[0 * 3 + 1]) + + RL * l[1 * 3 + 1] * cos(q[0 * 3 + 0]) * cos(q[0 * 3 + 1]) * + sin(q[0 * 3 + 2]) + + RL * l[1 * 3 + 1] * cos(q[0 * 3 + 0]) * cos(q[0 * 3 + 2]) * + sin(q[0 * 3 + 1]))) / + ((pow(cos(q[0 * 3 + 0]), 2.0) + pow(sin(q[0 * 3 + 0]), 2.0)) * + (pow(cos(q[0 * 3 + 0]), 2.0) * pow(cos(q[0 * 3 + 1]), 2.0) * + pow(sin(q[0 * 3 + 2]), 2.0) + + pow(cos(q[0 * 3 + 0]), 2.0) * pow(cos(q[0 * 3 + 2]), 2.0) * + pow(sin(q[0 * 3 + 1]), 2.0) + + pow(cos(q[0 * 3 + 1]), 2.0) * pow(cos(q[0 * 3 + 2]), 2.0) * + pow(sin(q[0 * 3 + 0]), 2.0) + + pow(cos(q[0 * 3 + 0]), 2.0) * pow(sin(q[0 * 3 + 1]), 2.0) * + pow(sin(q[0 * 3 + 2]), 2.0) + + pow(cos(q[0 * 3 + 1]), 2.0) * pow(sin(q[0 * 3 + 0]), 2.0) * + pow(sin(q[0 * 3 + 2]), 2.0) + + pow(cos(q[0 * 3 + 2]), 2.0) * pow(sin(q[0 * 3 + 0]), 2.0) * + pow(sin(q[0 * 3 + 1]), 2.0) + + pow(sin(q[0 * 3 + 0]), 2.0) * pow(sin(q[0 * 3 + 1]), 2.0) * + pow(sin(q[0 * 3 + 2]), 2.0) + + pow(cos(q[0 * 3 + 0]), 2.0) * pow(cos(q[0 * 3 + 1]), 2.0) * + pow(cos(q[0 * 3 + 2]), 2.0)) * + 2.0) - + ((cos(q[0 * 3 + 1]) * sin(q[0 * 3 + 2]) + + cos(q[0 * 3 + 2]) * sin(q[0 * 3 + 1])) * + (l[0 * 3 + 2] * pow(sin(q[0 * 3 + 1]), 2.0) + + l[1 * 3 + 2] * cos(q[0 * 3 + 1]) - + l[1 * 3 + 0] * sin(q[0 * 3 + 1]) + + l[0 * 3 + 2] * pow(cos(q[0 * 3 + 1]), 2.0))) / + ((pow(cos(q[0 * 3 + 1]), 2.0) + pow(sin(q[0 * 3 + 1]), 2.0)) * + (pow(cos(q[0 * 3 + 1]), 2.0) * pow(cos(q[0 * 3 + 2]), 2.0) + + pow(cos(q[0 * 3 + 1]), 2.0) * pow(sin(q[0 * 3 + 2]), 2.0) + + pow(cos(q[0 * 3 + 2]), 2.0) * pow(sin(q[0 * 3 + 1]), 2.0) + + pow(sin(q[0 * 3 + 1]), 2.0) * pow(sin(q[0 * 3 + 2]), 2.0)) * + 2.0) + + (pow(cos(q[0 * 3 + 0]), 2.0) * + (cos(q[0 * 3 + 1]) * cos(q[0 * 3 + 2]) - + sin(q[0 * 3 + 1]) * sin(q[0 * 3 + 2])) * + (l[0 * 3 + 0] * pow(sin(q[0 * 3 + 1]), 2.0) + + l[1 * 3 + 0] * cos(q[0 * 3 + 1]) + + l[1 * 3 + 2] * sin(q[0 * 3 + 1]) + + l[0 * 3 + 0] * pow(cos(q[0 * 3 + 1]), 2.0))) / + ((pow(cos(q[0 * 3 + 0]), 2.0) * pow(cos(q[0 * 3 + 1]), 2.0) + + pow(cos(q[0 * 3 + 0]), 2.0) * pow(sin(q[0 * 3 + 1]), 2.0) + + pow(cos(q[0 * 3 + 1]), 2.0) * pow(sin(q[0 * 3 + 0]), 2.0) + + pow(sin(q[0 * 3 + 0]), 2.0) * pow(sin(q[0 * 3 + 1]), 2.0)) * + (pow(cos(q[0 * 3 + 0]), 2.0) * pow(cos(q[0 * 3 + 1]), 2.0) * + pow(sin(q[0 * 3 + 2]), 2.0) + + pow(cos(q[0 * 3 + 0]), 2.0) * pow(cos(q[0 * 3 + 2]), 2.0) * + pow(sin(q[0 * 3 + 1]), 2.0) + + pow(cos(q[0 * 3 + 1]), 2.0) * pow(cos(q[0 * 3 + 2]), 2.0) * + pow(sin(q[0 * 3 + 0]), 2.0) + + pow(cos(q[0 * 3 + 0]), 2.0) * pow(sin(q[0 * 3 + 1]), 2.0) * + pow(sin(q[0 * 3 + 2]), 2.0) + + pow(cos(q[0 * 3 + 1]), 2.0) * pow(sin(q[0 * 3 + 0]), 2.0) * + pow(sin(q[0 * 3 + 2]), 2.0) + + pow(cos(q[0 * 3 + 2]), 2.0) * pow(sin(q[0 * 3 + 0]), 2.0) * + pow(sin(q[0 * 3 + 1]), 2.0) + + pow(sin(q[0 * 3 + 0]), 2.0) * pow(sin(q[0 * 3 + 1]), 2.0) * + pow(sin(q[0 * 3 + 2]), 2.0) + + pow(cos(q[0 * 3 + 0]), 2.0) * pow(cos(q[0 * 3 + 1]), 2.0) * + pow(cos(q[0 * 3 + 2]), 2.0)) * + 2.0) + + (pow(sin(q[0 * 3 + 0]), 2.0) * + (cos(q[0 * 3 + 1]) * cos(q[0 * 3 + 2]) - + sin(q[0 * 3 + 1]) * sin(q[0 * 3 + 2])) * + (l[0 * 3 + 0] * pow(sin(q[0 * 3 + 1]), 2.0) + + l[1 * 3 + 0] * cos(q[0 * 3 + 1]) + + l[1 * 3 + 2] * sin(q[0 * 3 + 1]) + + l[0 * 3 + 0] * pow(cos(q[0 * 3 + 1]), 2.0))) / + ((pow(cos(q[0 * 3 + 0]), 2.0) * pow(cos(q[0 * 3 + 1]), 2.0) + + pow(cos(q[0 * 3 + 0]), 2.0) * pow(sin(q[0 * 3 + 1]), 2.0) + + pow(cos(q[0 * 3 + 1]), 2.0) * pow(sin(q[0 * 3 + 0]), 2.0) + + pow(sin(q[0 * 3 + 0]), 2.0) * pow(sin(q[0 * 3 + 1]), 2.0)) * + (pow(cos(q[0 * 3 + 0]), 2.0) * pow(cos(q[0 * 3 + 1]), 2.0) * + pow(sin(q[0 * 3 + 2]), 2.0) + + pow(cos(q[0 * 3 + 0]), 2.0) * pow(cos(q[0 * 3 + 2]), 2.0) * + pow(sin(q[0 * 3 + 1]), 2.0) + + pow(cos(q[0 * 3 + 1]), 2.0) * pow(cos(q[0 * 3 + 2]), 2.0) * + pow(sin(q[0 * 3 + 0]), 2.0) + + pow(cos(q[0 * 3 + 0]), 2.0) * pow(sin(q[0 * 3 + 1]), 2.0) * + pow(sin(q[0 * 3 + 2]), 2.0) + + pow(cos(q[0 * 3 + 1]), 2.0) * pow(sin(q[0 * 3 + 0]), 2.0) * + pow(sin(q[0 * 3 + 2]), 2.0) + + pow(cos(q[0 * 3 + 2]), 2.0) * pow(sin(q[0 * 3 + 0]), 2.0) * + pow(sin(q[0 * 3 + 1]), 2.0) + + pow(sin(q[0 * 3 + 0]), 2.0) * pow(sin(q[0 * 3 + 1]), 2.0) * + pow(sin(q[0 * 3 + 2]), 2.0) + + pow(cos(q[0 * 3 + 0]), 2.0) * pow(cos(q[0 * 3 + 1]), 2.0) * + pow(cos(q[0 * 3 + 2]), 2.0)) * + 2.0))) / + (pow(cos(q[0 * 3 + 1]), 2.0) * pow(cos(q[0 * 3 + 2]), 2.0) + + pow(cos(q[0 * 3 + 1]), 2.0) * pow(sin(q[0 * 3 + 2]), 2.0) + + pow(cos(q[0 * 3 + 2]), 2.0) * pow(sin(q[0 * 3 + 1]), 2.0) + + pow(sin(q[0 * 3 + 1]), 2.0) * pow(sin(q[0 * 3 + 2]), 2.0)) + + ((cos(q[0 * 3 + 1]) * sin(q[0 * 3 + 2]) + + cos(q[0 * 3 + 2]) * sin(q[0 * 3 + 1])) * + (l[2 * 3 + 2] + + (cos(q[0 * 3 + 0]) * + (l[0 * 3 + 2] * cos(q[0 * 3 + 0]) * cos(q[0 * 3 + 1]) * + cos(q[0 * 3 + 2]) + + l[0 * 3 + 0] * cos(q[0 * 3 + 0]) * cos(q[0 * 3 + 1]) * + sin(q[0 * 3 + 2]) + + l[0 * 3 + 0] * cos(q[0 * 3 + 0]) * cos(q[0 * 3 + 2]) * + sin(q[0 * 3 + 1]) - + l[0 * 3 + 2] * cos(q[0 * 3 + 0]) * sin(q[0 * 3 + 1]) * + sin(q[0 * 3 + 2]) + + l[1 * 3 + 2] * cos(q[0 * 3 + 0]) * pow(cos(q[0 * 3 + 1]), 2.0) * + cos(q[0 * 3 + 2]) + + l[1 * 3 + 0] * cos(q[0 * 3 + 0]) * pow(cos(q[0 * 3 + 1]), 2.0) * + sin(q[0 * 3 + 2]) + + l[1 * 3 + 2] * cos(q[0 * 3 + 0]) * cos(q[0 * 3 + 2]) * + pow(sin(q[0 * 3 + 1]), 2.0) + + l[1 * 3 + 0] * cos(q[0 * 3 + 0]) * pow(sin(q[0 * 3 + 1]), 2.0) * + sin(q[0 * 3 + 2]) + + RL * l[0 * 3 + 1] * cos(q[0 * 3 + 1]) * cos(q[0 * 3 + 2]) * + sin(q[0 * 3 + 0]) + + RL * l[1 * 3 + 1] * cos(q[0 * 3 + 1]) * cos(q[0 * 3 + 2]) * + sin(q[0 * 3 + 0]) - + RL * l[0 * 3 + 1] * sin(q[0 * 3 + 0]) * sin(q[0 * 3 + 1]) * + sin(q[0 * 3 + 2]) - + RL * l[1 * 3 + 1] * sin(q[0 * 3 + 0]) * sin(q[0 * 3 + 1]) * + sin(q[0 * 3 + 2]))) / + ((pow(cos(q[0 * 3 + 0]), 2.0) + pow(sin(q[0 * 3 + 0]), 2.0)) * + (pow(cos(q[0 * 3 + 0]), 2.0) * pow(cos(q[0 * 3 + 1]), 2.0) * + pow(sin(q[0 * 3 + 2]), 2.0) + + pow(cos(q[0 * 3 + 0]), 2.0) * pow(cos(q[0 * 3 + 2]), 2.0) * + pow(sin(q[0 * 3 + 1]), 2.0) + + pow(cos(q[0 * 3 + 1]), 2.0) * pow(cos(q[0 * 3 + 2]), 2.0) * + pow(sin(q[0 * 3 + 0]), 2.0) + + pow(cos(q[0 * 3 + 0]), 2.0) * pow(sin(q[0 * 3 + 1]), 2.0) * + pow(sin(q[0 * 3 + 2]), 2.0) + + pow(cos(q[0 * 3 + 1]), 2.0) * pow(sin(q[0 * 3 + 0]), 2.0) * + pow(sin(q[0 * 3 + 2]), 2.0) + + pow(cos(q[0 * 3 + 2]), 2.0) * pow(sin(q[0 * 3 + 0]), 2.0) * + pow(sin(q[0 * 3 + 1]), 2.0) + + pow(sin(q[0 * 3 + 0]), 2.0) * pow(sin(q[0 * 3 + 1]), 2.0) * + pow(sin(q[0 * 3 + 2]), 2.0) + + pow(cos(q[0 * 3 + 0]), 2.0) * pow(cos(q[0 * 3 + 1]), 2.0) * + pow(cos(q[0 * 3 + 2]), 2.0)) * + 2.0) + + (sin(q[0 * 3 + 0]) * + (l[0 * 3 + 2] * cos(q[0 * 3 + 1]) * cos(q[0 * 3 + 2]) * + sin(q[0 * 3 + 0]) + + l[0 * 3 + 0] * cos(q[0 * 3 + 1]) * sin(q[0 * 3 + 0]) * + sin(q[0 * 3 + 2]) + + l[0 * 3 + 0] * cos(q[0 * 3 + 2]) * sin(q[0 * 3 + 0]) * + sin(q[0 * 3 + 1]) - + l[0 * 3 + 2] * sin(q[0 * 3 + 0]) * sin(q[0 * 3 + 1]) * + sin(q[0 * 3 + 2]) + + l[1 * 3 + 2] * pow(cos(q[0 * 3 + 1]), 2.0) * cos(q[0 * 3 + 2]) * + sin(q[0 * 3 + 0]) + + l[1 * 3 + 0] * pow(cos(q[0 * 3 + 1]), 2.0) * sin(q[0 * 3 + 0]) * + sin(q[0 * 3 + 2]) + + l[1 * 3 + 2] * cos(q[0 * 3 + 2]) * sin(q[0 * 3 + 0]) * + pow(sin(q[0 * 3 + 1]), 2.0) + + l[1 * 3 + 0] * sin(q[0 * 3 + 0]) * pow(sin(q[0 * 3 + 1]), 2.0) * + sin(q[0 * 3 + 2]) - + RL * l[0 * 3 + 1] * cos(q[0 * 3 + 0]) * cos(q[0 * 3 + 1]) * + cos(q[0 * 3 + 2]) - + RL * l[1 * 3 + 1] * cos(q[0 * 3 + 0]) * cos(q[0 * 3 + 1]) * + cos(q[0 * 3 + 2]) + + RL * l[0 * 3 + 1] * cos(q[0 * 3 + 0]) * sin(q[0 * 3 + 1]) * + sin(q[0 * 3 + 2]) + + RL * l[1 * 3 + 1] * cos(q[0 * 3 + 0]) * sin(q[0 * 3 + 1]) * + sin(q[0 * 3 + 2]))) / + ((pow(cos(q[0 * 3 + 0]), 2.0) + pow(sin(q[0 * 3 + 0]), 2.0)) * + (pow(cos(q[0 * 3 + 0]), 2.0) * pow(cos(q[0 * 3 + 1]), 2.0) * + pow(sin(q[0 * 3 + 2]), 2.0) + + pow(cos(q[0 * 3 + 0]), 2.0) * pow(cos(q[0 * 3 + 2]), 2.0) * + pow(sin(q[0 * 3 + 1]), 2.0) + + pow(cos(q[0 * 3 + 1]), 2.0) * pow(cos(q[0 * 3 + 2]), 2.0) * + pow(sin(q[0 * 3 + 0]), 2.0) + + pow(cos(q[0 * 3 + 0]), 2.0) * pow(sin(q[0 * 3 + 1]), 2.0) * + pow(sin(q[0 * 3 + 2]), 2.0) + + pow(cos(q[0 * 3 + 1]), 2.0) * pow(sin(q[0 * 3 + 0]), 2.0) * + pow(sin(q[0 * 3 + 2]), 2.0) + + pow(cos(q[0 * 3 + 2]), 2.0) * pow(sin(q[0 * 3 + 0]), 2.0) * + pow(sin(q[0 * 3 + 1]), 2.0) + + pow(sin(q[0 * 3 + 0]), 2.0) * pow(sin(q[0 * 3 + 1]), 2.0) * + pow(sin(q[0 * 3 + 2]), 2.0) + + pow(cos(q[0 * 3 + 0]), 2.0) * pow(cos(q[0 * 3 + 1]), 2.0) * + pow(cos(q[0 * 3 + 2]), 2.0)) * + 2.0) + + ((cos(q[0 * 3 + 1]) * cos(q[0 * 3 + 2]) - + sin(q[0 * 3 + 1]) * sin(q[0 * 3 + 2])) * + (l[0 * 3 + 2] * pow(sin(q[0 * 3 + 1]), 2.0) + + l[1 * 3 + 2] * cos(q[0 * 3 + 1]) - + l[1 * 3 + 0] * sin(q[0 * 3 + 1]) + + l[0 * 3 + 2] * pow(cos(q[0 * 3 + 1]), 2.0))) / + ((pow(cos(q[0 * 3 + 1]), 2.0) + pow(sin(q[0 * 3 + 1]), 2.0)) * + (pow(cos(q[0 * 3 + 1]), 2.0) * pow(cos(q[0 * 3 + 2]), 2.0) + + pow(cos(q[0 * 3 + 1]), 2.0) * pow(sin(q[0 * 3 + 2]), 2.0) + + pow(cos(q[0 * 3 + 2]), 2.0) * pow(sin(q[0 * 3 + 1]), 2.0) + + pow(sin(q[0 * 3 + 1]), 2.0) * pow(sin(q[0 * 3 + 2]), 2.0)) * + 2.0) + + (pow(cos(q[0 * 3 + 0]), 2.0) * + (cos(q[0 * 3 + 1]) * sin(q[0 * 3 + 2]) + + cos(q[0 * 3 + 2]) * sin(q[0 * 3 + 1])) * + (l[0 * 3 + 0] * pow(sin(q[0 * 3 + 1]), 2.0) + + l[1 * 3 + 0] * cos(q[0 * 3 + 1]) + + l[1 * 3 + 2] * sin(q[0 * 3 + 1]) + + l[0 * 3 + 0] * pow(cos(q[0 * 3 + 1]), 2.0))) / + ((pow(cos(q[0 * 3 + 0]), 2.0) * pow(cos(q[0 * 3 + 1]), 2.0) + + pow(cos(q[0 * 3 + 0]), 2.0) * pow(sin(q[0 * 3 + 1]), 2.0) + + pow(cos(q[0 * 3 + 1]), 2.0) * pow(sin(q[0 * 3 + 0]), 2.0) + + pow(sin(q[0 * 3 + 0]), 2.0) * pow(sin(q[0 * 3 + 1]), 2.0)) * + (pow(cos(q[0 * 3 + 0]), 2.0) * pow(cos(q[0 * 3 + 1]), 2.0) * + pow(sin(q[0 * 3 + 2]), 2.0) + + pow(cos(q[0 * 3 + 0]), 2.0) * pow(cos(q[0 * 3 + 2]), 2.0) * + pow(sin(q[0 * 3 + 1]), 2.0) + + pow(cos(q[0 * 3 + 1]), 2.0) * pow(cos(q[0 * 3 + 2]), 2.0) * + pow(sin(q[0 * 3 + 0]), 2.0) + + pow(cos(q[0 * 3 + 0]), 2.0) * pow(sin(q[0 * 3 + 1]), 2.0) * + pow(sin(q[0 * 3 + 2]), 2.0) + + pow(cos(q[0 * 3 + 1]), 2.0) * pow(sin(q[0 * 3 + 0]), 2.0) * + pow(sin(q[0 * 3 + 2]), 2.0) + + pow(cos(q[0 * 3 + 2]), 2.0) * pow(sin(q[0 * 3 + 0]), 2.0) * + pow(sin(q[0 * 3 + 1]), 2.0) + + pow(sin(q[0 * 3 + 0]), 2.0) * pow(sin(q[0 * 3 + 1]), 2.0) * + pow(sin(q[0 * 3 + 2]), 2.0) + + pow(cos(q[0 * 3 + 0]), 2.0) * pow(cos(q[0 * 3 + 1]), 2.0) * + pow(cos(q[0 * 3 + 2]), 2.0)) * + 2.0) + + (pow(sin(q[0 * 3 + 0]), 2.0) * + (cos(q[0 * 3 + 1]) * sin(q[0 * 3 + 2]) + + cos(q[0 * 3 + 2]) * sin(q[0 * 3 + 1])) * + (l[0 * 3 + 0] * pow(sin(q[0 * 3 + 1]), 2.0) + + l[1 * 3 + 0] * cos(q[0 * 3 + 1]) + + l[1 * 3 + 2] * sin(q[0 * 3 + 1]) + + l[0 * 3 + 0] * pow(cos(q[0 * 3 + 1]), 2.0))) / + ((pow(cos(q[0 * 3 + 0]), 2.0) * pow(cos(q[0 * 3 + 1]), 2.0) + + pow(cos(q[0 * 3 + 0]), 2.0) * pow(sin(q[0 * 3 + 1]), 2.0) + + pow(cos(q[0 * 3 + 1]), 2.0) * pow(sin(q[0 * 3 + 0]), 2.0) + + pow(sin(q[0 * 3 + 0]), 2.0) * pow(sin(q[0 * 3 + 1]), 2.0)) * + (pow(cos(q[0 * 3 + 0]), 2.0) * pow(cos(q[0 * 3 + 1]), 2.0) * + pow(sin(q[0 * 3 + 2]), 2.0) + + pow(cos(q[0 * 3 + 0]), 2.0) * pow(cos(q[0 * 3 + 2]), 2.0) * + pow(sin(q[0 * 3 + 1]), 2.0) + + pow(cos(q[0 * 3 + 1]), 2.0) * pow(cos(q[0 * 3 + 2]), 2.0) * + pow(sin(q[0 * 3 + 0]), 2.0) + + pow(cos(q[0 * 3 + 0]), 2.0) * pow(sin(q[0 * 3 + 1]), 2.0) * + pow(sin(q[0 * 3 + 2]), 2.0) + + pow(cos(q[0 * 3 + 1]), 2.0) * pow(sin(q[0 * 3 + 0]), 2.0) * + pow(sin(q[0 * 3 + 2]), 2.0) + + pow(cos(q[0 * 3 + 2]), 2.0) * pow(sin(q[0 * 3 + 0]), 2.0) * + pow(sin(q[0 * 3 + 1]), 2.0) + + pow(sin(q[0 * 3 + 0]), 2.0) * pow(sin(q[0 * 3 + 1]), 2.0) * + pow(sin(q[0 * 3 + 2]), 2.0) + + pow(cos(q[0 * 3 + 0]), 2.0) * pow(cos(q[0 * 3 + 1]), 2.0) * + pow(cos(q[0 * 3 + 2]), 2.0)) * + 2.0))) / + (pow(cos(q[0 * 3 + 1]), 2.0) * pow(cos(q[0 * 3 + 2]), 2.0) + + pow(cos(q[0 * 3 + 1]), 2.0) * pow(sin(q[0 * 3 + 2]), 2.0) + + pow(cos(q[0 * 3 + 2]), 2.0) * pow(sin(q[0 * 3 + 1]), 2.0) + + pow(sin(q[0 * 3 + 1]), 2.0) * pow(sin(q[0 * 3 + 2]), 2.0)))) / + (pow(cos(q[0 * 3 + 0]), 2.0) + pow(sin(q[0 * 3 + 0]), 2.0)) + + (sin(q[0 * 3 + 0]) * + (cos(q[0 * 3 + 1]) * sin(q[0 * 3 + 2]) + + cos(q[0 * 3 + 2]) * sin(q[0 * 3 + 1])) * + (l[1 * 3 + 2] * cos(q[0 * 3 + 2]) + + l[1 * 3 + 0] * sin(q[0 * 3 + 2]))) / + (pow(cos(q[0 * 3 + 0]), 2.0) * pow(cos(q[0 * 3 + 1]), 2.0) * + pow(sin(q[0 * 3 + 2]), 2.0) + + pow(cos(q[0 * 3 + 0]), 2.0) * pow(cos(q[0 * 3 + 2]), 2.0) * + pow(sin(q[0 * 3 + 1]), 2.0) + + pow(cos(q[0 * 3 + 1]), 2.0) * pow(cos(q[0 * 3 + 2]), 2.0) * + pow(sin(q[0 * 3 + 0]), 2.0) + + pow(cos(q[0 * 3 + 0]), 2.0) * pow(sin(q[0 * 3 + 1]), 2.0) * + pow(sin(q[0 * 3 + 2]), 2.0) + + pow(cos(q[0 * 3 + 1]), 2.0) * pow(sin(q[0 * 3 + 0]), 2.0) * + pow(sin(q[0 * 3 + 2]), 2.0) + + pow(cos(q[0 * 3 + 2]), 2.0) * pow(sin(q[0 * 3 + 0]), 2.0) * + pow(sin(q[0 * 3 + 1]), 2.0) + + pow(sin(q[0 * 3 + 0]), 2.0) * pow(sin(q[0 * 3 + 1]), 2.0) * + pow(sin(q[0 * 3 + 2]), 2.0) + + pow(cos(q[0 * 3 + 0]), 2.0) * pow(cos(q[0 * 3 + 1]), 2.0) * + pow(cos(q[0 * 3 + 2]), 2.0)) + + (sin(q[0 * 3 + 0]) * + (cos(q[0 * 3 + 1]) * cos(q[0 * 3 + 2]) - + sin(q[0 * 3 + 1]) * sin(q[0 * 3 + 2])) * + (l[1 * 3 + 0] * cos(q[0 * 3 + 2]) - + l[1 * 3 + 2] * sin(q[0 * 3 + 2]))) / + (pow(cos(q[0 * 3 + 0]), 2.0) * pow(cos(q[0 * 3 + 1]), 2.0) * + pow(sin(q[0 * 3 + 2]), 2.0) + + pow(cos(q[0 * 3 + 0]), 2.0) * pow(cos(q[0 * 3 + 2]), 2.0) * + pow(sin(q[0 * 3 + 1]), 2.0) + + pow(cos(q[0 * 3 + 1]), 2.0) * pow(cos(q[0 * 3 + 2]), 2.0) * + pow(sin(q[0 * 3 + 0]), 2.0) + + pow(cos(q[0 * 3 + 0]), 2.0) * pow(sin(q[0 * 3 + 1]), 2.0) * + pow(sin(q[0 * 3 + 2]), 2.0) + + pow(cos(q[0 * 3 + 1]), 2.0) * pow(sin(q[0 * 3 + 0]), 2.0) * + pow(sin(q[0 * 3 + 2]), 2.0) + + pow(cos(q[0 * 3 + 2]), 2.0) * pow(sin(q[0 * 3 + 0]), 2.0) * + pow(sin(q[0 * 3 + 1]), 2.0) + + pow(sin(q[0 * 3 + 0]), 2.0) * pow(sin(q[0 * 3 + 1]), 2.0) * + pow(sin(q[0 * 3 + 2]), 2.0) + + pow(cos(q[0 * 3 + 0]), 2.0) * pow(cos(q[0 * 3 + 1]), 2.0) * + pow(cos(q[0 * 3 + 2]), 2.0)); + F(2 * 3 + 1) = + -(sin(q[0 * 3 + 0]) * + (l[0 * 3 + 0] * pow(sin(q[0 * 3 + 1]), 2.0) + + l[1 * 3 + 0] * cos(q[0 * 3 + 1]) + l[1 * 3 + 2] * sin(q[0 * 3 + 1]) + + l[0 * 3 + 0] * pow(cos(q[0 * 3 + 1]), 2.0))) / + (pow(cos(q[0 * 3 + 0]), 2.0) * pow(cos(q[0 * 3 + 1]), 2.0) + + pow(cos(q[0 * 3 + 0]), 2.0) * pow(sin(q[0 * 3 + 1]), 2.0) + + pow(cos(q[0 * 3 + 1]), 2.0) * pow(sin(q[0 * 3 + 0]), 2.0) + + pow(sin(q[0 * 3 + 0]), 2.0) * pow(sin(q[0 * 3 + 1]), 2.0)) + + (sin(q[0 * 3 + 0]) * + (((cos(q[0 * 3 + 1]) * cos(q[0 * 3 + 2]) - + sin(q[0 * 3 + 1]) * sin(q[0 * 3 + 2])) * + (l[2 * 3 + 0] - + (cos(q[0 * 3 + 0]) * + (-l[0 * 3 + 0] * cos(q[0 * 3 + 0]) * cos(q[0 * 3 + 1]) * + cos(q[0 * 3 + 2]) + + l[0 * 3 + 2] * cos(q[0 * 3 + 0]) * cos(q[0 * 3 + 1]) * + sin(q[0 * 3 + 2]) + + l[0 * 3 + 2] * cos(q[0 * 3 + 0]) * cos(q[0 * 3 + 2]) * + sin(q[0 * 3 + 1]) + + l[0 * 3 + 0] * cos(q[0 * 3 + 0]) * sin(q[0 * 3 + 1]) * + sin(q[0 * 3 + 2]) - + l[1 * 3 + 0] * cos(q[0 * 3 + 0]) * pow(cos(q[0 * 3 + 1]), 2.0) * + cos(q[0 * 3 + 2]) - + l[1 * 3 + 0] * cos(q[0 * 3 + 0]) * cos(q[0 * 3 + 2]) * + pow(sin(q[0 * 3 + 1]), 2.0) + + l[1 * 3 + 2] * cos(q[0 * 3 + 0]) * pow(cos(q[0 * 3 + 1]), 2.0) * + sin(q[0 * 3 + 2]) + + l[1 * 3 + 2] * cos(q[0 * 3 + 0]) * pow(sin(q[0 * 3 + 1]), 2.0) * + sin(q[0 * 3 + 2]) + + RL * l[0 * 3 + 1] * cos(q[0 * 3 + 1]) * sin(q[0 * 3 + 0]) * + sin(q[0 * 3 + 2]) + + RL * l[0 * 3 + 1] * cos(q[0 * 3 + 2]) * sin(q[0 * 3 + 0]) * + sin(q[0 * 3 + 1]) + + RL * l[1 * 3 + 1] * cos(q[0 * 3 + 1]) * sin(q[0 * 3 + 0]) * + sin(q[0 * 3 + 2]) + + RL * l[1 * 3 + 1] * cos(q[0 * 3 + 2]) * sin(q[0 * 3 + 0]) * + sin(q[0 * 3 + 1]))) / + ((pow(cos(q[0 * 3 + 0]), 2.0) + pow(sin(q[0 * 3 + 0]), 2.0)) * + (pow(cos(q[0 * 3 + 0]), 2.0) * pow(cos(q[0 * 3 + 1]), 2.0) * + pow(sin(q[0 * 3 + 2]), 2.0) + + pow(cos(q[0 * 3 + 0]), 2.0) * pow(cos(q[0 * 3 + 2]), 2.0) * + pow(sin(q[0 * 3 + 1]), 2.0) + + pow(cos(q[0 * 3 + 1]), 2.0) * pow(cos(q[0 * 3 + 2]), 2.0) * + pow(sin(q[0 * 3 + 0]), 2.0) + + pow(cos(q[0 * 3 + 0]), 2.0) * pow(sin(q[0 * 3 + 1]), 2.0) * + pow(sin(q[0 * 3 + 2]), 2.0) + + pow(cos(q[0 * 3 + 1]), 2.0) * pow(sin(q[0 * 3 + 0]), 2.0) * + pow(sin(q[0 * 3 + 2]), 2.0) + + pow(cos(q[0 * 3 + 2]), 2.0) * pow(sin(q[0 * 3 + 0]), 2.0) * + pow(sin(q[0 * 3 + 1]), 2.0) + + pow(sin(q[0 * 3 + 0]), 2.0) * pow(sin(q[0 * 3 + 1]), 2.0) * + pow(sin(q[0 * 3 + 2]), 2.0) + + pow(cos(q[0 * 3 + 0]), 2.0) * pow(cos(q[0 * 3 + 1]), 2.0) * + pow(cos(q[0 * 3 + 2]), 2.0)) * + 2.0) + + (sin(q[0 * 3 + 0]) * + (l[0 * 3 + 0] * cos(q[0 * 3 + 1]) * cos(q[0 * 3 + 2]) * + sin(q[0 * 3 + 0]) - + l[0 * 3 + 2] * cos(q[0 * 3 + 1]) * sin(q[0 * 3 + 0]) * + sin(q[0 * 3 + 2]) - + l[0 * 3 + 2] * cos(q[0 * 3 + 2]) * sin(q[0 * 3 + 0]) * + sin(q[0 * 3 + 1]) - + l[0 * 3 + 0] * sin(q[0 * 3 + 0]) * sin(q[0 * 3 + 1]) * + sin(q[0 * 3 + 2]) + + l[1 * 3 + 0] * pow(cos(q[0 * 3 + 1]), 2.0) * cos(q[0 * 3 + 2]) * + sin(q[0 * 3 + 0]) + + l[1 * 3 + 0] * cos(q[0 * 3 + 2]) * sin(q[0 * 3 + 0]) * + pow(sin(q[0 * 3 + 1]), 2.0) - + l[1 * 3 + 2] * pow(cos(q[0 * 3 + 1]), 2.0) * sin(q[0 * 3 + 0]) * + sin(q[0 * 3 + 2]) - + l[1 * 3 + 2] * sin(q[0 * 3 + 0]) * pow(sin(q[0 * 3 + 1]), 2.0) * + sin(q[0 * 3 + 2]) + + RL * l[0 * 3 + 1] * cos(q[0 * 3 + 0]) * cos(q[0 * 3 + 1]) * + sin(q[0 * 3 + 2]) + + RL * l[0 * 3 + 1] * cos(q[0 * 3 + 0]) * cos(q[0 * 3 + 2]) * + sin(q[0 * 3 + 1]) + + RL * l[1 * 3 + 1] * cos(q[0 * 3 + 0]) * cos(q[0 * 3 + 1]) * + sin(q[0 * 3 + 2]) + + RL * l[1 * 3 + 1] * cos(q[0 * 3 + 0]) * cos(q[0 * 3 + 2]) * + sin(q[0 * 3 + 1]))) / + ((pow(cos(q[0 * 3 + 0]), 2.0) + pow(sin(q[0 * 3 + 0]), 2.0)) * + (pow(cos(q[0 * 3 + 0]), 2.0) * pow(cos(q[0 * 3 + 1]), 2.0) * + pow(sin(q[0 * 3 + 2]), 2.0) + + pow(cos(q[0 * 3 + 0]), 2.0) * pow(cos(q[0 * 3 + 2]), 2.0) * + pow(sin(q[0 * 3 + 1]), 2.0) + + pow(cos(q[0 * 3 + 1]), 2.0) * pow(cos(q[0 * 3 + 2]), 2.0) * + pow(sin(q[0 * 3 + 0]), 2.0) + + pow(cos(q[0 * 3 + 0]), 2.0) * pow(sin(q[0 * 3 + 1]), 2.0) * + pow(sin(q[0 * 3 + 2]), 2.0) + + pow(cos(q[0 * 3 + 1]), 2.0) * pow(sin(q[0 * 3 + 0]), 2.0) * + pow(sin(q[0 * 3 + 2]), 2.0) + + pow(cos(q[0 * 3 + 2]), 2.0) * pow(sin(q[0 * 3 + 0]), 2.0) * + pow(sin(q[0 * 3 + 1]), 2.0) + + pow(sin(q[0 * 3 + 0]), 2.0) * pow(sin(q[0 * 3 + 1]), 2.0) * + pow(sin(q[0 * 3 + 2]), 2.0) + + pow(cos(q[0 * 3 + 0]), 2.0) * pow(cos(q[0 * 3 + 1]), 2.0) * + pow(cos(q[0 * 3 + 2]), 2.0)) * + 2.0) - + ((cos(q[0 * 3 + 1]) * sin(q[0 * 3 + 2]) + + cos(q[0 * 3 + 2]) * sin(q[0 * 3 + 1])) * + (l[0 * 3 + 2] * pow(sin(q[0 * 3 + 1]), 2.0) + + l[1 * 3 + 2] * cos(q[0 * 3 + 1]) - + l[1 * 3 + 0] * sin(q[0 * 3 + 1]) + + l[0 * 3 + 2] * pow(cos(q[0 * 3 + 1]), 2.0))) / + ((pow(cos(q[0 * 3 + 1]), 2.0) + pow(sin(q[0 * 3 + 1]), 2.0)) * + (pow(cos(q[0 * 3 + 1]), 2.0) * pow(cos(q[0 * 3 + 2]), 2.0) + + pow(cos(q[0 * 3 + 1]), 2.0) * pow(sin(q[0 * 3 + 2]), 2.0) + + pow(cos(q[0 * 3 + 2]), 2.0) * pow(sin(q[0 * 3 + 1]), 2.0) + + pow(sin(q[0 * 3 + 1]), 2.0) * pow(sin(q[0 * 3 + 2]), 2.0)) * + 2.0) + + (pow(cos(q[0 * 3 + 0]), 2.0) * + (cos(q[0 * 3 + 1]) * cos(q[0 * 3 + 2]) - + sin(q[0 * 3 + 1]) * sin(q[0 * 3 + 2])) * + (l[0 * 3 + 0] * pow(sin(q[0 * 3 + 1]), 2.0) + + l[1 * 3 + 0] * cos(q[0 * 3 + 1]) + + l[1 * 3 + 2] * sin(q[0 * 3 + 1]) + + l[0 * 3 + 0] * pow(cos(q[0 * 3 + 1]), 2.0))) / + ((pow(cos(q[0 * 3 + 0]), 2.0) * pow(cos(q[0 * 3 + 1]), 2.0) + + pow(cos(q[0 * 3 + 0]), 2.0) * pow(sin(q[0 * 3 + 1]), 2.0) + + pow(cos(q[0 * 3 + 1]), 2.0) * pow(sin(q[0 * 3 + 0]), 2.0) + + pow(sin(q[0 * 3 + 0]), 2.0) * pow(sin(q[0 * 3 + 1]), 2.0)) * + (pow(cos(q[0 * 3 + 0]), 2.0) * pow(cos(q[0 * 3 + 1]), 2.0) * + pow(sin(q[0 * 3 + 2]), 2.0) + + pow(cos(q[0 * 3 + 0]), 2.0) * pow(cos(q[0 * 3 + 2]), 2.0) * + pow(sin(q[0 * 3 + 1]), 2.0) + + pow(cos(q[0 * 3 + 1]), 2.0) * pow(cos(q[0 * 3 + 2]), 2.0) * + pow(sin(q[0 * 3 + 0]), 2.0) + + pow(cos(q[0 * 3 + 0]), 2.0) * pow(sin(q[0 * 3 + 1]), 2.0) * + pow(sin(q[0 * 3 + 2]), 2.0) + + pow(cos(q[0 * 3 + 1]), 2.0) * pow(sin(q[0 * 3 + 0]), 2.0) * + pow(sin(q[0 * 3 + 2]), 2.0) + + pow(cos(q[0 * 3 + 2]), 2.0) * pow(sin(q[0 * 3 + 0]), 2.0) * + pow(sin(q[0 * 3 + 1]), 2.0) + + pow(sin(q[0 * 3 + 0]), 2.0) * pow(sin(q[0 * 3 + 1]), 2.0) * + pow(sin(q[0 * 3 + 2]), 2.0) + + pow(cos(q[0 * 3 + 0]), 2.0) * pow(cos(q[0 * 3 + 1]), 2.0) * + pow(cos(q[0 * 3 + 2]), 2.0)) * + 2.0) + + (pow(sin(q[0 * 3 + 0]), 2.0) * + (cos(q[0 * 3 + 1]) * cos(q[0 * 3 + 2]) - + sin(q[0 * 3 + 1]) * sin(q[0 * 3 + 2])) * + (l[0 * 3 + 0] * pow(sin(q[0 * 3 + 1]), 2.0) + + l[1 * 3 + 0] * cos(q[0 * 3 + 1]) + + l[1 * 3 + 2] * sin(q[0 * 3 + 1]) + + l[0 * 3 + 0] * pow(cos(q[0 * 3 + 1]), 2.0))) / + ((pow(cos(q[0 * 3 + 0]), 2.0) * pow(cos(q[0 * 3 + 1]), 2.0) + + pow(cos(q[0 * 3 + 0]), 2.0) * pow(sin(q[0 * 3 + 1]), 2.0) + + pow(cos(q[0 * 3 + 1]), 2.0) * pow(sin(q[0 * 3 + 0]), 2.0) + + pow(sin(q[0 * 3 + 0]), 2.0) * pow(sin(q[0 * 3 + 1]), 2.0)) * + (pow(cos(q[0 * 3 + 0]), 2.0) * pow(cos(q[0 * 3 + 1]), 2.0) * + pow(sin(q[0 * 3 + 2]), 2.0) + + pow(cos(q[0 * 3 + 0]), 2.0) * pow(cos(q[0 * 3 + 2]), 2.0) * + pow(sin(q[0 * 3 + 1]), 2.0) + + pow(cos(q[0 * 3 + 1]), 2.0) * pow(cos(q[0 * 3 + 2]), 2.0) * + pow(sin(q[0 * 3 + 0]), 2.0) + + pow(cos(q[0 * 3 + 0]), 2.0) * pow(sin(q[0 * 3 + 1]), 2.0) * + pow(sin(q[0 * 3 + 2]), 2.0) + + pow(cos(q[0 * 3 + 1]), 2.0) * pow(sin(q[0 * 3 + 0]), 2.0) * + pow(sin(q[0 * 3 + 2]), 2.0) + + pow(cos(q[0 * 3 + 2]), 2.0) * pow(sin(q[0 * 3 + 0]), 2.0) * + pow(sin(q[0 * 3 + 1]), 2.0) + + pow(sin(q[0 * 3 + 0]), 2.0) * pow(sin(q[0 * 3 + 1]), 2.0) * + pow(sin(q[0 * 3 + 2]), 2.0) + + pow(cos(q[0 * 3 + 0]), 2.0) * pow(cos(q[0 * 3 + 1]), 2.0) * + pow(cos(q[0 * 3 + 2]), 2.0)) * + 2.0))) / + (pow(cos(q[0 * 3 + 1]), 2.0) * pow(cos(q[0 * 3 + 2]), 2.0) + + pow(cos(q[0 * 3 + 1]), 2.0) * pow(sin(q[0 * 3 + 2]), 2.0) + + pow(cos(q[0 * 3 + 2]), 2.0) * pow(sin(q[0 * 3 + 1]), 2.0) + + pow(sin(q[0 * 3 + 1]), 2.0) * pow(sin(q[0 * 3 + 2]), 2.0)) + + ((cos(q[0 * 3 + 1]) * sin(q[0 * 3 + 2]) + + cos(q[0 * 3 + 2]) * sin(q[0 * 3 + 1])) * + (l[2 * 3 + 2] + + (cos(q[0 * 3 + 0]) * + (l[0 * 3 + 2] * cos(q[0 * 3 + 0]) * cos(q[0 * 3 + 1]) * + cos(q[0 * 3 + 2]) + + l[0 * 3 + 0] * cos(q[0 * 3 + 0]) * cos(q[0 * 3 + 1]) * + sin(q[0 * 3 + 2]) + + l[0 * 3 + 0] * cos(q[0 * 3 + 0]) * cos(q[0 * 3 + 2]) * + sin(q[0 * 3 + 1]) - + l[0 * 3 + 2] * cos(q[0 * 3 + 0]) * sin(q[0 * 3 + 1]) * + sin(q[0 * 3 + 2]) + + l[1 * 3 + 2] * cos(q[0 * 3 + 0]) * pow(cos(q[0 * 3 + 1]), 2.0) * + cos(q[0 * 3 + 2]) + + l[1 * 3 + 0] * cos(q[0 * 3 + 0]) * pow(cos(q[0 * 3 + 1]), 2.0) * + sin(q[0 * 3 + 2]) + + l[1 * 3 + 2] * cos(q[0 * 3 + 0]) * cos(q[0 * 3 + 2]) * + pow(sin(q[0 * 3 + 1]), 2.0) + + l[1 * 3 + 0] * cos(q[0 * 3 + 0]) * pow(sin(q[0 * 3 + 1]), 2.0) * + sin(q[0 * 3 + 2]) + + RL * l[0 * 3 + 1] * cos(q[0 * 3 + 1]) * cos(q[0 * 3 + 2]) * + sin(q[0 * 3 + 0]) + + RL * l[1 * 3 + 1] * cos(q[0 * 3 + 1]) * cos(q[0 * 3 + 2]) * + sin(q[0 * 3 + 0]) - + RL * l[0 * 3 + 1] * sin(q[0 * 3 + 0]) * sin(q[0 * 3 + 1]) * + sin(q[0 * 3 + 2]) - + RL * l[1 * 3 + 1] * sin(q[0 * 3 + 0]) * sin(q[0 * 3 + 1]) * + sin(q[0 * 3 + 2]))) / + ((pow(cos(q[0 * 3 + 0]), 2.0) + pow(sin(q[0 * 3 + 0]), 2.0)) * + (pow(cos(q[0 * 3 + 0]), 2.0) * pow(cos(q[0 * 3 + 1]), 2.0) * + pow(sin(q[0 * 3 + 2]), 2.0) + + pow(cos(q[0 * 3 + 0]), 2.0) * pow(cos(q[0 * 3 + 2]), 2.0) * + pow(sin(q[0 * 3 + 1]), 2.0) + + pow(cos(q[0 * 3 + 1]), 2.0) * pow(cos(q[0 * 3 + 2]), 2.0) * + pow(sin(q[0 * 3 + 0]), 2.0) + + pow(cos(q[0 * 3 + 0]), 2.0) * pow(sin(q[0 * 3 + 1]), 2.0) * + pow(sin(q[0 * 3 + 2]), 2.0) + + pow(cos(q[0 * 3 + 1]), 2.0) * pow(sin(q[0 * 3 + 0]), 2.0) * + pow(sin(q[0 * 3 + 2]), 2.0) + + pow(cos(q[0 * 3 + 2]), 2.0) * pow(sin(q[0 * 3 + 0]), 2.0) * + pow(sin(q[0 * 3 + 1]), 2.0) + + pow(sin(q[0 * 3 + 0]), 2.0) * pow(sin(q[0 * 3 + 1]), 2.0) * + pow(sin(q[0 * 3 + 2]), 2.0) + + pow(cos(q[0 * 3 + 0]), 2.0) * pow(cos(q[0 * 3 + 1]), 2.0) * + pow(cos(q[0 * 3 + 2]), 2.0)) * + 2.0) + + (sin(q[0 * 3 + 0]) * + (l[0 * 3 + 2] * cos(q[0 * 3 + 1]) * cos(q[0 * 3 + 2]) * + sin(q[0 * 3 + 0]) + + l[0 * 3 + 0] * cos(q[0 * 3 + 1]) * sin(q[0 * 3 + 0]) * + sin(q[0 * 3 + 2]) + + l[0 * 3 + 0] * cos(q[0 * 3 + 2]) * sin(q[0 * 3 + 0]) * + sin(q[0 * 3 + 1]) - + l[0 * 3 + 2] * sin(q[0 * 3 + 0]) * sin(q[0 * 3 + 1]) * + sin(q[0 * 3 + 2]) + + l[1 * 3 + 2] * pow(cos(q[0 * 3 + 1]), 2.0) * cos(q[0 * 3 + 2]) * + sin(q[0 * 3 + 0]) + + l[1 * 3 + 0] * pow(cos(q[0 * 3 + 1]), 2.0) * sin(q[0 * 3 + 0]) * + sin(q[0 * 3 + 2]) + + l[1 * 3 + 2] * cos(q[0 * 3 + 2]) * sin(q[0 * 3 + 0]) * + pow(sin(q[0 * 3 + 1]), 2.0) + + l[1 * 3 + 0] * sin(q[0 * 3 + 0]) * pow(sin(q[0 * 3 + 1]), 2.0) * + sin(q[0 * 3 + 2]) - + RL * l[0 * 3 + 1] * cos(q[0 * 3 + 0]) * cos(q[0 * 3 + 1]) * + cos(q[0 * 3 + 2]) - + RL * l[1 * 3 + 1] * cos(q[0 * 3 + 0]) * cos(q[0 * 3 + 1]) * + cos(q[0 * 3 + 2]) + + RL * l[0 * 3 + 1] * cos(q[0 * 3 + 0]) * sin(q[0 * 3 + 1]) * + sin(q[0 * 3 + 2]) + + RL * l[1 * 3 + 1] * cos(q[0 * 3 + 0]) * sin(q[0 * 3 + 1]) * + sin(q[0 * 3 + 2]))) / + ((pow(cos(q[0 * 3 + 0]), 2.0) + pow(sin(q[0 * 3 + 0]), 2.0)) * + (pow(cos(q[0 * 3 + 0]), 2.0) * pow(cos(q[0 * 3 + 1]), 2.0) * + pow(sin(q[0 * 3 + 2]), 2.0) + + pow(cos(q[0 * 3 + 0]), 2.0) * pow(cos(q[0 * 3 + 2]), 2.0) * + pow(sin(q[0 * 3 + 1]), 2.0) + + pow(cos(q[0 * 3 + 1]), 2.0) * pow(cos(q[0 * 3 + 2]), 2.0) * + pow(sin(q[0 * 3 + 0]), 2.0) + + pow(cos(q[0 * 3 + 0]), 2.0) * pow(sin(q[0 * 3 + 1]), 2.0) * + pow(sin(q[0 * 3 + 2]), 2.0) + + pow(cos(q[0 * 3 + 1]), 2.0) * pow(sin(q[0 * 3 + 0]), 2.0) * + pow(sin(q[0 * 3 + 2]), 2.0) + + pow(cos(q[0 * 3 + 2]), 2.0) * pow(sin(q[0 * 3 + 0]), 2.0) * + pow(sin(q[0 * 3 + 1]), 2.0) + + pow(sin(q[0 * 3 + 0]), 2.0) * pow(sin(q[0 * 3 + 1]), 2.0) * + pow(sin(q[0 * 3 + 2]), 2.0) + + pow(cos(q[0 * 3 + 0]), 2.0) * pow(cos(q[0 * 3 + 1]), 2.0) * + pow(cos(q[0 * 3 + 2]), 2.0)) * + 2.0) + + ((cos(q[0 * 3 + 1]) * cos(q[0 * 3 + 2]) - + sin(q[0 * 3 + 1]) * sin(q[0 * 3 + 2])) * + (l[0 * 3 + 2] * pow(sin(q[0 * 3 + 1]), 2.0) + + l[1 * 3 + 2] * cos(q[0 * 3 + 1]) - + l[1 * 3 + 0] * sin(q[0 * 3 + 1]) + + l[0 * 3 + 2] * pow(cos(q[0 * 3 + 1]), 2.0))) / + ((pow(cos(q[0 * 3 + 1]), 2.0) + pow(sin(q[0 * 3 + 1]), 2.0)) * + (pow(cos(q[0 * 3 + 1]), 2.0) * pow(cos(q[0 * 3 + 2]), 2.0) + + pow(cos(q[0 * 3 + 1]), 2.0) * pow(sin(q[0 * 3 + 2]), 2.0) + + pow(cos(q[0 * 3 + 2]), 2.0) * pow(sin(q[0 * 3 + 1]), 2.0) + + pow(sin(q[0 * 3 + 1]), 2.0) * pow(sin(q[0 * 3 + 2]), 2.0)) * + 2.0) + + (pow(cos(q[0 * 3 + 0]), 2.0) * + (cos(q[0 * 3 + 1]) * sin(q[0 * 3 + 2]) + + cos(q[0 * 3 + 2]) * sin(q[0 * 3 + 1])) * + (l[0 * 3 + 0] * pow(sin(q[0 * 3 + 1]), 2.0) + + l[1 * 3 + 0] * cos(q[0 * 3 + 1]) + + l[1 * 3 + 2] * sin(q[0 * 3 + 1]) + + l[0 * 3 + 0] * pow(cos(q[0 * 3 + 1]), 2.0))) / + ((pow(cos(q[0 * 3 + 0]), 2.0) * pow(cos(q[0 * 3 + 1]), 2.0) + + pow(cos(q[0 * 3 + 0]), 2.0) * pow(sin(q[0 * 3 + 1]), 2.0) + + pow(cos(q[0 * 3 + 1]), 2.0) * pow(sin(q[0 * 3 + 0]), 2.0) + + pow(sin(q[0 * 3 + 0]), 2.0) * pow(sin(q[0 * 3 + 1]), 2.0)) * + (pow(cos(q[0 * 3 + 0]), 2.0) * pow(cos(q[0 * 3 + 1]), 2.0) * + pow(sin(q[0 * 3 + 2]), 2.0) + + pow(cos(q[0 * 3 + 0]), 2.0) * pow(cos(q[0 * 3 + 2]), 2.0) * + pow(sin(q[0 * 3 + 1]), 2.0) + + pow(cos(q[0 * 3 + 1]), 2.0) * pow(cos(q[0 * 3 + 2]), 2.0) * + pow(sin(q[0 * 3 + 0]), 2.0) + + pow(cos(q[0 * 3 + 0]), 2.0) * pow(sin(q[0 * 3 + 1]), 2.0) * + pow(sin(q[0 * 3 + 2]), 2.0) + + pow(cos(q[0 * 3 + 1]), 2.0) * pow(sin(q[0 * 3 + 0]), 2.0) * + pow(sin(q[0 * 3 + 2]), 2.0) + + pow(cos(q[0 * 3 + 2]), 2.0) * pow(sin(q[0 * 3 + 0]), 2.0) * + pow(sin(q[0 * 3 + 1]), 2.0) + + pow(sin(q[0 * 3 + 0]), 2.0) * pow(sin(q[0 * 3 + 1]), 2.0) * + pow(sin(q[0 * 3 + 2]), 2.0) + + pow(cos(q[0 * 3 + 0]), 2.0) * pow(cos(q[0 * 3 + 1]), 2.0) * + pow(cos(q[0 * 3 + 2]), 2.0)) * + 2.0) + + (pow(sin(q[0 * 3 + 0]), 2.0) * + (cos(q[0 * 3 + 1]) * sin(q[0 * 3 + 2]) + + cos(q[0 * 3 + 2]) * sin(q[0 * 3 + 1])) * + (l[0 * 3 + 0] * pow(sin(q[0 * 3 + 1]), 2.0) + + l[1 * 3 + 0] * cos(q[0 * 3 + 1]) + + l[1 * 3 + 2] * sin(q[0 * 3 + 1]) + + l[0 * 3 + 0] * pow(cos(q[0 * 3 + 1]), 2.0))) / + ((pow(cos(q[0 * 3 + 0]), 2.0) * pow(cos(q[0 * 3 + 1]), 2.0) + + pow(cos(q[0 * 3 + 0]), 2.0) * pow(sin(q[0 * 3 + 1]), 2.0) + + pow(cos(q[0 * 3 + 1]), 2.0) * pow(sin(q[0 * 3 + 0]), 2.0) + + pow(sin(q[0 * 3 + 0]), 2.0) * pow(sin(q[0 * 3 + 1]), 2.0)) * + (pow(cos(q[0 * 3 + 0]), 2.0) * pow(cos(q[0 * 3 + 1]), 2.0) * + pow(sin(q[0 * 3 + 2]), 2.0) + + pow(cos(q[0 * 3 + 0]), 2.0) * pow(cos(q[0 * 3 + 2]), 2.0) * + pow(sin(q[0 * 3 + 1]), 2.0) + + pow(cos(q[0 * 3 + 1]), 2.0) * pow(cos(q[0 * 3 + 2]), 2.0) * + pow(sin(q[0 * 3 + 0]), 2.0) + + pow(cos(q[0 * 3 + 0]), 2.0) * pow(sin(q[0 * 3 + 1]), 2.0) * + pow(sin(q[0 * 3 + 2]), 2.0) + + pow(cos(q[0 * 3 + 1]), 2.0) * pow(sin(q[0 * 3 + 0]), 2.0) * + pow(sin(q[0 * 3 + 2]), 2.0) + + pow(cos(q[0 * 3 + 2]), 2.0) * pow(sin(q[0 * 3 + 0]), 2.0) * + pow(sin(q[0 * 3 + 1]), 2.0) + + pow(sin(q[0 * 3 + 0]), 2.0) * pow(sin(q[0 * 3 + 1]), 2.0) * + pow(sin(q[0 * 3 + 2]), 2.0) + + pow(cos(q[0 * 3 + 0]), 2.0) * pow(cos(q[0 * 3 + 1]), 2.0) * + pow(cos(q[0 * 3 + 2]), 2.0)) * + 2.0))) / + (pow(cos(q[0 * 3 + 1]), 2.0) * pow(cos(q[0 * 3 + 2]), 2.0) + + pow(cos(q[0 * 3 + 1]), 2.0) * pow(sin(q[0 * 3 + 2]), 2.0) + + pow(cos(q[0 * 3 + 2]), 2.0) * pow(sin(q[0 * 3 + 1]), 2.0) + + pow(sin(q[0 * 3 + 1]), 2.0) * pow(sin(q[0 * 3 + 2]), 2.0)))) / + (pow(cos(q[0 * 3 + 0]), 2.0) + pow(sin(q[0 * 3 + 0]), 2.0)); + F(0 * 3 + 2) = + (pow(cos(q[0 * 3 + 1]) * sin(q[0 * 3 + 2]) + + cos(q[0 * 3 + 2]) * sin(q[0 * 3 + 1]), + 2.0) / + (pow(cos(q[0 * 3 + 1]), 2.0) * pow(cos(q[0 * 3 + 2]), 2.0) + + pow(cos(q[0 * 3 + 1]), 2.0) * pow(sin(q[0 * 3 + 2]), 2.0) + + pow(cos(q[0 * 3 + 2]), 2.0) * pow(sin(q[0 * 3 + 1]), 2.0) + + pow(sin(q[0 * 3 + 1]), 2.0) * pow(sin(q[0 * 3 + 2]), 2.0)) + + pow(cos(q[0 * 3 + 1]) * cos(q[0 * 3 + 2]) - + sin(q[0 * 3 + 1]) * sin(q[0 * 3 + 2]), + 2.0) / + (pow(cos(q[0 * 3 + 1]), 2.0) * pow(cos(q[0 * 3 + 2]), 2.0) + + pow(cos(q[0 * 3 + 1]), 2.0) * pow(sin(q[0 * 3 + 2]), 2.0) + + pow(cos(q[0 * 3 + 2]), 2.0) * pow(sin(q[0 * 3 + 1]), 2.0) + + pow(sin(q[0 * 3 + 1]), 2.0) * pow(sin(q[0 * 3 + 2]), 2.0))) * + ((cos(q[0 * 3 + 0]) * + (RL * l[2 * 3 + 1] + + ((cos(q[0 * 3 + 1]) * sin(q[0 * 3 + 2]) + + cos(q[0 * 3 + 2]) * sin(q[0 * 3 + 1])) * + (RL * l[0 * 3 + 1] * cos(q[0 * 3 + 1]) * sin(q[0 * 3 + 2]) + + RL * l[0 * 3 + 1] * cos(q[0 * 3 + 2]) * sin(q[0 * 3 + 1]) + + RL * l[1 * 3 + 1] * cos(q[0 * 3 + 1]) * sin(q[0 * 3 + 2]) + + RL * l[1 * 3 + 1] * cos(q[0 * 3 + 2]) * sin(q[0 * 3 + 1])) * + 1.0 / + pow(pow(cos(q[0 * 3 + 1]), 2.0) * pow(cos(q[0 * 3 + 2]), 2.0) + + pow(cos(q[0 * 3 + 1]), 2.0) * + pow(sin(q[0 * 3 + 2]), 2.0) + + pow(cos(q[0 * 3 + 2]), 2.0) * + pow(sin(q[0 * 3 + 1]), 2.0) + + pow(sin(q[0 * 3 + 1]), 2.0) * + pow(sin(q[0 * 3 + 2]), 2.0), + 2.0)) / + 2.0 + + ((cos(q[0 * 3 + 1]) * cos(q[0 * 3 + 2]) - + sin(q[0 * 3 + 1]) * sin(q[0 * 3 + 2])) * + (RL * l[0 * 3 + 1] * cos(q[0 * 3 + 1]) * cos(q[0 * 3 + 2]) + + RL * l[1 * 3 + 1] * cos(q[0 * 3 + 1]) * cos(q[0 * 3 + 2]) - + RL * l[0 * 3 + 1] * sin(q[0 * 3 + 1]) * sin(q[0 * 3 + 2]) - + RL * l[1 * 3 + 1] * sin(q[0 * 3 + 1]) * sin(q[0 * 3 + 2])) * + 1.0 / + pow(pow(cos(q[0 * 3 + 1]), 2.0) * pow(cos(q[0 * 3 + 2]), 2.0) + + pow(cos(q[0 * 3 + 1]), 2.0) * + pow(sin(q[0 * 3 + 2]), 2.0) + + pow(cos(q[0 * 3 + 2]), 2.0) * + pow(sin(q[0 * 3 + 1]), 2.0) + + pow(sin(q[0 * 3 + 1]), 2.0) * + pow(sin(q[0 * 3 + 2]), 2.0), + 2.0)) / + 2.0 + + (sin(q[0 * 3 + 0]) * + (cos(q[0 * 3 + 1]) * sin(q[0 * 3 + 2]) + + cos(q[0 * 3 + 2]) * sin(q[0 * 3 + 1])) * + 1.0 / + pow(pow(cos(q[0 * 3 + 0]), 2.0) * pow(cos(q[0 * 3 + 1]), 2.0) * + pow(sin(q[0 * 3 + 2]), 2.0) + + pow(cos(q[0 * 3 + 0]), 2.0) * + pow(cos(q[0 * 3 + 2]), 2.0) * + pow(sin(q[0 * 3 + 1]), 2.0) + + pow(cos(q[0 * 3 + 1]), 2.0) * + pow(cos(q[0 * 3 + 2]), 2.0) * + pow(sin(q[0 * 3 + 0]), 2.0) + + pow(cos(q[0 * 3 + 0]), 2.0) * + pow(sin(q[0 * 3 + 1]), 2.0) * + pow(sin(q[0 * 3 + 2]), 2.0) + + pow(cos(q[0 * 3 + 1]), 2.0) * + pow(sin(q[0 * 3 + 0]), 2.0) * + pow(sin(q[0 * 3 + 2]), 2.0) + + pow(cos(q[0 * 3 + 2]), 2.0) * + pow(sin(q[0 * 3 + 0]), 2.0) * + pow(sin(q[0 * 3 + 1]), 2.0) + + pow(sin(q[0 * 3 + 0]), 2.0) * + pow(sin(q[0 * 3 + 1]), 2.0) * + pow(sin(q[0 * 3 + 2]), 2.0) + + pow(cos(q[0 * 3 + 0]), 2.0) * + pow(cos(q[0 * 3 + 1]), 2.0) * + pow(cos(q[0 * 3 + 2]), 2.0), + 2.0) * + (-l[0 * 3 + 0] * cos(q[0 * 3 + 0]) * cos(q[0 * 3 + 1]) * + cos(q[0 * 3 + 2]) + + l[0 * 3 + 2] * cos(q[0 * 3 + 0]) * cos(q[0 * 3 + 1]) * + sin(q[0 * 3 + 2]) + + l[0 * 3 + 2] * cos(q[0 * 3 + 0]) * cos(q[0 * 3 + 2]) * + sin(q[0 * 3 + 1]) + + l[0 * 3 + 0] * cos(q[0 * 3 + 0]) * sin(q[0 * 3 + 1]) * + sin(q[0 * 3 + 2]) - + l[1 * 3 + 0] * cos(q[0 * 3 + 0]) * + pow(cos(q[0 * 3 + 1]), 2.0) * cos(q[0 * 3 + 2]) - + l[1 * 3 + 0] * cos(q[0 * 3 + 0]) * cos(q[0 * 3 + 2]) * + pow(sin(q[0 * 3 + 1]), 2.0) + + l[1 * 3 + 2] * cos(q[0 * 3 + 0]) * + pow(cos(q[0 * 3 + 1]), 2.0) * sin(q[0 * 3 + 2]) + + l[1 * 3 + 2] * cos(q[0 * 3 + 0]) * + pow(sin(q[0 * 3 + 1]), 2.0) * sin(q[0 * 3 + 2]) + + RL * l[0 * 3 + 1] * cos(q[0 * 3 + 1]) * sin(q[0 * 3 + 0]) * + sin(q[0 * 3 + 2]) + + RL * l[0 * 3 + 1] * cos(q[0 * 3 + 2]) * sin(q[0 * 3 + 0]) * + sin(q[0 * 3 + 1]) + + RL * l[1 * 3 + 1] * cos(q[0 * 3 + 1]) * sin(q[0 * 3 + 0]) * + sin(q[0 * 3 + 2]) + + RL * l[1 * 3 + 1] * cos(q[0 * 3 + 2]) * sin(q[0 * 3 + 0]) * + sin(q[0 * 3 + 1]))) / + 2.0 + + (sin(q[0 * 3 + 0]) * + (cos(q[0 * 3 + 1]) * cos(q[0 * 3 + 2]) - + sin(q[0 * 3 + 1]) * sin(q[0 * 3 + 2])) * + 1.0 / + pow(pow(cos(q[0 * 3 + 0]), 2.0) * pow(cos(q[0 * 3 + 1]), 2.0) * + pow(sin(q[0 * 3 + 2]), 2.0) + + pow(cos(q[0 * 3 + 0]), 2.0) * + pow(cos(q[0 * 3 + 2]), 2.0) * + pow(sin(q[0 * 3 + 1]), 2.0) + + pow(cos(q[0 * 3 + 1]), 2.0) * + pow(cos(q[0 * 3 + 2]), 2.0) * + pow(sin(q[0 * 3 + 0]), 2.0) + + pow(cos(q[0 * 3 + 0]), 2.0) * + pow(sin(q[0 * 3 + 1]), 2.0) * + pow(sin(q[0 * 3 + 2]), 2.0) + + pow(cos(q[0 * 3 + 1]), 2.0) * + pow(sin(q[0 * 3 + 0]), 2.0) * + pow(sin(q[0 * 3 + 2]), 2.0) + + pow(cos(q[0 * 3 + 2]), 2.0) * + pow(sin(q[0 * 3 + 0]), 2.0) * + pow(sin(q[0 * 3 + 1]), 2.0) + + pow(sin(q[0 * 3 + 0]), 2.0) * + pow(sin(q[0 * 3 + 1]), 2.0) * + pow(sin(q[0 * 3 + 2]), 2.0) + + pow(cos(q[0 * 3 + 0]), 2.0) * + pow(cos(q[0 * 3 + 1]), 2.0) * + pow(cos(q[0 * 3 + 2]), 2.0), + 2.0) * + (l[0 * 3 + 2] * cos(q[0 * 3 + 0]) * cos(q[0 * 3 + 1]) * + cos(q[0 * 3 + 2]) + + l[0 * 3 + 0] * cos(q[0 * 3 + 0]) * cos(q[0 * 3 + 1]) * + sin(q[0 * 3 + 2]) + + l[0 * 3 + 0] * cos(q[0 * 3 + 0]) * cos(q[0 * 3 + 2]) * + sin(q[0 * 3 + 1]) - + l[0 * 3 + 2] * cos(q[0 * 3 + 0]) * sin(q[0 * 3 + 1]) * + sin(q[0 * 3 + 2]) + + l[1 * 3 + 2] * cos(q[0 * 3 + 0]) * + pow(cos(q[0 * 3 + 1]), 2.0) * cos(q[0 * 3 + 2]) + + l[1 * 3 + 0] * cos(q[0 * 3 + 0]) * + pow(cos(q[0 * 3 + 1]), 2.0) * sin(q[0 * 3 + 2]) + + l[1 * 3 + 2] * cos(q[0 * 3 + 0]) * cos(q[0 * 3 + 2]) * + pow(sin(q[0 * 3 + 1]), 2.0) + + l[1 * 3 + 0] * cos(q[0 * 3 + 0]) * + pow(sin(q[0 * 3 + 1]), 2.0) * sin(q[0 * 3 + 2]) + + RL * l[0 * 3 + 1] * cos(q[0 * 3 + 1]) * cos(q[0 * 3 + 2]) * + sin(q[0 * 3 + 0]) + + RL * l[1 * 3 + 1] * cos(q[0 * 3 + 1]) * cos(q[0 * 3 + 2]) * + sin(q[0 * 3 + 0]) - + RL * l[0 * 3 + 1] * sin(q[0 * 3 + 0]) * sin(q[0 * 3 + 1]) * + sin(q[0 * 3 + 2]) - + RL * l[1 * 3 + 1] * sin(q[0 * 3 + 0]) * sin(q[0 * 3 + 1]) * + sin(q[0 * 3 + 2]))) / + 2.0 - + (cos(q[0 * 3 + 0]) * + (cos(q[0 * 3 + 1]) * cos(q[0 * 3 + 2]) - + sin(q[0 * 3 + 1]) * sin(q[0 * 3 + 2])) * + 1.0 / + pow(pow(cos(q[0 * 3 + 0]), 2.0) * pow(cos(q[0 * 3 + 1]), 2.0) * + pow(sin(q[0 * 3 + 2]), 2.0) + + pow(cos(q[0 * 3 + 0]), 2.0) * + pow(cos(q[0 * 3 + 2]), 2.0) * + pow(sin(q[0 * 3 + 1]), 2.0) + + pow(cos(q[0 * 3 + 1]), 2.0) * + pow(cos(q[0 * 3 + 2]), 2.0) * + pow(sin(q[0 * 3 + 0]), 2.0) + + pow(cos(q[0 * 3 + 0]), 2.0) * + pow(sin(q[0 * 3 + 1]), 2.0) * + pow(sin(q[0 * 3 + 2]), 2.0) + + pow(cos(q[0 * 3 + 1]), 2.0) * + pow(sin(q[0 * 3 + 0]), 2.0) * + pow(sin(q[0 * 3 + 2]), 2.0) + + pow(cos(q[0 * 3 + 2]), 2.0) * + pow(sin(q[0 * 3 + 0]), 2.0) * + pow(sin(q[0 * 3 + 1]), 2.0) + + pow(sin(q[0 * 3 + 0]), 2.0) * + pow(sin(q[0 * 3 + 1]), 2.0) * + pow(sin(q[0 * 3 + 2]), 2.0) + + pow(cos(q[0 * 3 + 0]), 2.0) * + pow(cos(q[0 * 3 + 1]), 2.0) * + pow(cos(q[0 * 3 + 2]), 2.0), + 2.0) * + (l[0 * 3 + 2] * cos(q[0 * 3 + 1]) * cos(q[0 * 3 + 2]) * + sin(q[0 * 3 + 0]) + + l[0 * 3 + 0] * cos(q[0 * 3 + 1]) * sin(q[0 * 3 + 0]) * + sin(q[0 * 3 + 2]) + + l[0 * 3 + 0] * cos(q[0 * 3 + 2]) * sin(q[0 * 3 + 0]) * + sin(q[0 * 3 + 1]) - + l[0 * 3 + 2] * sin(q[0 * 3 + 0]) * sin(q[0 * 3 + 1]) * + sin(q[0 * 3 + 2]) + + l[1 * 3 + 2] * pow(cos(q[0 * 3 + 1]), 2.0) * + cos(q[0 * 3 + 2]) * sin(q[0 * 3 + 0]) + + l[1 * 3 + 0] * pow(cos(q[0 * 3 + 1]), 2.0) * + sin(q[0 * 3 + 0]) * sin(q[0 * 3 + 2]) + + l[1 * 3 + 2] * cos(q[0 * 3 + 2]) * sin(q[0 * 3 + 0]) * + pow(sin(q[0 * 3 + 1]), 2.0) + + l[1 * 3 + 0] * sin(q[0 * 3 + 0]) * + pow(sin(q[0 * 3 + 1]), 2.0) * sin(q[0 * 3 + 2]) - + RL * l[0 * 3 + 1] * cos(q[0 * 3 + 0]) * cos(q[0 * 3 + 1]) * + cos(q[0 * 3 + 2]) - + RL * l[1 * 3 + 1] * cos(q[0 * 3 + 0]) * cos(q[0 * 3 + 1]) * + cos(q[0 * 3 + 2]) + + RL * l[0 * 3 + 1] * cos(q[0 * 3 + 0]) * sin(q[0 * 3 + 1]) * + sin(q[0 * 3 + 2]) + + RL * l[1 * 3 + 1] * cos(q[0 * 3 + 0]) * sin(q[0 * 3 + 1]) * + sin(q[0 * 3 + 2]))) / + 2.0 + + (cos(q[0 * 3 + 0]) * + (cos(q[0 * 3 + 1]) * sin(q[0 * 3 + 2]) + + cos(q[0 * 3 + 2]) * sin(q[0 * 3 + 1])) * + 1.0 / + pow(pow(cos(q[0 * 3 + 0]), 2.0) * pow(cos(q[0 * 3 + 1]), 2.0) * + pow(sin(q[0 * 3 + 2]), 2.0) + + pow(cos(q[0 * 3 + 0]), 2.0) * + pow(cos(q[0 * 3 + 2]), 2.0) * + pow(sin(q[0 * 3 + 1]), 2.0) + + pow(cos(q[0 * 3 + 1]), 2.0) * + pow(cos(q[0 * 3 + 2]), 2.0) * + pow(sin(q[0 * 3 + 0]), 2.0) + + pow(cos(q[0 * 3 + 0]), 2.0) * + pow(sin(q[0 * 3 + 1]), 2.0) * + pow(sin(q[0 * 3 + 2]), 2.0) + + pow(cos(q[0 * 3 + 1]), 2.0) * + pow(sin(q[0 * 3 + 0]), 2.0) * + pow(sin(q[0 * 3 + 2]), 2.0) + + pow(cos(q[0 * 3 + 2]), 2.0) * + pow(sin(q[0 * 3 + 0]), 2.0) * + pow(sin(q[0 * 3 + 1]), 2.0) + + pow(sin(q[0 * 3 + 0]), 2.0) * + pow(sin(q[0 * 3 + 1]), 2.0) * + pow(sin(q[0 * 3 + 2]), 2.0) + + pow(cos(q[0 * 3 + 0]), 2.0) * + pow(cos(q[0 * 3 + 1]), 2.0) * + pow(cos(q[0 * 3 + 2]), 2.0), + 2.0) * + (l[0 * 3 + 0] * cos(q[0 * 3 + 1]) * cos(q[0 * 3 + 2]) * + sin(q[0 * 3 + 0]) - + l[0 * 3 + 2] * cos(q[0 * 3 + 1]) * sin(q[0 * 3 + 0]) * + sin(q[0 * 3 + 2]) - + l[0 * 3 + 2] * cos(q[0 * 3 + 2]) * sin(q[0 * 3 + 0]) * + sin(q[0 * 3 + 1]) - + l[0 * 3 + 0] * sin(q[0 * 3 + 0]) * sin(q[0 * 3 + 1]) * + sin(q[0 * 3 + 2]) + + l[1 * 3 + 0] * pow(cos(q[0 * 3 + 1]), 2.0) * + cos(q[0 * 3 + 2]) * sin(q[0 * 3 + 0]) + + l[1 * 3 + 0] * cos(q[0 * 3 + 2]) * sin(q[0 * 3 + 0]) * + pow(sin(q[0 * 3 + 1]), 2.0) - + l[1 * 3 + 2] * pow(cos(q[0 * 3 + 1]), 2.0) * + sin(q[0 * 3 + 0]) * sin(q[0 * 3 + 2]) - + l[1 * 3 + 2] * sin(q[0 * 3 + 0]) * + pow(sin(q[0 * 3 + 1]), 2.0) * sin(q[0 * 3 + 2]) + + RL * l[0 * 3 + 1] * cos(q[0 * 3 + 0]) * cos(q[0 * 3 + 1]) * + sin(q[0 * 3 + 2]) + + RL * l[0 * 3 + 1] * cos(q[0 * 3 + 0]) * cos(q[0 * 3 + 2]) * + sin(q[0 * 3 + 1]) + + RL * l[1 * 3 + 1] * cos(q[0 * 3 + 0]) * cos(q[0 * 3 + 1]) * + sin(q[0 * 3 + 2]) + + RL * l[1 * 3 + 1] * cos(q[0 * 3 + 0]) * cos(q[0 * 3 + 2]) * + sin(q[0 * 3 + 1]))) / + 2.0)) / + (pow(cos(q[0 * 3 + 0]), 2.0) + pow(sin(q[0 * 3 + 0]), 2.0)) + + (sin(q[0 * 3 + 0]) * + (cos(q[0 * 3 + 1]) * sin(q[0 * 3 + 2]) + + cos(q[0 * 3 + 2]) * sin(q[0 * 3 + 1])) * + (l[2 * 3 + 0] - + (cos(q[0 * 3 + 0]) * + (-l[0 * 3 + 0] * cos(q[0 * 3 + 0]) * cos(q[0 * 3 + 1]) * + cos(q[0 * 3 + 2]) + + l[0 * 3 + 2] * cos(q[0 * 3 + 0]) * cos(q[0 * 3 + 1]) * + sin(q[0 * 3 + 2]) + + l[0 * 3 + 2] * cos(q[0 * 3 + 0]) * cos(q[0 * 3 + 2]) * + sin(q[0 * 3 + 1]) + + l[0 * 3 + 0] * cos(q[0 * 3 + 0]) * sin(q[0 * 3 + 1]) * + sin(q[0 * 3 + 2]) - + l[1 * 3 + 0] * cos(q[0 * 3 + 0]) * + pow(cos(q[0 * 3 + 1]), 2.0) * cos(q[0 * 3 + 2]) - + l[1 * 3 + 0] * cos(q[0 * 3 + 0]) * cos(q[0 * 3 + 2]) * + pow(sin(q[0 * 3 + 1]), 2.0) + + l[1 * 3 + 2] * cos(q[0 * 3 + 0]) * + pow(cos(q[0 * 3 + 1]), 2.0) * sin(q[0 * 3 + 2]) + + l[1 * 3 + 2] * cos(q[0 * 3 + 0]) * + pow(sin(q[0 * 3 + 1]), 2.0) * sin(q[0 * 3 + 2]) + + RL * l[0 * 3 + 1] * cos(q[0 * 3 + 1]) * sin(q[0 * 3 + 0]) * + sin(q[0 * 3 + 2]) + + RL * l[0 * 3 + 1] * cos(q[0 * 3 + 2]) * sin(q[0 * 3 + 0]) * + sin(q[0 * 3 + 1]) + + RL * l[1 * 3 + 1] * cos(q[0 * 3 + 1]) * sin(q[0 * 3 + 0]) * + sin(q[0 * 3 + 2]) + + RL * l[1 * 3 + 1] * cos(q[0 * 3 + 2]) * sin(q[0 * 3 + 0]) * + sin(q[0 * 3 + 1]))) / + ((pow(cos(q[0 * 3 + 0]), 2.0) + + pow(sin(q[0 * 3 + 0]), 2.0)) * + (pow(cos(q[0 * 3 + 0]), 2.0) * pow(cos(q[0 * 3 + 1]), 2.0) * + pow(sin(q[0 * 3 + 2]), 2.0) + + pow(cos(q[0 * 3 + 0]), 2.0) * pow(cos(q[0 * 3 + 2]), 2.0) * + pow(sin(q[0 * 3 + 1]), 2.0) + + pow(cos(q[0 * 3 + 1]), 2.0) * pow(cos(q[0 * 3 + 2]), 2.0) * + pow(sin(q[0 * 3 + 0]), 2.0) + + pow(cos(q[0 * 3 + 0]), 2.0) * pow(sin(q[0 * 3 + 1]), 2.0) * + pow(sin(q[0 * 3 + 2]), 2.0) + + pow(cos(q[0 * 3 + 1]), 2.0) * pow(sin(q[0 * 3 + 0]), 2.0) * + pow(sin(q[0 * 3 + 2]), 2.0) + + pow(cos(q[0 * 3 + 2]), 2.0) * pow(sin(q[0 * 3 + 0]), 2.0) * + pow(sin(q[0 * 3 + 1]), 2.0) + + pow(sin(q[0 * 3 + 0]), 2.0) * pow(sin(q[0 * 3 + 1]), 2.0) * + pow(sin(q[0 * 3 + 2]), 2.0) + + pow(cos(q[0 * 3 + 0]), 2.0) * pow(cos(q[0 * 3 + 1]), 2.0) * + pow(cos(q[0 * 3 + 2]), 2.0)) * + 2.0) + + (sin(q[0 * 3 + 0]) * + (l[0 * 3 + 0] * cos(q[0 * 3 + 1]) * cos(q[0 * 3 + 2]) * + sin(q[0 * 3 + 0]) - + l[0 * 3 + 2] * cos(q[0 * 3 + 1]) * sin(q[0 * 3 + 0]) * + sin(q[0 * 3 + 2]) - + l[0 * 3 + 2] * cos(q[0 * 3 + 2]) * sin(q[0 * 3 + 0]) * + sin(q[0 * 3 + 1]) - + l[0 * 3 + 0] * sin(q[0 * 3 + 0]) * sin(q[0 * 3 + 1]) * + sin(q[0 * 3 + 2]) + + l[1 * 3 + 0] * pow(cos(q[0 * 3 + 1]), 2.0) * + cos(q[0 * 3 + 2]) * sin(q[0 * 3 + 0]) + + l[1 * 3 + 0] * cos(q[0 * 3 + 2]) * sin(q[0 * 3 + 0]) * + pow(sin(q[0 * 3 + 1]), 2.0) - + l[1 * 3 + 2] * pow(cos(q[0 * 3 + 1]), 2.0) * + sin(q[0 * 3 + 0]) * sin(q[0 * 3 + 2]) - + l[1 * 3 + 2] * sin(q[0 * 3 + 0]) * + pow(sin(q[0 * 3 + 1]), 2.0) * sin(q[0 * 3 + 2]) + + RL * l[0 * 3 + 1] * cos(q[0 * 3 + 0]) * cos(q[0 * 3 + 1]) * + sin(q[0 * 3 + 2]) + + RL * l[0 * 3 + 1] * cos(q[0 * 3 + 0]) * cos(q[0 * 3 + 2]) * + sin(q[0 * 3 + 1]) + + RL * l[1 * 3 + 1] * cos(q[0 * 3 + 0]) * cos(q[0 * 3 + 1]) * + sin(q[0 * 3 + 2]) + + RL * l[1 * 3 + 1] * cos(q[0 * 3 + 0]) * cos(q[0 * 3 + 2]) * + sin(q[0 * 3 + 1]))) / + ((pow(cos(q[0 * 3 + 0]), 2.0) + + pow(sin(q[0 * 3 + 0]), 2.0)) * + (pow(cos(q[0 * 3 + 0]), 2.0) * pow(cos(q[0 * 3 + 1]), 2.0) * + pow(sin(q[0 * 3 + 2]), 2.0) + + pow(cos(q[0 * 3 + 0]), 2.0) * pow(cos(q[0 * 3 + 2]), 2.0) * + pow(sin(q[0 * 3 + 1]), 2.0) + + pow(cos(q[0 * 3 + 1]), 2.0) * pow(cos(q[0 * 3 + 2]), 2.0) * + pow(sin(q[0 * 3 + 0]), 2.0) + + pow(cos(q[0 * 3 + 0]), 2.0) * pow(sin(q[0 * 3 + 1]), 2.0) * + pow(sin(q[0 * 3 + 2]), 2.0) + + pow(cos(q[0 * 3 + 1]), 2.0) * pow(sin(q[0 * 3 + 0]), 2.0) * + pow(sin(q[0 * 3 + 2]), 2.0) + + pow(cos(q[0 * 3 + 2]), 2.0) * pow(sin(q[0 * 3 + 0]), 2.0) * + pow(sin(q[0 * 3 + 1]), 2.0) + + pow(sin(q[0 * 3 + 0]), 2.0) * pow(sin(q[0 * 3 + 1]), 2.0) * + pow(sin(q[0 * 3 + 2]), 2.0) + + pow(cos(q[0 * 3 + 0]), 2.0) * pow(cos(q[0 * 3 + 1]), 2.0) * + pow(cos(q[0 * 3 + 2]), 2.0)) * + 2.0) - + ((cos(q[0 * 3 + 1]) * sin(q[0 * 3 + 2]) + + cos(q[0 * 3 + 2]) * sin(q[0 * 3 + 1])) * + (l[0 * 3 + 2] * pow(sin(q[0 * 3 + 1]), 2.0) + + l[1 * 3 + 2] * cos(q[0 * 3 + 1]) - + l[1 * 3 + 0] * sin(q[0 * 3 + 1]) + + l[0 * 3 + 2] * pow(cos(q[0 * 3 + 1]), 2.0))) / + ((pow(cos(q[0 * 3 + 1]), 2.0) + + pow(sin(q[0 * 3 + 1]), 2.0)) * + (pow(cos(q[0 * 3 + 1]), 2.0) * pow(cos(q[0 * 3 + 2]), 2.0) + + pow(cos(q[0 * 3 + 1]), 2.0) * pow(sin(q[0 * 3 + 2]), 2.0) + + pow(cos(q[0 * 3 + 2]), 2.0) * pow(sin(q[0 * 3 + 1]), 2.0) + + pow(sin(q[0 * 3 + 1]), 2.0) * + pow(sin(q[0 * 3 + 2]), 2.0)) * + 2.0) + + (pow(cos(q[0 * 3 + 0]), 2.0) * + (cos(q[0 * 3 + 1]) * cos(q[0 * 3 + 2]) - + sin(q[0 * 3 + 1]) * sin(q[0 * 3 + 2])) * + (l[0 * 3 + 0] * pow(sin(q[0 * 3 + 1]), 2.0) + + l[1 * 3 + 0] * cos(q[0 * 3 + 1]) + + l[1 * 3 + 2] * sin(q[0 * 3 + 1]) + + l[0 * 3 + 0] * pow(cos(q[0 * 3 + 1]), 2.0))) / + ((pow(cos(q[0 * 3 + 0]), 2.0) * pow(cos(q[0 * 3 + 1]), 2.0) + + pow(cos(q[0 * 3 + 0]), 2.0) * pow(sin(q[0 * 3 + 1]), 2.0) + + pow(cos(q[0 * 3 + 1]), 2.0) * pow(sin(q[0 * 3 + 0]), 2.0) + + pow(sin(q[0 * 3 + 0]), 2.0) * + pow(sin(q[0 * 3 + 1]), 2.0)) * + (pow(cos(q[0 * 3 + 0]), 2.0) * pow(cos(q[0 * 3 + 1]), 2.0) * + pow(sin(q[0 * 3 + 2]), 2.0) + + pow(cos(q[0 * 3 + 0]), 2.0) * pow(cos(q[0 * 3 + 2]), 2.0) * + pow(sin(q[0 * 3 + 1]), 2.0) + + pow(cos(q[0 * 3 + 1]), 2.0) * pow(cos(q[0 * 3 + 2]), 2.0) * + pow(sin(q[0 * 3 + 0]), 2.0) + + pow(cos(q[0 * 3 + 0]), 2.0) * pow(sin(q[0 * 3 + 1]), 2.0) * + pow(sin(q[0 * 3 + 2]), 2.0) + + pow(cos(q[0 * 3 + 1]), 2.0) * pow(sin(q[0 * 3 + 0]), 2.0) * + pow(sin(q[0 * 3 + 2]), 2.0) + + pow(cos(q[0 * 3 + 2]), 2.0) * pow(sin(q[0 * 3 + 0]), 2.0) * + pow(sin(q[0 * 3 + 1]), 2.0) + + pow(sin(q[0 * 3 + 0]), 2.0) * pow(sin(q[0 * 3 + 1]), 2.0) * + pow(sin(q[0 * 3 + 2]), 2.0) + + pow(cos(q[0 * 3 + 0]), 2.0) * pow(cos(q[0 * 3 + 1]), 2.0) * + pow(cos(q[0 * 3 + 2]), 2.0)) * + 2.0) + + (pow(sin(q[0 * 3 + 0]), 2.0) * + (cos(q[0 * 3 + 1]) * cos(q[0 * 3 + 2]) - + sin(q[0 * 3 + 1]) * sin(q[0 * 3 + 2])) * + (l[0 * 3 + 0] * pow(sin(q[0 * 3 + 1]), 2.0) + + l[1 * 3 + 0] * cos(q[0 * 3 + 1]) + + l[1 * 3 + 2] * sin(q[0 * 3 + 1]) + + l[0 * 3 + 0] * pow(cos(q[0 * 3 + 1]), 2.0))) / + ((pow(cos(q[0 * 3 + 0]), 2.0) * pow(cos(q[0 * 3 + 1]), 2.0) + + pow(cos(q[0 * 3 + 0]), 2.0) * pow(sin(q[0 * 3 + 1]), 2.0) + + pow(cos(q[0 * 3 + 1]), 2.0) * pow(sin(q[0 * 3 + 0]), 2.0) + + pow(sin(q[0 * 3 + 0]), 2.0) * + pow(sin(q[0 * 3 + 1]), 2.0)) * + (pow(cos(q[0 * 3 + 0]), 2.0) * pow(cos(q[0 * 3 + 1]), 2.0) * + pow(sin(q[0 * 3 + 2]), 2.0) + + pow(cos(q[0 * 3 + 0]), 2.0) * pow(cos(q[0 * 3 + 2]), 2.0) * + pow(sin(q[0 * 3 + 1]), 2.0) + + pow(cos(q[0 * 3 + 1]), 2.0) * pow(cos(q[0 * 3 + 2]), 2.0) * + pow(sin(q[0 * 3 + 0]), 2.0) + + pow(cos(q[0 * 3 + 0]), 2.0) * pow(sin(q[0 * 3 + 1]), 2.0) * + pow(sin(q[0 * 3 + 2]), 2.0) + + pow(cos(q[0 * 3 + 1]), 2.0) * pow(sin(q[0 * 3 + 0]), 2.0) * + pow(sin(q[0 * 3 + 2]), 2.0) + + pow(cos(q[0 * 3 + 2]), 2.0) * pow(sin(q[0 * 3 + 0]), 2.0) * + pow(sin(q[0 * 3 + 1]), 2.0) + + pow(sin(q[0 * 3 + 0]), 2.0) * pow(sin(q[0 * 3 + 1]), 2.0) * + pow(sin(q[0 * 3 + 2]), 2.0) + + pow(cos(q[0 * 3 + 0]), 2.0) * pow(cos(q[0 * 3 + 1]), 2.0) * + pow(cos(q[0 * 3 + 2]), 2.0)) * + 2.0))) / + (pow(cos(q[0 * 3 + 0]), 2.0) * pow(cos(q[0 * 3 + 1]), 2.0) * + pow(sin(q[0 * 3 + 2]), 2.0) + + pow(cos(q[0 * 3 + 0]), 2.0) * pow(cos(q[0 * 3 + 2]), 2.0) * + pow(sin(q[0 * 3 + 1]), 2.0) + + pow(cos(q[0 * 3 + 1]), 2.0) * pow(cos(q[0 * 3 + 2]), 2.0) * + pow(sin(q[0 * 3 + 0]), 2.0) + + pow(cos(q[0 * 3 + 0]), 2.0) * pow(sin(q[0 * 3 + 1]), 2.0) * + pow(sin(q[0 * 3 + 2]), 2.0) + + pow(cos(q[0 * 3 + 1]), 2.0) * pow(sin(q[0 * 3 + 0]), 2.0) * + pow(sin(q[0 * 3 + 2]), 2.0) + + pow(cos(q[0 * 3 + 2]), 2.0) * pow(sin(q[0 * 3 + 0]), 2.0) * + pow(sin(q[0 * 3 + 1]), 2.0) + + pow(sin(q[0 * 3 + 0]), 2.0) * pow(sin(q[0 * 3 + 1]), 2.0) * + pow(sin(q[0 * 3 + 2]), 2.0) + + pow(cos(q[0 * 3 + 0]), 2.0) * pow(cos(q[0 * 3 + 1]), 2.0) * + pow(cos(q[0 * 3 + 2]), 2.0)) - + (sin(q[0 * 3 + 0]) * + (cos(q[0 * 3 + 1]) * cos(q[0 * 3 + 2]) - + sin(q[0 * 3 + 1]) * sin(q[0 * 3 + 2])) * + (l[2 * 3 + 2] + + (cos(q[0 * 3 + 0]) * + (l[0 * 3 + 2] * cos(q[0 * 3 + 0]) * cos(q[0 * 3 + 1]) * + cos(q[0 * 3 + 2]) + + l[0 * 3 + 0] * cos(q[0 * 3 + 0]) * cos(q[0 * 3 + 1]) * + sin(q[0 * 3 + 2]) + + l[0 * 3 + 0] * cos(q[0 * 3 + 0]) * cos(q[0 * 3 + 2]) * + sin(q[0 * 3 + 1]) - + l[0 * 3 + 2] * cos(q[0 * 3 + 0]) * sin(q[0 * 3 + 1]) * + sin(q[0 * 3 + 2]) + + l[1 * 3 + 2] * cos(q[0 * 3 + 0]) * + pow(cos(q[0 * 3 + 1]), 2.0) * cos(q[0 * 3 + 2]) + + l[1 * 3 + 0] * cos(q[0 * 3 + 0]) * + pow(cos(q[0 * 3 + 1]), 2.0) * sin(q[0 * 3 + 2]) + + l[1 * 3 + 2] * cos(q[0 * 3 + 0]) * cos(q[0 * 3 + 2]) * + pow(sin(q[0 * 3 + 1]), 2.0) + + l[1 * 3 + 0] * cos(q[0 * 3 + 0]) * + pow(sin(q[0 * 3 + 1]), 2.0) * sin(q[0 * 3 + 2]) + + RL * l[0 * 3 + 1] * cos(q[0 * 3 + 1]) * cos(q[0 * 3 + 2]) * + sin(q[0 * 3 + 0]) + + RL * l[1 * 3 + 1] * cos(q[0 * 3 + 1]) * cos(q[0 * 3 + 2]) * + sin(q[0 * 3 + 0]) - + RL * l[0 * 3 + 1] * sin(q[0 * 3 + 0]) * sin(q[0 * 3 + 1]) * + sin(q[0 * 3 + 2]) - + RL * l[1 * 3 + 1] * sin(q[0 * 3 + 0]) * sin(q[0 * 3 + 1]) * + sin(q[0 * 3 + 2]))) / + ((pow(cos(q[0 * 3 + 0]), 2.0) + + pow(sin(q[0 * 3 + 0]), 2.0)) * + (pow(cos(q[0 * 3 + 0]), 2.0) * pow(cos(q[0 * 3 + 1]), 2.0) * + pow(sin(q[0 * 3 + 2]), 2.0) + + pow(cos(q[0 * 3 + 0]), 2.0) * pow(cos(q[0 * 3 + 2]), 2.0) * + pow(sin(q[0 * 3 + 1]), 2.0) + + pow(cos(q[0 * 3 + 1]), 2.0) * pow(cos(q[0 * 3 + 2]), 2.0) * + pow(sin(q[0 * 3 + 0]), 2.0) + + pow(cos(q[0 * 3 + 0]), 2.0) * pow(sin(q[0 * 3 + 1]), 2.0) * + pow(sin(q[0 * 3 + 2]), 2.0) + + pow(cos(q[0 * 3 + 1]), 2.0) * pow(sin(q[0 * 3 + 0]), 2.0) * + pow(sin(q[0 * 3 + 2]), 2.0) + + pow(cos(q[0 * 3 + 2]), 2.0) * pow(sin(q[0 * 3 + 0]), 2.0) * + pow(sin(q[0 * 3 + 1]), 2.0) + + pow(sin(q[0 * 3 + 0]), 2.0) * pow(sin(q[0 * 3 + 1]), 2.0) * + pow(sin(q[0 * 3 + 2]), 2.0) + + pow(cos(q[0 * 3 + 0]), 2.0) * pow(cos(q[0 * 3 + 1]), 2.0) * + pow(cos(q[0 * 3 + 2]), 2.0)) * + 2.0) + + (sin(q[0 * 3 + 0]) * + (l[0 * 3 + 2] * cos(q[0 * 3 + 1]) * cos(q[0 * 3 + 2]) * + sin(q[0 * 3 + 0]) + + l[0 * 3 + 0] * cos(q[0 * 3 + 1]) * sin(q[0 * 3 + 0]) * + sin(q[0 * 3 + 2]) + + l[0 * 3 + 0] * cos(q[0 * 3 + 2]) * sin(q[0 * 3 + 0]) * + sin(q[0 * 3 + 1]) - + l[0 * 3 + 2] * sin(q[0 * 3 + 0]) * sin(q[0 * 3 + 1]) * + sin(q[0 * 3 + 2]) + + l[1 * 3 + 2] * pow(cos(q[0 * 3 + 1]), 2.0) * + cos(q[0 * 3 + 2]) * sin(q[0 * 3 + 0]) + + l[1 * 3 + 0] * pow(cos(q[0 * 3 + 1]), 2.0) * + sin(q[0 * 3 + 0]) * sin(q[0 * 3 + 2]) + + l[1 * 3 + 2] * cos(q[0 * 3 + 2]) * sin(q[0 * 3 + 0]) * + pow(sin(q[0 * 3 + 1]), 2.0) + + l[1 * 3 + 0] * sin(q[0 * 3 + 0]) * + pow(sin(q[0 * 3 + 1]), 2.0) * sin(q[0 * 3 + 2]) - + RL * l[0 * 3 + 1] * cos(q[0 * 3 + 0]) * cos(q[0 * 3 + 1]) * + cos(q[0 * 3 + 2]) - + RL * l[1 * 3 + 1] * cos(q[0 * 3 + 0]) * cos(q[0 * 3 + 1]) * + cos(q[0 * 3 + 2]) + + RL * l[0 * 3 + 1] * cos(q[0 * 3 + 0]) * sin(q[0 * 3 + 1]) * + sin(q[0 * 3 + 2]) + + RL * l[1 * 3 + 1] * cos(q[0 * 3 + 0]) * sin(q[0 * 3 + 1]) * + sin(q[0 * 3 + 2]))) / + ((pow(cos(q[0 * 3 + 0]), 2.0) + + pow(sin(q[0 * 3 + 0]), 2.0)) * + (pow(cos(q[0 * 3 + 0]), 2.0) * pow(cos(q[0 * 3 + 1]), 2.0) * + pow(sin(q[0 * 3 + 2]), 2.0) + + pow(cos(q[0 * 3 + 0]), 2.0) * pow(cos(q[0 * 3 + 2]), 2.0) * + pow(sin(q[0 * 3 + 1]), 2.0) + + pow(cos(q[0 * 3 + 1]), 2.0) * pow(cos(q[0 * 3 + 2]), 2.0) * + pow(sin(q[0 * 3 + 0]), 2.0) + + pow(cos(q[0 * 3 + 0]), 2.0) * pow(sin(q[0 * 3 + 1]), 2.0) * + pow(sin(q[0 * 3 + 2]), 2.0) + + pow(cos(q[0 * 3 + 1]), 2.0) * pow(sin(q[0 * 3 + 0]), 2.0) * + pow(sin(q[0 * 3 + 2]), 2.0) + + pow(cos(q[0 * 3 + 2]), 2.0) * pow(sin(q[0 * 3 + 0]), 2.0) * + pow(sin(q[0 * 3 + 1]), 2.0) + + pow(sin(q[0 * 3 + 0]), 2.0) * pow(sin(q[0 * 3 + 1]), 2.0) * + pow(sin(q[0 * 3 + 2]), 2.0) + + pow(cos(q[0 * 3 + 0]), 2.0) * pow(cos(q[0 * 3 + 1]), 2.0) * + pow(cos(q[0 * 3 + 2]), 2.0)) * + 2.0) + + ((cos(q[0 * 3 + 1]) * cos(q[0 * 3 + 2]) - + sin(q[0 * 3 + 1]) * sin(q[0 * 3 + 2])) * + (l[0 * 3 + 2] * pow(sin(q[0 * 3 + 1]), 2.0) + + l[1 * 3 + 2] * cos(q[0 * 3 + 1]) - + l[1 * 3 + 0] * sin(q[0 * 3 + 1]) + + l[0 * 3 + 2] * pow(cos(q[0 * 3 + 1]), 2.0))) / + ((pow(cos(q[0 * 3 + 1]), 2.0) + + pow(sin(q[0 * 3 + 1]), 2.0)) * + (pow(cos(q[0 * 3 + 1]), 2.0) * pow(cos(q[0 * 3 + 2]), 2.0) + + pow(cos(q[0 * 3 + 1]), 2.0) * pow(sin(q[0 * 3 + 2]), 2.0) + + pow(cos(q[0 * 3 + 2]), 2.0) * pow(sin(q[0 * 3 + 1]), 2.0) + + pow(sin(q[0 * 3 + 1]), 2.0) * + pow(sin(q[0 * 3 + 2]), 2.0)) * + 2.0) + + (pow(cos(q[0 * 3 + 0]), 2.0) * + (cos(q[0 * 3 + 1]) * sin(q[0 * 3 + 2]) + + cos(q[0 * 3 + 2]) * sin(q[0 * 3 + 1])) * + (l[0 * 3 + 0] * pow(sin(q[0 * 3 + 1]), 2.0) + + l[1 * 3 + 0] * cos(q[0 * 3 + 1]) + + l[1 * 3 + 2] * sin(q[0 * 3 + 1]) + + l[0 * 3 + 0] * pow(cos(q[0 * 3 + 1]), 2.0))) / + ((pow(cos(q[0 * 3 + 0]), 2.0) * pow(cos(q[0 * 3 + 1]), 2.0) + + pow(cos(q[0 * 3 + 0]), 2.0) * pow(sin(q[0 * 3 + 1]), 2.0) + + pow(cos(q[0 * 3 + 1]), 2.0) * pow(sin(q[0 * 3 + 0]), 2.0) + + pow(sin(q[0 * 3 + 0]), 2.0) * + pow(sin(q[0 * 3 + 1]), 2.0)) * + (pow(cos(q[0 * 3 + 0]), 2.0) * pow(cos(q[0 * 3 + 1]), 2.0) * + pow(sin(q[0 * 3 + 2]), 2.0) + + pow(cos(q[0 * 3 + 0]), 2.0) * pow(cos(q[0 * 3 + 2]), 2.0) * + pow(sin(q[0 * 3 + 1]), 2.0) + + pow(cos(q[0 * 3 + 1]), 2.0) * pow(cos(q[0 * 3 + 2]), 2.0) * + pow(sin(q[0 * 3 + 0]), 2.0) + + pow(cos(q[0 * 3 + 0]), 2.0) * pow(sin(q[0 * 3 + 1]), 2.0) * + pow(sin(q[0 * 3 + 2]), 2.0) + + pow(cos(q[0 * 3 + 1]), 2.0) * pow(sin(q[0 * 3 + 0]), 2.0) * + pow(sin(q[0 * 3 + 2]), 2.0) + + pow(cos(q[0 * 3 + 2]), 2.0) * pow(sin(q[0 * 3 + 0]), 2.0) * + pow(sin(q[0 * 3 + 1]), 2.0) + + pow(sin(q[0 * 3 + 0]), 2.0) * pow(sin(q[0 * 3 + 1]), 2.0) * + pow(sin(q[0 * 3 + 2]), 2.0) + + pow(cos(q[0 * 3 + 0]), 2.0) * pow(cos(q[0 * 3 + 1]), 2.0) * + pow(cos(q[0 * 3 + 2]), 2.0)) * + 2.0) + + (pow(sin(q[0 * 3 + 0]), 2.0) * + (cos(q[0 * 3 + 1]) * sin(q[0 * 3 + 2]) + + cos(q[0 * 3 + 2]) * sin(q[0 * 3 + 1])) * + (l[0 * 3 + 0] * pow(sin(q[0 * 3 + 1]), 2.0) + + l[1 * 3 + 0] * cos(q[0 * 3 + 1]) + + l[1 * 3 + 2] * sin(q[0 * 3 + 1]) + + l[0 * 3 + 0] * pow(cos(q[0 * 3 + 1]), 2.0))) / + ((pow(cos(q[0 * 3 + 0]), 2.0) * pow(cos(q[0 * 3 + 1]), 2.0) + + pow(cos(q[0 * 3 + 0]), 2.0) * pow(sin(q[0 * 3 + 1]), 2.0) + + pow(cos(q[0 * 3 + 1]), 2.0) * pow(sin(q[0 * 3 + 0]), 2.0) + + pow(sin(q[0 * 3 + 0]), 2.0) * + pow(sin(q[0 * 3 + 1]), 2.0)) * + (pow(cos(q[0 * 3 + 0]), 2.0) * pow(cos(q[0 * 3 + 1]), 2.0) * + pow(sin(q[0 * 3 + 2]), 2.0) + + pow(cos(q[0 * 3 + 0]), 2.0) * pow(cos(q[0 * 3 + 2]), 2.0) * + pow(sin(q[0 * 3 + 1]), 2.0) + + pow(cos(q[0 * 3 + 1]), 2.0) * pow(cos(q[0 * 3 + 2]), 2.0) * + pow(sin(q[0 * 3 + 0]), 2.0) + + pow(cos(q[0 * 3 + 0]), 2.0) * pow(sin(q[0 * 3 + 1]), 2.0) * + pow(sin(q[0 * 3 + 2]), 2.0) + + pow(cos(q[0 * 3 + 1]), 2.0) * pow(sin(q[0 * 3 + 0]), 2.0) * + pow(sin(q[0 * 3 + 2]), 2.0) + + pow(cos(q[0 * 3 + 2]), 2.0) * pow(sin(q[0 * 3 + 0]), 2.0) * + pow(sin(q[0 * 3 + 1]), 2.0) + + pow(sin(q[0 * 3 + 0]), 2.0) * pow(sin(q[0 * 3 + 1]), 2.0) * + pow(sin(q[0 * 3 + 2]), 2.0) + + pow(cos(q[0 * 3 + 0]), 2.0) * pow(cos(q[0 * 3 + 1]), 2.0) * + pow(cos(q[0 * 3 + 2]), 2.0)) * + 2.0))) / + (pow(cos(q[0 * 3 + 0]), 2.0) * pow(cos(q[0 * 3 + 1]), 2.0) * + pow(sin(q[0 * 3 + 2]), 2.0) + + pow(cos(q[0 * 3 + 0]), 2.0) * pow(cos(q[0 * 3 + 2]), 2.0) * + pow(sin(q[0 * 3 + 1]), 2.0) + + pow(cos(q[0 * 3 + 1]), 2.0) * pow(cos(q[0 * 3 + 2]), 2.0) * + pow(sin(q[0 * 3 + 0]), 2.0) + + pow(cos(q[0 * 3 + 0]), 2.0) * pow(sin(q[0 * 3 + 1]), 2.0) * + pow(sin(q[0 * 3 + 2]), 2.0) + + pow(cos(q[0 * 3 + 1]), 2.0) * pow(sin(q[0 * 3 + 0]), 2.0) * + pow(sin(q[0 * 3 + 2]), 2.0) + + pow(cos(q[0 * 3 + 2]), 2.0) * pow(sin(q[0 * 3 + 0]), 2.0) * + pow(sin(q[0 * 3 + 1]), 2.0) + + pow(sin(q[0 * 3 + 0]), 2.0) * pow(sin(q[0 * 3 + 1]), 2.0) * + pow(sin(q[0 * 3 + 2]), 2.0) + + pow(cos(q[0 * 3 + 0]), 2.0) * pow(cos(q[0 * 3 + 1]), 2.0) * + pow(cos(q[0 * 3 + 2]), 2.0))) + + ((cos(q[0 * 3 + 1]) * cos(q[0 * 3 + 2]) - + sin(q[0 * 3 + 1]) * sin(q[0 * 3 + 2])) * + (l[0 * 3 + 2] * cos(q[0 * 3 + 1]) * cos(q[0 * 3 + 2]) * + sin(q[0 * 3 + 0]) + + l[0 * 3 + 0] * cos(q[0 * 3 + 1]) * sin(q[0 * 3 + 0]) * + sin(q[0 * 3 + 2]) + + l[0 * 3 + 0] * cos(q[0 * 3 + 2]) * sin(q[0 * 3 + 0]) * + sin(q[0 * 3 + 1]) - + l[0 * 3 + 2] * sin(q[0 * 3 + 0]) * sin(q[0 * 3 + 1]) * + sin(q[0 * 3 + 2]) + + l[1 * 3 + 2] * pow(cos(q[0 * 3 + 1]), 2.0) * cos(q[0 * 3 + 2]) * + sin(q[0 * 3 + 0]) + + l[1 * 3 + 0] * pow(cos(q[0 * 3 + 1]), 2.0) * sin(q[0 * 3 + 0]) * + sin(q[0 * 3 + 2]) + + l[1 * 3 + 2] * cos(q[0 * 3 + 2]) * sin(q[0 * 3 + 0]) * + pow(sin(q[0 * 3 + 1]), 2.0) + + l[1 * 3 + 0] * sin(q[0 * 3 + 0]) * pow(sin(q[0 * 3 + 1]), 2.0) * + sin(q[0 * 3 + 2]) - + RL * l[0 * 3 + 1] * cos(q[0 * 3 + 0]) * cos(q[0 * 3 + 1]) * + cos(q[0 * 3 + 2]) - + RL * l[1 * 3 + 1] * cos(q[0 * 3 + 0]) * cos(q[0 * 3 + 1]) * + cos(q[0 * 3 + 2]) + + RL * l[0 * 3 + 1] * cos(q[0 * 3 + 0]) * sin(q[0 * 3 + 1]) * + sin(q[0 * 3 + 2]) + + RL * l[1 * 3 + 1] * cos(q[0 * 3 + 0]) * sin(q[0 * 3 + 1]) * + sin(q[0 * 3 + 2]))) / + (pow(cos(q[0 * 3 + 0]), 2.0) * pow(cos(q[0 * 3 + 1]), 2.0) * + pow(sin(q[0 * 3 + 2]), 2.0) + + pow(cos(q[0 * 3 + 0]), 2.0) * pow(cos(q[0 * 3 + 2]), 2.0) * + pow(sin(q[0 * 3 + 1]), 2.0) + + pow(cos(q[0 * 3 + 1]), 2.0) * pow(cos(q[0 * 3 + 2]), 2.0) * + pow(sin(q[0 * 3 + 0]), 2.0) + + pow(cos(q[0 * 3 + 0]), 2.0) * pow(sin(q[0 * 3 + 1]), 2.0) * + pow(sin(q[0 * 3 + 2]), 2.0) + + pow(cos(q[0 * 3 + 1]), 2.0) * pow(sin(q[0 * 3 + 0]), 2.0) * + pow(sin(q[0 * 3 + 2]), 2.0) + + pow(cos(q[0 * 3 + 2]), 2.0) * pow(sin(q[0 * 3 + 0]), 2.0) * + pow(sin(q[0 * 3 + 1]), 2.0) + + pow(sin(q[0 * 3 + 0]), 2.0) * pow(sin(q[0 * 3 + 1]), 2.0) * + pow(sin(q[0 * 3 + 2]), 2.0) + + pow(cos(q[0 * 3 + 0]), 2.0) * pow(cos(q[0 * 3 + 1]), 2.0) * + pow(cos(q[0 * 3 + 2]), 2.0)) - + ((cos(q[0 * 3 + 1]) * sin(q[0 * 3 + 2]) + + cos(q[0 * 3 + 2]) * sin(q[0 * 3 + 1])) * + (l[0 * 3 + 0] * cos(q[0 * 3 + 1]) * cos(q[0 * 3 + 2]) * + sin(q[0 * 3 + 0]) - + l[0 * 3 + 2] * cos(q[0 * 3 + 1]) * sin(q[0 * 3 + 0]) * + sin(q[0 * 3 + 2]) - + l[0 * 3 + 2] * cos(q[0 * 3 + 2]) * sin(q[0 * 3 + 0]) * + sin(q[0 * 3 + 1]) - + l[0 * 3 + 0] * sin(q[0 * 3 + 0]) * sin(q[0 * 3 + 1]) * + sin(q[0 * 3 + 2]) + + l[1 * 3 + 0] * pow(cos(q[0 * 3 + 1]), 2.0) * cos(q[0 * 3 + 2]) * + sin(q[0 * 3 + 0]) + + l[1 * 3 + 0] * cos(q[0 * 3 + 2]) * sin(q[0 * 3 + 0]) * + pow(sin(q[0 * 3 + 1]), 2.0) - + l[1 * 3 + 2] * pow(cos(q[0 * 3 + 1]), 2.0) * sin(q[0 * 3 + 0]) * + sin(q[0 * 3 + 2]) - + l[1 * 3 + 2] * sin(q[0 * 3 + 0]) * pow(sin(q[0 * 3 + 1]), 2.0) * + sin(q[0 * 3 + 2]) + + RL * l[0 * 3 + 1] * cos(q[0 * 3 + 0]) * cos(q[0 * 3 + 1]) * + sin(q[0 * 3 + 2]) + + RL * l[0 * 3 + 1] * cos(q[0 * 3 + 0]) * cos(q[0 * 3 + 2]) * + sin(q[0 * 3 + 1]) + + RL * l[1 * 3 + 1] * cos(q[0 * 3 + 0]) * cos(q[0 * 3 + 1]) * + sin(q[0 * 3 + 2]) + + RL * l[1 * 3 + 1] * cos(q[0 * 3 + 0]) * cos(q[0 * 3 + 2]) * + sin(q[0 * 3 + 1]))) / + (pow(cos(q[0 * 3 + 0]), 2.0) * pow(cos(q[0 * 3 + 1]), 2.0) * + pow(sin(q[0 * 3 + 2]), 2.0) + + pow(cos(q[0 * 3 + 0]), 2.0) * pow(cos(q[0 * 3 + 2]), 2.0) * + pow(sin(q[0 * 3 + 1]), 2.0) + + pow(cos(q[0 * 3 + 1]), 2.0) * pow(cos(q[0 * 3 + 2]), 2.0) * + pow(sin(q[0 * 3 + 0]), 2.0) + + pow(cos(q[0 * 3 + 0]), 2.0) * pow(sin(q[0 * 3 + 1]), 2.0) * + pow(sin(q[0 * 3 + 2]), 2.0) + + pow(cos(q[0 * 3 + 1]), 2.0) * pow(sin(q[0 * 3 + 0]), 2.0) * + pow(sin(q[0 * 3 + 2]), 2.0) + + pow(cos(q[0 * 3 + 2]), 2.0) * pow(sin(q[0 * 3 + 0]), 2.0) * + pow(sin(q[0 * 3 + 1]), 2.0) + + pow(sin(q[0 * 3 + 0]), 2.0) * pow(sin(q[0 * 3 + 1]), 2.0) * + pow(sin(q[0 * 3 + 2]), 2.0) + + pow(cos(q[0 * 3 + 0]), 2.0) * pow(cos(q[0 * 3 + 1]), 2.0) * + pow(cos(q[0 * 3 + 2]), 2.0)) - + (sin(q[0 * 3 + 0]) * (l[0 * 3 + 2] + l[1 * 3 + 2] * cos(q[0 * 3 + 1]) - + l[1 * 3 + 0] * sin(q[0 * 3 + 1]))) / + (pow(cos(q[0 * 3 + 0]), 2.0) + pow(sin(q[0 * 3 + 0]), 2.0)) + + (cos(q[0 * 3 + 0]) * + (cos(q[0 * 3 + 1]) * sin(q[0 * 3 + 2]) + + cos(q[0 * 3 + 2]) * sin(q[0 * 3 + 1])) * + (RL * l[0 * 3 + 1] * cos(q[0 * 3 + 1]) * sin(q[0 * 3 + 2]) + + RL * l[0 * 3 + 1] * cos(q[0 * 3 + 2]) * sin(q[0 * 3 + 1]) + + RL * l[1 * 3 + 1] * cos(q[0 * 3 + 1]) * sin(q[0 * 3 + 2]) + + RL * l[1 * 3 + 1] * cos(q[0 * 3 + 2]) * sin(q[0 * 3 + 1]))) / + (pow(cos(q[0 * 3 + 0]), 2.0) * pow(cos(q[0 * 3 + 1]), 2.0) * + pow(sin(q[0 * 3 + 2]), 2.0) + + pow(cos(q[0 * 3 + 0]), 2.0) * pow(cos(q[0 * 3 + 2]), 2.0) * + pow(sin(q[0 * 3 + 1]), 2.0) + + pow(cos(q[0 * 3 + 1]), 2.0) * pow(cos(q[0 * 3 + 2]), 2.0) * + pow(sin(q[0 * 3 + 0]), 2.0) + + pow(cos(q[0 * 3 + 0]), 2.0) * pow(sin(q[0 * 3 + 1]), 2.0) * + pow(sin(q[0 * 3 + 2]), 2.0) + + pow(cos(q[0 * 3 + 1]), 2.0) * pow(sin(q[0 * 3 + 0]), 2.0) * + pow(sin(q[0 * 3 + 2]), 2.0) + + pow(cos(q[0 * 3 + 2]), 2.0) * pow(sin(q[0 * 3 + 0]), 2.0) * + pow(sin(q[0 * 3 + 1]), 2.0) + + pow(sin(q[0 * 3 + 0]), 2.0) * pow(sin(q[0 * 3 + 1]), 2.0) * + pow(sin(q[0 * 3 + 2]), 2.0) + + pow(cos(q[0 * 3 + 0]), 2.0) * pow(cos(q[0 * 3 + 1]), 2.0) * + pow(cos(q[0 * 3 + 2]), 2.0)) + + (cos(q[0 * 3 + 0]) * + (cos(q[0 * 3 + 1]) * cos(q[0 * 3 + 2]) - + sin(q[0 * 3 + 1]) * sin(q[0 * 3 + 2])) * + (RL * l[0 * 3 + 1] * cos(q[0 * 3 + 1]) * cos(q[0 * 3 + 2]) + + RL * l[1 * 3 + 1] * cos(q[0 * 3 + 1]) * cos(q[0 * 3 + 2]) - + RL * l[0 * 3 + 1] * sin(q[0 * 3 + 1]) * sin(q[0 * 3 + 2]) - + RL * l[1 * 3 + 1] * sin(q[0 * 3 + 1]) * sin(q[0 * 3 + 2]))) / + (pow(cos(q[0 * 3 + 0]), 2.0) * pow(cos(q[0 * 3 + 1]), 2.0) * + pow(sin(q[0 * 3 + 2]), 2.0) + + pow(cos(q[0 * 3 + 0]), 2.0) * pow(cos(q[0 * 3 + 2]), 2.0) * + pow(sin(q[0 * 3 + 1]), 2.0) + + pow(cos(q[0 * 3 + 1]), 2.0) * pow(cos(q[0 * 3 + 2]), 2.0) * + pow(sin(q[0 * 3 + 0]), 2.0) + + pow(cos(q[0 * 3 + 0]), 2.0) * pow(sin(q[0 * 3 + 1]), 2.0) * + pow(sin(q[0 * 3 + 2]), 2.0) + + pow(cos(q[0 * 3 + 1]), 2.0) * pow(sin(q[0 * 3 + 0]), 2.0) * + pow(sin(q[0 * 3 + 2]), 2.0) + + pow(cos(q[0 * 3 + 2]), 2.0) * pow(sin(q[0 * 3 + 0]), 2.0) * + pow(sin(q[0 * 3 + 1]), 2.0) + + pow(sin(q[0 * 3 + 0]), 2.0) * pow(sin(q[0 * 3 + 1]), 2.0) * + pow(sin(q[0 * 3 + 2]), 2.0) + + pow(cos(q[0 * 3 + 0]), 2.0) * pow(cos(q[0 * 3 + 1]), 2.0) * + pow(cos(q[0 * 3 + 2]), 2.0)); + F(1 * 3 + 2) = + (cos(q[0 * 3 + 0]) * + (l[0 * 3 + 0] * pow(sin(q[0 * 3 + 1]), 2.0) + + l[1 * 3 + 0] * cos(q[0 * 3 + 1]) + l[1 * 3 + 2] * sin(q[0 * 3 + 1]) + + l[0 * 3 + 0] * pow(cos(q[0 * 3 + 1]), 2.0))) / + (pow(cos(q[0 * 3 + 0]), 2.0) * pow(cos(q[0 * 3 + 1]), 2.0) + + pow(cos(q[0 * 3 + 0]), 2.0) * pow(sin(q[0 * 3 + 1]), 2.0) + + pow(cos(q[0 * 3 + 1]), 2.0) * pow(sin(q[0 * 3 + 0]), 2.0) + + pow(sin(q[0 * 3 + 0]), 2.0) * pow(sin(q[0 * 3 + 1]), 2.0)) - + (cos(q[0 * 3 + 0]) * + (((cos(q[0 * 3 + 1]) * cos(q[0 * 3 + 2]) - + sin(q[0 * 3 + 1]) * sin(q[0 * 3 + 2])) * + (l[2 * 3 + 0] - + (cos(q[0 * 3 + 0]) * + (-l[0 * 3 + 0] * cos(q[0 * 3 + 0]) * cos(q[0 * 3 + 1]) * + cos(q[0 * 3 + 2]) + + l[0 * 3 + 2] * cos(q[0 * 3 + 0]) * cos(q[0 * 3 + 1]) * + sin(q[0 * 3 + 2]) + + l[0 * 3 + 2] * cos(q[0 * 3 + 0]) * cos(q[0 * 3 + 2]) * + sin(q[0 * 3 + 1]) + + l[0 * 3 + 0] * cos(q[0 * 3 + 0]) * sin(q[0 * 3 + 1]) * + sin(q[0 * 3 + 2]) - + l[1 * 3 + 0] * cos(q[0 * 3 + 0]) * pow(cos(q[0 * 3 + 1]), 2.0) * + cos(q[0 * 3 + 2]) - + l[1 * 3 + 0] * cos(q[0 * 3 + 0]) * cos(q[0 * 3 + 2]) * + pow(sin(q[0 * 3 + 1]), 2.0) + + l[1 * 3 + 2] * cos(q[0 * 3 + 0]) * pow(cos(q[0 * 3 + 1]), 2.0) * + sin(q[0 * 3 + 2]) + + l[1 * 3 + 2] * cos(q[0 * 3 + 0]) * pow(sin(q[0 * 3 + 1]), 2.0) * + sin(q[0 * 3 + 2]) + + RL * l[0 * 3 + 1] * cos(q[0 * 3 + 1]) * sin(q[0 * 3 + 0]) * + sin(q[0 * 3 + 2]) + + RL * l[0 * 3 + 1] * cos(q[0 * 3 + 2]) * sin(q[0 * 3 + 0]) * + sin(q[0 * 3 + 1]) + + RL * l[1 * 3 + 1] * cos(q[0 * 3 + 1]) * sin(q[0 * 3 + 0]) * + sin(q[0 * 3 + 2]) + + RL * l[1 * 3 + 1] * cos(q[0 * 3 + 2]) * sin(q[0 * 3 + 0]) * + sin(q[0 * 3 + 1]))) / + ((pow(cos(q[0 * 3 + 0]), 2.0) + pow(sin(q[0 * 3 + 0]), 2.0)) * + (pow(cos(q[0 * 3 + 0]), 2.0) * pow(cos(q[0 * 3 + 1]), 2.0) * + pow(sin(q[0 * 3 + 2]), 2.0) + + pow(cos(q[0 * 3 + 0]), 2.0) * pow(cos(q[0 * 3 + 2]), 2.0) * + pow(sin(q[0 * 3 + 1]), 2.0) + + pow(cos(q[0 * 3 + 1]), 2.0) * pow(cos(q[0 * 3 + 2]), 2.0) * + pow(sin(q[0 * 3 + 0]), 2.0) + + pow(cos(q[0 * 3 + 0]), 2.0) * pow(sin(q[0 * 3 + 1]), 2.0) * + pow(sin(q[0 * 3 + 2]), 2.0) + + pow(cos(q[0 * 3 + 1]), 2.0) * pow(sin(q[0 * 3 + 0]), 2.0) * + pow(sin(q[0 * 3 + 2]), 2.0) + + pow(cos(q[0 * 3 + 2]), 2.0) * pow(sin(q[0 * 3 + 0]), 2.0) * + pow(sin(q[0 * 3 + 1]), 2.0) + + pow(sin(q[0 * 3 + 0]), 2.0) * pow(sin(q[0 * 3 + 1]), 2.0) * + pow(sin(q[0 * 3 + 2]), 2.0) + + pow(cos(q[0 * 3 + 0]), 2.0) * pow(cos(q[0 * 3 + 1]), 2.0) * + pow(cos(q[0 * 3 + 2]), 2.0)) * + 2.0) + + (sin(q[0 * 3 + 0]) * + (l[0 * 3 + 0] * cos(q[0 * 3 + 1]) * cos(q[0 * 3 + 2]) * + sin(q[0 * 3 + 0]) - + l[0 * 3 + 2] * cos(q[0 * 3 + 1]) * sin(q[0 * 3 + 0]) * + sin(q[0 * 3 + 2]) - + l[0 * 3 + 2] * cos(q[0 * 3 + 2]) * sin(q[0 * 3 + 0]) * + sin(q[0 * 3 + 1]) - + l[0 * 3 + 0] * sin(q[0 * 3 + 0]) * sin(q[0 * 3 + 1]) * + sin(q[0 * 3 + 2]) + + l[1 * 3 + 0] * pow(cos(q[0 * 3 + 1]), 2.0) * cos(q[0 * 3 + 2]) * + sin(q[0 * 3 + 0]) + + l[1 * 3 + 0] * cos(q[0 * 3 + 2]) * sin(q[0 * 3 + 0]) * + pow(sin(q[0 * 3 + 1]), 2.0) - + l[1 * 3 + 2] * pow(cos(q[0 * 3 + 1]), 2.0) * sin(q[0 * 3 + 0]) * + sin(q[0 * 3 + 2]) - + l[1 * 3 + 2] * sin(q[0 * 3 + 0]) * pow(sin(q[0 * 3 + 1]), 2.0) * + sin(q[0 * 3 + 2]) + + RL * l[0 * 3 + 1] * cos(q[0 * 3 + 0]) * cos(q[0 * 3 + 1]) * + sin(q[0 * 3 + 2]) + + RL * l[0 * 3 + 1] * cos(q[0 * 3 + 0]) * cos(q[0 * 3 + 2]) * + sin(q[0 * 3 + 1]) + + RL * l[1 * 3 + 1] * cos(q[0 * 3 + 0]) * cos(q[0 * 3 + 1]) * + sin(q[0 * 3 + 2]) + + RL * l[1 * 3 + 1] * cos(q[0 * 3 + 0]) * cos(q[0 * 3 + 2]) * + sin(q[0 * 3 + 1]))) / + ((pow(cos(q[0 * 3 + 0]), 2.0) + pow(sin(q[0 * 3 + 0]), 2.0)) * + (pow(cos(q[0 * 3 + 0]), 2.0) * pow(cos(q[0 * 3 + 1]), 2.0) * + pow(sin(q[0 * 3 + 2]), 2.0) + + pow(cos(q[0 * 3 + 0]), 2.0) * pow(cos(q[0 * 3 + 2]), 2.0) * + pow(sin(q[0 * 3 + 1]), 2.0) + + pow(cos(q[0 * 3 + 1]), 2.0) * pow(cos(q[0 * 3 + 2]), 2.0) * + pow(sin(q[0 * 3 + 0]), 2.0) + + pow(cos(q[0 * 3 + 0]), 2.0) * pow(sin(q[0 * 3 + 1]), 2.0) * + pow(sin(q[0 * 3 + 2]), 2.0) + + pow(cos(q[0 * 3 + 1]), 2.0) * pow(sin(q[0 * 3 + 0]), 2.0) * + pow(sin(q[0 * 3 + 2]), 2.0) + + pow(cos(q[0 * 3 + 2]), 2.0) * pow(sin(q[0 * 3 + 0]), 2.0) * + pow(sin(q[0 * 3 + 1]), 2.0) + + pow(sin(q[0 * 3 + 0]), 2.0) * pow(sin(q[0 * 3 + 1]), 2.0) * + pow(sin(q[0 * 3 + 2]), 2.0) + + pow(cos(q[0 * 3 + 0]), 2.0) * pow(cos(q[0 * 3 + 1]), 2.0) * + pow(cos(q[0 * 3 + 2]), 2.0)) * + 2.0) - + ((cos(q[0 * 3 + 1]) * sin(q[0 * 3 + 2]) + + cos(q[0 * 3 + 2]) * sin(q[0 * 3 + 1])) * + (l[0 * 3 + 2] * pow(sin(q[0 * 3 + 1]), 2.0) + + l[1 * 3 + 2] * cos(q[0 * 3 + 1]) - + l[1 * 3 + 0] * sin(q[0 * 3 + 1]) + + l[0 * 3 + 2] * pow(cos(q[0 * 3 + 1]), 2.0))) / + ((pow(cos(q[0 * 3 + 1]), 2.0) + pow(sin(q[0 * 3 + 1]), 2.0)) * + (pow(cos(q[0 * 3 + 1]), 2.0) * pow(cos(q[0 * 3 + 2]), 2.0) + + pow(cos(q[0 * 3 + 1]), 2.0) * pow(sin(q[0 * 3 + 2]), 2.0) + + pow(cos(q[0 * 3 + 2]), 2.0) * pow(sin(q[0 * 3 + 1]), 2.0) + + pow(sin(q[0 * 3 + 1]), 2.0) * pow(sin(q[0 * 3 + 2]), 2.0)) * + 2.0) + + (pow(cos(q[0 * 3 + 0]), 2.0) * + (cos(q[0 * 3 + 1]) * cos(q[0 * 3 + 2]) - + sin(q[0 * 3 + 1]) * sin(q[0 * 3 + 2])) * + (l[0 * 3 + 0] * pow(sin(q[0 * 3 + 1]), 2.0) + + l[1 * 3 + 0] * cos(q[0 * 3 + 1]) + + l[1 * 3 + 2] * sin(q[0 * 3 + 1]) + + l[0 * 3 + 0] * pow(cos(q[0 * 3 + 1]), 2.0))) / + ((pow(cos(q[0 * 3 + 0]), 2.0) * pow(cos(q[0 * 3 + 1]), 2.0) + + pow(cos(q[0 * 3 + 0]), 2.0) * pow(sin(q[0 * 3 + 1]), 2.0) + + pow(cos(q[0 * 3 + 1]), 2.0) * pow(sin(q[0 * 3 + 0]), 2.0) + + pow(sin(q[0 * 3 + 0]), 2.0) * pow(sin(q[0 * 3 + 1]), 2.0)) * + (pow(cos(q[0 * 3 + 0]), 2.0) * pow(cos(q[0 * 3 + 1]), 2.0) * + pow(sin(q[0 * 3 + 2]), 2.0) + + pow(cos(q[0 * 3 + 0]), 2.0) * pow(cos(q[0 * 3 + 2]), 2.0) * + pow(sin(q[0 * 3 + 1]), 2.0) + + pow(cos(q[0 * 3 + 1]), 2.0) * pow(cos(q[0 * 3 + 2]), 2.0) * + pow(sin(q[0 * 3 + 0]), 2.0) + + pow(cos(q[0 * 3 + 0]), 2.0) * pow(sin(q[0 * 3 + 1]), 2.0) * + pow(sin(q[0 * 3 + 2]), 2.0) + + pow(cos(q[0 * 3 + 1]), 2.0) * pow(sin(q[0 * 3 + 0]), 2.0) * + pow(sin(q[0 * 3 + 2]), 2.0) + + pow(cos(q[0 * 3 + 2]), 2.0) * pow(sin(q[0 * 3 + 0]), 2.0) * + pow(sin(q[0 * 3 + 1]), 2.0) + + pow(sin(q[0 * 3 + 0]), 2.0) * pow(sin(q[0 * 3 + 1]), 2.0) * + pow(sin(q[0 * 3 + 2]), 2.0) + + pow(cos(q[0 * 3 + 0]), 2.0) * pow(cos(q[0 * 3 + 1]), 2.0) * + pow(cos(q[0 * 3 + 2]), 2.0)) * + 2.0) + + (pow(sin(q[0 * 3 + 0]), 2.0) * + (cos(q[0 * 3 + 1]) * cos(q[0 * 3 + 2]) - + sin(q[0 * 3 + 1]) * sin(q[0 * 3 + 2])) * + (l[0 * 3 + 0] * pow(sin(q[0 * 3 + 1]), 2.0) + + l[1 * 3 + 0] * cos(q[0 * 3 + 1]) + + l[1 * 3 + 2] * sin(q[0 * 3 + 1]) + + l[0 * 3 + 0] * pow(cos(q[0 * 3 + 1]), 2.0))) / + ((pow(cos(q[0 * 3 + 0]), 2.0) * pow(cos(q[0 * 3 + 1]), 2.0) + + pow(cos(q[0 * 3 + 0]), 2.0) * pow(sin(q[0 * 3 + 1]), 2.0) + + pow(cos(q[0 * 3 + 1]), 2.0) * pow(sin(q[0 * 3 + 0]), 2.0) + + pow(sin(q[0 * 3 + 0]), 2.0) * pow(sin(q[0 * 3 + 1]), 2.0)) * + (pow(cos(q[0 * 3 + 0]), 2.0) * pow(cos(q[0 * 3 + 1]), 2.0) * + pow(sin(q[0 * 3 + 2]), 2.0) + + pow(cos(q[0 * 3 + 0]), 2.0) * pow(cos(q[0 * 3 + 2]), 2.0) * + pow(sin(q[0 * 3 + 1]), 2.0) + + pow(cos(q[0 * 3 + 1]), 2.0) * pow(cos(q[0 * 3 + 2]), 2.0) * + pow(sin(q[0 * 3 + 0]), 2.0) + + pow(cos(q[0 * 3 + 0]), 2.0) * pow(sin(q[0 * 3 + 1]), 2.0) * + pow(sin(q[0 * 3 + 2]), 2.0) + + pow(cos(q[0 * 3 + 1]), 2.0) * pow(sin(q[0 * 3 + 0]), 2.0) * + pow(sin(q[0 * 3 + 2]), 2.0) + + pow(cos(q[0 * 3 + 2]), 2.0) * pow(sin(q[0 * 3 + 0]), 2.0) * + pow(sin(q[0 * 3 + 1]), 2.0) + + pow(sin(q[0 * 3 + 0]), 2.0) * pow(sin(q[0 * 3 + 1]), 2.0) * + pow(sin(q[0 * 3 + 2]), 2.0) + + pow(cos(q[0 * 3 + 0]), 2.0) * pow(cos(q[0 * 3 + 1]), 2.0) * + pow(cos(q[0 * 3 + 2]), 2.0)) * + 2.0))) / + (pow(cos(q[0 * 3 + 1]), 2.0) * pow(cos(q[0 * 3 + 2]), 2.0) + + pow(cos(q[0 * 3 + 1]), 2.0) * pow(sin(q[0 * 3 + 2]), 2.0) + + pow(cos(q[0 * 3 + 2]), 2.0) * pow(sin(q[0 * 3 + 1]), 2.0) + + pow(sin(q[0 * 3 + 1]), 2.0) * pow(sin(q[0 * 3 + 2]), 2.0)) + + ((cos(q[0 * 3 + 1]) * sin(q[0 * 3 + 2]) + + cos(q[0 * 3 + 2]) * sin(q[0 * 3 + 1])) * + (l[2 * 3 + 2] + + (cos(q[0 * 3 + 0]) * + (l[0 * 3 + 2] * cos(q[0 * 3 + 0]) * cos(q[0 * 3 + 1]) * + cos(q[0 * 3 + 2]) + + l[0 * 3 + 0] * cos(q[0 * 3 + 0]) * cos(q[0 * 3 + 1]) * + sin(q[0 * 3 + 2]) + + l[0 * 3 + 0] * cos(q[0 * 3 + 0]) * cos(q[0 * 3 + 2]) * + sin(q[0 * 3 + 1]) - + l[0 * 3 + 2] * cos(q[0 * 3 + 0]) * sin(q[0 * 3 + 1]) * + sin(q[0 * 3 + 2]) + + l[1 * 3 + 2] * cos(q[0 * 3 + 0]) * pow(cos(q[0 * 3 + 1]), 2.0) * + cos(q[0 * 3 + 2]) + + l[1 * 3 + 0] * cos(q[0 * 3 + 0]) * pow(cos(q[0 * 3 + 1]), 2.0) * + sin(q[0 * 3 + 2]) + + l[1 * 3 + 2] * cos(q[0 * 3 + 0]) * cos(q[0 * 3 + 2]) * + pow(sin(q[0 * 3 + 1]), 2.0) + + l[1 * 3 + 0] * cos(q[0 * 3 + 0]) * pow(sin(q[0 * 3 + 1]), 2.0) * + sin(q[0 * 3 + 2]) + + RL * l[0 * 3 + 1] * cos(q[0 * 3 + 1]) * cos(q[0 * 3 + 2]) * + sin(q[0 * 3 + 0]) + + RL * l[1 * 3 + 1] * cos(q[0 * 3 + 1]) * cos(q[0 * 3 + 2]) * + sin(q[0 * 3 + 0]) - + RL * l[0 * 3 + 1] * sin(q[0 * 3 + 0]) * sin(q[0 * 3 + 1]) * + sin(q[0 * 3 + 2]) - + RL * l[1 * 3 + 1] * sin(q[0 * 3 + 0]) * sin(q[0 * 3 + 1]) * + sin(q[0 * 3 + 2]))) / + ((pow(cos(q[0 * 3 + 0]), 2.0) + pow(sin(q[0 * 3 + 0]), 2.0)) * + (pow(cos(q[0 * 3 + 0]), 2.0) * pow(cos(q[0 * 3 + 1]), 2.0) * + pow(sin(q[0 * 3 + 2]), 2.0) + + pow(cos(q[0 * 3 + 0]), 2.0) * pow(cos(q[0 * 3 + 2]), 2.0) * + pow(sin(q[0 * 3 + 1]), 2.0) + + pow(cos(q[0 * 3 + 1]), 2.0) * pow(cos(q[0 * 3 + 2]), 2.0) * + pow(sin(q[0 * 3 + 0]), 2.0) + + pow(cos(q[0 * 3 + 0]), 2.0) * pow(sin(q[0 * 3 + 1]), 2.0) * + pow(sin(q[0 * 3 + 2]), 2.0) + + pow(cos(q[0 * 3 + 1]), 2.0) * pow(sin(q[0 * 3 + 0]), 2.0) * + pow(sin(q[0 * 3 + 2]), 2.0) + + pow(cos(q[0 * 3 + 2]), 2.0) * pow(sin(q[0 * 3 + 0]), 2.0) * + pow(sin(q[0 * 3 + 1]), 2.0) + + pow(sin(q[0 * 3 + 0]), 2.0) * pow(sin(q[0 * 3 + 1]), 2.0) * + pow(sin(q[0 * 3 + 2]), 2.0) + + pow(cos(q[0 * 3 + 0]), 2.0) * pow(cos(q[0 * 3 + 1]), 2.0) * + pow(cos(q[0 * 3 + 2]), 2.0)) * + 2.0) + + (sin(q[0 * 3 + 0]) * + (l[0 * 3 + 2] * cos(q[0 * 3 + 1]) * cos(q[0 * 3 + 2]) * + sin(q[0 * 3 + 0]) + + l[0 * 3 + 0] * cos(q[0 * 3 + 1]) * sin(q[0 * 3 + 0]) * + sin(q[0 * 3 + 2]) + + l[0 * 3 + 0] * cos(q[0 * 3 + 2]) * sin(q[0 * 3 + 0]) * + sin(q[0 * 3 + 1]) - + l[0 * 3 + 2] * sin(q[0 * 3 + 0]) * sin(q[0 * 3 + 1]) * + sin(q[0 * 3 + 2]) + + l[1 * 3 + 2] * pow(cos(q[0 * 3 + 1]), 2.0) * cos(q[0 * 3 + 2]) * + sin(q[0 * 3 + 0]) + + l[1 * 3 + 0] * pow(cos(q[0 * 3 + 1]), 2.0) * sin(q[0 * 3 + 0]) * + sin(q[0 * 3 + 2]) + + l[1 * 3 + 2] * cos(q[0 * 3 + 2]) * sin(q[0 * 3 + 0]) * + pow(sin(q[0 * 3 + 1]), 2.0) + + l[1 * 3 + 0] * sin(q[0 * 3 + 0]) * pow(sin(q[0 * 3 + 1]), 2.0) * + sin(q[0 * 3 + 2]) - + RL * l[0 * 3 + 1] * cos(q[0 * 3 + 0]) * cos(q[0 * 3 + 1]) * + cos(q[0 * 3 + 2]) - + RL * l[1 * 3 + 1] * cos(q[0 * 3 + 0]) * cos(q[0 * 3 + 1]) * + cos(q[0 * 3 + 2]) + + RL * l[0 * 3 + 1] * cos(q[0 * 3 + 0]) * sin(q[0 * 3 + 1]) * + sin(q[0 * 3 + 2]) + + RL * l[1 * 3 + 1] * cos(q[0 * 3 + 0]) * sin(q[0 * 3 + 1]) * + sin(q[0 * 3 + 2]))) / + ((pow(cos(q[0 * 3 + 0]), 2.0) + pow(sin(q[0 * 3 + 0]), 2.0)) * + (pow(cos(q[0 * 3 + 0]), 2.0) * pow(cos(q[0 * 3 + 1]), 2.0) * + pow(sin(q[0 * 3 + 2]), 2.0) + + pow(cos(q[0 * 3 + 0]), 2.0) * pow(cos(q[0 * 3 + 2]), 2.0) * + pow(sin(q[0 * 3 + 1]), 2.0) + + pow(cos(q[0 * 3 + 1]), 2.0) * pow(cos(q[0 * 3 + 2]), 2.0) * + pow(sin(q[0 * 3 + 0]), 2.0) + + pow(cos(q[0 * 3 + 0]), 2.0) * pow(sin(q[0 * 3 + 1]), 2.0) * + pow(sin(q[0 * 3 + 2]), 2.0) + + pow(cos(q[0 * 3 + 1]), 2.0) * pow(sin(q[0 * 3 + 0]), 2.0) * + pow(sin(q[0 * 3 + 2]), 2.0) + + pow(cos(q[0 * 3 + 2]), 2.0) * pow(sin(q[0 * 3 + 0]), 2.0) * + pow(sin(q[0 * 3 + 1]), 2.0) + + pow(sin(q[0 * 3 + 0]), 2.0) * pow(sin(q[0 * 3 + 1]), 2.0) * + pow(sin(q[0 * 3 + 2]), 2.0) + + pow(cos(q[0 * 3 + 0]), 2.0) * pow(cos(q[0 * 3 + 1]), 2.0) * + pow(cos(q[0 * 3 + 2]), 2.0)) * + 2.0) + + ((cos(q[0 * 3 + 1]) * cos(q[0 * 3 + 2]) - + sin(q[0 * 3 + 1]) * sin(q[0 * 3 + 2])) * + (l[0 * 3 + 2] * pow(sin(q[0 * 3 + 1]), 2.0) + + l[1 * 3 + 2] * cos(q[0 * 3 + 1]) - + l[1 * 3 + 0] * sin(q[0 * 3 + 1]) + + l[0 * 3 + 2] * pow(cos(q[0 * 3 + 1]), 2.0))) / + ((pow(cos(q[0 * 3 + 1]), 2.0) + pow(sin(q[0 * 3 + 1]), 2.0)) * + (pow(cos(q[0 * 3 + 1]), 2.0) * pow(cos(q[0 * 3 + 2]), 2.0) + + pow(cos(q[0 * 3 + 1]), 2.0) * pow(sin(q[0 * 3 + 2]), 2.0) + + pow(cos(q[0 * 3 + 2]), 2.0) * pow(sin(q[0 * 3 + 1]), 2.0) + + pow(sin(q[0 * 3 + 1]), 2.0) * pow(sin(q[0 * 3 + 2]), 2.0)) * + 2.0) + + (pow(cos(q[0 * 3 + 0]), 2.0) * + (cos(q[0 * 3 + 1]) * sin(q[0 * 3 + 2]) + + cos(q[0 * 3 + 2]) * sin(q[0 * 3 + 1])) * + (l[0 * 3 + 0] * pow(sin(q[0 * 3 + 1]), 2.0) + + l[1 * 3 + 0] * cos(q[0 * 3 + 1]) + + l[1 * 3 + 2] * sin(q[0 * 3 + 1]) + + l[0 * 3 + 0] * pow(cos(q[0 * 3 + 1]), 2.0))) / + ((pow(cos(q[0 * 3 + 0]), 2.0) * pow(cos(q[0 * 3 + 1]), 2.0) + + pow(cos(q[0 * 3 + 0]), 2.0) * pow(sin(q[0 * 3 + 1]), 2.0) + + pow(cos(q[0 * 3 + 1]), 2.0) * pow(sin(q[0 * 3 + 0]), 2.0) + + pow(sin(q[0 * 3 + 0]), 2.0) * pow(sin(q[0 * 3 + 1]), 2.0)) * + (pow(cos(q[0 * 3 + 0]), 2.0) * pow(cos(q[0 * 3 + 1]), 2.0) * + pow(sin(q[0 * 3 + 2]), 2.0) + + pow(cos(q[0 * 3 + 0]), 2.0) * pow(cos(q[0 * 3 + 2]), 2.0) * + pow(sin(q[0 * 3 + 1]), 2.0) + + pow(cos(q[0 * 3 + 1]), 2.0) * pow(cos(q[0 * 3 + 2]), 2.0) * + pow(sin(q[0 * 3 + 0]), 2.0) + + pow(cos(q[0 * 3 + 0]), 2.0) * pow(sin(q[0 * 3 + 1]), 2.0) * + pow(sin(q[0 * 3 + 2]), 2.0) + + pow(cos(q[0 * 3 + 1]), 2.0) * pow(sin(q[0 * 3 + 0]), 2.0) * + pow(sin(q[0 * 3 + 2]), 2.0) + + pow(cos(q[0 * 3 + 2]), 2.0) * pow(sin(q[0 * 3 + 0]), 2.0) * + pow(sin(q[0 * 3 + 1]), 2.0) + + pow(sin(q[0 * 3 + 0]), 2.0) * pow(sin(q[0 * 3 + 1]), 2.0) * + pow(sin(q[0 * 3 + 2]), 2.0) + + pow(cos(q[0 * 3 + 0]), 2.0) * pow(cos(q[0 * 3 + 1]), 2.0) * + pow(cos(q[0 * 3 + 2]), 2.0)) * + 2.0) + + (pow(sin(q[0 * 3 + 0]), 2.0) * + (cos(q[0 * 3 + 1]) * sin(q[0 * 3 + 2]) + + cos(q[0 * 3 + 2]) * sin(q[0 * 3 + 1])) * + (l[0 * 3 + 0] * pow(sin(q[0 * 3 + 1]), 2.0) + + l[1 * 3 + 0] * cos(q[0 * 3 + 1]) + + l[1 * 3 + 2] * sin(q[0 * 3 + 1]) + + l[0 * 3 + 0] * pow(cos(q[0 * 3 + 1]), 2.0))) / + ((pow(cos(q[0 * 3 + 0]), 2.0) * pow(cos(q[0 * 3 + 1]), 2.0) + + pow(cos(q[0 * 3 + 0]), 2.0) * pow(sin(q[0 * 3 + 1]), 2.0) + + pow(cos(q[0 * 3 + 1]), 2.0) * pow(sin(q[0 * 3 + 0]), 2.0) + + pow(sin(q[0 * 3 + 0]), 2.0) * pow(sin(q[0 * 3 + 1]), 2.0)) * + (pow(cos(q[0 * 3 + 0]), 2.0) * pow(cos(q[0 * 3 + 1]), 2.0) * + pow(sin(q[0 * 3 + 2]), 2.0) + + pow(cos(q[0 * 3 + 0]), 2.0) * pow(cos(q[0 * 3 + 2]), 2.0) * + pow(sin(q[0 * 3 + 1]), 2.0) + + pow(cos(q[0 * 3 + 1]), 2.0) * pow(cos(q[0 * 3 + 2]), 2.0) * + pow(sin(q[0 * 3 + 0]), 2.0) + + pow(cos(q[0 * 3 + 0]), 2.0) * pow(sin(q[0 * 3 + 1]), 2.0) * + pow(sin(q[0 * 3 + 2]), 2.0) + + pow(cos(q[0 * 3 + 1]), 2.0) * pow(sin(q[0 * 3 + 0]), 2.0) * + pow(sin(q[0 * 3 + 2]), 2.0) + + pow(cos(q[0 * 3 + 2]), 2.0) * pow(sin(q[0 * 3 + 0]), 2.0) * + pow(sin(q[0 * 3 + 1]), 2.0) + + pow(sin(q[0 * 3 + 0]), 2.0) * pow(sin(q[0 * 3 + 1]), 2.0) * + pow(sin(q[0 * 3 + 2]), 2.0) + + pow(cos(q[0 * 3 + 0]), 2.0) * pow(cos(q[0 * 3 + 1]), 2.0) * + pow(cos(q[0 * 3 + 2]), 2.0)) * + 2.0))) / + (pow(cos(q[0 * 3 + 1]), 2.0) * pow(cos(q[0 * 3 + 2]), 2.0) + + pow(cos(q[0 * 3 + 1]), 2.0) * pow(sin(q[0 * 3 + 2]), 2.0) + + pow(cos(q[0 * 3 + 2]), 2.0) * pow(sin(q[0 * 3 + 1]), 2.0) + + pow(sin(q[0 * 3 + 1]), 2.0) * pow(sin(q[0 * 3 + 2]), 2.0)))) / + (pow(cos(q[0 * 3 + 0]), 2.0) + pow(sin(q[0 * 3 + 0]), 2.0)) - + (cos(q[0 * 3 + 0]) * + (cos(q[0 * 3 + 1]) * sin(q[0 * 3 + 2]) + + cos(q[0 * 3 + 2]) * sin(q[0 * 3 + 1])) * + (l[1 * 3 + 2] * cos(q[0 * 3 + 2]) + + l[1 * 3 + 0] * sin(q[0 * 3 + 2]))) / + (pow(cos(q[0 * 3 + 0]), 2.0) * pow(cos(q[0 * 3 + 1]), 2.0) * + pow(sin(q[0 * 3 + 2]), 2.0) + + pow(cos(q[0 * 3 + 0]), 2.0) * pow(cos(q[0 * 3 + 2]), 2.0) * + pow(sin(q[0 * 3 + 1]), 2.0) + + pow(cos(q[0 * 3 + 1]), 2.0) * pow(cos(q[0 * 3 + 2]), 2.0) * + pow(sin(q[0 * 3 + 0]), 2.0) + + pow(cos(q[0 * 3 + 0]), 2.0) * pow(sin(q[0 * 3 + 1]), 2.0) * + pow(sin(q[0 * 3 + 2]), 2.0) + + pow(cos(q[0 * 3 + 1]), 2.0) * pow(sin(q[0 * 3 + 0]), 2.0) * + pow(sin(q[0 * 3 + 2]), 2.0) + + pow(cos(q[0 * 3 + 2]), 2.0) * pow(sin(q[0 * 3 + 0]), 2.0) * + pow(sin(q[0 * 3 + 1]), 2.0) + + pow(sin(q[0 * 3 + 0]), 2.0) * pow(sin(q[0 * 3 + 1]), 2.0) * + pow(sin(q[0 * 3 + 2]), 2.0) + + pow(cos(q[0 * 3 + 0]), 2.0) * pow(cos(q[0 * 3 + 1]), 2.0) * + pow(cos(q[0 * 3 + 2]), 2.0)) - + (cos(q[0 * 3 + 0]) * + (cos(q[0 * 3 + 1]) * cos(q[0 * 3 + 2]) - + sin(q[0 * 3 + 1]) * sin(q[0 * 3 + 2])) * + (l[1 * 3 + 0] * cos(q[0 * 3 + 2]) - + l[1 * 3 + 2] * sin(q[0 * 3 + 2]))) / + (pow(cos(q[0 * 3 + 0]), 2.0) * pow(cos(q[0 * 3 + 1]), 2.0) * + pow(sin(q[0 * 3 + 2]), 2.0) + + pow(cos(q[0 * 3 + 0]), 2.0) * pow(cos(q[0 * 3 + 2]), 2.0) * + pow(sin(q[0 * 3 + 1]), 2.0) + + pow(cos(q[0 * 3 + 1]), 2.0) * pow(cos(q[0 * 3 + 2]), 2.0) * + pow(sin(q[0 * 3 + 0]), 2.0) + + pow(cos(q[0 * 3 + 0]), 2.0) * pow(sin(q[0 * 3 + 1]), 2.0) * + pow(sin(q[0 * 3 + 2]), 2.0) + + pow(cos(q[0 * 3 + 1]), 2.0) * pow(sin(q[0 * 3 + 0]), 2.0) * + pow(sin(q[0 * 3 + 2]), 2.0) + + pow(cos(q[0 * 3 + 2]), 2.0) * pow(sin(q[0 * 3 + 0]), 2.0) * + pow(sin(q[0 * 3 + 1]), 2.0) + + pow(sin(q[0 * 3 + 0]), 2.0) * pow(sin(q[0 * 3 + 1]), 2.0) * + pow(sin(q[0 * 3 + 2]), 2.0) + + pow(cos(q[0 * 3 + 0]), 2.0) * pow(cos(q[0 * 3 + 1]), 2.0) * + pow(cos(q[0 * 3 + 2]), 2.0)); + F(2 * 3 + 2) = + (cos(q[0 * 3 + 0]) * + (l[0 * 3 + 0] * pow(sin(q[0 * 3 + 1]), 2.0) + + l[1 * 3 + 0] * cos(q[0 * 3 + 1]) + l[1 * 3 + 2] * sin(q[0 * 3 + 1]) + + l[0 * 3 + 0] * pow(cos(q[0 * 3 + 1]), 2.0))) / + (pow(cos(q[0 * 3 + 0]), 2.0) * pow(cos(q[0 * 3 + 1]), 2.0) + + pow(cos(q[0 * 3 + 0]), 2.0) * pow(sin(q[0 * 3 + 1]), 2.0) + + pow(cos(q[0 * 3 + 1]), 2.0) * pow(sin(q[0 * 3 + 0]), 2.0) + + pow(sin(q[0 * 3 + 0]), 2.0) * pow(sin(q[0 * 3 + 1]), 2.0)) - + (cos(q[0 * 3 + 0]) * + (((cos(q[0 * 3 + 1]) * cos(q[0 * 3 + 2]) - + sin(q[0 * 3 + 1]) * sin(q[0 * 3 + 2])) * + (l[2 * 3 + 0] - + (cos(q[0 * 3 + 0]) * + (-l[0 * 3 + 0] * cos(q[0 * 3 + 0]) * cos(q[0 * 3 + 1]) * + cos(q[0 * 3 + 2]) + + l[0 * 3 + 2] * cos(q[0 * 3 + 0]) * cos(q[0 * 3 + 1]) * + sin(q[0 * 3 + 2]) + + l[0 * 3 + 2] * cos(q[0 * 3 + 0]) * cos(q[0 * 3 + 2]) * + sin(q[0 * 3 + 1]) + + l[0 * 3 + 0] * cos(q[0 * 3 + 0]) * sin(q[0 * 3 + 1]) * + sin(q[0 * 3 + 2]) - + l[1 * 3 + 0] * cos(q[0 * 3 + 0]) * pow(cos(q[0 * 3 + 1]), 2.0) * + cos(q[0 * 3 + 2]) - + l[1 * 3 + 0] * cos(q[0 * 3 + 0]) * cos(q[0 * 3 + 2]) * + pow(sin(q[0 * 3 + 1]), 2.0) + + l[1 * 3 + 2] * cos(q[0 * 3 + 0]) * pow(cos(q[0 * 3 + 1]), 2.0) * + sin(q[0 * 3 + 2]) + + l[1 * 3 + 2] * cos(q[0 * 3 + 0]) * pow(sin(q[0 * 3 + 1]), 2.0) * + sin(q[0 * 3 + 2]) + + RL * l[0 * 3 + 1] * cos(q[0 * 3 + 1]) * sin(q[0 * 3 + 0]) * + sin(q[0 * 3 + 2]) + + RL * l[0 * 3 + 1] * cos(q[0 * 3 + 2]) * sin(q[0 * 3 + 0]) * + sin(q[0 * 3 + 1]) + + RL * l[1 * 3 + 1] * cos(q[0 * 3 + 1]) * sin(q[0 * 3 + 0]) * + sin(q[0 * 3 + 2]) + + RL * l[1 * 3 + 1] * cos(q[0 * 3 + 2]) * sin(q[0 * 3 + 0]) * + sin(q[0 * 3 + 1]))) / + ((pow(cos(q[0 * 3 + 0]), 2.0) + pow(sin(q[0 * 3 + 0]), 2.0)) * + (pow(cos(q[0 * 3 + 0]), 2.0) * pow(cos(q[0 * 3 + 1]), 2.0) * + pow(sin(q[0 * 3 + 2]), 2.0) + + pow(cos(q[0 * 3 + 0]), 2.0) * pow(cos(q[0 * 3 + 2]), 2.0) * + pow(sin(q[0 * 3 + 1]), 2.0) + + pow(cos(q[0 * 3 + 1]), 2.0) * pow(cos(q[0 * 3 + 2]), 2.0) * + pow(sin(q[0 * 3 + 0]), 2.0) + + pow(cos(q[0 * 3 + 0]), 2.0) * pow(sin(q[0 * 3 + 1]), 2.0) * + pow(sin(q[0 * 3 + 2]), 2.0) + + pow(cos(q[0 * 3 + 1]), 2.0) * pow(sin(q[0 * 3 + 0]), 2.0) * + pow(sin(q[0 * 3 + 2]), 2.0) + + pow(cos(q[0 * 3 + 2]), 2.0) * pow(sin(q[0 * 3 + 0]), 2.0) * + pow(sin(q[0 * 3 + 1]), 2.0) + + pow(sin(q[0 * 3 + 0]), 2.0) * pow(sin(q[0 * 3 + 1]), 2.0) * + pow(sin(q[0 * 3 + 2]), 2.0) + + pow(cos(q[0 * 3 + 0]), 2.0) * pow(cos(q[0 * 3 + 1]), 2.0) * + pow(cos(q[0 * 3 + 2]), 2.0)) * + 2.0) + + (sin(q[0 * 3 + 0]) * + (l[0 * 3 + 0] * cos(q[0 * 3 + 1]) * cos(q[0 * 3 + 2]) * + sin(q[0 * 3 + 0]) - + l[0 * 3 + 2] * cos(q[0 * 3 + 1]) * sin(q[0 * 3 + 0]) * + sin(q[0 * 3 + 2]) - + l[0 * 3 + 2] * cos(q[0 * 3 + 2]) * sin(q[0 * 3 + 0]) * + sin(q[0 * 3 + 1]) - + l[0 * 3 + 0] * sin(q[0 * 3 + 0]) * sin(q[0 * 3 + 1]) * + sin(q[0 * 3 + 2]) + + l[1 * 3 + 0] * pow(cos(q[0 * 3 + 1]), 2.0) * cos(q[0 * 3 + 2]) * + sin(q[0 * 3 + 0]) + + l[1 * 3 + 0] * cos(q[0 * 3 + 2]) * sin(q[0 * 3 + 0]) * + pow(sin(q[0 * 3 + 1]), 2.0) - + l[1 * 3 + 2] * pow(cos(q[0 * 3 + 1]), 2.0) * sin(q[0 * 3 + 0]) * + sin(q[0 * 3 + 2]) - + l[1 * 3 + 2] * sin(q[0 * 3 + 0]) * pow(sin(q[0 * 3 + 1]), 2.0) * + sin(q[0 * 3 + 2]) + + RL * l[0 * 3 + 1] * cos(q[0 * 3 + 0]) * cos(q[0 * 3 + 1]) * + sin(q[0 * 3 + 2]) + + RL * l[0 * 3 + 1] * cos(q[0 * 3 + 0]) * cos(q[0 * 3 + 2]) * + sin(q[0 * 3 + 1]) + + RL * l[1 * 3 + 1] * cos(q[0 * 3 + 0]) * cos(q[0 * 3 + 1]) * + sin(q[0 * 3 + 2]) + + RL * l[1 * 3 + 1] * cos(q[0 * 3 + 0]) * cos(q[0 * 3 + 2]) * + sin(q[0 * 3 + 1]))) / + ((pow(cos(q[0 * 3 + 0]), 2.0) + pow(sin(q[0 * 3 + 0]), 2.0)) * + (pow(cos(q[0 * 3 + 0]), 2.0) * pow(cos(q[0 * 3 + 1]), 2.0) * + pow(sin(q[0 * 3 + 2]), 2.0) + + pow(cos(q[0 * 3 + 0]), 2.0) * pow(cos(q[0 * 3 + 2]), 2.0) * + pow(sin(q[0 * 3 + 1]), 2.0) + + pow(cos(q[0 * 3 + 1]), 2.0) * pow(cos(q[0 * 3 + 2]), 2.0) * + pow(sin(q[0 * 3 + 0]), 2.0) + + pow(cos(q[0 * 3 + 0]), 2.0) * pow(sin(q[0 * 3 + 1]), 2.0) * + pow(sin(q[0 * 3 + 2]), 2.0) + + pow(cos(q[0 * 3 + 1]), 2.0) * pow(sin(q[0 * 3 + 0]), 2.0) * + pow(sin(q[0 * 3 + 2]), 2.0) + + pow(cos(q[0 * 3 + 2]), 2.0) * pow(sin(q[0 * 3 + 0]), 2.0) * + pow(sin(q[0 * 3 + 1]), 2.0) + + pow(sin(q[0 * 3 + 0]), 2.0) * pow(sin(q[0 * 3 + 1]), 2.0) * + pow(sin(q[0 * 3 + 2]), 2.0) + + pow(cos(q[0 * 3 + 0]), 2.0) * pow(cos(q[0 * 3 + 1]), 2.0) * + pow(cos(q[0 * 3 + 2]), 2.0)) * + 2.0) - + ((cos(q[0 * 3 + 1]) * sin(q[0 * 3 + 2]) + + cos(q[0 * 3 + 2]) * sin(q[0 * 3 + 1])) * + (l[0 * 3 + 2] * pow(sin(q[0 * 3 + 1]), 2.0) + + l[1 * 3 + 2] * cos(q[0 * 3 + 1]) - + l[1 * 3 + 0] * sin(q[0 * 3 + 1]) + + l[0 * 3 + 2] * pow(cos(q[0 * 3 + 1]), 2.0))) / + ((pow(cos(q[0 * 3 + 1]), 2.0) + pow(sin(q[0 * 3 + 1]), 2.0)) * + (pow(cos(q[0 * 3 + 1]), 2.0) * pow(cos(q[0 * 3 + 2]), 2.0) + + pow(cos(q[0 * 3 + 1]), 2.0) * pow(sin(q[0 * 3 + 2]), 2.0) + + pow(cos(q[0 * 3 + 2]), 2.0) * pow(sin(q[0 * 3 + 1]), 2.0) + + pow(sin(q[0 * 3 + 1]), 2.0) * pow(sin(q[0 * 3 + 2]), 2.0)) * + 2.0) + + (pow(cos(q[0 * 3 + 0]), 2.0) * + (cos(q[0 * 3 + 1]) * cos(q[0 * 3 + 2]) - + sin(q[0 * 3 + 1]) * sin(q[0 * 3 + 2])) * + (l[0 * 3 + 0] * pow(sin(q[0 * 3 + 1]), 2.0) + + l[1 * 3 + 0] * cos(q[0 * 3 + 1]) + + l[1 * 3 + 2] * sin(q[0 * 3 + 1]) + + l[0 * 3 + 0] * pow(cos(q[0 * 3 + 1]), 2.0))) / + ((pow(cos(q[0 * 3 + 0]), 2.0) * pow(cos(q[0 * 3 + 1]), 2.0) + + pow(cos(q[0 * 3 + 0]), 2.0) * pow(sin(q[0 * 3 + 1]), 2.0) + + pow(cos(q[0 * 3 + 1]), 2.0) * pow(sin(q[0 * 3 + 0]), 2.0) + + pow(sin(q[0 * 3 + 0]), 2.0) * pow(sin(q[0 * 3 + 1]), 2.0)) * + (pow(cos(q[0 * 3 + 0]), 2.0) * pow(cos(q[0 * 3 + 1]), 2.0) * + pow(sin(q[0 * 3 + 2]), 2.0) + + pow(cos(q[0 * 3 + 0]), 2.0) * pow(cos(q[0 * 3 + 2]), 2.0) * + pow(sin(q[0 * 3 + 1]), 2.0) + + pow(cos(q[0 * 3 + 1]), 2.0) * pow(cos(q[0 * 3 + 2]), 2.0) * + pow(sin(q[0 * 3 + 0]), 2.0) + + pow(cos(q[0 * 3 + 0]), 2.0) * pow(sin(q[0 * 3 + 1]), 2.0) * + pow(sin(q[0 * 3 + 2]), 2.0) + + pow(cos(q[0 * 3 + 1]), 2.0) * pow(sin(q[0 * 3 + 0]), 2.0) * + pow(sin(q[0 * 3 + 2]), 2.0) + + pow(cos(q[0 * 3 + 2]), 2.0) * pow(sin(q[0 * 3 + 0]), 2.0) * + pow(sin(q[0 * 3 + 1]), 2.0) + + pow(sin(q[0 * 3 + 0]), 2.0) * pow(sin(q[0 * 3 + 1]), 2.0) * + pow(sin(q[0 * 3 + 2]), 2.0) + + pow(cos(q[0 * 3 + 0]), 2.0) * pow(cos(q[0 * 3 + 1]), 2.0) * + pow(cos(q[0 * 3 + 2]), 2.0)) * + 2.0) + + (pow(sin(q[0 * 3 + 0]), 2.0) * + (cos(q[0 * 3 + 1]) * cos(q[0 * 3 + 2]) - + sin(q[0 * 3 + 1]) * sin(q[0 * 3 + 2])) * + (l[0 * 3 + 0] * pow(sin(q[0 * 3 + 1]), 2.0) + + l[1 * 3 + 0] * cos(q[0 * 3 + 1]) + + l[1 * 3 + 2] * sin(q[0 * 3 + 1]) + + l[0 * 3 + 0] * pow(cos(q[0 * 3 + 1]), 2.0))) / + ((pow(cos(q[0 * 3 + 0]), 2.0) * pow(cos(q[0 * 3 + 1]), 2.0) + + pow(cos(q[0 * 3 + 0]), 2.0) * pow(sin(q[0 * 3 + 1]), 2.0) + + pow(cos(q[0 * 3 + 1]), 2.0) * pow(sin(q[0 * 3 + 0]), 2.0) + + pow(sin(q[0 * 3 + 0]), 2.0) * pow(sin(q[0 * 3 + 1]), 2.0)) * + (pow(cos(q[0 * 3 + 0]), 2.0) * pow(cos(q[0 * 3 + 1]), 2.0) * + pow(sin(q[0 * 3 + 2]), 2.0) + + pow(cos(q[0 * 3 + 0]), 2.0) * pow(cos(q[0 * 3 + 2]), 2.0) * + pow(sin(q[0 * 3 + 1]), 2.0) + + pow(cos(q[0 * 3 + 1]), 2.0) * pow(cos(q[0 * 3 + 2]), 2.0) * + pow(sin(q[0 * 3 + 0]), 2.0) + + pow(cos(q[0 * 3 + 0]), 2.0) * pow(sin(q[0 * 3 + 1]), 2.0) * + pow(sin(q[0 * 3 + 2]), 2.0) + + pow(cos(q[0 * 3 + 1]), 2.0) * pow(sin(q[0 * 3 + 0]), 2.0) * + pow(sin(q[0 * 3 + 2]), 2.0) + + pow(cos(q[0 * 3 + 2]), 2.0) * pow(sin(q[0 * 3 + 0]), 2.0) * + pow(sin(q[0 * 3 + 1]), 2.0) + + pow(sin(q[0 * 3 + 0]), 2.0) * pow(sin(q[0 * 3 + 1]), 2.0) * + pow(sin(q[0 * 3 + 2]), 2.0) + + pow(cos(q[0 * 3 + 0]), 2.0) * pow(cos(q[0 * 3 + 1]), 2.0) * + pow(cos(q[0 * 3 + 2]), 2.0)) * + 2.0))) / + (pow(cos(q[0 * 3 + 1]), 2.0) * pow(cos(q[0 * 3 + 2]), 2.0) + + pow(cos(q[0 * 3 + 1]), 2.0) * pow(sin(q[0 * 3 + 2]), 2.0) + + pow(cos(q[0 * 3 + 2]), 2.0) * pow(sin(q[0 * 3 + 1]), 2.0) + + pow(sin(q[0 * 3 + 1]), 2.0) * pow(sin(q[0 * 3 + 2]), 2.0)) + + ((cos(q[0 * 3 + 1]) * sin(q[0 * 3 + 2]) + + cos(q[0 * 3 + 2]) * sin(q[0 * 3 + 1])) * + (l[2 * 3 + 2] + + (cos(q[0 * 3 + 0]) * + (l[0 * 3 + 2] * cos(q[0 * 3 + 0]) * cos(q[0 * 3 + 1]) * + cos(q[0 * 3 + 2]) + + l[0 * 3 + 0] * cos(q[0 * 3 + 0]) * cos(q[0 * 3 + 1]) * + sin(q[0 * 3 + 2]) + + l[0 * 3 + 0] * cos(q[0 * 3 + 0]) * cos(q[0 * 3 + 2]) * + sin(q[0 * 3 + 1]) - + l[0 * 3 + 2] * cos(q[0 * 3 + 0]) * sin(q[0 * 3 + 1]) * + sin(q[0 * 3 + 2]) + + l[1 * 3 + 2] * cos(q[0 * 3 + 0]) * pow(cos(q[0 * 3 + 1]), 2.0) * + cos(q[0 * 3 + 2]) + + l[1 * 3 + 0] * cos(q[0 * 3 + 0]) * pow(cos(q[0 * 3 + 1]), 2.0) * + sin(q[0 * 3 + 2]) + + l[1 * 3 + 2] * cos(q[0 * 3 + 0]) * cos(q[0 * 3 + 2]) * + pow(sin(q[0 * 3 + 1]), 2.0) + + l[1 * 3 + 0] * cos(q[0 * 3 + 0]) * pow(sin(q[0 * 3 + 1]), 2.0) * + sin(q[0 * 3 + 2]) + + RL * l[0 * 3 + 1] * cos(q[0 * 3 + 1]) * cos(q[0 * 3 + 2]) * + sin(q[0 * 3 + 0]) + + RL * l[1 * 3 + 1] * cos(q[0 * 3 + 1]) * cos(q[0 * 3 + 2]) * + sin(q[0 * 3 + 0]) - + RL * l[0 * 3 + 1] * sin(q[0 * 3 + 0]) * sin(q[0 * 3 + 1]) * + sin(q[0 * 3 + 2]) - + RL * l[1 * 3 + 1] * sin(q[0 * 3 + 0]) * sin(q[0 * 3 + 1]) * + sin(q[0 * 3 + 2]))) / + ((pow(cos(q[0 * 3 + 0]), 2.0) + pow(sin(q[0 * 3 + 0]), 2.0)) * + (pow(cos(q[0 * 3 + 0]), 2.0) * pow(cos(q[0 * 3 + 1]), 2.0) * + pow(sin(q[0 * 3 + 2]), 2.0) + + pow(cos(q[0 * 3 + 0]), 2.0) * pow(cos(q[0 * 3 + 2]), 2.0) * + pow(sin(q[0 * 3 + 1]), 2.0) + + pow(cos(q[0 * 3 + 1]), 2.0) * pow(cos(q[0 * 3 + 2]), 2.0) * + pow(sin(q[0 * 3 + 0]), 2.0) + + pow(cos(q[0 * 3 + 0]), 2.0) * pow(sin(q[0 * 3 + 1]), 2.0) * + pow(sin(q[0 * 3 + 2]), 2.0) + + pow(cos(q[0 * 3 + 1]), 2.0) * pow(sin(q[0 * 3 + 0]), 2.0) * + pow(sin(q[0 * 3 + 2]), 2.0) + + pow(cos(q[0 * 3 + 2]), 2.0) * pow(sin(q[0 * 3 + 0]), 2.0) * + pow(sin(q[0 * 3 + 1]), 2.0) + + pow(sin(q[0 * 3 + 0]), 2.0) * pow(sin(q[0 * 3 + 1]), 2.0) * + pow(sin(q[0 * 3 + 2]), 2.0) + + pow(cos(q[0 * 3 + 0]), 2.0) * pow(cos(q[0 * 3 + 1]), 2.0) * + pow(cos(q[0 * 3 + 2]), 2.0)) * + 2.0) + + (sin(q[0 * 3 + 0]) * + (l[0 * 3 + 2] * cos(q[0 * 3 + 1]) * cos(q[0 * 3 + 2]) * + sin(q[0 * 3 + 0]) + + l[0 * 3 + 0] * cos(q[0 * 3 + 1]) * sin(q[0 * 3 + 0]) * + sin(q[0 * 3 + 2]) + + l[0 * 3 + 0] * cos(q[0 * 3 + 2]) * sin(q[0 * 3 + 0]) * + sin(q[0 * 3 + 1]) - + l[0 * 3 + 2] * sin(q[0 * 3 + 0]) * sin(q[0 * 3 + 1]) * + sin(q[0 * 3 + 2]) + + l[1 * 3 + 2] * pow(cos(q[0 * 3 + 1]), 2.0) * cos(q[0 * 3 + 2]) * + sin(q[0 * 3 + 0]) + + l[1 * 3 + 0] * pow(cos(q[0 * 3 + 1]), 2.0) * sin(q[0 * 3 + 0]) * + sin(q[0 * 3 + 2]) + + l[1 * 3 + 2] * cos(q[0 * 3 + 2]) * sin(q[0 * 3 + 0]) * + pow(sin(q[0 * 3 + 1]), 2.0) + + l[1 * 3 + 0] * sin(q[0 * 3 + 0]) * pow(sin(q[0 * 3 + 1]), 2.0) * + sin(q[0 * 3 + 2]) - + RL * l[0 * 3 + 1] * cos(q[0 * 3 + 0]) * cos(q[0 * 3 + 1]) * + cos(q[0 * 3 + 2]) - + RL * l[1 * 3 + 1] * cos(q[0 * 3 + 0]) * cos(q[0 * 3 + 1]) * + cos(q[0 * 3 + 2]) + + RL * l[0 * 3 + 1] * cos(q[0 * 3 + 0]) * sin(q[0 * 3 + 1]) * + sin(q[0 * 3 + 2]) + + RL * l[1 * 3 + 1] * cos(q[0 * 3 + 0]) * sin(q[0 * 3 + 1]) * + sin(q[0 * 3 + 2]))) / + ((pow(cos(q[0 * 3 + 0]), 2.0) + pow(sin(q[0 * 3 + 0]), 2.0)) * + (pow(cos(q[0 * 3 + 0]), 2.0) * pow(cos(q[0 * 3 + 1]), 2.0) * + pow(sin(q[0 * 3 + 2]), 2.0) + + pow(cos(q[0 * 3 + 0]), 2.0) * pow(cos(q[0 * 3 + 2]), 2.0) * + pow(sin(q[0 * 3 + 1]), 2.0) + + pow(cos(q[0 * 3 + 1]), 2.0) * pow(cos(q[0 * 3 + 2]), 2.0) * + pow(sin(q[0 * 3 + 0]), 2.0) + + pow(cos(q[0 * 3 + 0]), 2.0) * pow(sin(q[0 * 3 + 1]), 2.0) * + pow(sin(q[0 * 3 + 2]), 2.0) + + pow(cos(q[0 * 3 + 1]), 2.0) * pow(sin(q[0 * 3 + 0]), 2.0) * + pow(sin(q[0 * 3 + 2]), 2.0) + + pow(cos(q[0 * 3 + 2]), 2.0) * pow(sin(q[0 * 3 + 0]), 2.0) * + pow(sin(q[0 * 3 + 1]), 2.0) + + pow(sin(q[0 * 3 + 0]), 2.0) * pow(sin(q[0 * 3 + 1]), 2.0) * + pow(sin(q[0 * 3 + 2]), 2.0) + + pow(cos(q[0 * 3 + 0]), 2.0) * pow(cos(q[0 * 3 + 1]), 2.0) * + pow(cos(q[0 * 3 + 2]), 2.0)) * + 2.0) + + ((cos(q[0 * 3 + 1]) * cos(q[0 * 3 + 2]) - + sin(q[0 * 3 + 1]) * sin(q[0 * 3 + 2])) * + (l[0 * 3 + 2] * pow(sin(q[0 * 3 + 1]), 2.0) + + l[1 * 3 + 2] * cos(q[0 * 3 + 1]) - + l[1 * 3 + 0] * sin(q[0 * 3 + 1]) + + l[0 * 3 + 2] * pow(cos(q[0 * 3 + 1]), 2.0))) / + ((pow(cos(q[0 * 3 + 1]), 2.0) + pow(sin(q[0 * 3 + 1]), 2.0)) * + (pow(cos(q[0 * 3 + 1]), 2.0) * pow(cos(q[0 * 3 + 2]), 2.0) + + pow(cos(q[0 * 3 + 1]), 2.0) * pow(sin(q[0 * 3 + 2]), 2.0) + + pow(cos(q[0 * 3 + 2]), 2.0) * pow(sin(q[0 * 3 + 1]), 2.0) + + pow(sin(q[0 * 3 + 1]), 2.0) * pow(sin(q[0 * 3 + 2]), 2.0)) * + 2.0) + + (pow(cos(q[0 * 3 + 0]), 2.0) * + (cos(q[0 * 3 + 1]) * sin(q[0 * 3 + 2]) + + cos(q[0 * 3 + 2]) * sin(q[0 * 3 + 1])) * + (l[0 * 3 + 0] * pow(sin(q[0 * 3 + 1]), 2.0) + + l[1 * 3 + 0] * cos(q[0 * 3 + 1]) + + l[1 * 3 + 2] * sin(q[0 * 3 + 1]) + + l[0 * 3 + 0] * pow(cos(q[0 * 3 + 1]), 2.0))) / + ((pow(cos(q[0 * 3 + 0]), 2.0) * pow(cos(q[0 * 3 + 1]), 2.0) + + pow(cos(q[0 * 3 + 0]), 2.0) * pow(sin(q[0 * 3 + 1]), 2.0) + + pow(cos(q[0 * 3 + 1]), 2.0) * pow(sin(q[0 * 3 + 0]), 2.0) + + pow(sin(q[0 * 3 + 0]), 2.0) * pow(sin(q[0 * 3 + 1]), 2.0)) * + (pow(cos(q[0 * 3 + 0]), 2.0) * pow(cos(q[0 * 3 + 1]), 2.0) * + pow(sin(q[0 * 3 + 2]), 2.0) + + pow(cos(q[0 * 3 + 0]), 2.0) * pow(cos(q[0 * 3 + 2]), 2.0) * + pow(sin(q[0 * 3 + 1]), 2.0) + + pow(cos(q[0 * 3 + 1]), 2.0) * pow(cos(q[0 * 3 + 2]), 2.0) * + pow(sin(q[0 * 3 + 0]), 2.0) + + pow(cos(q[0 * 3 + 0]), 2.0) * pow(sin(q[0 * 3 + 1]), 2.0) * + pow(sin(q[0 * 3 + 2]), 2.0) + + pow(cos(q[0 * 3 + 1]), 2.0) * pow(sin(q[0 * 3 + 0]), 2.0) * + pow(sin(q[0 * 3 + 2]), 2.0) + + pow(cos(q[0 * 3 + 2]), 2.0) * pow(sin(q[0 * 3 + 0]), 2.0) * + pow(sin(q[0 * 3 + 1]), 2.0) + + pow(sin(q[0 * 3 + 0]), 2.0) * pow(sin(q[0 * 3 + 1]), 2.0) * + pow(sin(q[0 * 3 + 2]), 2.0) + + pow(cos(q[0 * 3 + 0]), 2.0) * pow(cos(q[0 * 3 + 1]), 2.0) * + pow(cos(q[0 * 3 + 2]), 2.0)) * + 2.0) + + (pow(sin(q[0 * 3 + 0]), 2.0) * + (cos(q[0 * 3 + 1]) * sin(q[0 * 3 + 2]) + + cos(q[0 * 3 + 2]) * sin(q[0 * 3 + 1])) * + (l[0 * 3 + 0] * pow(sin(q[0 * 3 + 1]), 2.0) + + l[1 * 3 + 0] * cos(q[0 * 3 + 1]) + + l[1 * 3 + 2] * sin(q[0 * 3 + 1]) + + l[0 * 3 + 0] * pow(cos(q[0 * 3 + 1]), 2.0))) / + ((pow(cos(q[0 * 3 + 0]), 2.0) * pow(cos(q[0 * 3 + 1]), 2.0) + + pow(cos(q[0 * 3 + 0]), 2.0) * pow(sin(q[0 * 3 + 1]), 2.0) + + pow(cos(q[0 * 3 + 1]), 2.0) * pow(sin(q[0 * 3 + 0]), 2.0) + + pow(sin(q[0 * 3 + 0]), 2.0) * pow(sin(q[0 * 3 + 1]), 2.0)) * + (pow(cos(q[0 * 3 + 0]), 2.0) * pow(cos(q[0 * 3 + 1]), 2.0) * + pow(sin(q[0 * 3 + 2]), 2.0) + + pow(cos(q[0 * 3 + 0]), 2.0) * pow(cos(q[0 * 3 + 2]), 2.0) * + pow(sin(q[0 * 3 + 1]), 2.0) + + pow(cos(q[0 * 3 + 1]), 2.0) * pow(cos(q[0 * 3 + 2]), 2.0) * + pow(sin(q[0 * 3 + 0]), 2.0) + + pow(cos(q[0 * 3 + 0]), 2.0) * pow(sin(q[0 * 3 + 1]), 2.0) * + pow(sin(q[0 * 3 + 2]), 2.0) + + pow(cos(q[0 * 3 + 1]), 2.0) * pow(sin(q[0 * 3 + 0]), 2.0) * + pow(sin(q[0 * 3 + 2]), 2.0) + + pow(cos(q[0 * 3 + 2]), 2.0) * pow(sin(q[0 * 3 + 0]), 2.0) * + pow(sin(q[0 * 3 + 1]), 2.0) + + pow(sin(q[0 * 3 + 0]), 2.0) * pow(sin(q[0 * 3 + 1]), 2.0) * + pow(sin(q[0 * 3 + 2]), 2.0) + + pow(cos(q[0 * 3 + 0]), 2.0) * pow(cos(q[0 * 3 + 1]), 2.0) * + pow(cos(q[0 * 3 + 2]), 2.0)) * + 2.0))) / + (pow(cos(q[0 * 3 + 1]), 2.0) * pow(cos(q[0 * 3 + 2]), 2.0) + + pow(cos(q[0 * 3 + 1]), 2.0) * pow(sin(q[0 * 3 + 2]), 2.0) + + pow(cos(q[0 * 3 + 2]), 2.0) * pow(sin(q[0 * 3 + 1]), 2.0) + + pow(sin(q[0 * 3 + 1]), 2.0) * pow(sin(q[0 * 3 + 2]), 2.0)))) / + (pow(cos(q[0 * 3 + 0]), 2.0) + pow(sin(q[0 * 3 + 0]), 2.0)); +} + +#if 0 +void f_M(Eigen::Vector3d q, int RL, Eigen::Matrix3d &F) { + F(0, 0) = + -sin(q[0 * 3 + 1]) * + (cos(q[0 * 3 + 1]) * + (cos(q[0 * 3 + 2]) * 1.463296047418464E-11 + + sin(q[0 * 3 + 2]) * 2.515259716742376E-3 + + sin(q[0 * 3 + 2]) * (cos(q[0 * 3 + 2]) * 1.736636096535468E-5 - + sin(q[0 * 3 + 2]) * 6.535566164070341E-5) + + cos(q[0 * 3 + 2]) * (cos(q[0 * 3 + 2]) * 6.535566164070341E-5 - + sin(q[0 * 3 + 2]) * 2.00164060021257E-3) + + cos(q[0 * 3 + 2] * 1.2E+1) * + (cos(q[0 * 3 + 2] * 1.2E+1) * 5.133772622539436E-10 - + sin(q[0 * 3 + 2] * 1.2E+1) / 1.0E+4 + + (RL * RL) * sin(q[0 * 3 + 2] * 1.2E+1) * + 4.695222322285431E-13) - + sin(q[0 * 3 + 2] * 1.2E+1) * + (cos(q[0 * 3 + 2] * 1.2E+1) * (-1.0E-4) + + sin(q[0 * 3 + 2] * 1.2E+1) * 5.133772622539436E-10 + + (RL * RL) * cos(q[0 * 3 + 2] * 1.2E+1) * + 4.695222322285431E-13) - + 7.355881041257671E-4) - + sin(q[0 * 3 + 1]) * + (cos(q[0 * 3 + 2]) * (-2.515259716742376E-3) + + sin(q[0 * 3 + 2]) * 1.463296047418464E-11 + + sin(q[0 * 3 + 2] * 1.2E+1) * + (cos(q[0 * 3 + 2] * 1.2E+1) * 5.133772622539436E-10 + + sin(q[0 * 3 + 2] * 1.2E+1) / 1.0E+4 - + (RL * RL) * sin(q[0 * 3 + 2] * 1.2E+1) * + 4.695222322285431E-13) + + cos(q[0 * 3 + 2]) * (cos(q[0 * 3 + 2]) * 2.00164060021257E-3 + + sin(q[0 * 3 + 2]) * 6.535566164070341E-5 - + 2.515259716742376E-3) + + sin(q[0 * 3 + 2]) * (cos(q[0 * 3 + 2]) * 6.535566164070341E-5 + + sin(q[0 * 3 + 2]) * 1.736636096535468E-5 + + 1.463296047418464E-11) + + (RL * RL) * pow(cos(q[0 * 3 + 2] * 1.2E+1), 2.0) * + 9.599999995304778E-4 + + (RL * RL) * pow(sin(q[0 * 3 + 2] * 1.2E+1), 2.0) * + 9.599999995304778E-4 + + cos(q[0 * 3 + 2] * 1.2E+1) * + (cos(q[0 * 3 + 2] * 1.2E+1) / 1.0E+4 + + sin(q[0 * 3 + 2] * 1.2E+1) * 5.133772622539436E-10 - + (RL * RL) * cos(q[0 * 3 + 2] * 1.2E+1) * + 4.695222322285431E-13) + + 1.182575965665761E-2) + + RL * sin(q[0 * 3 + 1]) * + (RL * 6.33521130433211E-3 + + RL * pow(cos(q[0 * 3 + 2] * 1.2E+1), 2.0) * + 1.199999999413097E-2 + + RL * pow(sin(q[0 * 3 + 2] * 1.2E+1), 2.0) * + 1.199999999413097E-2 + + RL * pow(cos(q[0 * 3 + 2]), 2.0) * 9.485213488252606E-10 + + RL * pow(sin(q[0 * 3 + 2]), 2.0) * 9.485213488252606E-10) * + 1.0098E-1) + + cos(q[0 * 3 + 1]) * + (cos(q[0 * 3 + 1]) * + (-sin(q[0 * 3 + 2] * 1.2E+1) * + (cos(q[0 * 3 + 2] * 1.2E+1) * 5.133772622539436E-10 - + sin(q[0 * 3 + 2] * 1.2E+1) / 1.0E+4 + + (RL * RL) * sin(q[0 * 3 + 2] * 1.2E+1) * + 4.695222322285431E-13) - + sin(q[0 * 3 + 2]) * (cos(q[0 * 3 + 2]) * 6.535566164070341E-5 - + sin(q[0 * 3 + 2]) * 2.00164060021257E-3) + + (RL * RL) * pow(cos(q[0 * 3 + 2] * 1.2E+1), 2.0) * + 9.599999995304778E-4 + + (RL * RL) * pow(sin(q[0 * 3 + 2] * 1.2E+1), 2.0) * + 9.599999995304778E-4 - + cos(q[0 * 3 + 2] * 1.2E+1) * + (cos(q[0 * 3 + 2] * 1.2E+1) * (-1.0E-4) + + sin(q[0 * 3 + 2] * 1.2E+1) * 5.133772622539436E-10 + + (RL * RL) * cos(q[0 * 3 + 2] * 1.2E+1) * + 4.695222322285431E-13) + + cos(q[0 * 3 + 2]) * (cos(q[0 * 3 + 2]) * 1.736636096535468E-5 - + sin(q[0 * 3 + 2]) * 6.535566164070341E-5) + + 4.120174052057458E-4) + + sin(q[0 * 3 + 1]) * + (-cos(q[0 * 3 + 2]) * (cos(q[0 * 3 + 2]) * 6.535566164070341E-5 + + sin(q[0 * 3 + 2]) * 1.736636096535468E-5 + + 1.463296047418464E-11) + + sin(q[0 * 3 + 2]) * (cos(q[0 * 3 + 2]) * 2.00164060021257E-3 + + sin(q[0 * 3 + 2]) * 6.535566164070341E-5 - + 2.515259716742376E-3) - + cos(q[0 * 3 + 2] * 1.2E+1) * + (cos(q[0 * 3 + 2] * 1.2E+1) * 5.133772622539436E-10 + + sin(q[0 * 3 + 2] * 1.2E+1) / 1.0E+4 - + (RL * RL) * sin(q[0 * 3 + 2] * 1.2E+1) * + 4.695222322285431E-13) + + sin(q[0 * 3 + 2] * 1.2E+1) * + (cos(q[0 * 3 + 2] * 1.2E+1) / 1.0E+4 + + sin(q[0 * 3 + 2] * 1.2E+1) * 5.133772622539436E-10 - + (RL * RL) * cos(q[0 * 3 + 2] * 1.2E+1) * + 4.695222322285431E-13) + + 7.355881041257671E-4) - + RL * cos(q[0 * 3 + 1]) * + (RL * 6.33521130433211E-3 + + RL * pow(cos(q[0 * 3 + 2] * 1.2E+1), 2.0) * + 1.199999999413097E-2 + + RL * pow(sin(q[0 * 3 + 2] * 1.2E+1), 2.0) * + 1.199999999413097E-2 + + RL * pow(cos(q[0 * 3 + 2]), 2.0) * 9.485213488252606E-10 + + RL * pow(sin(q[0 * 3 + 2]), 2.0) * 9.485213488252606E-10) * + 1.0098E-1) + + (RL * RL) * pow(cos(q[0 * 3 + 1] * 6.0), 2.0) * 6.0E-5 + + (RL * RL) * pow(sin(q[0 * 3 + 1] * 6.0), 2.0) * 6.0E-5 + + cos(q[0 * 3 + 1] * 6.0) * + (cos(q[0 * 3 + 1] * 6.0) / 1.0E+4 + + sin(q[0 * 3 + 1] * 6.0) * 7.211167181803015E-10) + + sin(q[0 * 3 + 1] * 6.0) * + (cos(q[0 * 3 + 1] * 6.0) * 7.211167181803015E-10 + + sin(q[0 * 3 + 1] * 6.0) / 1.0E+4) - + RL * cos(q[0 * 3 + 1]) * + (cos(q[0 * 3 + 1]) * + (RL * 6.33521130433211E-3 + + RL * pow(cos(q[0 * 3 + 2] * 1.2E+1), 2.0) * + 1.199999999413097E-2 + + RL * pow(sin(q[0 * 3 + 2] * 1.2E+1), 2.0) * + 1.199999999413097E-2 + + RL * pow(cos(q[0 * 3 + 2]), 2.0) * 9.485213488252606E-10 + + RL * pow(sin(q[0 * 3 + 2]), 2.0) * 9.485213488252606E-10) - + RL * cos(q[0 * 3 + 1]) * + (pow(cos(q[0 * 3 + 2]), 2.0) * 1.109999993677436E-1 + + pow(sin(q[0 * 3 + 2]), 2.0) * 1.109999993677436E-1 + + pow(cos(q[0 * 3 + 2] * 1.2E+1), 2.0) * (3.0 / 2.0E+1) + + pow(sin(q[0 * 3 + 2] * 1.2E+1), 2.0) * (3.0 / 2.0E+1) + + 2.677382786369361E-1) * + 1.0098E-1) * + 1.0098E-1 - + RL * sin(q[0 * 3 + 1]) * + (sin(q[0 * 3 + 1]) * + (RL * 6.33521130433211E-3 + + RL * pow(cos(q[0 * 3 + 2] * 1.2E+1), 2.0) * + 1.199999999413097E-2 + + RL * pow(sin(q[0 * 3 + 2] * 1.2E+1), 2.0) * + 1.199999999413097E-2 + + RL * pow(cos(q[0 * 3 + 2]), 2.0) * 9.485213488252606E-10 + + RL * pow(sin(q[0 * 3 + 2]), 2.0) * 9.485213488252606E-10) - + RL * sin(q[0 * 3 + 1]) * + (pow(cos(q[0 * 3 + 2]), 2.0) * 1.109999993677436E-1 + + pow(sin(q[0 * 3 + 2]), 2.0) * 1.109999993677436E-1 + + pow(cos(q[0 * 3 + 2] * 1.2E+1), 2.0) * (3.0 / 2.0E+1) + + pow(sin(q[0 * 3 + 2] * 1.2E+1), 2.0) * (3.0 / 2.0E+1) + + 2.677382786369361E-1) * + 1.0098E-1) * + 1.0098E-1 + + 5.084135445146919E-3; + F(0, 1) = + -sin(q[0 * 3 + 1]) * + (RL * 1.121534523796973E-5 + + RL * cos(q[0 * 3 + 2]) * 5.909849367362282E-6 - + RL * sin(q[0 * 3 + 2]) * 6.000834499971923E-5 + + RL * cos(q[0 * 3 + 2] * 1.2E+1) * 2.046586469068882E-9 - + RL * sin(q[0 * 3 + 2] * 1.2E+1) * 3.092703555787573E-9) - + cos(q[0 * 3 + 1]) * + (RL * (-5.386630108894173E-5) + + RL * cos(q[0 * 3 + 2]) * 6.000834499971923E-5 + + RL * sin(q[0 * 3 + 2]) * 5.909849367362282E-6 + + RL * cos(q[0 * 3 + 2] * 1.2E+1) * 3.092703555787573E-9 - + RL * pow(cos(q[0 * 3 + 2]), 2.0) * 1.953953978580037E-10 + + RL * sin(q[0 * 3 + 2] * 1.2E+1) * 2.046586469068882E-9 - + RL * pow(sin(q[0 * 3 + 2]), 2.0) * 1.953953978580037E-10) + + RL * cos(q[0 * 3 + 1] * 6.0) * 1.046133449327708E-9 - + RL * sin(q[0 * 3 + 1] * 6.0) * 1.37802405976267E-8 - + RL * cos(q[0 * 3 + 1]) * + (cos(q[0 * 3 + 2] * 1.2E+1) * (-7.383500983694232E-10) + + sin(q[0 * 3 + 2] * 1.2E+1) * 1.061923352497383E-9 - + cos(q[0 * 3 + 2]) * 1.220999862496299E-2 + + sin(q[0 * 3 + 2]) * 7.103378870963419E-11 + + pow(cos(q[0 * 3 + 2]), 2.0) * 2.286599986975518E-2 + + pow(sin(q[0 * 3 + 2]), 2.0) * 2.286599986975518E-2 + + 3.14785127673052E-2) * + 1.0098E-1 - + RL * sin(q[0 * 3 + 1]) * + (cos(q[0 * 3 + 2] * 1.2E+1) * 1.061923352497383E-9 + + sin(q[0 * 3 + 2] * 1.2E+1) * 7.383500983694232E-10 + + cos(q[0 * 3 + 2]) * 7.103378870963419E-11 + + sin(q[0 * 3 + 2]) * 1.220999862496299E-2 - 3.502030359531471E-3) * + 1.0098E-1; + F(0, 2) = + cos(q[0 * 3 + 1]) * + (RL * cos(q[0 * 3 + 2] * 1.2E+1) * 3.092703555787573E-9 + + RL * sin(q[0 * 3 + 2] * 1.2E+1) * 2.046586469068882E-9) * + -6.0 - + cos(q[0 * 3 + 1]) * (RL * cos(q[0 * 3 + 2]) * 6.000834499971923E-5 + + RL * sin(q[0 * 3 + 2]) * 5.909849367362282E-6) - + sin(q[0 * 3 + 1]) * + (RL * cos(q[0 * 3 + 2] * 1.2E+1) * 2.046586469068882E-9 - + RL * sin(q[0 * 3 + 2] * 1.2E+1) * 3.092703555787573E-9) * + 6.0 - + sin(q[0 * 3 + 1]) * (RL * cos(q[0 * 3 + 2]) * 5.909849367362282E-6 - + RL * sin(q[0 * 3 + 2]) * 6.000834499971923E-5) + + RL * cos(q[0 * 3 + 1]) * + (cos(q[0 * 3 + 2]) * 1.220999862496299E-2 - + sin(q[0 * 3 + 2]) * 7.103378870963419E-11) * + 1.0098E-1 - + RL * sin(q[0 * 3 + 1]) * + (cos(q[0 * 3 + 2]) * 7.103378870963419E-11 + + sin(q[0 * 3 + 2]) * 1.220999862496299E-2) * + 1.0098E-1 + + RL * cos(q[0 * 3 + 1]) * + (cos(q[0 * 3 + 2] * 1.2E+1) * 7.383500983694232E-10 - + sin(q[0 * 3 + 2] * 1.2E+1) * 1.061923352497383E-9) * + 6.0588E-1 - + RL * sin(q[0 * 3 + 1]) * + (cos(q[0 * 3 + 2] * 1.2E+1) * 1.061923352497383E-9 + + sin(q[0 * 3 + 2] * 1.2E+1) * 7.383500983694232E-10) * + 6.0588E-1; + F(1, 0) = + -sin(q[0 * 3 + 1]) * + (RL * 1.121534523796973E-5 + + RL * cos(q[0 * 3 + 2]) * 5.909849367362282E-6 - + RL * sin(q[0 * 3 + 2]) * 6.000834499971923E-5 + + RL * cos(q[0 * 3 + 2] * 1.2E+1) * 2.046586469068882E-9 - + RL * sin(q[0 * 3 + 2] * 1.2E+1) * 3.092703555787573E-9) - + cos(q[0 * 3 + 1]) * + (RL * (-5.386630108894173E-5) + + RL * cos(q[0 * 3 + 2]) * 6.000834499971923E-5 + + RL * sin(q[0 * 3 + 2]) * 5.909849367362282E-6 + + RL * cos(q[0 * 3 + 2] * 1.2E+1) * 3.092703555787573E-9 - + RL * pow(cos(q[0 * 3 + 2]), 2.0) * 1.953953978580037E-10 + + RL * sin(q[0 * 3 + 2] * 1.2E+1) * 2.046586469068882E-9 - + RL * pow(sin(q[0 * 3 + 2]), 2.0) * 1.953953978580037E-10) + + RL * cos(q[0 * 3 + 1] * 6.0) * 1.046133449327708E-9 - + RL * sin(q[0 * 3 + 1] * 6.0) * 1.37802405976267E-8 - + RL * cos(q[0 * 3 + 1]) * + (cos(q[0 * 3 + 2] * 1.2E+1) * (-7.383500983694232E-10) + + sin(q[0 * 3 + 2] * 1.2E+1) * 1.061923352497383E-9 - + cos(q[0 * 3 + 2]) * 1.220999862496299E-2 + + sin(q[0 * 3 + 2]) * 7.103378870963419E-11 + + pow(cos(q[0 * 3 + 2]), 2.0) * 2.286599986975518E-2 + + pow(sin(q[0 * 3 + 2]), 2.0) * 2.286599986975518E-2 + + 3.14785127673052E-2) * + 1.0098E-1 - + RL * sin(q[0 * 3 + 1]) * + (cos(q[0 * 3 + 2] * 1.2E+1) * 1.061923352497383E-9 + + sin(q[0 * 3 + 2] * 1.2E+1) * 7.383500983694232E-10 + + cos(q[0 * 3 + 2]) * 7.103378870963419E-11 + + sin(q[0 * 3 + 2]) * 1.220999862496299E-2 - 3.502030359531471E-3) * + 1.0098E-1; + F(1, 1) = + cos(q[0 * 3 + 2]) * (-2.515259716742376E-3) + + sin(q[0 * 3 + 2]) * 1.463296047418464E-11 + + sin(q[0 * 3 + 2]) * + (sin(q[0 * 3 + 2]) * 2.286599986975518E-2 + 7.103378870963419E-11) * + (1.03E+2 / 5.0E+2) + + cos(q[0 * 3 + 2]) * + (cos(q[0 * 3 + 2]) * 2.286599986975518E-2 - 1.220999862496299E-2) * + (1.03E+2 / 5.0E+2) + + 1.349729066626227E-2; + F(1, 2) = cos(q[0 * 3 + 2]) * (-2.515259716742376E-3) + + sin(q[0 * 3 + 2]) * 1.463296047418464E-11 + 2.750678488557605E-3; + F(2, 0) = + cos(q[0 * 3 + 1]) * + (RL * cos(q[0 * 3 + 2] * 1.2E+1) * 3.092703555787573E-9 + + RL * sin(q[0 * 3 + 2] * 1.2E+1) * 2.046586469068882E-9) * + -1.2E+1 - + cos(q[0 * 3 + 1]) * (RL * cos(q[0 * 3 + 2]) * 6.000834499971923E-5 + + RL * sin(q[0 * 3 + 2]) * 5.909849367362282E-6) - + sin(q[0 * 3 + 1]) * + (RL * cos(q[0 * 3 + 2] * 1.2E+1) * 2.046586469068882E-9 - + RL * sin(q[0 * 3 + 2] * 1.2E+1) * 3.092703555787573E-9) * + 1.2E+1 - + sin(q[0 * 3 + 1]) * (RL * cos(q[0 * 3 + 2]) * 5.909849367362282E-6 - + RL * sin(q[0 * 3 + 2]) * 6.000834499971923E-5) + + RL * cos(q[0 * 3 + 1]) * + (cos(q[0 * 3 + 2]) * 1.220999862496299E-2 - + sin(q[0 * 3 + 2]) * 7.103378870963419E-11) * + 1.0098E-1 - + RL * sin(q[0 * 3 + 1]) * + (cos(q[0 * 3 + 2]) * 7.103378870963419E-11 + + sin(q[0 * 3 + 2]) * 1.220999862496299E-2) * + 1.0098E-1 + + RL * cos(q[0 * 3 + 1]) * + (cos(q[0 * 3 + 2] * 1.2E+1) * 7.383500983694232E-10 - + sin(q[0 * 3 + 2] * 1.2E+1) * 1.061923352497383E-9) * + 1.21176 - + RL * sin(q[0 * 3 + 1]) * + (cos(q[0 * 3 + 2] * 1.2E+1) * 1.061923352497383E-9 + + sin(q[0 * 3 + 2] * 1.2E+1) * 7.383500983694232E-10) * + 1.21176; + F(2, 1) = cos(q[0 * 3 + 2]) * (-2.515259716742376E-3) + + sin(q[0 * 3 + 2]) * 1.463296047418464E-11 + 3.498204812978887E-3; + F(2, 2) = 1.994378395024709E-2; +} + +void f_beta(Eigen::Vector3d q, Eigen::Vector3d qd, int RL, Eigen::Vector3d &F) { + F[0 * 3 + 0] = + sin(q[0 * 3 + 0] * 6.0) * (-3.710406914629634E-8) + + sin(q[0 * 3 + 0]) * 4.885285059173943E-2 - + cos(q[0 * 3 + 1]) * sin(q[0 * 3 + 0]) * 3.435491782700373E-2 - + sin(q[0 * 3 + 0]) * sin(q[0 * 3 + 1]) * 5.331196689695623E-1 + + qd[0 * 3 + 1] * + (qd[0 * 3 + 0] * + ((cos(q[0 * 3 + 1] * 6.0) * + (cos(q[0 * 3 + 1] * 6.0) * 7.211167181803015E-10 - + sin(q[0 * 3 + 1] * 6.0) / 1.0E+4)) / + 2.0 + + (cos(q[0 * 3 + 1] * 6.0) * + (cos(q[0 * 3 + 1] * 6.0) * 7.211167181803015E-10 + + sin(q[0 * 3 + 1] * 6.0) / 1.0E+4)) / + 2.0 + + (sin(q[0 * 3 + 1] * 6.0) * + (cos(q[0 * 3 + 1] * 6.0) / 1.0E+4 - + sin(q[0 * 3 + 1] * 6.0) * 7.211167181803015E-10)) / + 2.0 - + (sin(q[0 * 3 + 1] * 6.0) * + (cos(q[0 * 3 + 1] * 6.0) / 1.0E+4 + + sin(q[0 * 3 + 1] * 6.0) * 7.211167181803015E-10)) / + 2.0) * + 6.0 + + qd[0 * 3 + 0] * + ((sin(q[0 * 3 + 1]) * + (sin(q[0 * 3 + 1]) * + (cos(q[0 * 3 + 2]) * 1.463296047418464E-11 + + sin(q[0 * 3 + 2]) * 2.515259716742376E-3 + + sin(q[0 * 3 + 2]) * + (cos(q[0 * 3 + 2]) * 1.736636096535468E-5 - + sin(q[0 * 3 + 2]) * 6.535566164070341E-5) + + cos(q[0 * 3 + 2]) * + (cos(q[0 * 3 + 2]) * 6.535566164070341E-5 - + sin(q[0 * 3 + 2]) * 2.00164060021257E-3) + + cos(q[0 * 3 + 2] * 1.2E+1) * + (cos(q[0 * 3 + 2] * 1.2E+1) * 5.133772622539436E-10 - + sin(q[0 * 3 + 2] * 1.2E+1) / 1.0E+4 + + (RL * RL) * sin(q[0 * 3 + 2] * 1.2E+1) * + 4.695222322285431E-13) - + sin(q[0 * 3 + 2] * 1.2E+1) * + (cos(q[0 * 3 + 2] * 1.2E+1) * (-1.0E-4) + + sin(q[0 * 3 + 2] * 1.2E+1) * 5.133772622539436E-10 + + (RL * RL) * cos(q[0 * 3 + 2] * 1.2E+1) * + 4.695222322285431E-13) - + 7.355881041257671E-4) + + cos(q[0 * 3 + 1]) * + (cos(q[0 * 3 + 2]) * (-2.515259716742376E-3) + + sin(q[0 * 3 + 2]) * 1.463296047418464E-11 + + sin(q[0 * 3 + 2] * 1.2E+1) * + (cos(q[0 * 3 + 2] * 1.2E+1) * 5.133772622539436E-10 + + sin(q[0 * 3 + 2] * 1.2E+1) / 1.0E+4 - + (RL * RL) * sin(q[0 * 3 + 2] * 1.2E+1) * + 4.695222322285431E-13) + + cos(q[0 * 3 + 2]) * + (cos(q[0 * 3 + 2]) * 2.00164060021257E-3 + + sin(q[0 * 3 + 2]) * 6.535566164070341E-5 - + 2.515259716742376E-3) + + sin(q[0 * 3 + 2]) * + (cos(q[0 * 3 + 2]) * 6.535566164070341E-5 + + sin(q[0 * 3 + 2]) * 1.736636096535468E-5 + + 1.463296047418464E-11) + + (RL * RL) * pow(cos(q[0 * 3 + 2] * 1.2E+1), 2.0) * + 9.599999995304778E-4 + + (RL * RL) * pow(sin(q[0 * 3 + 2] * 1.2E+1), 2.0) * + 9.599999995304778E-4 + + cos(q[0 * 3 + 2] * 1.2E+1) * + (cos(q[0 * 3 + 2] * 1.2E+1) / 1.0E+4 + + sin(q[0 * 3 + 2] * 1.2E+1) * 5.133772622539436E-10 - + (RL * RL) * cos(q[0 * 3 + 2] * 1.2E+1) * + 4.695222322285431E-13) + + 1.182575965665761E-2) - + RL * cos(q[0 * 3 + 1]) * + (RL * 6.33521130433211E-3 + + RL * pow(cos(q[0 * 3 + 2] * 1.2E+1), 2.0) * + 1.199999999413097E-2 + + RL * pow(sin(q[0 * 3 + 2] * 1.2E+1), 2.0) * + 1.199999999413097E-2 + + RL * pow(cos(q[0 * 3 + 2]), 2.0) * + 9.485213488252606E-10 + + RL * pow(sin(q[0 * 3 + 2]), 2.0) * + 9.485213488252606E-10) * + 1.0098E-1)) / + 2.0 - + (cos(q[0 * 3 + 1]) * + (cos(q[0 * 3 + 1]) * + (cos(q[0 * 3 + 2]) * 1.463296047418464E-11 + + sin(q[0 * 3 + 2]) * 2.515259716742376E-3 + + sin(q[0 * 3 + 2]) * + (cos(q[0 * 3 + 2]) * 1.736636096535468E-5 - + sin(q[0 * 3 + 2]) * 6.535566164070341E-5) + + cos(q[0 * 3 + 2]) * + (cos(q[0 * 3 + 2]) * 6.535566164070341E-5 - + sin(q[0 * 3 + 2]) * 2.00164060021257E-3) + + cos(q[0 * 3 + 2] * 1.2E+1) * + (cos(q[0 * 3 + 2] * 1.2E+1) * 5.133772622539436E-10 - + sin(q[0 * 3 + 2] * 1.2E+1) / 1.0E+4 + + (RL * RL) * sin(q[0 * 3 + 2] * 1.2E+1) * + 4.695222322285431E-13) - + sin(q[0 * 3 + 2] * 1.2E+1) * + (cos(q[0 * 3 + 2] * 1.2E+1) * (-1.0E-4) + + sin(q[0 * 3 + 2] * 1.2E+1) * 5.133772622539436E-10 + + (RL * RL) * cos(q[0 * 3 + 2] * 1.2E+1) * + 4.695222322285431E-13) - + 7.355881041257671E-4) - + sin(q[0 * 3 + 1]) * + (cos(q[0 * 3 + 2]) * (-2.515259716742376E-3) + + sin(q[0 * 3 + 2]) * 1.463296047418464E-11 + + sin(q[0 * 3 + 2] * 1.2E+1) * + (cos(q[0 * 3 + 2] * 1.2E+1) * 5.133772622539436E-10 + + sin(q[0 * 3 + 2] * 1.2E+1) / 1.0E+4 - + (RL * RL) * sin(q[0 * 3 + 2] * 1.2E+1) * + 4.695222322285431E-13) + + cos(q[0 * 3 + 2]) * + (cos(q[0 * 3 + 2]) * 2.00164060021257E-3 + + sin(q[0 * 3 + 2]) * 6.535566164070341E-5 - + 2.515259716742376E-3) + + sin(q[0 * 3 + 2]) * + (cos(q[0 * 3 + 2]) * 6.535566164070341E-5 + + sin(q[0 * 3 + 2]) * 1.736636096535468E-5 + + 1.463296047418464E-11) + + (RL * RL) * pow(cos(q[0 * 3 + 2] * 1.2E+1), 2.0) * + 9.599999995304778E-4 + + (RL * RL) * pow(sin(q[0 * 3 + 2] * 1.2E+1), 2.0) * + 9.599999995304778E-4 + + cos(q[0 * 3 + 2] * 1.2E+1) * + (cos(q[0 * 3 + 2] * 1.2E+1) / 1.0E+4 + + sin(q[0 * 3 + 2] * 1.2E+1) * 5.133772622539436E-10 - + (RL * RL) * cos(q[0 * 3 + 2] * 1.2E+1) * + 4.695222322285431E-13) + + 1.182575965665761E-2) + + RL * sin(q[0 * 3 + 1]) * + (RL * 6.33521130433211E-3 + + RL * pow(cos(q[0 * 3 + 2] * 1.2E+1), 2.0) * + 1.199999999413097E-2 + + RL * pow(sin(q[0 * 3 + 2] * 1.2E+1), 2.0) * + 1.199999999413097E-2 + + RL * pow(cos(q[0 * 3 + 2]), 2.0) * + 9.485213488252606E-10 + + RL * pow(sin(q[0 * 3 + 2]), 2.0) * + 9.485213488252606E-10) * + 1.0098E-1)) / + 2.0 - + (sin(q[0 * 3 + 1]) * + (cos(q[0 * 3 + 1]) * + (-sin(q[0 * 3 + 2] * 1.2E+1) * + (cos(q[0 * 3 + 2] * 1.2E+1) * 5.133772622539436E-10 - + sin(q[0 * 3 + 2] * 1.2E+1) / 1.0E+4 + + (RL * RL) * sin(q[0 * 3 + 2] * 1.2E+1) * + 4.695222322285431E-13) - + sin(q[0 * 3 + 2]) * + (cos(q[0 * 3 + 2]) * 6.535566164070341E-5 - + sin(q[0 * 3 + 2]) * 2.00164060021257E-3) + + (RL * RL) * pow(cos(q[0 * 3 + 2] * 1.2E+1), 2.0) * + 9.599999995304778E-4 + + (RL * RL) * pow(sin(q[0 * 3 + 2] * 1.2E+1), 2.0) * + 9.599999995304778E-4 - + cos(q[0 * 3 + 2] * 1.2E+1) * + (cos(q[0 * 3 + 2] * 1.2E+1) * (-1.0E-4) + + sin(q[0 * 3 + 2] * 1.2E+1) * 5.133772622539436E-10 + + (RL * RL) * cos(q[0 * 3 + 2] * 1.2E+1) * + 4.695222322285431E-13) + + cos(q[0 * 3 + 2]) * + (cos(q[0 * 3 + 2]) * 1.736636096535468E-5 - + sin(q[0 * 3 + 2]) * 6.535566164070341E-5) + + 4.120174052057458E-4) + + sin(q[0 * 3 + 1]) * + (-cos(q[0 * 3 + 2]) * + (cos(q[0 * 3 + 2]) * 6.535566164070341E-5 + + sin(q[0 * 3 + 2]) * 1.736636096535468E-5 + + 1.463296047418464E-11) + + sin(q[0 * 3 + 2]) * + (cos(q[0 * 3 + 2]) * 2.00164060021257E-3 + + sin(q[0 * 3 + 2]) * 6.535566164070341E-5 - + 2.515259716742376E-3) - + cos(q[0 * 3 + 2] * 1.2E+1) * + (cos(q[0 * 3 + 2] * 1.2E+1) * 5.133772622539436E-10 + + sin(q[0 * 3 + 2] * 1.2E+1) / 1.0E+4 - + (RL * RL) * sin(q[0 * 3 + 2] * 1.2E+1) * + 4.695222322285431E-13) + + sin(q[0 * 3 + 2] * 1.2E+1) * + (cos(q[0 * 3 + 2] * 1.2E+1) / 1.0E+4 + + sin(q[0 * 3 + 2] * 1.2E+1) * 5.133772622539436E-10 - + (RL * RL) * cos(q[0 * 3 + 2] * 1.2E+1) * + 4.695222322285431E-13) + + 7.355881041257671E-4) - + RL * cos(q[0 * 3 + 1]) * + (RL * 6.33521130433211E-3 + + RL * pow(cos(q[0 * 3 + 2] * 1.2E+1), 2.0) * + 1.199999999413097E-2 + + RL * pow(sin(q[0 * 3 + 2] * 1.2E+1), 2.0) * + 1.199999999413097E-2 + + RL * pow(cos(q[0 * 3 + 2]), 2.0) * + 9.485213488252606E-10 + + RL * pow(sin(q[0 * 3 + 2]), 2.0) * + 9.485213488252606E-10) * + 1.0098E-1)) / + 2.0 + + (cos(q[0 * 3 + 1]) * + (-sin(q[0 * 3 + 1]) * + (-sin(q[0 * 3 + 2] * 1.2E+1) * + (cos(q[0 * 3 + 2] * 1.2E+1) * 5.133772622539436E-10 - + sin(q[0 * 3 + 2] * 1.2E+1) / 1.0E+4 + + (RL * RL) * sin(q[0 * 3 + 2] * 1.2E+1) * + 4.695222322285431E-13) - + sin(q[0 * 3 + 2]) * + (cos(q[0 * 3 + 2]) * 6.535566164070341E-5 - + sin(q[0 * 3 + 2]) * 2.00164060021257E-3) + + (RL * RL) * pow(cos(q[0 * 3 + 2] * 1.2E+1), 2.0) * + 9.599999995304778E-4 + + (RL * RL) * pow(sin(q[0 * 3 + 2] * 1.2E+1), 2.0) * + 9.599999995304778E-4 - + cos(q[0 * 3 + 2] * 1.2E+1) * + (cos(q[0 * 3 + 2] * 1.2E+1) * (-1.0E-4) + + sin(q[0 * 3 + 2] * 1.2E+1) * 5.133772622539436E-10 + + (RL * RL) * cos(q[0 * 3 + 2] * 1.2E+1) * + 4.695222322285431E-13) + + cos(q[0 * 3 + 2]) * + (cos(q[0 * 3 + 2]) * 1.736636096535468E-5 - + sin(q[0 * 3 + 2]) * 6.535566164070341E-5) + + 4.120174052057458E-4) + + cos(q[0 * 3 + 1]) * + (-cos(q[0 * 3 + 2]) * + (cos(q[0 * 3 + 2]) * 6.535566164070341E-5 + + sin(q[0 * 3 + 2]) * 1.736636096535468E-5 + + 1.463296047418464E-11) + + sin(q[0 * 3 + 2]) * + (cos(q[0 * 3 + 2]) * 2.00164060021257E-3 + + sin(q[0 * 3 + 2]) * 6.535566164070341E-5 - + 2.515259716742376E-3) - + cos(q[0 * 3 + 2] * 1.2E+1) * + (cos(q[0 * 3 + 2] * 1.2E+1) * 5.133772622539436E-10 + + sin(q[0 * 3 + 2] * 1.2E+1) / 1.0E+4 - + (RL * RL) * sin(q[0 * 3 + 2] * 1.2E+1) * + 4.695222322285431E-13) + + sin(q[0 * 3 + 2] * 1.2E+1) * + (cos(q[0 * 3 + 2] * 1.2E+1) / 1.0E+4 + + sin(q[0 * 3 + 2] * 1.2E+1) * 5.133772622539436E-10 - + (RL * RL) * cos(q[0 * 3 + 2] * 1.2E+1) * + 4.695222322285431E-13) + + 7.355881041257671E-4) + + RL * sin(q[0 * 3 + 1]) * + (RL * 6.33521130433211E-3 + + RL * pow(cos(q[0 * 3 + 2] * 1.2E+1), 2.0) * + 1.199999999413097E-2 + + RL * pow(sin(q[0 * 3 + 2] * 1.2E+1), 2.0) * + 1.199999999413097E-2 + + RL * pow(cos(q[0 * 3 + 2]), 2.0) * + 9.485213488252606E-10 + + RL * pow(sin(q[0 * 3 + 2]), 2.0) * + 9.485213488252606E-10) * + 1.0098E-1)) / + 2.0)) + + cos(q[0 * 3 + 1] * 6.0) * sin(q[0 * 3 + 0]) * 3.555887503428222E-9 - + RL * cos(q[0 * 3 + 0]) * 6.906818617516898E-1 - + sin(q[0 * 3 + 1] * 6.0) * sin(q[0 * 3 + 0]) * 2.426349129295663E-9 + + qd[0 * 3 + 2] * + (qd[0 * 3 + 0] * + ((sin(q[0 * 3 + 1]) * + (sin(q[0 * 3 + 1]) * + (cos(q[0 * 3 + 2]) * 1.463296047418464E-11 + + sin(q[0 * 3 + 2]) * 2.515259716742376E-3 + + sin(q[0 * 3 + 2]) * + (cos(q[0 * 3 + 2]) * 1.736636096535468E-5 - + sin(q[0 * 3 + 2]) * 6.535566164070341E-5) + + cos(q[0 * 3 + 2]) * + (cos(q[0 * 3 + 2]) * 6.535566164070341E-5 - + sin(q[0 * 3 + 2]) * 2.00164060021257E-3) + + cos(q[0 * 3 + 2]) * + (cos(q[0 * 3 + 2]) * 6.535566164070341E-5 + + sin(q[0 * 3 + 2]) * 1.736636096535468E-5 + + 1.463296047418464E-11) - + sin(q[0 * 3 + 2]) * + (cos(q[0 * 3 + 2]) * 2.00164060021257E-3 + + sin(q[0 * 3 + 2]) * 6.535566164070341E-5 - + 2.515259716742376E-3)) + + cos(q[0 * 3 + 1]) * + (cos(q[0 * 3 + 2]) * (-2.515259716742376E-3) + + sin(q[0 * 3 + 2]) * 1.463296047418464E-11 + + sin(q[0 * 3 + 2]) * + (cos(q[0 * 3 + 2]) * 6.535566164070341E-5 + + sin(q[0 * 3 + 2]) * 1.736636096535468E-5) + + cos(q[0 * 3 + 2]) * + (cos(q[0 * 3 + 2]) * 2.00164060021257E-3 + + sin(q[0 * 3 + 2]) * 6.535566164070341E-5) + + sin(q[0 * 3 + 2]) * + (cos(q[0 * 3 + 2]) * 6.535566164070341E-5 - + sin(q[0 * 3 + 2]) * 2.00164060021257E-3) - + cos(q[0 * 3 + 2]) * + (cos(q[0 * 3 + 2]) * 1.736636096535468E-5 - + sin(q[0 * 3 + 2]) * 6.535566164070341E-5)))) / + 2.0 + + (cos(q[0 * 3 + 1]) * + (sin(q[0 * 3 + 1]) * + (sin(q[0 * 3 + 2]) * + (cos(q[0 * 3 + 2]) * 6.535566164070341E-5 - + sin(q[0 * 3 + 2]) * 2.00164060021257E-3) + + cos(q[0 * 3 + 2]) * + (cos(q[0 * 3 + 2]) * 2.00164060021257E-3 + + sin(q[0 * 3 + 2]) * 6.535566164070341E-5 - + 2.515259716742376E-3) + + sin(q[0 * 3 + 2]) * + (cos(q[0 * 3 + 2]) * 6.535566164070341E-5 + + sin(q[0 * 3 + 2]) * 1.736636096535468E-5 + + 1.463296047418464E-11) - + cos(q[0 * 3 + 2]) * + (cos(q[0 * 3 + 2]) * 1.736636096535468E-5 - + sin(q[0 * 3 + 2]) * 6.535566164070341E-5)) - + cos(q[0 * 3 + 1]) * + (sin(q[0 * 3 + 2]) * + (cos(q[0 * 3 + 2]) * 1.736636096535468E-5 - + sin(q[0 * 3 + 2]) * 6.535566164070341E-5) + + cos(q[0 * 3 + 2]) * + (cos(q[0 * 3 + 2]) * 6.535566164070341E-5 - + sin(q[0 * 3 + 2]) * 2.00164060021257E-3) - + sin(q[0 * 3 + 2]) * + (cos(q[0 * 3 + 2]) * 2.00164060021257E-3 + + sin(q[0 * 3 + 2]) * 6.535566164070341E-5) + + cos(q[0 * 3 + 2]) * + (cos(q[0 * 3 + 2]) * 6.535566164070341E-5 + + sin(q[0 * 3 + 2]) * 1.736636096535468E-5)))) / + 2.0) - + qd[0 * 3 + 0] * + ((cos(q[0 * 3 + 1]) * + (cos(q[0 * 3 + 1]) * + (cos(q[0 * 3 + 2] * 1.2E+1) * + (cos(q[0 * 3 + 2] * 1.2E+1) * 5.133772622539436E-10 - + sin(q[0 * 3 + 2] * 1.2E+1) / 1.0E+4 + + (RL * RL) * sin(q[0 * 3 + 2] * 1.2E+1) * + 4.695222322285431E-13) + + cos(q[0 * 3 + 2] * 1.2E+1) * + (cos(q[0 * 3 + 2] * 1.2E+1) * 5.133772622539436E-10 + + sin(q[0 * 3 + 2] * 1.2E+1) / 1.0E+4 - + (RL * RL) * sin(q[0 * 3 + 2] * 1.2E+1) * + 4.695222322285431E-13) - + sin(q[0 * 3 + 2] * 1.2E+1) * + (cos(q[0 * 3 + 2] * 1.2E+1) * (-1.0E-4) + + sin(q[0 * 3 + 2] * 1.2E+1) * 5.133772622539436E-10 + + (RL * RL) * cos(q[0 * 3 + 2] * 1.2E+1) * + 4.695222322285431E-13) - + sin(q[0 * 3 + 2] * 1.2E+1) * + (cos(q[0 * 3 + 2] * 1.2E+1) / 1.0E+4 + + sin(q[0 * 3 + 2] * 1.2E+1) * 5.133772622539436E-10 - + (RL * RL) * cos(q[0 * 3 + 2] * 1.2E+1) * + 4.695222322285431E-13)) - + sin(q[0 * 3 + 1]) * + (sin(q[0 * 3 + 2] * 1.2E+1) * + (cos(q[0 * 3 + 2] * 1.2E+1) * 5.133772622539436E-10 - + sin(q[0 * 3 + 2] * 1.2E+1) / 1.0E+4 + + (RL * RL) * sin(q[0 * 3 + 2] * 1.2E+1) * + 4.695222322285431E-13) + + sin(q[0 * 3 + 2] * 1.2E+1) * + (cos(q[0 * 3 + 2] * 1.2E+1) * 5.133772622539436E-10 + + sin(q[0 * 3 + 2] * 1.2E+1) / 1.0E+4 - + (RL * RL) * sin(q[0 * 3 + 2] * 1.2E+1) * + 4.695222322285431E-13) + + cos(q[0 * 3 + 2] * 1.2E+1) * + (cos(q[0 * 3 + 2] * 1.2E+1) * (-1.0E-4) + + sin(q[0 * 3 + 2] * 1.2E+1) * 5.133772622539436E-10 + + (RL * RL) * cos(q[0 * 3 + 2] * 1.2E+1) * + 4.695222322285431E-13) + + cos(q[0 * 3 + 2] * 1.2E+1) * + (cos(q[0 * 3 + 2] * 1.2E+1) / 1.0E+4 + + sin(q[0 * 3 + 2] * 1.2E+1) * 5.133772622539436E-10 - + (RL * RL) * cos(q[0 * 3 + 2] * 1.2E+1) * + 4.695222322285431E-13)))) / + 2.0 - + (sin(q[0 * 3 + 1]) * + (cos(q[0 * 3 + 1]) * + (sin(q[0 * 3 + 2] * 1.2E+1) * + (cos(q[0 * 3 + 2] * 1.2E+1) * 5.133772622539436E-10 - + sin(q[0 * 3 + 2] * 1.2E+1) / 1.0E+4 + + (RL * RL) * sin(q[0 * 3 + 2] * 1.2E+1) * + 4.695222322285431E-13) + + sin(q[0 * 3 + 2] * 1.2E+1) * + (cos(q[0 * 3 + 2] * 1.2E+1) * 5.133772622539436E-10 + + sin(q[0 * 3 + 2] * 1.2E+1) / 1.0E+4 - + (RL * RL) * sin(q[0 * 3 + 2] * 1.2E+1) * + 4.695222322285431E-13) + + cos(q[0 * 3 + 2] * 1.2E+1) * + (cos(q[0 * 3 + 2] * 1.2E+1) * (-1.0E-4) + + sin(q[0 * 3 + 2] * 1.2E+1) * 5.133772622539436E-10 + + (RL * RL) * cos(q[0 * 3 + 2] * 1.2E+1) * + 4.695222322285431E-13) + + cos(q[0 * 3 + 2] * 1.2E+1) * + (cos(q[0 * 3 + 2] * 1.2E+1) / 1.0E+4 + + sin(q[0 * 3 + 2] * 1.2E+1) * 5.133772622539436E-10 - + (RL * RL) * cos(q[0 * 3 + 2] * 1.2E+1) * + 4.695222322285431E-13)) + + sin(q[0 * 3 + 1]) * + (cos(q[0 * 3 + 2] * 1.2E+1) * + (cos(q[0 * 3 + 2] * 1.2E+1) * 5.133772622539436E-10 - + sin(q[0 * 3 + 2] * 1.2E+1) / 1.0E+4 + + (RL * RL) * sin(q[0 * 3 + 2] * 1.2E+1) * + 4.695222322285431E-13) + + cos(q[0 * 3 + 2] * 1.2E+1) * + (cos(q[0 * 3 + 2] * 1.2E+1) * 5.133772622539436E-10 + + sin(q[0 * 3 + 2] * 1.2E+1) / 1.0E+4 - + (RL * RL) * sin(q[0 * 3 + 2] * 1.2E+1) * + 4.695222322285431E-13) - + sin(q[0 * 3 + 2] * 1.2E+1) * + (cos(q[0 * 3 + 2] * 1.2E+1) * (-1.0E-4) + + sin(q[0 * 3 + 2] * 1.2E+1) * 5.133772622539436E-10 + + (RL * RL) * cos(q[0 * 3 + 2] * 1.2E+1) * + 4.695222322285431E-13) - + sin(q[0 * 3 + 2] * 1.2E+1) * + (cos(q[0 * 3 + 2] * 1.2E+1) / 1.0E+4 + + sin(q[0 * 3 + 2] * 1.2E+1) * 5.133772622539436E-10 - + (RL * RL) * cos(q[0 * 3 + 2] * 1.2E+1) * + 4.695222322285431E-13)))) / + 2.0) * + 1.2E+1) - + RL * cos(q[0 * 3 + 0] * 6.0) * 2.202837492322188E-8 - + qd[0 * 3 + 0] * + (qd[0 * 3 + 2] * + ((sin(q[0 * 3 + 1]) * + (sin(q[0 * 3 + 1]) * + (cos(q[0 * 3 + 2]) * 1.463296047418464E-11 + + sin(q[0 * 3 + 2]) * 2.515259716742376E-3 + + sin(q[0 * 3 + 2]) * + (cos(q[0 * 3 + 2]) * 1.736636096535468E-5 - + sin(q[0 * 3 + 2]) * 6.535566164070341E-5) + + cos(q[0 * 3 + 2]) * + (cos(q[0 * 3 + 2]) * 6.535566164070341E-5 - + sin(q[0 * 3 + 2]) * 2.00164060021257E-3) + + cos(q[0 * 3 + 2]) * + (cos(q[0 * 3 + 2]) * 6.535566164070341E-5 + + sin(q[0 * 3 + 2]) * 1.736636096535468E-5 + + 1.463296047418464E-11) - + sin(q[0 * 3 + 2]) * + (cos(q[0 * 3 + 2]) * 2.00164060021257E-3 + + sin(q[0 * 3 + 2]) * 6.535566164070341E-5 - + 2.515259716742376E-3)) + + cos(q[0 * 3 + 1]) * + (cos(q[0 * 3 + 2]) * (-2.515259716742376E-3) + + sin(q[0 * 3 + 2]) * 1.463296047418464E-11 + + sin(q[0 * 3 + 2]) * + (cos(q[0 * 3 + 2]) * 6.535566164070341E-5 + + sin(q[0 * 3 + 2]) * 1.736636096535468E-5) + + cos(q[0 * 3 + 2]) * + (cos(q[0 * 3 + 2]) * 2.00164060021257E-3 + + sin(q[0 * 3 + 2]) * 6.535566164070341E-5) + + sin(q[0 * 3 + 2]) * + (cos(q[0 * 3 + 2]) * 6.535566164070341E-5 - + sin(q[0 * 3 + 2]) * 2.00164060021257E-3) - + cos(q[0 * 3 + 2]) * + (cos(q[0 * 3 + 2]) * 1.736636096535468E-5 - + sin(q[0 * 3 + 2]) * 6.535566164070341E-5)))) / + 2.0 + + (cos(q[0 * 3 + 1]) * + (sin(q[0 * 3 + 1]) * + (sin(q[0 * 3 + 2]) * + (cos(q[0 * 3 + 2]) * 6.535566164070341E-5 - + sin(q[0 * 3 + 2]) * 2.00164060021257E-3) + + cos(q[0 * 3 + 2]) * + (cos(q[0 * 3 + 2]) * 2.00164060021257E-3 + + sin(q[0 * 3 + 2]) * 6.535566164070341E-5 - + 2.515259716742376E-3) + + sin(q[0 * 3 + 2]) * + (cos(q[0 * 3 + 2]) * 6.535566164070341E-5 + + sin(q[0 * 3 + 2]) * 1.736636096535468E-5 + + 1.463296047418464E-11) - + cos(q[0 * 3 + 2]) * + (cos(q[0 * 3 + 2]) * 1.736636096535468E-5 - + sin(q[0 * 3 + 2]) * 6.535566164070341E-5)) - + cos(q[0 * 3 + 1]) * + (sin(q[0 * 3 + 2]) * + (cos(q[0 * 3 + 2]) * 1.736636096535468E-5 - + sin(q[0 * 3 + 2]) * 6.535566164070341E-5) + + cos(q[0 * 3 + 2]) * + (cos(q[0 * 3 + 2]) * 6.535566164070341E-5 - + sin(q[0 * 3 + 2]) * 2.00164060021257E-3) - + sin(q[0 * 3 + 2]) * + (cos(q[0 * 3 + 2]) * 2.00164060021257E-3 + + sin(q[0 * 3 + 2]) * 6.535566164070341E-5) + + cos(q[0 * 3 + 2]) * + (cos(q[0 * 3 + 2]) * 6.535566164070341E-5 + + sin(q[0 * 3 + 2]) * 1.736636096535468E-5)))) / + 2.0) - + qd[0 * 3 + 2] * + ((cos(q[0 * 3 + 1]) * + (cos(q[0 * 3 + 1]) * + (cos(q[0 * 3 + 2] * 1.2E+1) * + (cos(q[0 * 3 + 2] * 1.2E+1) * 5.133772622539436E-10 - + sin(q[0 * 3 + 2] * 1.2E+1) / 1.0E+4 + + (RL * RL) * sin(q[0 * 3 + 2] * 1.2E+1) * + 4.695222322285431E-13) + + cos(q[0 * 3 + 2] * 1.2E+1) * + (cos(q[0 * 3 + 2] * 1.2E+1) * 5.133772622539436E-10 + + sin(q[0 * 3 + 2] * 1.2E+1) / 1.0E+4 - + (RL * RL) * sin(q[0 * 3 + 2] * 1.2E+1) * + 4.695222322285431E-13) - + sin(q[0 * 3 + 2] * 1.2E+1) * + (cos(q[0 * 3 + 2] * 1.2E+1) * (-1.0E-4) + + sin(q[0 * 3 + 2] * 1.2E+1) * 5.133772622539436E-10 + + (RL * RL) * cos(q[0 * 3 + 2] * 1.2E+1) * + 4.695222322285431E-13) - + sin(q[0 * 3 + 2] * 1.2E+1) * + (cos(q[0 * 3 + 2] * 1.2E+1) / 1.0E+4 + + sin(q[0 * 3 + 2] * 1.2E+1) * 5.133772622539436E-10 - + (RL * RL) * cos(q[0 * 3 + 2] * 1.2E+1) * + 4.695222322285431E-13)) - + sin(q[0 * 3 + 1]) * + (sin(q[0 * 3 + 2] * 1.2E+1) * + (cos(q[0 * 3 + 2] * 1.2E+1) * 5.133772622539436E-10 - + sin(q[0 * 3 + 2] * 1.2E+1) / 1.0E+4 + + (RL * RL) * sin(q[0 * 3 + 2] * 1.2E+1) * + 4.695222322285431E-13) + + sin(q[0 * 3 + 2] * 1.2E+1) * + (cos(q[0 * 3 + 2] * 1.2E+1) * 5.133772622539436E-10 + + sin(q[0 * 3 + 2] * 1.2E+1) / 1.0E+4 - + (RL * RL) * sin(q[0 * 3 + 2] * 1.2E+1) * + 4.695222322285431E-13) + + cos(q[0 * 3 + 2] * 1.2E+1) * + (cos(q[0 * 3 + 2] * 1.2E+1) * (-1.0E-4) + + sin(q[0 * 3 + 2] * 1.2E+1) * 5.133772622539436E-10 + + (RL * RL) * cos(q[0 * 3 + 2] * 1.2E+1) * + 4.695222322285431E-13) + + cos(q[0 * 3 + 2] * 1.2E+1) * + (cos(q[0 * 3 + 2] * 1.2E+1) / 1.0E+4 + + sin(q[0 * 3 + 2] * 1.2E+1) * 5.133772622539436E-10 - + (RL * RL) * cos(q[0 * 3 + 2] * 1.2E+1) * + 4.695222322285431E-13)))) / + 2.0 - + (sin(q[0 * 3 + 1]) * + (cos(q[0 * 3 + 1]) * + (sin(q[0 * 3 + 2] * 1.2E+1) * + (cos(q[0 * 3 + 2] * 1.2E+1) * 5.133772622539436E-10 - + sin(q[0 * 3 + 2] * 1.2E+1) / 1.0E+4 + + (RL * RL) * sin(q[0 * 3 + 2] * 1.2E+1) * + 4.695222322285431E-13) + + sin(q[0 * 3 + 2] * 1.2E+1) * + (cos(q[0 * 3 + 2] * 1.2E+1) * 5.133772622539436E-10 + + sin(q[0 * 3 + 2] * 1.2E+1) / 1.0E+4 - + (RL * RL) * sin(q[0 * 3 + 2] * 1.2E+1) * + 4.695222322285431E-13) + + cos(q[0 * 3 + 2] * 1.2E+1) * + (cos(q[0 * 3 + 2] * 1.2E+1) * (-1.0E-4) + + sin(q[0 * 3 + 2] * 1.2E+1) * 5.133772622539436E-10 + + (RL * RL) * cos(q[0 * 3 + 2] * 1.2E+1) * + 4.695222322285431E-13) + + cos(q[0 * 3 + 2] * 1.2E+1) * + (cos(q[0 * 3 + 2] * 1.2E+1) / 1.0E+4 + + sin(q[0 * 3 + 2] * 1.2E+1) * 5.133772622539436E-10 - + (RL * RL) * cos(q[0 * 3 + 2] * 1.2E+1) * + 4.695222322285431E-13)) + + sin(q[0 * 3 + 1]) * + (cos(q[0 * 3 + 2] * 1.2E+1) * + (cos(q[0 * 3 + 2] * 1.2E+1) * 5.133772622539436E-10 - + sin(q[0 * 3 + 2] * 1.2E+1) / 1.0E+4 + + (RL * RL) * sin(q[0 * 3 + 2] * 1.2E+1) * + 4.695222322285431E-13) + + cos(q[0 * 3 + 2] * 1.2E+1) * + (cos(q[0 * 3 + 2] * 1.2E+1) * 5.133772622539436E-10 + + sin(q[0 * 3 + 2] * 1.2E+1) / 1.0E+4 - + (RL * RL) * sin(q[0 * 3 + 2] * 1.2E+1) * + 4.695222322285431E-13) - + sin(q[0 * 3 + 2] * 1.2E+1) * + (cos(q[0 * 3 + 2] * 1.2E+1) * (-1.0E-4) + + sin(q[0 * 3 + 2] * 1.2E+1) * 5.133772622539436E-10 + + (RL * RL) * cos(q[0 * 3 + 2] * 1.2E+1) * + 4.695222322285431E-13) - + sin(q[0 * 3 + 2] * 1.2E+1) * + (cos(q[0 * 3 + 2] * 1.2E+1) / 1.0E+4 + + sin(q[0 * 3 + 2] * 1.2E+1) * 5.133772622539436E-10 - + (RL * RL) * cos(q[0 * 3 + 2] * 1.2E+1) * + 4.695222322285431E-13)))) / + 2.0) * + 1.2E+1 + + qd[0 * 3 + 1] * + ((cos(q[0 * 3 + 1] * 6.0) * + (cos(q[0 * 3 + 1] * 6.0) * 7.211167181803015E-10 - + sin(q[0 * 3 + 1] * 6.0) / 1.0E+4)) / + 2.0 + + (cos(q[0 * 3 + 1] * 6.0) * + (cos(q[0 * 3 + 1] * 6.0) * 7.211167181803015E-10 + + sin(q[0 * 3 + 1] * 6.0) / 1.0E+4)) / + 2.0 + + (sin(q[0 * 3 + 1] * 6.0) * + (cos(q[0 * 3 + 1] * 6.0) / 1.0E+4 - + sin(q[0 * 3 + 1] * 6.0) * 7.211167181803015E-10)) / + 2.0 - + (sin(q[0 * 3 + 1] * 6.0) * + (cos(q[0 * 3 + 1] * 6.0) / 1.0E+4 + + sin(q[0 * 3 + 1] * 6.0) * 7.211167181803015E-10)) / + 2.0) * + 6.0 + + qd[0 * 3 + 1] * + ((sin(q[0 * 3 + 1]) * + (sin(q[0 * 3 + 1]) * + (cos(q[0 * 3 + 2]) * 1.463296047418464E-11 + + sin(q[0 * 3 + 2]) * 2.515259716742376E-3 + + sin(q[0 * 3 + 2]) * + (cos(q[0 * 3 + 2]) * 1.736636096535468E-5 - + sin(q[0 * 3 + 2]) * 6.535566164070341E-5) + + cos(q[0 * 3 + 2]) * + (cos(q[0 * 3 + 2]) * 6.535566164070341E-5 - + sin(q[0 * 3 + 2]) * 2.00164060021257E-3) + + cos(q[0 * 3 + 2] * 1.2E+1) * + (cos(q[0 * 3 + 2] * 1.2E+1) * 5.133772622539436E-10 - + sin(q[0 * 3 + 2] * 1.2E+1) / 1.0E+4 + + (RL * RL) * sin(q[0 * 3 + 2] * 1.2E+1) * + 4.695222322285431E-13) - + sin(q[0 * 3 + 2] * 1.2E+1) * + (cos(q[0 * 3 + 2] * 1.2E+1) * (-1.0E-4) + + sin(q[0 * 3 + 2] * 1.2E+1) * 5.133772622539436E-10 + + (RL * RL) * cos(q[0 * 3 + 2] * 1.2E+1) * + 4.695222322285431E-13) - + 7.355881041257671E-4) + + cos(q[0 * 3 + 1]) * + (cos(q[0 * 3 + 2]) * (-2.515259716742376E-3) + + sin(q[0 * 3 + 2]) * 1.463296047418464E-11 + + sin(q[0 * 3 + 2] * 1.2E+1) * + (cos(q[0 * 3 + 2] * 1.2E+1) * 5.133772622539436E-10 + + sin(q[0 * 3 + 2] * 1.2E+1) / 1.0E+4 - + (RL * RL) * sin(q[0 * 3 + 2] * 1.2E+1) * + 4.695222322285431E-13) + + cos(q[0 * 3 + 2]) * + (cos(q[0 * 3 + 2]) * 2.00164060021257E-3 + + sin(q[0 * 3 + 2]) * 6.535566164070341E-5 - + 2.515259716742376E-3) + + sin(q[0 * 3 + 2]) * + (cos(q[0 * 3 + 2]) * 6.535566164070341E-5 + + sin(q[0 * 3 + 2]) * 1.736636096535468E-5 + + 1.463296047418464E-11) + + (RL * RL) * pow(cos(q[0 * 3 + 2] * 1.2E+1), 2.0) * + 9.599999995304778E-4 + + (RL * RL) * pow(sin(q[0 * 3 + 2] * 1.2E+1), 2.0) * + 9.599999995304778E-4 + + cos(q[0 * 3 + 2] * 1.2E+1) * + (cos(q[0 * 3 + 2] * 1.2E+1) / 1.0E+4 + + sin(q[0 * 3 + 2] * 1.2E+1) * 5.133772622539436E-10 - + (RL * RL) * cos(q[0 * 3 + 2] * 1.2E+1) * + 4.695222322285431E-13) + + 1.182575965665761E-2) - + RL * cos(q[0 * 3 + 1]) * + (RL * 6.33521130433211E-3 + + RL * pow(cos(q[0 * 3 + 2] * 1.2E+1), 2.0) * + 1.199999999413097E-2 + + RL * pow(sin(q[0 * 3 + 2] * 1.2E+1), 2.0) * + 1.199999999413097E-2 + + RL * pow(cos(q[0 * 3 + 2]), 2.0) * + 9.485213488252606E-10 + + RL * pow(sin(q[0 * 3 + 2]), 2.0) * + 9.485213488252606E-10) * + 1.0098E-1)) / + 2.0 - + (cos(q[0 * 3 + 1]) * + (cos(q[0 * 3 + 1]) * + (cos(q[0 * 3 + 2]) * 1.463296047418464E-11 + + sin(q[0 * 3 + 2]) * 2.515259716742376E-3 + + sin(q[0 * 3 + 2]) * + (cos(q[0 * 3 + 2]) * 1.736636096535468E-5 - + sin(q[0 * 3 + 2]) * 6.535566164070341E-5) + + cos(q[0 * 3 + 2]) * + (cos(q[0 * 3 + 2]) * 6.535566164070341E-5 - + sin(q[0 * 3 + 2]) * 2.00164060021257E-3) + + cos(q[0 * 3 + 2] * 1.2E+1) * + (cos(q[0 * 3 + 2] * 1.2E+1) * 5.133772622539436E-10 - + sin(q[0 * 3 + 2] * 1.2E+1) / 1.0E+4 + + (RL * RL) * sin(q[0 * 3 + 2] * 1.2E+1) * + 4.695222322285431E-13) - + sin(q[0 * 3 + 2] * 1.2E+1) * + (cos(q[0 * 3 + 2] * 1.2E+1) * (-1.0E-4) + + sin(q[0 * 3 + 2] * 1.2E+1) * 5.133772622539436E-10 + + (RL * RL) * cos(q[0 * 3 + 2] * 1.2E+1) * + 4.695222322285431E-13) - + 7.355881041257671E-4) - + sin(q[0 * 3 + 1]) * + (cos(q[0 * 3 + 2]) * (-2.515259716742376E-3) + + sin(q[0 * 3 + 2]) * 1.463296047418464E-11 + + sin(q[0 * 3 + 2] * 1.2E+1) * + (cos(q[0 * 3 + 2] * 1.2E+1) * 5.133772622539436E-10 + + sin(q[0 * 3 + 2] * 1.2E+1) / 1.0E+4 - + (RL * RL) * sin(q[0 * 3 + 2] * 1.2E+1) * + 4.695222322285431E-13) + + cos(q[0 * 3 + 2]) * + (cos(q[0 * 3 + 2]) * 2.00164060021257E-3 + + sin(q[0 * 3 + 2]) * 6.535566164070341E-5 - + 2.515259716742376E-3) + + sin(q[0 * 3 + 2]) * + (cos(q[0 * 3 + 2]) * 6.535566164070341E-5 + + sin(q[0 * 3 + 2]) * 1.736636096535468E-5 + + 1.463296047418464E-11) + + (RL * RL) * pow(cos(q[0 * 3 + 2] * 1.2E+1), 2.0) * + 9.599999995304778E-4 + + (RL * RL) * pow(sin(q[0 * 3 + 2] * 1.2E+1), 2.0) * + 9.599999995304778E-4 + + cos(q[0 * 3 + 2] * 1.2E+1) * + (cos(q[0 * 3 + 2] * 1.2E+1) / 1.0E+4 + + sin(q[0 * 3 + 2] * 1.2E+1) * 5.133772622539436E-10 - + (RL * RL) * cos(q[0 * 3 + 2] * 1.2E+1) * + 4.695222322285431E-13) + + 1.182575965665761E-2) + + RL * sin(q[0 * 3 + 1]) * + (RL * 6.33521130433211E-3 + + RL * pow(cos(q[0 * 3 + 2] * 1.2E+1), 2.0) * + 1.199999999413097E-2 + + RL * pow(sin(q[0 * 3 + 2] * 1.2E+1), 2.0) * + 1.199999999413097E-2 + + RL * pow(cos(q[0 * 3 + 2]), 2.0) * + 9.485213488252606E-10 + + RL * pow(sin(q[0 * 3 + 2]), 2.0) * + 9.485213488252606E-10) * + 1.0098E-1)) / + 2.0 - + (sin(q[0 * 3 + 1]) * + (cos(q[0 * 3 + 1]) * + (-sin(q[0 * 3 + 2] * 1.2E+1) * + (cos(q[0 * 3 + 2] * 1.2E+1) * 5.133772622539436E-10 - + sin(q[0 * 3 + 2] * 1.2E+1) / 1.0E+4 + + (RL * RL) * sin(q[0 * 3 + 2] * 1.2E+1) * + 4.695222322285431E-13) - + sin(q[0 * 3 + 2]) * + (cos(q[0 * 3 + 2]) * 6.535566164070341E-5 - + sin(q[0 * 3 + 2]) * 2.00164060021257E-3) + + (RL * RL) * pow(cos(q[0 * 3 + 2] * 1.2E+1), 2.0) * + 9.599999995304778E-4 + + (RL * RL) * pow(sin(q[0 * 3 + 2] * 1.2E+1), 2.0) * + 9.599999995304778E-4 - + cos(q[0 * 3 + 2] * 1.2E+1) * + (cos(q[0 * 3 + 2] * 1.2E+1) * (-1.0E-4) + + sin(q[0 * 3 + 2] * 1.2E+1) * 5.133772622539436E-10 + + (RL * RL) * cos(q[0 * 3 + 2] * 1.2E+1) * + 4.695222322285431E-13) + + cos(q[0 * 3 + 2]) * + (cos(q[0 * 3 + 2]) * 1.736636096535468E-5 - + sin(q[0 * 3 + 2]) * 6.535566164070341E-5) + + 4.120174052057458E-4) + + sin(q[0 * 3 + 1]) * + (-cos(q[0 * 3 + 2]) * + (cos(q[0 * 3 + 2]) * 6.535566164070341E-5 + + sin(q[0 * 3 + 2]) * 1.736636096535468E-5 + + 1.463296047418464E-11) + + sin(q[0 * 3 + 2]) * + (cos(q[0 * 3 + 2]) * 2.00164060021257E-3 + + sin(q[0 * 3 + 2]) * 6.535566164070341E-5 - + 2.515259716742376E-3) - + cos(q[0 * 3 + 2] * 1.2E+1) * + (cos(q[0 * 3 + 2] * 1.2E+1) * 5.133772622539436E-10 + + sin(q[0 * 3 + 2] * 1.2E+1) / 1.0E+4 - + (RL * RL) * sin(q[0 * 3 + 2] * 1.2E+1) * + 4.695222322285431E-13) + + sin(q[0 * 3 + 2] * 1.2E+1) * + (cos(q[0 * 3 + 2] * 1.2E+1) / 1.0E+4 + + sin(q[0 * 3 + 2] * 1.2E+1) * 5.133772622539436E-10 - + (RL * RL) * cos(q[0 * 3 + 2] * 1.2E+1) * + 4.695222322285431E-13) + + 7.355881041257671E-4) - + RL * cos(q[0 * 3 + 1]) * + (RL * 6.33521130433211E-3 + + RL * pow(cos(q[0 * 3 + 2] * 1.2E+1), 2.0) * + 1.199999999413097E-2 + + RL * pow(sin(q[0 * 3 + 2] * 1.2E+1), 2.0) * + 1.199999999413097E-2 + + RL * pow(cos(q[0 * 3 + 2]), 2.0) * + 9.485213488252606E-10 + + RL * pow(sin(q[0 * 3 + 2]), 2.0) * + 9.485213488252606E-10) * + 1.0098E-1)) / + 2.0 + + (cos(q[0 * 3 + 1]) * + (-sin(q[0 * 3 + 1]) * + (-sin(q[0 * 3 + 2] * 1.2E+1) * + (cos(q[0 * 3 + 2] * 1.2E+1) * 5.133772622539436E-10 - + sin(q[0 * 3 + 2] * 1.2E+1) / 1.0E+4 + + (RL * RL) * sin(q[0 * 3 + 2] * 1.2E+1) * + 4.695222322285431E-13) - + sin(q[0 * 3 + 2]) * + (cos(q[0 * 3 + 2]) * 6.535566164070341E-5 - + sin(q[0 * 3 + 2]) * 2.00164060021257E-3) + + (RL * RL) * pow(cos(q[0 * 3 + 2] * 1.2E+1), 2.0) * + 9.599999995304778E-4 + + (RL * RL) * pow(sin(q[0 * 3 + 2] * 1.2E+1), 2.0) * + 9.599999995304778E-4 - + cos(q[0 * 3 + 2] * 1.2E+1) * + (cos(q[0 * 3 + 2] * 1.2E+1) * (-1.0E-4) + + sin(q[0 * 3 + 2] * 1.2E+1) * 5.133772622539436E-10 + + (RL * RL) * cos(q[0 * 3 + 2] * 1.2E+1) * + 4.695222322285431E-13) + + cos(q[0 * 3 + 2]) * + (cos(q[0 * 3 + 2]) * 1.736636096535468E-5 - + sin(q[0 * 3 + 2]) * 6.535566164070341E-5) + + 4.120174052057458E-4) + + cos(q[0 * 3 + 1]) * + (-cos(q[0 * 3 + 2]) * + (cos(q[0 * 3 + 2]) * 6.535566164070341E-5 + + sin(q[0 * 3 + 2]) * 1.736636096535468E-5 + + 1.463296047418464E-11) + + sin(q[0 * 3 + 2]) * + (cos(q[0 * 3 + 2]) * 2.00164060021257E-3 + + sin(q[0 * 3 + 2]) * 6.535566164070341E-5 - + 2.515259716742376E-3) - + cos(q[0 * 3 + 2] * 1.2E+1) * + (cos(q[0 * 3 + 2] * 1.2E+1) * 5.133772622539436E-10 + + sin(q[0 * 3 + 2] * 1.2E+1) / 1.0E+4 - + (RL * RL) * sin(q[0 * 3 + 2] * 1.2E+1) * + 4.695222322285431E-13) + + sin(q[0 * 3 + 2] * 1.2E+1) * + (cos(q[0 * 3 + 2] * 1.2E+1) / 1.0E+4 + + sin(q[0 * 3 + 2] * 1.2E+1) * 5.133772622539436E-10 - + (RL * RL) * cos(q[0 * 3 + 2] * 1.2E+1) * + 4.695222322285431E-13) + + 7.355881041257671E-4) + + RL * sin(q[0 * 3 + 1]) * + (RL * 6.33521130433211E-3 + + RL * pow(cos(q[0 * 3 + 2] * 1.2E+1), 2.0) * + 1.199999999413097E-2 + + RL * pow(sin(q[0 * 3 + 2] * 1.2E+1), 2.0) * + 1.199999999413097E-2 + + RL * pow(cos(q[0 * 3 + 2]), 2.0) * + 9.485213488252606E-10 + + RL * pow(sin(q[0 * 3 + 2]), 2.0) * + 9.485213488252606E-10) * + 1.0098E-1)) / + 2.0)) - + sin(q[0 * 3 + 0]) * sin(q[0 * 3 + 1]) * sin(q[0 * 3 + 2]) * + 6.968414672415114E-10 + + cos(q[0 * 3 + 2] * 1.2E+1) * cos(q[0 * 3 + 1]) * sin(q[0 * 3 + 0]) * + 1.041746808799933E-8 + + cos(q[0 * 3 + 2] * 1.2E+1) * sin(q[0 * 3 + 0]) * sin(q[0 * 3 + 1]) * + 7.243214465004041E-9 + + sin(q[0 * 3 + 2] * 1.2E+1) * cos(q[0 * 3 + 1]) * sin(q[0 * 3 + 0]) * + 7.243214465004041E-9 - + sin(q[0 * 3 + 2] * 1.2E+1) * sin(q[0 * 3 + 0]) * sin(q[0 * 3 + 1]) * + 1.041746808799933E-8 + + cos(q[0 * 3 + 1]) * cos(q[0 * 3 + 2]) * sin(q[0 * 3 + 0]) * + 6.968414672415114E-10 + + cos(q[0 * 3 + 1]) * sin(q[0 * 3 + 0]) * sin(q[0 * 3 + 2]) * + 1.197800865108869E-1 + + cos(q[0 * 3 + 2]) * sin(q[0 * 3 + 0]) * sin(q[0 * 3 + 1]) * + 1.197800865108869E-1; + F[0 * 3 + 1] = + cos(q[0 * 3 + 0]) * + (cos(q[0 * 3 + 1] * 6.0) * 4.784140444126403E+15 + + sin(q[0 * 3 + 1] * 6.0) * 7.011301471216116E+15) * + 3.042990678429451E-24 + + qd[0 * 3 + 0] * + (qd[0 * 3 + 2] * + ((cos(q[0 * 3 + 1]) * + (RL * cos(q[0 * 3 + 2] * 1.2E+1) * 2.046586469068882E-9 - + RL * sin(q[0 * 3 + 2] * 1.2E+1) * 3.092703555787573E-9)) / + 2.0 - + (sin(q[0 * 3 + 1]) * + (RL * cos(q[0 * 3 + 2] * 1.2E+1) * 3.092703555787573E-9 + + RL * sin(q[0 * 3 + 2] * 1.2E+1) * 2.046586469068882E-9)) / + 2.0 + + RL * cos(q[0 * 3 + 1]) * + (cos(q[0 * 3 + 2] * 1.2E+1) * 1.061923352497383E-9 + + sin(q[0 * 3 + 2] * 1.2E+1) * 7.383500983694232E-10) * + 5.049E-2 + + RL * sin(q[0 * 3 + 1]) * + (cos(q[0 * 3 + 2] * 1.2E+1) * 7.383500983694232E-10 - + sin(q[0 * 3 + 2] * 1.2E+1) * 1.061923352497383E-9) * + 5.049E-2) * + 2.4E+1 + + qd[0 * 3 + 2] * + ((cos(q[0 * 3 + 1]) * + (RL * cos(q[0 * 3 + 2]) * 5.909849367362282E-6 - + RL * sin(q[0 * 3 + 2]) * 6.000834499971923E-5)) / + 2.0 - + (sin(q[0 * 3 + 1]) * + (RL * cos(q[0 * 3 + 2]) * 6.000834499971923E-5 + + RL * sin(q[0 * 3 + 2]) * 5.909849367362282E-6)) / + 2.0 + + RL * cos(q[0 * 3 + 1]) * + (cos(q[0 * 3 + 2]) * 7.103378870963419E-11 + + sin(q[0 * 3 + 2]) * 1.220999862496299E-2) * + 5.049E-2 + + RL * sin(q[0 * 3 + 1]) * + (cos(q[0 * 3 + 2]) * 1.220999862496299E-2 - + sin(q[0 * 3 + 2]) * 7.103378870963419E-11) * + 5.049E-2) * + 2.0 - + qd[0 * 3 + 0] * + ((cos(q[0 * 3 + 1] * 6.0) * + (cos(q[0 * 3 + 1] * 6.0) * 7.211167181803015E-10 - + sin(q[0 * 3 + 1] * 6.0) / 1.0E+4)) / + 2.0 + + (cos(q[0 * 3 + 1] * 6.0) * + (cos(q[0 * 3 + 1] * 6.0) * 7.211167181803015E-10 + + sin(q[0 * 3 + 1] * 6.0) / 1.0E+4)) / + 2.0 + + (sin(q[0 * 3 + 1] * 6.0) * + (cos(q[0 * 3 + 1] * 6.0) / 1.0E+4 - + sin(q[0 * 3 + 1] * 6.0) * 7.211167181803015E-10)) / + 2.0 - + (sin(q[0 * 3 + 1] * 6.0) * + (cos(q[0 * 3 + 1] * 6.0) / 1.0E+4 + + sin(q[0 * 3 + 1] * 6.0) * 7.211167181803015E-10)) / + 2.0) * + 6.0 + + qd[0 * 3 + 1] * + ((cos(q[0 * 3 + 1]) * + (RL * 1.121534523796973E-5 + + RL * cos(q[0 * 3 + 2]) * 5.909849367362282E-6 - + RL * sin(q[0 * 3 + 2]) * 6.000834499971923E-5 + + RL * cos(q[0 * 3 + 2] * 1.2E+1) * 2.046586469068882E-9 - + RL * sin(q[0 * 3 + 2] * 1.2E+1) * 3.092703555787573E-9)) / + 2.0 - + (sin(q[0 * 3 + 1]) * + (RL * (-5.386630108894173E-5) + + RL * cos(q[0 * 3 + 2]) * 6.000834499971923E-5 + + RL * sin(q[0 * 3 + 2]) * 5.909849367362282E-6 + + RL * cos(q[0 * 3 + 2] * 1.2E+1) * 3.092703555787573E-9 - + RL * pow(cos(q[0 * 3 + 2]), 2.0) * 1.953953978580037E-10 + + RL * sin(q[0 * 3 + 2] * 1.2E+1) * 2.046586469068882E-9 - + RL * pow(sin(q[0 * 3 + 2]), 2.0) * 1.953953978580037E-10)) / + 2.0 + + RL * cos(q[0 * 3 + 1]) * + (cos(q[0 * 3 + 2] * 1.2E+1) * 1.061923352497383E-9 + + sin(q[0 * 3 + 2] * 1.2E+1) * 7.383500983694232E-10 + + cos(q[0 * 3 + 2]) * 7.103378870963419E-11 + + sin(q[0 * 3 + 2]) * 1.220999862496299E-2 - + 3.502030359531471E-3) * + 5.049E-2 - + RL * sin(q[0 * 3 + 1]) * + (cos(q[0 * 3 + 2] * 1.2E+1) * (-7.383500983694232E-10) + + sin(q[0 * 3 + 2] * 1.2E+1) * 1.061923352497383E-9 - + cos(q[0 * 3 + 2]) * 1.220999862496299E-2 + + sin(q[0 * 3 + 2]) * 7.103378870963419E-11 + + pow(cos(q[0 * 3 + 2]), 2.0) * 2.286599986975518E-2 + + pow(sin(q[0 * 3 + 2]), 2.0) * 2.286599986975518E-2 + + 3.14785127673052E-2) * + 5.049E-2) * + 2.0 - + qd[0 * 3 + 0] * + ((sin(q[0 * 3 + 1]) * + (sin(q[0 * 3 + 1]) * + (cos(q[0 * 3 + 2]) * 1.463296047418464E-11 + + sin(q[0 * 3 + 2]) * 2.515259716742376E-3 + + sin(q[0 * 3 + 2]) * + (cos(q[0 * 3 + 2]) * 1.736636096535468E-5 - + sin(q[0 * 3 + 2]) * 6.535566164070341E-5) + + cos(q[0 * 3 + 2]) * + (cos(q[0 * 3 + 2]) * 6.535566164070341E-5 - + sin(q[0 * 3 + 2]) * 2.00164060021257E-3) + + cos(q[0 * 3 + 2] * 1.2E+1) * + (cos(q[0 * 3 + 2] * 1.2E+1) * 5.133772622539436E-10 - + sin(q[0 * 3 + 2] * 1.2E+1) / 1.0E+4 + + (RL * RL) * sin(q[0 * 3 + 2] * 1.2E+1) * + 4.695222322285431E-13) - + sin(q[0 * 3 + 2] * 1.2E+1) * + (cos(q[0 * 3 + 2] * 1.2E+1) * (-1.0E-4) + + sin(q[0 * 3 + 2] * 1.2E+1) * 5.133772622539436E-10 + + (RL * RL) * cos(q[0 * 3 + 2] * 1.2E+1) * + 4.695222322285431E-13) - + 7.355881041257671E-4) + + cos(q[0 * 3 + 1]) * + (cos(q[0 * 3 + 2]) * (-2.515259716742376E-3) + + sin(q[0 * 3 + 2]) * 1.463296047418464E-11 + + sin(q[0 * 3 + 2] * 1.2E+1) * + (cos(q[0 * 3 + 2] * 1.2E+1) * 5.133772622539436E-10 + + sin(q[0 * 3 + 2] * 1.2E+1) / 1.0E+4 - + (RL * RL) * sin(q[0 * 3 + 2] * 1.2E+1) * + 4.695222322285431E-13) + + cos(q[0 * 3 + 2]) * + (cos(q[0 * 3 + 2]) * 2.00164060021257E-3 + + sin(q[0 * 3 + 2]) * 6.535566164070341E-5 - + 2.515259716742376E-3) + + sin(q[0 * 3 + 2]) * + (cos(q[0 * 3 + 2]) * 6.535566164070341E-5 + + sin(q[0 * 3 + 2]) * 1.736636096535468E-5 + + 1.463296047418464E-11) + + (RL * RL) * pow(cos(q[0 * 3 + 2] * 1.2E+1), 2.0) * + 9.599999995304778E-4 + + (RL * RL) * pow(sin(q[0 * 3 + 2] * 1.2E+1), 2.0) * + 9.599999995304778E-4 + + cos(q[0 * 3 + 2] * 1.2E+1) * + (cos(q[0 * 3 + 2] * 1.2E+1) / 1.0E+4 + + sin(q[0 * 3 + 2] * 1.2E+1) * 5.133772622539436E-10 - + (RL * RL) * cos(q[0 * 3 + 2] * 1.2E+1) * + 4.695222322285431E-13) + + 1.182575965665761E-2) - + RL * cos(q[0 * 3 + 1]) * + (RL * 6.33521130433211E-3 + + RL * pow(cos(q[0 * 3 + 2] * 1.2E+1), 2.0) * + 1.199999999413097E-2 + + RL * pow(sin(q[0 * 3 + 2] * 1.2E+1), 2.0) * + 1.199999999413097E-2 + + RL * pow(cos(q[0 * 3 + 2]), 2.0) * + 9.485213488252606E-10 + + RL * pow(sin(q[0 * 3 + 2]), 2.0) * + 9.485213488252606E-10) * + 1.0098E-1)) / + 2.0 - + (cos(q[0 * 3 + 1]) * + (cos(q[0 * 3 + 1]) * + (cos(q[0 * 3 + 2]) * 1.463296047418464E-11 + + sin(q[0 * 3 + 2]) * 2.515259716742376E-3 + + sin(q[0 * 3 + 2]) * + (cos(q[0 * 3 + 2]) * 1.736636096535468E-5 - + sin(q[0 * 3 + 2]) * 6.535566164070341E-5) + + cos(q[0 * 3 + 2]) * + (cos(q[0 * 3 + 2]) * 6.535566164070341E-5 - + sin(q[0 * 3 + 2]) * 2.00164060021257E-3) + + cos(q[0 * 3 + 2] * 1.2E+1) * + (cos(q[0 * 3 + 2] * 1.2E+1) * 5.133772622539436E-10 - + sin(q[0 * 3 + 2] * 1.2E+1) / 1.0E+4 + + (RL * RL) * sin(q[0 * 3 + 2] * 1.2E+1) * + 4.695222322285431E-13) - + sin(q[0 * 3 + 2] * 1.2E+1) * + (cos(q[0 * 3 + 2] * 1.2E+1) * (-1.0E-4) + + sin(q[0 * 3 + 2] * 1.2E+1) * 5.133772622539436E-10 + + (RL * RL) * cos(q[0 * 3 + 2] * 1.2E+1) * + 4.695222322285431E-13) - + 7.355881041257671E-4) - + sin(q[0 * 3 + 1]) * + (cos(q[0 * 3 + 2]) * (-2.515259716742376E-3) + + sin(q[0 * 3 + 2]) * 1.463296047418464E-11 + + sin(q[0 * 3 + 2] * 1.2E+1) * + (cos(q[0 * 3 + 2] * 1.2E+1) * 5.133772622539436E-10 + + sin(q[0 * 3 + 2] * 1.2E+1) / 1.0E+4 - + (RL * RL) * sin(q[0 * 3 + 2] * 1.2E+1) * + 4.695222322285431E-13) + + cos(q[0 * 3 + 2]) * + (cos(q[0 * 3 + 2]) * 2.00164060021257E-3 + + sin(q[0 * 3 + 2]) * 6.535566164070341E-5 - + 2.515259716742376E-3) + + sin(q[0 * 3 + 2]) * + (cos(q[0 * 3 + 2]) * 6.535566164070341E-5 + + sin(q[0 * 3 + 2]) * 1.736636096535468E-5 + + 1.463296047418464E-11) + + (RL * RL) * pow(cos(q[0 * 3 + 2] * 1.2E+1), 2.0) * + 9.599999995304778E-4 + + (RL * RL) * pow(sin(q[0 * 3 + 2] * 1.2E+1), 2.0) * + 9.599999995304778E-4 + + cos(q[0 * 3 + 2] * 1.2E+1) * + (cos(q[0 * 3 + 2] * 1.2E+1) / 1.0E+4 + + sin(q[0 * 3 + 2] * 1.2E+1) * 5.133772622539436E-10 - + (RL * RL) * cos(q[0 * 3 + 2] * 1.2E+1) * + 4.695222322285431E-13) + + 1.182575965665761E-2) + + RL * sin(q[0 * 3 + 1]) * + (RL * 6.33521130433211E-3 + + RL * pow(cos(q[0 * 3 + 2] * 1.2E+1), 2.0) * + 1.199999999413097E-2 + + RL * pow(sin(q[0 * 3 + 2] * 1.2E+1), 2.0) * + 1.199999999413097E-2 + + RL * pow(cos(q[0 * 3 + 2]), 2.0) * + 9.485213488252606E-10 + + RL * pow(sin(q[0 * 3 + 2]), 2.0) * + 9.485213488252606E-10) * + 1.0098E-1)) / + 2.0 - + (sin(q[0 * 3 + 1]) * + (cos(q[0 * 3 + 1]) * + (-sin(q[0 * 3 + 2] * 1.2E+1) * + (cos(q[0 * 3 + 2] * 1.2E+1) * 5.133772622539436E-10 - + sin(q[0 * 3 + 2] * 1.2E+1) / 1.0E+4 + + (RL * RL) * sin(q[0 * 3 + 2] * 1.2E+1) * + 4.695222322285431E-13) - + sin(q[0 * 3 + 2]) * + (cos(q[0 * 3 + 2]) * 6.535566164070341E-5 - + sin(q[0 * 3 + 2]) * 2.00164060021257E-3) + + (RL * RL) * pow(cos(q[0 * 3 + 2] * 1.2E+1), 2.0) * + 9.599999995304778E-4 + + (RL * RL) * pow(sin(q[0 * 3 + 2] * 1.2E+1), 2.0) * + 9.599999995304778E-4 - + cos(q[0 * 3 + 2] * 1.2E+1) * + (cos(q[0 * 3 + 2] * 1.2E+1) * (-1.0E-4) + + sin(q[0 * 3 + 2] * 1.2E+1) * 5.133772622539436E-10 + + (RL * RL) * cos(q[0 * 3 + 2] * 1.2E+1) * + 4.695222322285431E-13) + + cos(q[0 * 3 + 2]) * + (cos(q[0 * 3 + 2]) * 1.736636096535468E-5 - + sin(q[0 * 3 + 2]) * 6.535566164070341E-5) + + 4.120174052057458E-4) + + sin(q[0 * 3 + 1]) * + (-cos(q[0 * 3 + 2]) * + (cos(q[0 * 3 + 2]) * 6.535566164070341E-5 + + sin(q[0 * 3 + 2]) * 1.736636096535468E-5 + + 1.463296047418464E-11) + + sin(q[0 * 3 + 2]) * + (cos(q[0 * 3 + 2]) * 2.00164060021257E-3 + + sin(q[0 * 3 + 2]) * 6.535566164070341E-5 - + 2.515259716742376E-3) - + cos(q[0 * 3 + 2] * 1.2E+1) * + (cos(q[0 * 3 + 2] * 1.2E+1) * 5.133772622539436E-10 + + sin(q[0 * 3 + 2] * 1.2E+1) / 1.0E+4 - + (RL * RL) * sin(q[0 * 3 + 2] * 1.2E+1) * + 4.695222322285431E-13) + + sin(q[0 * 3 + 2] * 1.2E+1) * + (cos(q[0 * 3 + 2] * 1.2E+1) / 1.0E+4 + + sin(q[0 * 3 + 2] * 1.2E+1) * 5.133772622539436E-10 - + (RL * RL) * cos(q[0 * 3 + 2] * 1.2E+1) * + 4.695222322285431E-13) + + 7.355881041257671E-4) - + RL * cos(q[0 * 3 + 1]) * + (RL * 6.33521130433211E-3 + + RL * pow(cos(q[0 * 3 + 2] * 1.2E+1), 2.0) * + 1.199999999413097E-2 + + RL * pow(sin(q[0 * 3 + 2] * 1.2E+1), 2.0) * + 1.199999999413097E-2 + + RL * pow(cos(q[0 * 3 + 2]), 2.0) * + 9.485213488252606E-10 + + RL * pow(sin(q[0 * 3 + 2]), 2.0) * + 9.485213488252606E-10) * + 1.0098E-1)) / + 2.0 + + (cos(q[0 * 3 + 1]) * + (-sin(q[0 * 3 + 1]) * + (-sin(q[0 * 3 + 2] * 1.2E+1) * + (cos(q[0 * 3 + 2] * 1.2E+1) * 5.133772622539436E-10 - + sin(q[0 * 3 + 2] * 1.2E+1) / 1.0E+4 + + (RL * RL) * sin(q[0 * 3 + 2] * 1.2E+1) * + 4.695222322285431E-13) - + sin(q[0 * 3 + 2]) * + (cos(q[0 * 3 + 2]) * 6.535566164070341E-5 - + sin(q[0 * 3 + 2]) * 2.00164060021257E-3) + + (RL * RL) * pow(cos(q[0 * 3 + 2] * 1.2E+1), 2.0) * + 9.599999995304778E-4 + + (RL * RL) * pow(sin(q[0 * 3 + 2] * 1.2E+1), 2.0) * + 9.599999995304778E-4 - + cos(q[0 * 3 + 2] * 1.2E+1) * + (cos(q[0 * 3 + 2] * 1.2E+1) * (-1.0E-4) + + sin(q[0 * 3 + 2] * 1.2E+1) * 5.133772622539436E-10 + + (RL * RL) * cos(q[0 * 3 + 2] * 1.2E+1) * + 4.695222322285431E-13) + + cos(q[0 * 3 + 2]) * + (cos(q[0 * 3 + 2]) * 1.736636096535468E-5 - + sin(q[0 * 3 + 2]) * 6.535566164070341E-5) + + 4.120174052057458E-4) + + cos(q[0 * 3 + 1]) * + (-cos(q[0 * 3 + 2]) * + (cos(q[0 * 3 + 2]) * 6.535566164070341E-5 + + sin(q[0 * 3 + 2]) * 1.736636096535468E-5 + + 1.463296047418464E-11) + + sin(q[0 * 3 + 2]) * + (cos(q[0 * 3 + 2]) * 2.00164060021257E-3 + + sin(q[0 * 3 + 2]) * 6.535566164070341E-5 - + 2.515259716742376E-3) - + cos(q[0 * 3 + 2] * 1.2E+1) * + (cos(q[0 * 3 + 2] * 1.2E+1) * 5.133772622539436E-10 + + sin(q[0 * 3 + 2] * 1.2E+1) / 1.0E+4 - + (RL * RL) * sin(q[0 * 3 + 2] * 1.2E+1) * + 4.695222322285431E-13) + + sin(q[0 * 3 + 2] * 1.2E+1) * + (cos(q[0 * 3 + 2] * 1.2E+1) / 1.0E+4 + + sin(q[0 * 3 + 2] * 1.2E+1) * 5.133772622539436E-10 - + (RL * RL) * cos(q[0 * 3 + 2] * 1.2E+1) * + 4.695222322285431E-13) + + 7.355881041257671E-4) + + RL * sin(q[0 * 3 + 1]) * + (RL * 6.33521130433211E-3 + + RL * pow(cos(q[0 * 3 + 2] * 1.2E+1), 2.0) * + 1.199999999413097E-2 + + RL * pow(sin(q[0 * 3 + 2] * 1.2E+1), 2.0) * + 1.199999999413097E-2 + + RL * pow(cos(q[0 * 3 + 2]), 2.0) * + 9.485213488252606E-10 + + RL * pow(sin(q[0 * 3 + 2]), 2.0) * + 9.485213488252606E-10) * + 1.0098E-1)) / + 2.0) + + qd[0 * 3 + 1] * + (RL * cos(q[0 * 3 + 1] * 6.0) * 1.148353383135558E-9 + + RL * sin(q[0 * 3 + 1] * 6.0) * 8.717778744397566E-11) * + 7.2E+1) - + cos(q[0 * 3 + 0]) * + (cos(q[0 * 3 + 1] + q[0 * 3 + 2] * 1.2E+1) * 3.570441991335188E+18 - + sin(q[0 * 3 + 1] + q[0 * 3 + 2] * 1.2E+1) * 5.135146237143255E+18 + + cos(q[0 * 3 + 1] + q[0 * 3 + 2]) * 5.904393038070751E+25 - + sin(q[0 * 3 + 1] + q[0 * 3 + 2]) * 3.434983249445076E+17 - + cos(q[0 * 3 + 1]) * 2.627939379252632E+26 + + sin(q[0 * 3 + 1]) * 1.693477969084759E+25) * + 2.0286604522863E-27; + F[0 * 3 + 2] = + cos(q[0 * 3 + 0]) * + (cos(q[0 * 3 + 1] + q[0 * 3 + 2] * 1.2E+1) * 3.570441991335188E+15 - + sin(q[0 * 3 + 1] + q[0 * 3 + 2] * 1.2E+1) * 5.135146237143255E+15) * + (-2.43439254274356E-23) + + qd[0 * 3 + 0] * + (qd[0 * 3 + 1] * + ((cos(q[0 * 3 + 1]) * + (RL * cos(q[0 * 3 + 2] * 1.2E+1) * 2.046586469068882E-9 - + RL * sin(q[0 * 3 + 2] * 1.2E+1) * 3.092703555787573E-9)) / + 2.0 - + (sin(q[0 * 3 + 1]) * + (RL * cos(q[0 * 3 + 2] * 1.2E+1) * 3.092703555787573E-9 + + RL * sin(q[0 * 3 + 2] * 1.2E+1) * 2.046586469068882E-9)) / + 2.0 + + RL * cos(q[0 * 3 + 1]) * + (cos(q[0 * 3 + 2] * 1.2E+1) * 1.061923352497383E-9 + + sin(q[0 * 3 + 2] * 1.2E+1) * 7.383500983694232E-10) * + 5.049E-2 + + RL * sin(q[0 * 3 + 1]) * + (cos(q[0 * 3 + 2] * 1.2E+1) * 7.383500983694232E-10 - + sin(q[0 * 3 + 2] * 1.2E+1) * 1.061923352497383E-9) * + 5.049E-2) * + 1.2E+1 + + qd[0 * 3 + 2] * + ((cos(q[0 * 3 + 1]) * + (RL * cos(q[0 * 3 + 2] * 1.2E+1) * 2.046586469068882E-9 - + RL * sin(q[0 * 3 + 2] * 1.2E+1) * 3.092703555787573E-9)) / + 2.0 - + (sin(q[0 * 3 + 1]) * + (RL * cos(q[0 * 3 + 2] * 1.2E+1) * 3.092703555787573E-9 + + RL * sin(q[0 * 3 + 2] * 1.2E+1) * 2.046586469068882E-9)) / + 2.0 + + RL * cos(q[0 * 3 + 1]) * + (cos(q[0 * 3 + 2] * 1.2E+1) * 1.061923352497383E-9 + + sin(q[0 * 3 + 2] * 1.2E+1) * 7.383500983694232E-10) * + 5.049E-2 + + RL * sin(q[0 * 3 + 1]) * + (cos(q[0 * 3 + 2] * 1.2E+1) * 7.383500983694232E-10 - + sin(q[0 * 3 + 2] * 1.2E+1) * 1.061923352497383E-9) * + 5.049E-2) * + 1.44E+2 - + qd[0 * 3 + 0] * + ((sin(q[0 * 3 + 1]) * + (sin(q[0 * 3 + 1]) * + (cos(q[0 * 3 + 2]) * 1.463296047418464E-11 + + sin(q[0 * 3 + 2]) * 2.515259716742376E-3 + + sin(q[0 * 3 + 2]) * + (cos(q[0 * 3 + 2]) * 1.736636096535468E-5 - + sin(q[0 * 3 + 2]) * 6.535566164070341E-5) + + cos(q[0 * 3 + 2]) * + (cos(q[0 * 3 + 2]) * 6.535566164070341E-5 - + sin(q[0 * 3 + 2]) * 2.00164060021257E-3) + + cos(q[0 * 3 + 2]) * + (cos(q[0 * 3 + 2]) * 6.535566164070341E-5 + + sin(q[0 * 3 + 2]) * 1.736636096535468E-5 + + 1.463296047418464E-11) - + sin(q[0 * 3 + 2]) * + (cos(q[0 * 3 + 2]) * 2.00164060021257E-3 + + sin(q[0 * 3 + 2]) * 6.535566164070341E-5 - + 2.515259716742376E-3)) + + cos(q[0 * 3 + 1]) * + (cos(q[0 * 3 + 2]) * (-2.515259716742376E-3) + + sin(q[0 * 3 + 2]) * 1.463296047418464E-11 + + sin(q[0 * 3 + 2]) * + (cos(q[0 * 3 + 2]) * 6.535566164070341E-5 + + sin(q[0 * 3 + 2]) * 1.736636096535468E-5) + + cos(q[0 * 3 + 2]) * + (cos(q[0 * 3 + 2]) * 2.00164060021257E-3 + + sin(q[0 * 3 + 2]) * 6.535566164070341E-5) + + sin(q[0 * 3 + 2]) * + (cos(q[0 * 3 + 2]) * 6.535566164070341E-5 - + sin(q[0 * 3 + 2]) * 2.00164060021257E-3) - + cos(q[0 * 3 + 2]) * + (cos(q[0 * 3 + 2]) * 1.736636096535468E-5 - + sin(q[0 * 3 + 2]) * 6.535566164070341E-5)))) / + 2.0 + + (cos(q[0 * 3 + 1]) * + (sin(q[0 * 3 + 1]) * + (sin(q[0 * 3 + 2]) * + (cos(q[0 * 3 + 2]) * 6.535566164070341E-5 - + sin(q[0 * 3 + 2]) * 2.00164060021257E-3) + + cos(q[0 * 3 + 2]) * + (cos(q[0 * 3 + 2]) * 2.00164060021257E-3 + + sin(q[0 * 3 + 2]) * 6.535566164070341E-5 - + 2.515259716742376E-3) + + sin(q[0 * 3 + 2]) * + (cos(q[0 * 3 + 2]) * 6.535566164070341E-5 + + sin(q[0 * 3 + 2]) * 1.736636096535468E-5 + + 1.463296047418464E-11) - + cos(q[0 * 3 + 2]) * + (cos(q[0 * 3 + 2]) * 1.736636096535468E-5 - + sin(q[0 * 3 + 2]) * 6.535566164070341E-5)) - + cos(q[0 * 3 + 1]) * + (sin(q[0 * 3 + 2]) * + (cos(q[0 * 3 + 2]) * 1.736636096535468E-5 - + sin(q[0 * 3 + 2]) * 6.535566164070341E-5) + + cos(q[0 * 3 + 2]) * + (cos(q[0 * 3 + 2]) * 6.535566164070341E-5 - + sin(q[0 * 3 + 2]) * 2.00164060021257E-3) - + sin(q[0 * 3 + 2]) * + (cos(q[0 * 3 + 2]) * 2.00164060021257E-3 + + sin(q[0 * 3 + 2]) * 6.535566164070341E-5) + + cos(q[0 * 3 + 2]) * + (cos(q[0 * 3 + 2]) * 6.535566164070341E-5 + + sin(q[0 * 3 + 2]) * 1.736636096535468E-5)))) / + 2.0) + + qd[0 * 3 + 1] * + ((cos(q[0 * 3 + 1]) * + (RL * cos(q[0 * 3 + 2]) * 5.909849367362282E-6 - + RL * sin(q[0 * 3 + 2]) * 6.000834499971923E-5)) / + 2.0 - + (sin(q[0 * 3 + 1]) * + (RL * cos(q[0 * 3 + 2]) * 6.000834499971923E-5 + + RL * sin(q[0 * 3 + 2]) * 5.909849367362282E-6)) / + 2.0 + + RL * cos(q[0 * 3 + 1]) * + (cos(q[0 * 3 + 2]) * 7.103378870963419E-11 + + sin(q[0 * 3 + 2]) * 1.220999862496299E-2) * + 5.049E-2 + + RL * sin(q[0 * 3 + 1]) * + (cos(q[0 * 3 + 2]) * 1.220999862496299E-2 - + sin(q[0 * 3 + 2]) * 7.103378870963419E-11) * + 5.049E-2) * + 2.0 + + qd[0 * 3 + 2] * + ((cos(q[0 * 3 + 1]) * + (RL * cos(q[0 * 3 + 2]) * 5.909849367362282E-6 - + RL * sin(q[0 * 3 + 2]) * 6.000834499971923E-5)) / + 2.0 - + (sin(q[0 * 3 + 1]) * + (RL * cos(q[0 * 3 + 2]) * 6.000834499971923E-5 + + RL * sin(q[0 * 3 + 2]) * 5.909849367362282E-6)) / + 2.0 + + RL * cos(q[0 * 3 + 1]) * + (cos(q[0 * 3 + 2]) * 7.103378870963419E-11 + + sin(q[0 * 3 + 2]) * 1.220999862496299E-2) * + 5.049E-2 + + RL * sin(q[0 * 3 + 1]) * + (cos(q[0 * 3 + 2]) * 1.220999862496299E-2 - + sin(q[0 * 3 + 2]) * 7.103378870963419E-11) * + 5.049E-2) * + 2.0 + + qd[0 * 3 + 0] * + ((cos(q[0 * 3 + 1]) * + (cos(q[0 * 3 + 1]) * + (cos(q[0 * 3 + 2] * 1.2E+1) * + (cos(q[0 * 3 + 2] * 1.2E+1) * 5.133772622539436E-10 - + sin(q[0 * 3 + 2] * 1.2E+1) / 1.0E+4 + + (RL * RL) * sin(q[0 * 3 + 2] * 1.2E+1) * + 4.695222322285431E-13) + + cos(q[0 * 3 + 2] * 1.2E+1) * + (cos(q[0 * 3 + 2] * 1.2E+1) * 5.133772622539436E-10 + + sin(q[0 * 3 + 2] * 1.2E+1) / 1.0E+4 - + (RL * RL) * sin(q[0 * 3 + 2] * 1.2E+1) * + 4.695222322285431E-13) - + sin(q[0 * 3 + 2] * 1.2E+1) * + (cos(q[0 * 3 + 2] * 1.2E+1) * (-1.0E-4) + + sin(q[0 * 3 + 2] * 1.2E+1) * 5.133772622539436E-10 + + (RL * RL) * cos(q[0 * 3 + 2] * 1.2E+1) * + 4.695222322285431E-13) - + sin(q[0 * 3 + 2] * 1.2E+1) * + (cos(q[0 * 3 + 2] * 1.2E+1) / 1.0E+4 + + sin(q[0 * 3 + 2] * 1.2E+1) * 5.133772622539436E-10 - + (RL * RL) * cos(q[0 * 3 + 2] * 1.2E+1) * + 4.695222322285431E-13)) - + sin(q[0 * 3 + 1]) * + (sin(q[0 * 3 + 2] * 1.2E+1) * + (cos(q[0 * 3 + 2] * 1.2E+1) * 5.133772622539436E-10 - + sin(q[0 * 3 + 2] * 1.2E+1) / 1.0E+4 + + (RL * RL) * sin(q[0 * 3 + 2] * 1.2E+1) * + 4.695222322285431E-13) + + sin(q[0 * 3 + 2] * 1.2E+1) * + (cos(q[0 * 3 + 2] * 1.2E+1) * 5.133772622539436E-10 + + sin(q[0 * 3 + 2] * 1.2E+1) / 1.0E+4 - + (RL * RL) * sin(q[0 * 3 + 2] * 1.2E+1) * + 4.695222322285431E-13) + + cos(q[0 * 3 + 2] * 1.2E+1) * + (cos(q[0 * 3 + 2] * 1.2E+1) * (-1.0E-4) + + sin(q[0 * 3 + 2] * 1.2E+1) * 5.133772622539436E-10 + + (RL * RL) * cos(q[0 * 3 + 2] * 1.2E+1) * + 4.695222322285431E-13) + + cos(q[0 * 3 + 2] * 1.2E+1) * + (cos(q[0 * 3 + 2] * 1.2E+1) / 1.0E+4 + + sin(q[0 * 3 + 2] * 1.2E+1) * 5.133772622539436E-10 - + (RL * RL) * cos(q[0 * 3 + 2] * 1.2E+1) * + 4.695222322285431E-13)))) / + 2.0 - + (sin(q[0 * 3 + 1]) * + (cos(q[0 * 3 + 1]) * + (sin(q[0 * 3 + 2] * 1.2E+1) * + (cos(q[0 * 3 + 2] * 1.2E+1) * 5.133772622539436E-10 - + sin(q[0 * 3 + 2] * 1.2E+1) / 1.0E+4 + + (RL * RL) * sin(q[0 * 3 + 2] * 1.2E+1) * + 4.695222322285431E-13) + + sin(q[0 * 3 + 2] * 1.2E+1) * + (cos(q[0 * 3 + 2] * 1.2E+1) * 5.133772622539436E-10 + + sin(q[0 * 3 + 2] * 1.2E+1) / 1.0E+4 - + (RL * RL) * sin(q[0 * 3 + 2] * 1.2E+1) * + 4.695222322285431E-13) + + cos(q[0 * 3 + 2] * 1.2E+1) * + (cos(q[0 * 3 + 2] * 1.2E+1) * (-1.0E-4) + + sin(q[0 * 3 + 2] * 1.2E+1) * 5.133772622539436E-10 + + (RL * RL) * cos(q[0 * 3 + 2] * 1.2E+1) * + 4.695222322285431E-13) + + cos(q[0 * 3 + 2] * 1.2E+1) * + (cos(q[0 * 3 + 2] * 1.2E+1) / 1.0E+4 + + sin(q[0 * 3 + 2] * 1.2E+1) * 5.133772622539436E-10 - + (RL * RL) * cos(q[0 * 3 + 2] * 1.2E+1) * + 4.695222322285431E-13)) + + sin(q[0 * 3 + 1]) * + (cos(q[0 * 3 + 2] * 1.2E+1) * + (cos(q[0 * 3 + 2] * 1.2E+1) * 5.133772622539436E-10 - + sin(q[0 * 3 + 2] * 1.2E+1) / 1.0E+4 + + (RL * RL) * sin(q[0 * 3 + 2] * 1.2E+1) * + 4.695222322285431E-13) + + cos(q[0 * 3 + 2] * 1.2E+1) * + (cos(q[0 * 3 + 2] * 1.2E+1) * 5.133772622539436E-10 + + sin(q[0 * 3 + 2] * 1.2E+1) / 1.0E+4 - + (RL * RL) * sin(q[0 * 3 + 2] * 1.2E+1) * + 4.695222322285431E-13) - + sin(q[0 * 3 + 2] * 1.2E+1) * + (cos(q[0 * 3 + 2] * 1.2E+1) * (-1.0E-4) + + sin(q[0 * 3 + 2] * 1.2E+1) * 5.133772622539436E-10 + + (RL * RL) * cos(q[0 * 3 + 2] * 1.2E+1) * + 4.695222322285431E-13) - + sin(q[0 * 3 + 2] * 1.2E+1) * + (cos(q[0 * 3 + 2] * 1.2E+1) / 1.0E+4 + + sin(q[0 * 3 + 2] * 1.2E+1) * 5.133772622539436E-10 - + (RL * RL) * cos(q[0 * 3 + 2] * 1.2E+1) * + 4.695222322285431E-13)))) / + 2.0) * + 6.0) - + cos(q[0 * 3 + 0]) * + (cos(q[0 * 3 + 1] + q[0 * 3 + 2]) * 4.723514430456601E+23 - + sin(q[0 * 3 + 1] + q[0 * 3 + 2]) * 2.747986599556061E+15) * + 2.535825565357875E-25 - + qd[0 * 3 + 1] * + (qd[0 * 3 + 1] * (cos(q[0 * 3 + 2]) * 7.316480237092322E-12 + + sin(q[0 * 3 + 2]) * 1.257629858371188E-3 + + cos(q[0 * 3 + 2]) * + (sin(q[0 * 3 + 2]) * 2.286599986975518E-2 + + 7.103378870963419E-11) * + (1.03E+2 / 1.0E+3) - + sin(q[0 * 3 + 2]) * + (cos(q[0 * 3 + 2]) * 2.286599986975518E-2 - + 1.220999862496299E-2) * + (1.03E+2 / 1.0E+3)) + + qd[0 * 3 + 2] * + (cos(q[0 * 3 + 2]) * 7.316480237092322E-12 + + sin(q[0 * 3 + 2]) * 1.257629858371188E-3) * + 2.0); +} + +void f_J_MO(Eigen::Vector3d q, int RL, Eigen::Matrix3d &F) { + F(0, 0) = -((RL * cos(q[0 * 3 + 1]) * cos(q[0 * 3 + 2]) * 1.0098E-1 - + RL * sin(q[0 * 3 + 1]) * sin(q[0 * 3 + 2]) * 1.0098E-1) * + (cos(q[0 * 3 + 1]) * sin(q[0 * 3 + 2]) + + cos(q[0 * 3 + 2]) * sin(q[0 * 3 + 1]))) / + (pow(cos(q[0 * 3 + 1]), 2.0) * pow(cos(q[0 * 3 + 2]), 2.0) + + pow(cos(q[0 * 3 + 1]), 2.0) * pow(sin(q[0 * 3 + 2]), 2.0) + + pow(cos(q[0 * 3 + 2]), 2.0) * pow(sin(q[0 * 3 + 1]), 2.0) + + pow(sin(q[0 * 3 + 1]), 2.0) * pow(sin(q[0 * 3 + 2]), 2.0)) + + ((RL * cos(q[0 * 3 + 1]) * sin(q[0 * 3 + 2]) * 1.0098E-1 + + RL * cos(q[0 * 3 + 2]) * sin(q[0 * 3 + 1]) * 1.0098E-1) * + (cos(q[0 * 3 + 1]) * cos(q[0 * 3 + 2]) - + sin(q[0 * 3 + 1]) * sin(q[0 * 3 + 2]))) / + (pow(cos(q[0 * 3 + 1]), 2.0) * pow(cos(q[0 * 3 + 2]), 2.0) + + pow(cos(q[0 * 3 + 1]), 2.0) * pow(sin(q[0 * 3 + 2]), 2.0) + + pow(cos(q[0 * 3 + 2]), 2.0) * pow(sin(q[0 * 3 + 1]), 2.0) + + pow(sin(q[0 * 3 + 1]), 2.0) * pow(sin(q[0 * 3 + 2]), 2.0)) - + ((cos(q[0 * 3 + 1]) * sin(q[0 * 3 + 2]) + + cos(q[0 * 3 + 2]) * sin(q[0 * 3 + 1])) * + (RL * cos(q[0 * 3 + 0]) * cos(q[0 * 3 + 1]) * cos(q[0 * 3 + 2]) * + 3.5E+3 - + RL * cos(q[0 * 3 + 0]) * sin(q[0 * 3 + 1]) * sin(q[0 * 3 + 2]) * + 3.5E+3 + + RL * pow(cos(q[0 * 3 + 0]), 2.0) * cos(q[0 * 3 + 1]) * + cos(q[0 * 3 + 2]) * 5.049E+3 + + RL * cos(q[0 * 3 + 1]) * cos(q[0 * 3 + 2]) * + pow(sin(q[0 * 3 + 0]), 2.0) * 5.049E+3 - + RL * pow(cos(q[0 * 3 + 0]), 2.0) * sin(q[0 * 3 + 1]) * + sin(q[0 * 3 + 2]) * 5.049E+3 - + RL * pow(sin(q[0 * 3 + 0]), 2.0) * sin(q[0 * 3 + 1]) * + sin(q[0 * 3 + 2]) * 5.049E+3)) / + (pow(cos(q[0 * 3 + 0]), 2.0) * pow(cos(q[0 * 3 + 1]), 2.0) * + pow(sin(q[0 * 3 + 2]), 2.0) * 5.0E+4 + + pow(cos(q[0 * 3 + 0]), 2.0) * pow(cos(q[0 * 3 + 2]), 2.0) * + pow(sin(q[0 * 3 + 1]), 2.0) * 5.0E+4 + + pow(cos(q[0 * 3 + 1]), 2.0) * pow(cos(q[0 * 3 + 2]), 2.0) * + pow(sin(q[0 * 3 + 0]), 2.0) * 5.0E+4 + + pow(cos(q[0 * 3 + 0]), 2.0) * pow(sin(q[0 * 3 + 1]), 2.0) * + pow(sin(q[0 * 3 + 2]), 2.0) * 5.0E+4 + + pow(cos(q[0 * 3 + 1]), 2.0) * pow(sin(q[0 * 3 + 0]), 2.0) * + pow(sin(q[0 * 3 + 2]), 2.0) * 5.0E+4 + + pow(cos(q[0 * 3 + 2]), 2.0) * pow(sin(q[0 * 3 + 0]), 2.0) * + pow(sin(q[0 * 3 + 1]), 2.0) * 5.0E+4 + + pow(sin(q[0 * 3 + 0]), 2.0) * pow(sin(q[0 * 3 + 1]), 2.0) * + pow(sin(q[0 * 3 + 2]), 2.0) * 5.0E+4 + + pow(cos(q[0 * 3 + 0]), 2.0) * pow(cos(q[0 * 3 + 1]), 2.0) * + pow(cos(q[0 * 3 + 2]), 2.0) * 5.0E+4) + + ((cos(q[0 * 3 + 1]) * cos(q[0 * 3 + 2]) - + sin(q[0 * 3 + 1]) * sin(q[0 * 3 + 2])) * + (RL * cos(q[0 * 3 + 0]) * cos(q[0 * 3 + 1]) * sin(q[0 * 3 + 2]) * + 3.5E+3 + + RL * cos(q[0 * 3 + 0]) * cos(q[0 * 3 + 2]) * sin(q[0 * 3 + 1]) * + 3.5E+3 + + RL * pow(cos(q[0 * 3 + 0]), 2.0) * cos(q[0 * 3 + 1]) * + sin(q[0 * 3 + 2]) * 5.049E+3 + + RL * pow(cos(q[0 * 3 + 0]), 2.0) * cos(q[0 * 3 + 2]) * + sin(q[0 * 3 + 1]) * 5.049E+3 + + RL * cos(q[0 * 3 + 1]) * pow(sin(q[0 * 3 + 0]), 2.0) * + sin(q[0 * 3 + 2]) * 5.049E+3 + + RL * cos(q[0 * 3 + 2]) * pow(sin(q[0 * 3 + 0]), 2.0) * + sin(q[0 * 3 + 1]) * 5.049E+3)) / + (pow(cos(q[0 * 3 + 0]), 2.0) * pow(cos(q[0 * 3 + 1]), 2.0) * + pow(sin(q[0 * 3 + 2]), 2.0) * 5.0E+4 + + pow(cos(q[0 * 3 + 0]), 2.0) * pow(cos(q[0 * 3 + 2]), 2.0) * + pow(sin(q[0 * 3 + 1]), 2.0) * 5.0E+4 + + pow(cos(q[0 * 3 + 1]), 2.0) * pow(cos(q[0 * 3 + 2]), 2.0) * + pow(sin(q[0 * 3 + 0]), 2.0) * 5.0E+4 + + pow(cos(q[0 * 3 + 0]), 2.0) * pow(sin(q[0 * 3 + 1]), 2.0) * + pow(sin(q[0 * 3 + 2]), 2.0) * 5.0E+4 + + pow(cos(q[0 * 3 + 1]), 2.0) * pow(sin(q[0 * 3 + 0]), 2.0) * + pow(sin(q[0 * 3 + 2]), 2.0) * 5.0E+4 + + pow(cos(q[0 * 3 + 2]), 2.0) * pow(sin(q[0 * 3 + 0]), 2.0) * + pow(sin(q[0 * 3 + 1]), 2.0) * 5.0E+4 + + pow(sin(q[0 * 3 + 0]), 2.0) * pow(sin(q[0 * 3 + 1]), 2.0) * + pow(sin(q[0 * 3 + 2]), 2.0) * 5.0E+4 + + pow(cos(q[0 * 3 + 0]), 2.0) * pow(cos(q[0 * 3 + 1]), 2.0) * + pow(cos(q[0 * 3 + 2]), 2.0) * 5.0E+4); + F(0, 1) = + ((pow(cos(q[0 * 3 + 0]), 2.0) * sin(q[0 * 3 + 1]) * 1.03E+2 + + pow(sin(q[0 * 3 + 0]), 2.0) * sin(q[0 * 3 + 1]) * 1.03E+2 + + RL * pow(cos(q[0 * 3 + 1]), 2.0) * sin(q[0 * 3 + 0]) * 3.5E+1 + + RL * sin(q[0 * 3 + 0]) * pow(sin(q[0 * 3 + 1]), 2.0) * 3.5E+1) * + (-1.0 / 5.0E+2)) / + (pow(cos(q[0 * 3 + 0]), 2.0) * pow(cos(q[0 * 3 + 1]), 2.0) + + pow(cos(q[0 * 3 + 0]), 2.0) * pow(sin(q[0 * 3 + 1]), 2.0) + + pow(cos(q[0 * 3 + 1]), 2.0) * pow(sin(q[0 * 3 + 0]), 2.0) + + pow(sin(q[0 * 3 + 0]), 2.0) * pow(sin(q[0 * 3 + 1]), 2.0)) + + (cos(q[0 * 3 + 0]) * + (-(sin(q[0 * 3 + 0]) * + ((cos(q[0 * 3 + 0]) * + (cos(q[0 * 3 + 1]) * sin(q[0 * 3 + 2]) + + cos(q[0 * 3 + 2]) * sin(q[0 * 3 + 1])) * + 1.0 / + pow(pow(cos(q[0 * 3 + 0]), 2.0) * pow(cos(q[0 * 3 + 1]), 2.0) * + pow(sin(q[0 * 3 + 2]), 2.0) + + pow(cos(q[0 * 3 + 0]), 2.0) * pow(cos(q[0 * 3 + 2]), 2.0) * + pow(sin(q[0 * 3 + 1]), 2.0) + + pow(cos(q[0 * 3 + 1]), 2.0) * pow(cos(q[0 * 3 + 2]), 2.0) * + pow(sin(q[0 * 3 + 0]), 2.0) + + pow(cos(q[0 * 3 + 0]), 2.0) * pow(sin(q[0 * 3 + 1]), 2.0) * + pow(sin(q[0 * 3 + 2]), 2.0) + + pow(cos(q[0 * 3 + 1]), 2.0) * pow(sin(q[0 * 3 + 0]), 2.0) * + pow(sin(q[0 * 3 + 2]), 2.0) + + pow(cos(q[0 * 3 + 2]), 2.0) * pow(sin(q[0 * 3 + 0]), 2.0) * + pow(sin(q[0 * 3 + 1]), 2.0) + + pow(sin(q[0 * 3 + 0]), 2.0) * pow(sin(q[0 * 3 + 1]), 2.0) * + pow(sin(q[0 * 3 + 2]), 2.0) + + pow(cos(q[0 * 3 + 0]), 2.0) * pow(cos(q[0 * 3 + 1]), 2.0) * + pow(cos(q[0 * 3 + 2]), 2.0), + 2.0) * + (sin(q[0 * 3 + 0]) * sin(q[0 * 3 + 1]) * sin(q[0 * 3 + 2]) * + 1.1315E+4 + + pow(cos(q[0 * 3 + 1]), 2.0) * cos(q[0 * 3 + 2]) * + sin(q[0 * 3 + 0]) * 1.03E+4 + + cos(q[0 * 3 + 2]) * sin(q[0 * 3 + 0]) * + pow(sin(q[0 * 3 + 1]), 2.0) * 1.03E+4 - + cos(q[0 * 3 + 1]) * cos(q[0 * 3 + 2]) * sin(q[0 * 3 + 0]) * + 1.1315E+4 + + RL * cos(q[0 * 3 + 0]) * cos(q[0 * 3 + 1]) * sin(q[0 * 3 + 2]) * + 5.049E+3 + + RL * cos(q[0 * 3 + 0]) * cos(q[0 * 3 + 2]) * sin(q[0 * 3 + 1]) * + 5.049E+3 + + RL * pow(cos(q[0 * 3 + 0]), 2.0) * cos(q[0 * 3 + 1]) * + sin(q[0 * 3 + 2]) * 3.5E+3 + + RL * pow(cos(q[0 * 3 + 0]), 2.0) * cos(q[0 * 3 + 2]) * + sin(q[0 * 3 + 1]) * 3.5E+3 + + RL * cos(q[0 * 3 + 1]) * pow(sin(q[0 * 3 + 0]), 2.0) * + sin(q[0 * 3 + 2]) * 3.5E+3 + + RL * cos(q[0 * 3 + 2]) * pow(sin(q[0 * 3 + 0]), 2.0) * + sin(q[0 * 3 + 1]) * 3.5E+3)) / + 1.0E+5 - + (cos(q[0 * 3 + 0]) * + (cos(q[0 * 3 + 1]) * cos(q[0 * 3 + 2]) - + sin(q[0 * 3 + 1]) * sin(q[0 * 3 + 2])) * + 1.0 / + pow(pow(cos(q[0 * 3 + 0]), 2.0) * pow(cos(q[0 * 3 + 1]), 2.0) * + pow(sin(q[0 * 3 + 2]), 2.0) + + pow(cos(q[0 * 3 + 0]), 2.0) * pow(cos(q[0 * 3 + 2]), 2.0) * + pow(sin(q[0 * 3 + 1]), 2.0) + + pow(cos(q[0 * 3 + 1]), 2.0) * pow(cos(q[0 * 3 + 2]), 2.0) * + pow(sin(q[0 * 3 + 0]), 2.0) + + pow(cos(q[0 * 3 + 0]), 2.0) * pow(sin(q[0 * 3 + 1]), 2.0) * + pow(sin(q[0 * 3 + 2]), 2.0) + + pow(cos(q[0 * 3 + 1]), 2.0) * pow(sin(q[0 * 3 + 0]), 2.0) * + pow(sin(q[0 * 3 + 2]), 2.0) + + pow(cos(q[0 * 3 + 2]), 2.0) * pow(sin(q[0 * 3 + 0]), 2.0) * + pow(sin(q[0 * 3 + 1]), 2.0) + + pow(sin(q[0 * 3 + 0]), 2.0) * pow(sin(q[0 * 3 + 1]), 2.0) * + pow(sin(q[0 * 3 + 2]), 2.0) + + pow(cos(q[0 * 3 + 0]), 2.0) * pow(cos(q[0 * 3 + 1]), 2.0) * + pow(cos(q[0 * 3 + 2]), 2.0), + 2.0) * + (pow(cos(q[0 * 3 + 1]), 2.0) * sin(q[0 * 3 + 0]) * + sin(q[0 * 3 + 2]) * 1.03E+4 + + sin(q[0 * 3 + 0]) * pow(sin(q[0 * 3 + 1]), 2.0) * + sin(q[0 * 3 + 2]) * 1.03E+4 - + cos(q[0 * 3 + 1]) * sin(q[0 * 3 + 0]) * sin(q[0 * 3 + 2]) * + 1.1315E+4 - + cos(q[0 * 3 + 2]) * sin(q[0 * 3 + 0]) * sin(q[0 * 3 + 1]) * + 1.1315E+4 - + RL * cos(q[0 * 3 + 0]) * cos(q[0 * 3 + 1]) * cos(q[0 * 3 + 2]) * + 5.049E+3 + + RL * cos(q[0 * 3 + 0]) * sin(q[0 * 3 + 1]) * sin(q[0 * 3 + 2]) * + 5.049E+3 - + RL * pow(cos(q[0 * 3 + 0]), 2.0) * cos(q[0 * 3 + 1]) * + cos(q[0 * 3 + 2]) * 3.5E+3 - + RL * cos(q[0 * 3 + 1]) * cos(q[0 * 3 + 2]) * + pow(sin(q[0 * 3 + 0]), 2.0) * 3.5E+3 + + RL * pow(cos(q[0 * 3 + 0]), 2.0) * sin(q[0 * 3 + 1]) * + sin(q[0 * 3 + 2]) * 3.5E+3 + + RL * pow(sin(q[0 * 3 + 0]), 2.0) * sin(q[0 * 3 + 1]) * + sin(q[0 * 3 + 2]) * 3.5E+3)) / + 1.0E+5 - + (sin(q[0 * 3 + 0]) * + (cos(q[0 * 3 + 1]) * sin(q[0 * 3 + 2]) + + cos(q[0 * 3 + 2]) * sin(q[0 * 3 + 1])) * + (cos(q[0 * 3 + 0]) * pow(cos(q[0 * 3 + 1]), 2.0) * + cos(q[0 * 3 + 2]) * 1.03E+4 + + cos(q[0 * 3 + 0]) * cos(q[0 * 3 + 2]) * + pow(sin(q[0 * 3 + 1]), 2.0) * 1.03E+4 - + cos(q[0 * 3 + 0]) * cos(q[0 * 3 + 1]) * cos(q[0 * 3 + 2]) * + 1.1315E+4 + + cos(q[0 * 3 + 0]) * sin(q[0 * 3 + 1]) * sin(q[0 * 3 + 2]) * + 1.1315E+4 - + RL * cos(q[0 * 3 + 1]) * sin(q[0 * 3 + 0]) * sin(q[0 * 3 + 2]) * + 5.049E+3 - + RL * cos(q[0 * 3 + 2]) * sin(q[0 * 3 + 0]) * sin(q[0 * 3 + 1]) * + 5.049E+3) * + 1.0 / + pow(pow(cos(q[0 * 3 + 0]), 2.0) * pow(cos(q[0 * 3 + 1]), 2.0) * + pow(sin(q[0 * 3 + 2]), 2.0) + + pow(cos(q[0 * 3 + 0]), 2.0) * pow(cos(q[0 * 3 + 2]), 2.0) * + pow(sin(q[0 * 3 + 1]), 2.0) + + pow(cos(q[0 * 3 + 1]), 2.0) * pow(cos(q[0 * 3 + 2]), 2.0) * + pow(sin(q[0 * 3 + 0]), 2.0) + + pow(cos(q[0 * 3 + 0]), 2.0) * pow(sin(q[0 * 3 + 1]), 2.0) * + pow(sin(q[0 * 3 + 2]), 2.0) + + pow(cos(q[0 * 3 + 1]), 2.0) * pow(sin(q[0 * 3 + 0]), 2.0) * + pow(sin(q[0 * 3 + 2]), 2.0) + + pow(cos(q[0 * 3 + 2]), 2.0) * pow(sin(q[0 * 3 + 0]), 2.0) * + pow(sin(q[0 * 3 + 1]), 2.0) + + pow(sin(q[0 * 3 + 0]), 2.0) * pow(sin(q[0 * 3 + 1]), 2.0) * + pow(sin(q[0 * 3 + 2]), 2.0) + + pow(cos(q[0 * 3 + 0]), 2.0) * pow(cos(q[0 * 3 + 1]), 2.0) * + pow(cos(q[0 * 3 + 2]), 2.0), + 2.0)) / + 1.0E+5 + + (sin(q[0 * 3 + 0]) * + (cos(q[0 * 3 + 1]) * cos(q[0 * 3 + 2]) - + sin(q[0 * 3 + 1]) * sin(q[0 * 3 + 2])) * + (cos(q[0 * 3 + 0]) * pow(cos(q[0 * 3 + 1]), 2.0) * + sin(q[0 * 3 + 2]) * 1.03E+4 + + cos(q[0 * 3 + 0]) * pow(sin(q[0 * 3 + 1]), 2.0) * + sin(q[0 * 3 + 2]) * 1.03E+4 - + cos(q[0 * 3 + 0]) * cos(q[0 * 3 + 1]) * sin(q[0 * 3 + 2]) * + 1.1315E+4 - + cos(q[0 * 3 + 0]) * cos(q[0 * 3 + 2]) * sin(q[0 * 3 + 1]) * + 1.1315E+4 + + RL * cos(q[0 * 3 + 1]) * cos(q[0 * 3 + 2]) * sin(q[0 * 3 + 0]) * + 5.049E+3 - + RL * sin(q[0 * 3 + 0]) * sin(q[0 * 3 + 1]) * sin(q[0 * 3 + 2]) * + 5.049E+3) * + 1.0 / + pow(pow(cos(q[0 * 3 + 0]), 2.0) * pow(cos(q[0 * 3 + 1]), 2.0) * + pow(sin(q[0 * 3 + 2]), 2.0) + + pow(cos(q[0 * 3 + 0]), 2.0) * pow(cos(q[0 * 3 + 2]), 2.0) * + pow(sin(q[0 * 3 + 1]), 2.0) + + pow(cos(q[0 * 3 + 1]), 2.0) * pow(cos(q[0 * 3 + 2]), 2.0) * + pow(sin(q[0 * 3 + 0]), 2.0) + + pow(cos(q[0 * 3 + 0]), 2.0) * pow(sin(q[0 * 3 + 1]), 2.0) * + pow(sin(q[0 * 3 + 2]), 2.0) + + pow(cos(q[0 * 3 + 1]), 2.0) * pow(sin(q[0 * 3 + 0]), 2.0) * + pow(sin(q[0 * 3 + 2]), 2.0) + + pow(cos(q[0 * 3 + 2]), 2.0) * pow(sin(q[0 * 3 + 0]), 2.0) * + pow(sin(q[0 * 3 + 1]), 2.0) + + pow(sin(q[0 * 3 + 0]), 2.0) * pow(sin(q[0 * 3 + 1]), 2.0) * + pow(sin(q[0 * 3 + 2]), 2.0) + + pow(cos(q[0 * 3 + 0]), 2.0) * pow(cos(q[0 * 3 + 1]), 2.0) * + pow(cos(q[0 * 3 + 2]), 2.0), + 2.0)) / + 1.0E+5 + + ((cos(q[0 * 3 + 1]) * sin(q[0 * 3 + 2]) + + cos(q[0 * 3 + 2]) * sin(q[0 * 3 + 1])) * + (RL * cos(q[0 * 3 + 0]) * cos(q[0 * 3 + 1]) * sin(q[0 * 3 + 2]) * + 3.5E+3 + + RL * cos(q[0 * 3 + 0]) * cos(q[0 * 3 + 2]) * sin(q[0 * 3 + 1]) * + 3.5E+3 + + RL * pow(cos(q[0 * 3 + 0]), 2.0) * cos(q[0 * 3 + 1]) * + sin(q[0 * 3 + 2]) * 5.049E+3 + + RL * pow(cos(q[0 * 3 + 0]), 2.0) * cos(q[0 * 3 + 2]) * + sin(q[0 * 3 + 1]) * 5.049E+3 + + RL * cos(q[0 * 3 + 1]) * pow(sin(q[0 * 3 + 0]), 2.0) * + sin(q[0 * 3 + 2]) * 5.049E+3 + + RL * cos(q[0 * 3 + 2]) * pow(sin(q[0 * 3 + 0]), 2.0) * + sin(q[0 * 3 + 1]) * 5.049E+3)) / + ((pow(cos(q[0 * 3 + 1]), 2.0) * pow(cos(q[0 * 3 + 2]), 2.0) + + pow(cos(q[0 * 3 + 1]), 2.0) * pow(sin(q[0 * 3 + 2]), 2.0) + + pow(cos(q[0 * 3 + 2]), 2.0) * pow(sin(q[0 * 3 + 1]), 2.0) + + pow(sin(q[0 * 3 + 1]), 2.0) * pow(sin(q[0 * 3 + 2]), 2.0)) * + (pow(cos(q[0 * 3 + 0]), 2.0) * pow(cos(q[0 * 3 + 1]), 2.0) * + pow(sin(q[0 * 3 + 2]), 2.0) + + pow(cos(q[0 * 3 + 0]), 2.0) * pow(cos(q[0 * 3 + 2]), 2.0) * + pow(sin(q[0 * 3 + 1]), 2.0) + + pow(cos(q[0 * 3 + 1]), 2.0) * pow(cos(q[0 * 3 + 2]), 2.0) * + pow(sin(q[0 * 3 + 0]), 2.0) + + pow(cos(q[0 * 3 + 0]), 2.0) * pow(sin(q[0 * 3 + 1]), 2.0) * + pow(sin(q[0 * 3 + 2]), 2.0) + + pow(cos(q[0 * 3 + 1]), 2.0) * pow(sin(q[0 * 3 + 0]), 2.0) * + pow(sin(q[0 * 3 + 2]), 2.0) + + pow(cos(q[0 * 3 + 2]), 2.0) * pow(sin(q[0 * 3 + 0]), 2.0) * + pow(sin(q[0 * 3 + 1]), 2.0) + + pow(sin(q[0 * 3 + 0]), 2.0) * pow(sin(q[0 * 3 + 1]), 2.0) * + pow(sin(q[0 * 3 + 2]), 2.0) + + pow(cos(q[0 * 3 + 0]), 2.0) * pow(cos(q[0 * 3 + 1]), 2.0) * + pow(cos(q[0 * 3 + 2]), 2.0)) * + 1.0E+5) + + ((cos(q[0 * 3 + 1]) * cos(q[0 * 3 + 2]) - + sin(q[0 * 3 + 1]) * sin(q[0 * 3 + 2])) * + (RL * cos(q[0 * 3 + 0]) * cos(q[0 * 3 + 1]) * cos(q[0 * 3 + 2]) * + 3.5E+3 - + RL * cos(q[0 * 3 + 0]) * sin(q[0 * 3 + 1]) * sin(q[0 * 3 + 2]) * + 3.5E+3 + + RL * pow(cos(q[0 * 3 + 0]), 2.0) * cos(q[0 * 3 + 1]) * + cos(q[0 * 3 + 2]) * 5.049E+3 + + RL * cos(q[0 * 3 + 1]) * cos(q[0 * 3 + 2]) * + pow(sin(q[0 * 3 + 0]), 2.0) * 5.049E+3 - + RL * pow(cos(q[0 * 3 + 0]), 2.0) * sin(q[0 * 3 + 1]) * + sin(q[0 * 3 + 2]) * 5.049E+3 - + RL * pow(sin(q[0 * 3 + 0]), 2.0) * sin(q[0 * 3 + 1]) * + sin(q[0 * 3 + 2]) * 5.049E+3)) / + ((pow(cos(q[0 * 3 + 1]), 2.0) * pow(cos(q[0 * 3 + 2]), 2.0) + + pow(cos(q[0 * 3 + 1]), 2.0) * pow(sin(q[0 * 3 + 2]), 2.0) + + pow(cos(q[0 * 3 + 2]), 2.0) * pow(sin(q[0 * 3 + 1]), 2.0) + + pow(sin(q[0 * 3 + 1]), 2.0) * pow(sin(q[0 * 3 + 2]), 2.0)) * + (pow(cos(q[0 * 3 + 0]), 2.0) * pow(cos(q[0 * 3 + 1]), 2.0) * + pow(sin(q[0 * 3 + 2]), 2.0) + + pow(cos(q[0 * 3 + 0]), 2.0) * pow(cos(q[0 * 3 + 2]), 2.0) * + pow(sin(q[0 * 3 + 1]), 2.0) + + pow(cos(q[0 * 3 + 1]), 2.0) * pow(cos(q[0 * 3 + 2]), 2.0) * + pow(sin(q[0 * 3 + 0]), 2.0) + + pow(cos(q[0 * 3 + 0]), 2.0) * pow(sin(q[0 * 3 + 1]), 2.0) * + pow(sin(q[0 * 3 + 2]), 2.0) + + pow(cos(q[0 * 3 + 1]), 2.0) * pow(sin(q[0 * 3 + 0]), 2.0) * + pow(sin(q[0 * 3 + 2]), 2.0) + + pow(cos(q[0 * 3 + 2]), 2.0) * pow(sin(q[0 * 3 + 0]), 2.0) * + pow(sin(q[0 * 3 + 1]), 2.0) + + pow(sin(q[0 * 3 + 0]), 2.0) * pow(sin(q[0 * 3 + 1]), 2.0) * + pow(sin(q[0 * 3 + 2]), 2.0) + + pow(cos(q[0 * 3 + 0]), 2.0) * pow(cos(q[0 * 3 + 1]), 2.0) * + pow(cos(q[0 * 3 + 2]), 2.0)) * + 1.0E+5))) / + (pow(cos(q[0 * 3 + 0]), 2.0) + pow(sin(q[0 * 3 + 0]), 2.0)) + + (cos(q[0 * 3 + 0]) * + (cos(q[0 * 3 + 1]) * sin(q[0 * 3 + 2]) + + cos(q[0 * 3 + 2]) * sin(q[0 * 3 + 1])) * + ((cos(q[0 * 3 + 0]) * + (cos(q[0 * 3 + 0]) * pow(cos(q[0 * 3 + 1]), 2.0) * + cos(q[0 * 3 + 2]) * 1.03E+4 + + cos(q[0 * 3 + 0]) * cos(q[0 * 3 + 2]) * + pow(sin(q[0 * 3 + 1]), 2.0) * 1.03E+4 - + cos(q[0 * 3 + 0]) * cos(q[0 * 3 + 1]) * cos(q[0 * 3 + 2]) * + 1.1315E+4 + + cos(q[0 * 3 + 0]) * sin(q[0 * 3 + 1]) * sin(q[0 * 3 + 2]) * + 1.1315E+4 - + RL * cos(q[0 * 3 + 1]) * sin(q[0 * 3 + 0]) * sin(q[0 * 3 + 2]) * + 5.049E+3 - + RL * cos(q[0 * 3 + 2]) * sin(q[0 * 3 + 0]) * sin(q[0 * 3 + 1]) * + 5.049E+3)) / + ((pow(cos(q[0 * 3 + 0]), 2.0) + pow(sin(q[0 * 3 + 0]), 2.0)) * + (pow(cos(q[0 * 3 + 0]), 2.0) * pow(cos(q[0 * 3 + 1]), 2.0) * + pow(sin(q[0 * 3 + 2]), 2.0) + + pow(cos(q[0 * 3 + 0]), 2.0) * pow(cos(q[0 * 3 + 2]), 2.0) * + pow(sin(q[0 * 3 + 1]), 2.0) + + pow(cos(q[0 * 3 + 1]), 2.0) * pow(cos(q[0 * 3 + 2]), 2.0) * + pow(sin(q[0 * 3 + 0]), 2.0) + + pow(cos(q[0 * 3 + 0]), 2.0) * pow(sin(q[0 * 3 + 1]), 2.0) * + pow(sin(q[0 * 3 + 2]), 2.0) + + pow(cos(q[0 * 3 + 1]), 2.0) * pow(sin(q[0 * 3 + 0]), 2.0) * + pow(sin(q[0 * 3 + 2]), 2.0) + + pow(cos(q[0 * 3 + 2]), 2.0) * pow(sin(q[0 * 3 + 0]), 2.0) * + pow(sin(q[0 * 3 + 1]), 2.0) + + pow(sin(q[0 * 3 + 0]), 2.0) * pow(sin(q[0 * 3 + 1]), 2.0) * + pow(sin(q[0 * 3 + 2]), 2.0) + + pow(cos(q[0 * 3 + 0]), 2.0) * pow(cos(q[0 * 3 + 1]), 2.0) * + pow(cos(q[0 * 3 + 2]), 2.0)) * + 1.0E+5) + + ((cos(q[0 * 3 + 1]) * sin(q[0 * 3 + 2]) + + cos(q[0 * 3 + 2]) * sin(q[0 * 3 + 1])) * + (pow(cos(q[0 * 3 + 0]), 2.0) * sin(q[0 * 3 + 1]) * 1.03E+2 + + pow(sin(q[0 * 3 + 0]), 2.0) * sin(q[0 * 3 + 1]) * 1.03E+2 + + RL * pow(cos(q[0 * 3 + 1]), 2.0) * sin(q[0 * 3 + 0]) * 3.5E+1 + + RL * sin(q[0 * 3 + 0]) * pow(sin(q[0 * 3 + 1]), 2.0) * 3.5E+1)) / + ((pow(cos(q[0 * 3 + 0]), 2.0) * pow(cos(q[0 * 3 + 1]), 2.0) + + pow(cos(q[0 * 3 + 0]), 2.0) * pow(sin(q[0 * 3 + 1]), 2.0) + + pow(cos(q[0 * 3 + 1]), 2.0) * pow(sin(q[0 * 3 + 0]), 2.0) + + pow(sin(q[0 * 3 + 0]), 2.0) * pow(sin(q[0 * 3 + 1]), 2.0)) * + (pow(cos(q[0 * 3 + 1]), 2.0) * pow(cos(q[0 * 3 + 2]), 2.0) + + pow(cos(q[0 * 3 + 1]), 2.0) * pow(sin(q[0 * 3 + 2]), 2.0) + + pow(cos(q[0 * 3 + 2]), 2.0) * pow(sin(q[0 * 3 + 1]), 2.0) + + pow(sin(q[0 * 3 + 1]), 2.0) * pow(sin(q[0 * 3 + 2]), 2.0)) * + 1.0E+3) + + (sin(q[0 * 3 + 0]) * + (sin(q[0 * 3 + 0]) * sin(q[0 * 3 + 1]) * sin(q[0 * 3 + 2]) * + 1.1315E+4 + + pow(cos(q[0 * 3 + 1]), 2.0) * cos(q[0 * 3 + 2]) * + sin(q[0 * 3 + 0]) * 1.03E+4 + + cos(q[0 * 3 + 2]) * sin(q[0 * 3 + 0]) * + pow(sin(q[0 * 3 + 1]), 2.0) * 1.03E+4 - + cos(q[0 * 3 + 1]) * cos(q[0 * 3 + 2]) * sin(q[0 * 3 + 0]) * + 1.1315E+4 + + RL * cos(q[0 * 3 + 0]) * cos(q[0 * 3 + 1]) * sin(q[0 * 3 + 2]) * + 5.049E+3 + + RL * cos(q[0 * 3 + 0]) * cos(q[0 * 3 + 2]) * sin(q[0 * 3 + 1]) * + 5.049E+3 + + RL * pow(cos(q[0 * 3 + 0]), 2.0) * cos(q[0 * 3 + 1]) * + sin(q[0 * 3 + 2]) * 3.5E+3 + + RL * pow(cos(q[0 * 3 + 0]), 2.0) * cos(q[0 * 3 + 2]) * + sin(q[0 * 3 + 1]) * 3.5E+3 + + RL * cos(q[0 * 3 + 1]) * pow(sin(q[0 * 3 + 0]), 2.0) * + sin(q[0 * 3 + 2]) * 3.5E+3 + + RL * cos(q[0 * 3 + 2]) * pow(sin(q[0 * 3 + 0]), 2.0) * + sin(q[0 * 3 + 1]) * 3.5E+3)) / + ((pow(cos(q[0 * 3 + 0]), 2.0) + pow(sin(q[0 * 3 + 0]), 2.0)) * + (pow(cos(q[0 * 3 + 0]), 2.0) * pow(cos(q[0 * 3 + 1]), 2.0) * + pow(sin(q[0 * 3 + 2]), 2.0) + + pow(cos(q[0 * 3 + 0]), 2.0) * pow(cos(q[0 * 3 + 2]), 2.0) * + pow(sin(q[0 * 3 + 1]), 2.0) + + pow(cos(q[0 * 3 + 1]), 2.0) * pow(cos(q[0 * 3 + 2]), 2.0) * + pow(sin(q[0 * 3 + 0]), 2.0) + + pow(cos(q[0 * 3 + 0]), 2.0) * pow(sin(q[0 * 3 + 1]), 2.0) * + pow(sin(q[0 * 3 + 2]), 2.0) + + pow(cos(q[0 * 3 + 1]), 2.0) * pow(sin(q[0 * 3 + 0]), 2.0) * + pow(sin(q[0 * 3 + 2]), 2.0) + + pow(cos(q[0 * 3 + 2]), 2.0) * pow(sin(q[0 * 3 + 0]), 2.0) * + pow(sin(q[0 * 3 + 1]), 2.0) + + pow(sin(q[0 * 3 + 0]), 2.0) * pow(sin(q[0 * 3 + 1]), 2.0) * + pow(sin(q[0 * 3 + 2]), 2.0) + + pow(cos(q[0 * 3 + 0]), 2.0) * pow(cos(q[0 * 3 + 1]), 2.0) * + pow(cos(q[0 * 3 + 2]), 2.0)) * + 1.0E+5) - + (pow(cos(q[0 * 3 + 0]), 2.0) * + (cos(q[0 * 3 + 1]) * cos(q[0 * 3 + 2]) - + sin(q[0 * 3 + 1]) * sin(q[0 * 3 + 2])) * + (cos(q[0 * 3 + 1]) * -2.06E+3 + + pow(cos(q[0 * 3 + 1]), 2.0) * 2.263E+3 + + pow(sin(q[0 * 3 + 1]), 2.0) * 2.263E+3)) / + ((pow(cos(q[0 * 3 + 0]), 2.0) * pow(cos(q[0 * 3 + 1]), 2.0) + + pow(cos(q[0 * 3 + 0]), 2.0) * pow(sin(q[0 * 3 + 1]), 2.0) + + pow(cos(q[0 * 3 + 1]), 2.0) * pow(sin(q[0 * 3 + 0]), 2.0) + + pow(sin(q[0 * 3 + 0]), 2.0) * pow(sin(q[0 * 3 + 1]), 2.0)) * + (pow(cos(q[0 * 3 + 0]), 2.0) * pow(cos(q[0 * 3 + 1]), 2.0) * + pow(sin(q[0 * 3 + 2]), 2.0) + + pow(cos(q[0 * 3 + 0]), 2.0) * pow(cos(q[0 * 3 + 2]), 2.0) * + pow(sin(q[0 * 3 + 1]), 2.0) + + pow(cos(q[0 * 3 + 1]), 2.0) * pow(cos(q[0 * 3 + 2]), 2.0) * + pow(sin(q[0 * 3 + 0]), 2.0) + + pow(cos(q[0 * 3 + 0]), 2.0) * pow(sin(q[0 * 3 + 1]), 2.0) * + pow(sin(q[0 * 3 + 2]), 2.0) + + pow(cos(q[0 * 3 + 1]), 2.0) * pow(sin(q[0 * 3 + 0]), 2.0) * + pow(sin(q[0 * 3 + 2]), 2.0) + + pow(cos(q[0 * 3 + 2]), 2.0) * pow(sin(q[0 * 3 + 0]), 2.0) * + pow(sin(q[0 * 3 + 1]), 2.0) + + pow(sin(q[0 * 3 + 0]), 2.0) * pow(sin(q[0 * 3 + 1]), 2.0) * + pow(sin(q[0 * 3 + 2]), 2.0) + + pow(cos(q[0 * 3 + 0]), 2.0) * pow(cos(q[0 * 3 + 1]), 2.0) * + pow(cos(q[0 * 3 + 2]), 2.0)) * + 2.0E+4) - + (pow(sin(q[0 * 3 + 0]), 2.0) * + (cos(q[0 * 3 + 1]) * cos(q[0 * 3 + 2]) - + sin(q[0 * 3 + 1]) * sin(q[0 * 3 + 2])) * + (cos(q[0 * 3 + 1]) * -2.06E+3 + + pow(cos(q[0 * 3 + 1]), 2.0) * 2.263E+3 + + pow(sin(q[0 * 3 + 1]), 2.0) * 2.263E+3)) / + ((pow(cos(q[0 * 3 + 0]), 2.0) * pow(cos(q[0 * 3 + 1]), 2.0) + + pow(cos(q[0 * 3 + 0]), 2.0) * pow(sin(q[0 * 3 + 1]), 2.0) + + pow(cos(q[0 * 3 + 1]), 2.0) * pow(sin(q[0 * 3 + 0]), 2.0) + + pow(sin(q[0 * 3 + 0]), 2.0) * pow(sin(q[0 * 3 + 1]), 2.0)) * + (pow(cos(q[0 * 3 + 0]), 2.0) * pow(cos(q[0 * 3 + 1]), 2.0) * + pow(sin(q[0 * 3 + 2]), 2.0) + + pow(cos(q[0 * 3 + 0]), 2.0) * pow(cos(q[0 * 3 + 2]), 2.0) * + pow(sin(q[0 * 3 + 1]), 2.0) + + pow(cos(q[0 * 3 + 1]), 2.0) * pow(cos(q[0 * 3 + 2]), 2.0) * + pow(sin(q[0 * 3 + 0]), 2.0) + + pow(cos(q[0 * 3 + 0]), 2.0) * pow(sin(q[0 * 3 + 1]), 2.0) * + pow(sin(q[0 * 3 + 2]), 2.0) + + pow(cos(q[0 * 3 + 1]), 2.0) * pow(sin(q[0 * 3 + 0]), 2.0) * + pow(sin(q[0 * 3 + 2]), 2.0) + + pow(cos(q[0 * 3 + 2]), 2.0) * pow(sin(q[0 * 3 + 0]), 2.0) * + pow(sin(q[0 * 3 + 1]), 2.0) + + pow(sin(q[0 * 3 + 0]), 2.0) * pow(sin(q[0 * 3 + 1]), 2.0) * + pow(sin(q[0 * 3 + 2]), 2.0) + + pow(cos(q[0 * 3 + 0]), 2.0) * pow(cos(q[0 * 3 + 1]), 2.0) * + pow(cos(q[0 * 3 + 2]), 2.0)) * + 2.0E+4) - + 1.03E+2 / 5.0E+2)) / + (pow(cos(q[0 * 3 + 0]), 2.0) * pow(cos(q[0 * 3 + 1]), 2.0) * + pow(sin(q[0 * 3 + 2]), 2.0) + + pow(cos(q[0 * 3 + 0]), 2.0) * pow(cos(q[0 * 3 + 2]), 2.0) * + pow(sin(q[0 * 3 + 1]), 2.0) + + pow(cos(q[0 * 3 + 1]), 2.0) * pow(cos(q[0 * 3 + 2]), 2.0) * + pow(sin(q[0 * 3 + 0]), 2.0) + + pow(cos(q[0 * 3 + 0]), 2.0) * pow(sin(q[0 * 3 + 1]), 2.0) * + pow(sin(q[0 * 3 + 2]), 2.0) + + pow(cos(q[0 * 3 + 1]), 2.0) * pow(sin(q[0 * 3 + 0]), 2.0) * + pow(sin(q[0 * 3 + 2]), 2.0) + + pow(cos(q[0 * 3 + 2]), 2.0) * pow(sin(q[0 * 3 + 0]), 2.0) * + pow(sin(q[0 * 3 + 1]), 2.0) + + pow(sin(q[0 * 3 + 0]), 2.0) * pow(sin(q[0 * 3 + 1]), 2.0) * + pow(sin(q[0 * 3 + 2]), 2.0) + + pow(cos(q[0 * 3 + 0]), 2.0) * pow(cos(q[0 * 3 + 1]), 2.0) * + pow(cos(q[0 * 3 + 2]), 2.0)) + + (cos(q[0 * 3 + 0]) * + (cos(q[0 * 3 + 1]) * cos(q[0 * 3 + 2]) - + sin(q[0 * 3 + 1]) * sin(q[0 * 3 + 2])) * + ((sin(q[0 * 3 + 0]) * + (pow(cos(q[0 * 3 + 1]), 2.0) * sin(q[0 * 3 + 0]) * + sin(q[0 * 3 + 2]) * 1.03E+4 + + sin(q[0 * 3 + 0]) * pow(sin(q[0 * 3 + 1]), 2.0) * + sin(q[0 * 3 + 2]) * 1.03E+4 - + cos(q[0 * 3 + 1]) * sin(q[0 * 3 + 0]) * sin(q[0 * 3 + 2]) * + 1.1315E+4 - + cos(q[0 * 3 + 2]) * sin(q[0 * 3 + 0]) * sin(q[0 * 3 + 1]) * + 1.1315E+4 - + RL * cos(q[0 * 3 + 0]) * cos(q[0 * 3 + 1]) * cos(q[0 * 3 + 2]) * + 5.049E+3 + + RL * cos(q[0 * 3 + 0]) * sin(q[0 * 3 + 1]) * sin(q[0 * 3 + 2]) * + 5.049E+3 - + RL * pow(cos(q[0 * 3 + 0]), 2.0) * cos(q[0 * 3 + 1]) * + cos(q[0 * 3 + 2]) * 3.5E+3 - + RL * cos(q[0 * 3 + 1]) * cos(q[0 * 3 + 2]) * + pow(sin(q[0 * 3 + 0]), 2.0) * 3.5E+3 + + RL * pow(cos(q[0 * 3 + 0]), 2.0) * sin(q[0 * 3 + 1]) * + sin(q[0 * 3 + 2]) * 3.5E+3 + + RL * pow(sin(q[0 * 3 + 0]), 2.0) * sin(q[0 * 3 + 1]) * + sin(q[0 * 3 + 2]) * 3.5E+3) * + (-1.0E-5)) / + ((pow(cos(q[0 * 3 + 0]), 2.0) + pow(sin(q[0 * 3 + 0]), 2.0)) * + (pow(cos(q[0 * 3 + 0]), 2.0) * pow(cos(q[0 * 3 + 1]), 2.0) * + pow(sin(q[0 * 3 + 2]), 2.0) + + pow(cos(q[0 * 3 + 0]), 2.0) * pow(cos(q[0 * 3 + 2]), 2.0) * + pow(sin(q[0 * 3 + 1]), 2.0) + + pow(cos(q[0 * 3 + 1]), 2.0) * pow(cos(q[0 * 3 + 2]), 2.0) * + pow(sin(q[0 * 3 + 0]), 2.0) + + pow(cos(q[0 * 3 + 0]), 2.0) * pow(sin(q[0 * 3 + 1]), 2.0) * + pow(sin(q[0 * 3 + 2]), 2.0) + + pow(cos(q[0 * 3 + 1]), 2.0) * pow(sin(q[0 * 3 + 0]), 2.0) * + pow(sin(q[0 * 3 + 2]), 2.0) + + pow(cos(q[0 * 3 + 2]), 2.0) * pow(sin(q[0 * 3 + 0]), 2.0) * + pow(sin(q[0 * 3 + 1]), 2.0) + + pow(sin(q[0 * 3 + 0]), 2.0) * pow(sin(q[0 * 3 + 1]), 2.0) * + pow(sin(q[0 * 3 + 2]), 2.0) + + pow(cos(q[0 * 3 + 0]), 2.0) * pow(cos(q[0 * 3 + 1]), 2.0) * + pow(cos(q[0 * 3 + 2]), 2.0))) + + ((cos(q[0 * 3 + 1]) * cos(q[0 * 3 + 2]) - + sin(q[0 * 3 + 1]) * sin(q[0 * 3 + 2])) * + (pow(cos(q[0 * 3 + 0]), 2.0) * sin(q[0 * 3 + 1]) * 1.03E+2 + + pow(sin(q[0 * 3 + 0]), 2.0) * sin(q[0 * 3 + 1]) * 1.03E+2 + + RL * pow(cos(q[0 * 3 + 1]), 2.0) * sin(q[0 * 3 + 0]) * 3.5E+1 + + RL * sin(q[0 * 3 + 0]) * pow(sin(q[0 * 3 + 1]), 2.0) * 3.5E+1)) / + ((pow(cos(q[0 * 3 + 0]), 2.0) * pow(cos(q[0 * 3 + 1]), 2.0) + + pow(cos(q[0 * 3 + 0]), 2.0) * pow(sin(q[0 * 3 + 1]), 2.0) + + pow(cos(q[0 * 3 + 1]), 2.0) * pow(sin(q[0 * 3 + 0]), 2.0) + + pow(sin(q[0 * 3 + 0]), 2.0) * pow(sin(q[0 * 3 + 1]), 2.0)) * + (pow(cos(q[0 * 3 + 1]), 2.0) * pow(cos(q[0 * 3 + 2]), 2.0) + + pow(cos(q[0 * 3 + 1]), 2.0) * pow(sin(q[0 * 3 + 2]), 2.0) + + pow(cos(q[0 * 3 + 2]), 2.0) * pow(sin(q[0 * 3 + 1]), 2.0) + + pow(sin(q[0 * 3 + 1]), 2.0) * pow(sin(q[0 * 3 + 2]), 2.0)) * + 1.0E+3) - + (cos(q[0 * 3 + 0]) * + (cos(q[0 * 3 + 0]) * pow(cos(q[0 * 3 + 1]), 2.0) * + sin(q[0 * 3 + 2]) * 1.03E+4 + + cos(q[0 * 3 + 0]) * pow(sin(q[0 * 3 + 1]), 2.0) * + sin(q[0 * 3 + 2]) * 1.03E+4 - + cos(q[0 * 3 + 0]) * cos(q[0 * 3 + 1]) * sin(q[0 * 3 + 2]) * + 1.1315E+4 - + cos(q[0 * 3 + 0]) * cos(q[0 * 3 + 2]) * sin(q[0 * 3 + 1]) * + 1.1315E+4 + + RL * cos(q[0 * 3 + 1]) * cos(q[0 * 3 + 2]) * sin(q[0 * 3 + 0]) * + 5.049E+3 - + RL * sin(q[0 * 3 + 0]) * sin(q[0 * 3 + 1]) * sin(q[0 * 3 + 2]) * + 5.049E+3)) / + ((pow(cos(q[0 * 3 + 0]), 2.0) + pow(sin(q[0 * 3 + 0]), 2.0)) * + (pow(cos(q[0 * 3 + 0]), 2.0) * pow(cos(q[0 * 3 + 1]), 2.0) * + pow(sin(q[0 * 3 + 2]), 2.0) + + pow(cos(q[0 * 3 + 0]), 2.0) * pow(cos(q[0 * 3 + 2]), 2.0) * + pow(sin(q[0 * 3 + 1]), 2.0) + + pow(cos(q[0 * 3 + 1]), 2.0) * pow(cos(q[0 * 3 + 2]), 2.0) * + pow(sin(q[0 * 3 + 0]), 2.0) + + pow(cos(q[0 * 3 + 0]), 2.0) * pow(sin(q[0 * 3 + 1]), 2.0) * + pow(sin(q[0 * 3 + 2]), 2.0) + + pow(cos(q[0 * 3 + 1]), 2.0) * pow(sin(q[0 * 3 + 0]), 2.0) * + pow(sin(q[0 * 3 + 2]), 2.0) + + pow(cos(q[0 * 3 + 2]), 2.0) * pow(sin(q[0 * 3 + 0]), 2.0) * + pow(sin(q[0 * 3 + 1]), 2.0) + + pow(sin(q[0 * 3 + 0]), 2.0) * pow(sin(q[0 * 3 + 1]), 2.0) * + pow(sin(q[0 * 3 + 2]), 2.0) + + pow(cos(q[0 * 3 + 0]), 2.0) * pow(cos(q[0 * 3 + 1]), 2.0) * + pow(cos(q[0 * 3 + 2]), 2.0)) * + 1.0E+5) + + (pow(cos(q[0 * 3 + 0]), 2.0) * + (cos(q[0 * 3 + 1]) * sin(q[0 * 3 + 2]) + + cos(q[0 * 3 + 2]) * sin(q[0 * 3 + 1])) * + (cos(q[0 * 3 + 1]) * -2.06E+3 + + pow(cos(q[0 * 3 + 1]), 2.0) * 2.263E+3 + + pow(sin(q[0 * 3 + 1]), 2.0) * 2.263E+3)) / + ((pow(cos(q[0 * 3 + 0]), 2.0) * pow(cos(q[0 * 3 + 1]), 2.0) + + pow(cos(q[0 * 3 + 0]), 2.0) * pow(sin(q[0 * 3 + 1]), 2.0) + + pow(cos(q[0 * 3 + 1]), 2.0) * pow(sin(q[0 * 3 + 0]), 2.0) + + pow(sin(q[0 * 3 + 0]), 2.0) * pow(sin(q[0 * 3 + 1]), 2.0)) * + (pow(cos(q[0 * 3 + 0]), 2.0) * pow(cos(q[0 * 3 + 1]), 2.0) * + pow(sin(q[0 * 3 + 2]), 2.0) + + pow(cos(q[0 * 3 + 0]), 2.0) * pow(cos(q[0 * 3 + 2]), 2.0) * + pow(sin(q[0 * 3 + 1]), 2.0) + + pow(cos(q[0 * 3 + 1]), 2.0) * pow(cos(q[0 * 3 + 2]), 2.0) * + pow(sin(q[0 * 3 + 0]), 2.0) + + pow(cos(q[0 * 3 + 0]), 2.0) * pow(sin(q[0 * 3 + 1]), 2.0) * + pow(sin(q[0 * 3 + 2]), 2.0) + + pow(cos(q[0 * 3 + 1]), 2.0) * pow(sin(q[0 * 3 + 0]), 2.0) * + pow(sin(q[0 * 3 + 2]), 2.0) + + pow(cos(q[0 * 3 + 2]), 2.0) * pow(sin(q[0 * 3 + 0]), 2.0) * + pow(sin(q[0 * 3 + 1]), 2.0) + + pow(sin(q[0 * 3 + 0]), 2.0) * pow(sin(q[0 * 3 + 1]), 2.0) * + pow(sin(q[0 * 3 + 2]), 2.0) + + pow(cos(q[0 * 3 + 0]), 2.0) * pow(cos(q[0 * 3 + 1]), 2.0) * + pow(cos(q[0 * 3 + 2]), 2.0)) * + 2.0E+4) + + (pow(sin(q[0 * 3 + 0]), 2.0) * + (cos(q[0 * 3 + 1]) * sin(q[0 * 3 + 2]) + + cos(q[0 * 3 + 2]) * sin(q[0 * 3 + 1])) * + (cos(q[0 * 3 + 1]) * -2.06E+3 + + pow(cos(q[0 * 3 + 1]), 2.0) * 2.263E+3 + + pow(sin(q[0 * 3 + 1]), 2.0) * 2.263E+3)) / + ((pow(cos(q[0 * 3 + 0]), 2.0) * pow(cos(q[0 * 3 + 1]), 2.0) + + pow(cos(q[0 * 3 + 0]), 2.0) * pow(sin(q[0 * 3 + 1]), 2.0) + + pow(cos(q[0 * 3 + 1]), 2.0) * pow(sin(q[0 * 3 + 0]), 2.0) + + pow(sin(q[0 * 3 + 0]), 2.0) * pow(sin(q[0 * 3 + 1]), 2.0)) * + (pow(cos(q[0 * 3 + 0]), 2.0) * pow(cos(q[0 * 3 + 1]), 2.0) * + pow(sin(q[0 * 3 + 2]), 2.0) + + pow(cos(q[0 * 3 + 0]), 2.0) * pow(cos(q[0 * 3 + 2]), 2.0) * + pow(sin(q[0 * 3 + 1]), 2.0) + + pow(cos(q[0 * 3 + 1]), 2.0) * pow(cos(q[0 * 3 + 2]), 2.0) * + pow(sin(q[0 * 3 + 0]), 2.0) + + pow(cos(q[0 * 3 + 0]), 2.0) * pow(sin(q[0 * 3 + 1]), 2.0) * + pow(sin(q[0 * 3 + 2]), 2.0) + + pow(cos(q[0 * 3 + 1]), 2.0) * pow(sin(q[0 * 3 + 0]), 2.0) * + pow(sin(q[0 * 3 + 2]), 2.0) + + pow(cos(q[0 * 3 + 2]), 2.0) * pow(sin(q[0 * 3 + 0]), 2.0) * + pow(sin(q[0 * 3 + 1]), 2.0) + + pow(sin(q[0 * 3 + 0]), 2.0) * pow(sin(q[0 * 3 + 1]), 2.0) * + pow(sin(q[0 * 3 + 2]), 2.0) + + pow(cos(q[0 * 3 + 0]), 2.0) * pow(cos(q[0 * 3 + 1]), 2.0) * + pow(cos(q[0 * 3 + 2]), 2.0)) * + 2.0E+4))) / + (pow(cos(q[0 * 3 + 0]), 2.0) * pow(cos(q[0 * 3 + 1]), 2.0) * + pow(sin(q[0 * 3 + 2]), 2.0) + + pow(cos(q[0 * 3 + 0]), 2.0) * pow(cos(q[0 * 3 + 2]), 2.0) * + pow(sin(q[0 * 3 + 1]), 2.0) + + pow(cos(q[0 * 3 + 1]), 2.0) * pow(cos(q[0 * 3 + 2]), 2.0) * + pow(sin(q[0 * 3 + 0]), 2.0) + + pow(cos(q[0 * 3 + 0]), 2.0) * pow(sin(q[0 * 3 + 1]), 2.0) * + pow(sin(q[0 * 3 + 2]), 2.0) + + pow(cos(q[0 * 3 + 1]), 2.0) * pow(sin(q[0 * 3 + 0]), 2.0) * + pow(sin(q[0 * 3 + 2]), 2.0) + + pow(cos(q[0 * 3 + 2]), 2.0) * pow(sin(q[0 * 3 + 0]), 2.0) * + pow(sin(q[0 * 3 + 1]), 2.0) + + pow(sin(q[0 * 3 + 0]), 2.0) * pow(sin(q[0 * 3 + 1]), 2.0) * + pow(sin(q[0 * 3 + 2]), 2.0) + + pow(cos(q[0 * 3 + 0]), 2.0) * pow(cos(q[0 * 3 + 1]), 2.0) * + pow(cos(q[0 * 3 + 2]), 2.0)))) / + (pow(cos(q[0 * 3 + 0]), 2.0) + pow(sin(q[0 * 3 + 0]), 2.0)) + + (sin(q[0 * 3 + 0]) * + ((cos(q[0 * 3 + 0]) * + ((cos(q[0 * 3 + 0]) * + (cos(q[0 * 3 + 1]) * sin(q[0 * 3 + 2]) + + cos(q[0 * 3 + 2]) * sin(q[0 * 3 + 1])) * + 1.0 / + pow(pow(cos(q[0 * 3 + 0]), 2.0) * pow(cos(q[0 * 3 + 1]), 2.0) * + pow(sin(q[0 * 3 + 2]), 2.0) + + pow(cos(q[0 * 3 + 0]), 2.0) * pow(cos(q[0 * 3 + 2]), 2.0) * + pow(sin(q[0 * 3 + 1]), 2.0) + + pow(cos(q[0 * 3 + 1]), 2.0) * pow(cos(q[0 * 3 + 2]), 2.0) * + pow(sin(q[0 * 3 + 0]), 2.0) + + pow(cos(q[0 * 3 + 0]), 2.0) * pow(sin(q[0 * 3 + 1]), 2.0) * + pow(sin(q[0 * 3 + 2]), 2.0) + + pow(cos(q[0 * 3 + 1]), 2.0) * pow(sin(q[0 * 3 + 0]), 2.0) * + pow(sin(q[0 * 3 + 2]), 2.0) + + pow(cos(q[0 * 3 + 2]), 2.0) * pow(sin(q[0 * 3 + 0]), 2.0) * + pow(sin(q[0 * 3 + 1]), 2.0) + + pow(sin(q[0 * 3 + 0]), 2.0) * pow(sin(q[0 * 3 + 1]), 2.0) * + pow(sin(q[0 * 3 + 2]), 2.0) + + pow(cos(q[0 * 3 + 0]), 2.0) * pow(cos(q[0 * 3 + 1]), 2.0) * + pow(cos(q[0 * 3 + 2]), 2.0), + 2.0) * + (sin(q[0 * 3 + 0]) * sin(q[0 * 3 + 1]) * sin(q[0 * 3 + 2]) * + 1.1315E+4 + + pow(cos(q[0 * 3 + 1]), 2.0) * cos(q[0 * 3 + 2]) * + sin(q[0 * 3 + 0]) * 1.03E+4 + + cos(q[0 * 3 + 2]) * sin(q[0 * 3 + 0]) * + pow(sin(q[0 * 3 + 1]), 2.0) * 1.03E+4 - + cos(q[0 * 3 + 1]) * cos(q[0 * 3 + 2]) * sin(q[0 * 3 + 0]) * + 1.1315E+4 + + RL * cos(q[0 * 3 + 0]) * cos(q[0 * 3 + 1]) * sin(q[0 * 3 + 2]) * + 5.049E+3 + + RL * cos(q[0 * 3 + 0]) * cos(q[0 * 3 + 2]) * sin(q[0 * 3 + 1]) * + 5.049E+3 + + RL * pow(cos(q[0 * 3 + 0]), 2.0) * cos(q[0 * 3 + 1]) * + sin(q[0 * 3 + 2]) * 3.5E+3 + + RL * pow(cos(q[0 * 3 + 0]), 2.0) * cos(q[0 * 3 + 2]) * + sin(q[0 * 3 + 1]) * 3.5E+3 + + RL * cos(q[0 * 3 + 1]) * pow(sin(q[0 * 3 + 0]), 2.0) * + sin(q[0 * 3 + 2]) * 3.5E+3 + + RL * cos(q[0 * 3 + 2]) * pow(sin(q[0 * 3 + 0]), 2.0) * + sin(q[0 * 3 + 1]) * 3.5E+3)) / + 1.0E+5 - + (cos(q[0 * 3 + 0]) * + (cos(q[0 * 3 + 1]) * cos(q[0 * 3 + 2]) - + sin(q[0 * 3 + 1]) * sin(q[0 * 3 + 2])) * + 1.0 / + pow(pow(cos(q[0 * 3 + 0]), 2.0) * pow(cos(q[0 * 3 + 1]), 2.0) * + pow(sin(q[0 * 3 + 2]), 2.0) + + pow(cos(q[0 * 3 + 0]), 2.0) * pow(cos(q[0 * 3 + 2]), 2.0) * + pow(sin(q[0 * 3 + 1]), 2.0) + + pow(cos(q[0 * 3 + 1]), 2.0) * pow(cos(q[0 * 3 + 2]), 2.0) * + pow(sin(q[0 * 3 + 0]), 2.0) + + pow(cos(q[0 * 3 + 0]), 2.0) * pow(sin(q[0 * 3 + 1]), 2.0) * + pow(sin(q[0 * 3 + 2]), 2.0) + + pow(cos(q[0 * 3 + 1]), 2.0) * pow(sin(q[0 * 3 + 0]), 2.0) * + pow(sin(q[0 * 3 + 2]), 2.0) + + pow(cos(q[0 * 3 + 2]), 2.0) * pow(sin(q[0 * 3 + 0]), 2.0) * + pow(sin(q[0 * 3 + 1]), 2.0) + + pow(sin(q[0 * 3 + 0]), 2.0) * pow(sin(q[0 * 3 + 1]), 2.0) * + pow(sin(q[0 * 3 + 2]), 2.0) + + pow(cos(q[0 * 3 + 0]), 2.0) * pow(cos(q[0 * 3 + 1]), 2.0) * + pow(cos(q[0 * 3 + 2]), 2.0), + 2.0) * + (pow(cos(q[0 * 3 + 1]), 2.0) * sin(q[0 * 3 + 0]) * + sin(q[0 * 3 + 2]) * 1.03E+4 + + sin(q[0 * 3 + 0]) * pow(sin(q[0 * 3 + 1]), 2.0) * + sin(q[0 * 3 + 2]) * 1.03E+4 - + cos(q[0 * 3 + 1]) * sin(q[0 * 3 + 0]) * sin(q[0 * 3 + 2]) * + 1.1315E+4 - + cos(q[0 * 3 + 2]) * sin(q[0 * 3 + 0]) * sin(q[0 * 3 + 1]) * + 1.1315E+4 - + RL * cos(q[0 * 3 + 0]) * cos(q[0 * 3 + 1]) * cos(q[0 * 3 + 2]) * + 5.049E+3 + + RL * cos(q[0 * 3 + 0]) * sin(q[0 * 3 + 1]) * sin(q[0 * 3 + 2]) * + 5.049E+3 - + RL * pow(cos(q[0 * 3 + 0]), 2.0) * cos(q[0 * 3 + 1]) * + cos(q[0 * 3 + 2]) * 3.5E+3 - + RL * cos(q[0 * 3 + 1]) * cos(q[0 * 3 + 2]) * + pow(sin(q[0 * 3 + 0]), 2.0) * 3.5E+3 + + RL * pow(cos(q[0 * 3 + 0]), 2.0) * sin(q[0 * 3 + 1]) * + sin(q[0 * 3 + 2]) * 3.5E+3 + + RL * pow(sin(q[0 * 3 + 0]), 2.0) * sin(q[0 * 3 + 1]) * + sin(q[0 * 3 + 2]) * 3.5E+3)) / + 1.0E+5 - + (sin(q[0 * 3 + 0]) * + (cos(q[0 * 3 + 1]) * sin(q[0 * 3 + 2]) + + cos(q[0 * 3 + 2]) * sin(q[0 * 3 + 1])) * + (cos(q[0 * 3 + 0]) * pow(cos(q[0 * 3 + 1]), 2.0) * + cos(q[0 * 3 + 2]) * 1.03E+4 + + cos(q[0 * 3 + 0]) * cos(q[0 * 3 + 2]) * + pow(sin(q[0 * 3 + 1]), 2.0) * 1.03E+4 - + cos(q[0 * 3 + 0]) * cos(q[0 * 3 + 1]) * cos(q[0 * 3 + 2]) * + 1.1315E+4 + + cos(q[0 * 3 + 0]) * sin(q[0 * 3 + 1]) * sin(q[0 * 3 + 2]) * + 1.1315E+4 - + RL * cos(q[0 * 3 + 1]) * sin(q[0 * 3 + 0]) * sin(q[0 * 3 + 2]) * + 5.049E+3 - + RL * cos(q[0 * 3 + 2]) * sin(q[0 * 3 + 0]) * sin(q[0 * 3 + 1]) * + 5.049E+3) * + 1.0 / + pow(pow(cos(q[0 * 3 + 0]), 2.0) * pow(cos(q[0 * 3 + 1]), 2.0) * + pow(sin(q[0 * 3 + 2]), 2.0) + + pow(cos(q[0 * 3 + 0]), 2.0) * pow(cos(q[0 * 3 + 2]), 2.0) * + pow(sin(q[0 * 3 + 1]), 2.0) + + pow(cos(q[0 * 3 + 1]), 2.0) * pow(cos(q[0 * 3 + 2]), 2.0) * + pow(sin(q[0 * 3 + 0]), 2.0) + + pow(cos(q[0 * 3 + 0]), 2.0) * pow(sin(q[0 * 3 + 1]), 2.0) * + pow(sin(q[0 * 3 + 2]), 2.0) + + pow(cos(q[0 * 3 + 1]), 2.0) * pow(sin(q[0 * 3 + 0]), 2.0) * + pow(sin(q[0 * 3 + 2]), 2.0) + + pow(cos(q[0 * 3 + 2]), 2.0) * pow(sin(q[0 * 3 + 0]), 2.0) * + pow(sin(q[0 * 3 + 1]), 2.0) + + pow(sin(q[0 * 3 + 0]), 2.0) * pow(sin(q[0 * 3 + 1]), 2.0) * + pow(sin(q[0 * 3 + 2]), 2.0) + + pow(cos(q[0 * 3 + 0]), 2.0) * pow(cos(q[0 * 3 + 1]), 2.0) * + pow(cos(q[0 * 3 + 2]), 2.0), + 2.0)) / + 1.0E+5 + + (sin(q[0 * 3 + 0]) * + (cos(q[0 * 3 + 1]) * cos(q[0 * 3 + 2]) - + sin(q[0 * 3 + 1]) * sin(q[0 * 3 + 2])) * + (cos(q[0 * 3 + 0]) * pow(cos(q[0 * 3 + 1]), 2.0) * + sin(q[0 * 3 + 2]) * 1.03E+4 + + cos(q[0 * 3 + 0]) * pow(sin(q[0 * 3 + 1]), 2.0) * + sin(q[0 * 3 + 2]) * 1.03E+4 - + cos(q[0 * 3 + 0]) * cos(q[0 * 3 + 1]) * sin(q[0 * 3 + 2]) * + 1.1315E+4 - + cos(q[0 * 3 + 0]) * cos(q[0 * 3 + 2]) * sin(q[0 * 3 + 1]) * + 1.1315E+4 + + RL * cos(q[0 * 3 + 1]) * cos(q[0 * 3 + 2]) * sin(q[0 * 3 + 0]) * + 5.049E+3 - + RL * sin(q[0 * 3 + 0]) * sin(q[0 * 3 + 1]) * sin(q[0 * 3 + 2]) * + 5.049E+3) * + 1.0 / + pow(pow(cos(q[0 * 3 + 0]), 2.0) * pow(cos(q[0 * 3 + 1]), 2.0) * + pow(sin(q[0 * 3 + 2]), 2.0) + + pow(cos(q[0 * 3 + 0]), 2.0) * pow(cos(q[0 * 3 + 2]), 2.0) * + pow(sin(q[0 * 3 + 1]), 2.0) + + pow(cos(q[0 * 3 + 1]), 2.0) * pow(cos(q[0 * 3 + 2]), 2.0) * + pow(sin(q[0 * 3 + 0]), 2.0) + + pow(cos(q[0 * 3 + 0]), 2.0) * pow(sin(q[0 * 3 + 1]), 2.0) * + pow(sin(q[0 * 3 + 2]), 2.0) + + pow(cos(q[0 * 3 + 1]), 2.0) * pow(sin(q[0 * 3 + 0]), 2.0) * + pow(sin(q[0 * 3 + 2]), 2.0) + + pow(cos(q[0 * 3 + 2]), 2.0) * pow(sin(q[0 * 3 + 0]), 2.0) * + pow(sin(q[0 * 3 + 1]), 2.0) + + pow(sin(q[0 * 3 + 0]), 2.0) * pow(sin(q[0 * 3 + 1]), 2.0) * + pow(sin(q[0 * 3 + 2]), 2.0) + + pow(cos(q[0 * 3 + 0]), 2.0) * pow(cos(q[0 * 3 + 1]), 2.0) * + pow(cos(q[0 * 3 + 2]), 2.0), + 2.0)) / + 1.0E+5 + + ((cos(q[0 * 3 + 1]) * sin(q[0 * 3 + 2]) + + cos(q[0 * 3 + 2]) * sin(q[0 * 3 + 1])) * + (RL * cos(q[0 * 3 + 0]) * cos(q[0 * 3 + 1]) * sin(q[0 * 3 + 2]) * + 3.5E+3 + + RL * cos(q[0 * 3 + 0]) * cos(q[0 * 3 + 2]) * sin(q[0 * 3 + 1]) * + 3.5E+3 + + RL * pow(cos(q[0 * 3 + 0]), 2.0) * cos(q[0 * 3 + 1]) * + sin(q[0 * 3 + 2]) * 5.049E+3 + + RL * pow(cos(q[0 * 3 + 0]), 2.0) * cos(q[0 * 3 + 2]) * + sin(q[0 * 3 + 1]) * 5.049E+3 + + RL * cos(q[0 * 3 + 1]) * pow(sin(q[0 * 3 + 0]), 2.0) * + sin(q[0 * 3 + 2]) * 5.049E+3 + + RL * cos(q[0 * 3 + 2]) * pow(sin(q[0 * 3 + 0]), 2.0) * + sin(q[0 * 3 + 1]) * 5.049E+3)) / + ((pow(cos(q[0 * 3 + 1]), 2.0) * pow(cos(q[0 * 3 + 2]), 2.0) + + pow(cos(q[0 * 3 + 1]), 2.0) * pow(sin(q[0 * 3 + 2]), 2.0) + + pow(cos(q[0 * 3 + 2]), 2.0) * pow(sin(q[0 * 3 + 1]), 2.0) + + pow(sin(q[0 * 3 + 1]), 2.0) * pow(sin(q[0 * 3 + 2]), 2.0)) * + (pow(cos(q[0 * 3 + 0]), 2.0) * pow(cos(q[0 * 3 + 1]), 2.0) * + pow(sin(q[0 * 3 + 2]), 2.0) + + pow(cos(q[0 * 3 + 0]), 2.0) * pow(cos(q[0 * 3 + 2]), 2.0) * + pow(sin(q[0 * 3 + 1]), 2.0) + + pow(cos(q[0 * 3 + 1]), 2.0) * pow(cos(q[0 * 3 + 2]), 2.0) * + pow(sin(q[0 * 3 + 0]), 2.0) + + pow(cos(q[0 * 3 + 0]), 2.0) * pow(sin(q[0 * 3 + 1]), 2.0) * + pow(sin(q[0 * 3 + 2]), 2.0) + + pow(cos(q[0 * 3 + 1]), 2.0) * pow(sin(q[0 * 3 + 0]), 2.0) * + pow(sin(q[0 * 3 + 2]), 2.0) + + pow(cos(q[0 * 3 + 2]), 2.0) * pow(sin(q[0 * 3 + 0]), 2.0) * + pow(sin(q[0 * 3 + 1]), 2.0) + + pow(sin(q[0 * 3 + 0]), 2.0) * pow(sin(q[0 * 3 + 1]), 2.0) * + pow(sin(q[0 * 3 + 2]), 2.0) + + pow(cos(q[0 * 3 + 0]), 2.0) * pow(cos(q[0 * 3 + 1]), 2.0) * + pow(cos(q[0 * 3 + 2]), 2.0)) * + 1.0E+5) + + ((cos(q[0 * 3 + 1]) * cos(q[0 * 3 + 2]) - + sin(q[0 * 3 + 1]) * sin(q[0 * 3 + 2])) * + (RL * cos(q[0 * 3 + 0]) * cos(q[0 * 3 + 1]) * cos(q[0 * 3 + 2]) * + 3.5E+3 - + RL * cos(q[0 * 3 + 0]) * sin(q[0 * 3 + 1]) * sin(q[0 * 3 + 2]) * + 3.5E+3 + + RL * pow(cos(q[0 * 3 + 0]), 2.0) * cos(q[0 * 3 + 1]) * + cos(q[0 * 3 + 2]) * 5.049E+3 + + RL * cos(q[0 * 3 + 1]) * cos(q[0 * 3 + 2]) * + pow(sin(q[0 * 3 + 0]), 2.0) * 5.049E+3 - + RL * pow(cos(q[0 * 3 + 0]), 2.0) * sin(q[0 * 3 + 1]) * + sin(q[0 * 3 + 2]) * 5.049E+3 - + RL * pow(sin(q[0 * 3 + 0]), 2.0) * sin(q[0 * 3 + 1]) * + sin(q[0 * 3 + 2]) * 5.049E+3)) / + ((pow(cos(q[0 * 3 + 1]), 2.0) * pow(cos(q[0 * 3 + 2]), 2.0) + + pow(cos(q[0 * 3 + 1]), 2.0) * pow(sin(q[0 * 3 + 2]), 2.0) + + pow(cos(q[0 * 3 + 2]), 2.0) * pow(sin(q[0 * 3 + 1]), 2.0) + + pow(sin(q[0 * 3 + 1]), 2.0) * pow(sin(q[0 * 3 + 2]), 2.0)) * + (pow(cos(q[0 * 3 + 0]), 2.0) * pow(cos(q[0 * 3 + 1]), 2.0) * + pow(sin(q[0 * 3 + 2]), 2.0) + + pow(cos(q[0 * 3 + 0]), 2.0) * pow(cos(q[0 * 3 + 2]), 2.0) * + pow(sin(q[0 * 3 + 1]), 2.0) + + pow(cos(q[0 * 3 + 1]), 2.0) * pow(cos(q[0 * 3 + 2]), 2.0) * + pow(sin(q[0 * 3 + 0]), 2.0) + + pow(cos(q[0 * 3 + 0]), 2.0) * pow(sin(q[0 * 3 + 1]), 2.0) * + pow(sin(q[0 * 3 + 2]), 2.0) + + pow(cos(q[0 * 3 + 1]), 2.0) * pow(sin(q[0 * 3 + 0]), 2.0) * + pow(sin(q[0 * 3 + 2]), 2.0) + + pow(cos(q[0 * 3 + 2]), 2.0) * pow(sin(q[0 * 3 + 0]), 2.0) * + pow(sin(q[0 * 3 + 1]), 2.0) + + pow(sin(q[0 * 3 + 0]), 2.0) * pow(sin(q[0 * 3 + 1]), 2.0) * + pow(sin(q[0 * 3 + 2]), 2.0) + + pow(cos(q[0 * 3 + 0]), 2.0) * pow(cos(q[0 * 3 + 1]), 2.0) * + pow(cos(q[0 * 3 + 2]), 2.0)) * + 1.0E+5))) / + (pow(cos(q[0 * 3 + 0]), 2.0) + pow(sin(q[0 * 3 + 0]), 2.0)) + + (sin(q[0 * 3 + 0]) * + (cos(q[0 * 3 + 1]) * sin(q[0 * 3 + 2]) + + cos(q[0 * 3 + 2]) * sin(q[0 * 3 + 1])) * + ((cos(q[0 * 3 + 0]) * + (cos(q[0 * 3 + 0]) * pow(cos(q[0 * 3 + 1]), 2.0) * + cos(q[0 * 3 + 2]) * 1.03E+4 + + cos(q[0 * 3 + 0]) * cos(q[0 * 3 + 2]) * + pow(sin(q[0 * 3 + 1]), 2.0) * 1.03E+4 - + cos(q[0 * 3 + 0]) * cos(q[0 * 3 + 1]) * cos(q[0 * 3 + 2]) * + 1.1315E+4 + + cos(q[0 * 3 + 0]) * sin(q[0 * 3 + 1]) * sin(q[0 * 3 + 2]) * + 1.1315E+4 - + RL * cos(q[0 * 3 + 1]) * sin(q[0 * 3 + 0]) * sin(q[0 * 3 + 2]) * + 5.049E+3 - + RL * cos(q[0 * 3 + 2]) * sin(q[0 * 3 + 0]) * sin(q[0 * 3 + 1]) * + 5.049E+3)) / + ((pow(cos(q[0 * 3 + 0]), 2.0) + pow(sin(q[0 * 3 + 0]), 2.0)) * + (pow(cos(q[0 * 3 + 0]), 2.0) * pow(cos(q[0 * 3 + 1]), 2.0) * + pow(sin(q[0 * 3 + 2]), 2.0) + + pow(cos(q[0 * 3 + 0]), 2.0) * pow(cos(q[0 * 3 + 2]), 2.0) * + pow(sin(q[0 * 3 + 1]), 2.0) + + pow(cos(q[0 * 3 + 1]), 2.0) * pow(cos(q[0 * 3 + 2]), 2.0) * + pow(sin(q[0 * 3 + 0]), 2.0) + + pow(cos(q[0 * 3 + 0]), 2.0) * pow(sin(q[0 * 3 + 1]), 2.0) * + pow(sin(q[0 * 3 + 2]), 2.0) + + pow(cos(q[0 * 3 + 1]), 2.0) * pow(sin(q[0 * 3 + 0]), 2.0) * + pow(sin(q[0 * 3 + 2]), 2.0) + + pow(cos(q[0 * 3 + 2]), 2.0) * pow(sin(q[0 * 3 + 0]), 2.0) * + pow(sin(q[0 * 3 + 1]), 2.0) + + pow(sin(q[0 * 3 + 0]), 2.0) * pow(sin(q[0 * 3 + 1]), 2.0) * + pow(sin(q[0 * 3 + 2]), 2.0) + + pow(cos(q[0 * 3 + 0]), 2.0) * pow(cos(q[0 * 3 + 1]), 2.0) * + pow(cos(q[0 * 3 + 2]), 2.0)) * + 1.0E+5) + + ((cos(q[0 * 3 + 1]) * sin(q[0 * 3 + 2]) + + cos(q[0 * 3 + 2]) * sin(q[0 * 3 + 1])) * + (pow(cos(q[0 * 3 + 0]), 2.0) * sin(q[0 * 3 + 1]) * 1.03E+2 + + pow(sin(q[0 * 3 + 0]), 2.0) * sin(q[0 * 3 + 1]) * 1.03E+2 + + RL * pow(cos(q[0 * 3 + 1]), 2.0) * sin(q[0 * 3 + 0]) * 3.5E+1 + + RL * sin(q[0 * 3 + 0]) * pow(sin(q[0 * 3 + 1]), 2.0) * 3.5E+1)) / + ((pow(cos(q[0 * 3 + 0]), 2.0) * pow(cos(q[0 * 3 + 1]), 2.0) + + pow(cos(q[0 * 3 + 0]), 2.0) * pow(sin(q[0 * 3 + 1]), 2.0) + + pow(cos(q[0 * 3 + 1]), 2.0) * pow(sin(q[0 * 3 + 0]), 2.0) + + pow(sin(q[0 * 3 + 0]), 2.0) * pow(sin(q[0 * 3 + 1]), 2.0)) * + (pow(cos(q[0 * 3 + 1]), 2.0) * pow(cos(q[0 * 3 + 2]), 2.0) + + pow(cos(q[0 * 3 + 1]), 2.0) * pow(sin(q[0 * 3 + 2]), 2.0) + + pow(cos(q[0 * 3 + 2]), 2.0) * pow(sin(q[0 * 3 + 1]), 2.0) + + pow(sin(q[0 * 3 + 1]), 2.0) * pow(sin(q[0 * 3 + 2]), 2.0)) * + 1.0E+3) + + (sin(q[0 * 3 + 0]) * + (sin(q[0 * 3 + 0]) * sin(q[0 * 3 + 1]) * sin(q[0 * 3 + 2]) * + 1.1315E+4 + + pow(cos(q[0 * 3 + 1]), 2.0) * cos(q[0 * 3 + 2]) * + sin(q[0 * 3 + 0]) * 1.03E+4 + + cos(q[0 * 3 + 2]) * sin(q[0 * 3 + 0]) * + pow(sin(q[0 * 3 + 1]), 2.0) * 1.03E+4 - + cos(q[0 * 3 + 1]) * cos(q[0 * 3 + 2]) * sin(q[0 * 3 + 0]) * + 1.1315E+4 + + RL * cos(q[0 * 3 + 0]) * cos(q[0 * 3 + 1]) * sin(q[0 * 3 + 2]) * + 5.049E+3 + + RL * cos(q[0 * 3 + 0]) * cos(q[0 * 3 + 2]) * sin(q[0 * 3 + 1]) * + 5.049E+3 + + RL * pow(cos(q[0 * 3 + 0]), 2.0) * cos(q[0 * 3 + 1]) * + sin(q[0 * 3 + 2]) * 3.5E+3 + + RL * pow(cos(q[0 * 3 + 0]), 2.0) * cos(q[0 * 3 + 2]) * + sin(q[0 * 3 + 1]) * 3.5E+3 + + RL * cos(q[0 * 3 + 1]) * pow(sin(q[0 * 3 + 0]), 2.0) * + sin(q[0 * 3 + 2]) * 3.5E+3 + + RL * cos(q[0 * 3 + 2]) * pow(sin(q[0 * 3 + 0]), 2.0) * + sin(q[0 * 3 + 1]) * 3.5E+3)) / + ((pow(cos(q[0 * 3 + 0]), 2.0) + pow(sin(q[0 * 3 + 0]), 2.0)) * + (pow(cos(q[0 * 3 + 0]), 2.0) * pow(cos(q[0 * 3 + 1]), 2.0) * + pow(sin(q[0 * 3 + 2]), 2.0) + + pow(cos(q[0 * 3 + 0]), 2.0) * pow(cos(q[0 * 3 + 2]), 2.0) * + pow(sin(q[0 * 3 + 1]), 2.0) + + pow(cos(q[0 * 3 + 1]), 2.0) * pow(cos(q[0 * 3 + 2]), 2.0) * + pow(sin(q[0 * 3 + 0]), 2.0) + + pow(cos(q[0 * 3 + 0]), 2.0) * pow(sin(q[0 * 3 + 1]), 2.0) * + pow(sin(q[0 * 3 + 2]), 2.0) + + pow(cos(q[0 * 3 + 1]), 2.0) * pow(sin(q[0 * 3 + 0]), 2.0) * + pow(sin(q[0 * 3 + 2]), 2.0) + + pow(cos(q[0 * 3 + 2]), 2.0) * pow(sin(q[0 * 3 + 0]), 2.0) * + pow(sin(q[0 * 3 + 1]), 2.0) + + pow(sin(q[0 * 3 + 0]), 2.0) * pow(sin(q[0 * 3 + 1]), 2.0) * + pow(sin(q[0 * 3 + 2]), 2.0) + + pow(cos(q[0 * 3 + 0]), 2.0) * pow(cos(q[0 * 3 + 1]), 2.0) * + pow(cos(q[0 * 3 + 2]), 2.0)) * + 1.0E+5) - + (pow(cos(q[0 * 3 + 0]), 2.0) * + (cos(q[0 * 3 + 1]) * cos(q[0 * 3 + 2]) - + sin(q[0 * 3 + 1]) * sin(q[0 * 3 + 2])) * + (cos(q[0 * 3 + 1]) * -2.06E+3 + + pow(cos(q[0 * 3 + 1]), 2.0) * 2.263E+3 + + pow(sin(q[0 * 3 + 1]), 2.0) * 2.263E+3)) / + ((pow(cos(q[0 * 3 + 0]), 2.0) * pow(cos(q[0 * 3 + 1]), 2.0) + + pow(cos(q[0 * 3 + 0]), 2.0) * pow(sin(q[0 * 3 + 1]), 2.0) + + pow(cos(q[0 * 3 + 1]), 2.0) * pow(sin(q[0 * 3 + 0]), 2.0) + + pow(sin(q[0 * 3 + 0]), 2.0) * pow(sin(q[0 * 3 + 1]), 2.0)) * + (pow(cos(q[0 * 3 + 0]), 2.0) * pow(cos(q[0 * 3 + 1]), 2.0) * + pow(sin(q[0 * 3 + 2]), 2.0) + + pow(cos(q[0 * 3 + 0]), 2.0) * pow(cos(q[0 * 3 + 2]), 2.0) * + pow(sin(q[0 * 3 + 1]), 2.0) + + pow(cos(q[0 * 3 + 1]), 2.0) * pow(cos(q[0 * 3 + 2]), 2.0) * + pow(sin(q[0 * 3 + 0]), 2.0) + + pow(cos(q[0 * 3 + 0]), 2.0) * pow(sin(q[0 * 3 + 1]), 2.0) * + pow(sin(q[0 * 3 + 2]), 2.0) + + pow(cos(q[0 * 3 + 1]), 2.0) * pow(sin(q[0 * 3 + 0]), 2.0) * + pow(sin(q[0 * 3 + 2]), 2.0) + + pow(cos(q[0 * 3 + 2]), 2.0) * pow(sin(q[0 * 3 + 0]), 2.0) * + pow(sin(q[0 * 3 + 1]), 2.0) + + pow(sin(q[0 * 3 + 0]), 2.0) * pow(sin(q[0 * 3 + 1]), 2.0) * + pow(sin(q[0 * 3 + 2]), 2.0) + + pow(cos(q[0 * 3 + 0]), 2.0) * pow(cos(q[0 * 3 + 1]), 2.0) * + pow(cos(q[0 * 3 + 2]), 2.0)) * + 2.0E+4) - + (pow(sin(q[0 * 3 + 0]), 2.0) * + (cos(q[0 * 3 + 1]) * cos(q[0 * 3 + 2]) - + sin(q[0 * 3 + 1]) * sin(q[0 * 3 + 2])) * + (cos(q[0 * 3 + 1]) * -2.06E+3 + + pow(cos(q[0 * 3 + 1]), 2.0) * 2.263E+3 + + pow(sin(q[0 * 3 + 1]), 2.0) * 2.263E+3)) / + ((pow(cos(q[0 * 3 + 0]), 2.0) * pow(cos(q[0 * 3 + 1]), 2.0) + + pow(cos(q[0 * 3 + 0]), 2.0) * pow(sin(q[0 * 3 + 1]), 2.0) + + pow(cos(q[0 * 3 + 1]), 2.0) * pow(sin(q[0 * 3 + 0]), 2.0) + + pow(sin(q[0 * 3 + 0]), 2.0) * pow(sin(q[0 * 3 + 1]), 2.0)) * + (pow(cos(q[0 * 3 + 0]), 2.0) * pow(cos(q[0 * 3 + 1]), 2.0) * + pow(sin(q[0 * 3 + 2]), 2.0) + + pow(cos(q[0 * 3 + 0]), 2.0) * pow(cos(q[0 * 3 + 2]), 2.0) * + pow(sin(q[0 * 3 + 1]), 2.0) + + pow(cos(q[0 * 3 + 1]), 2.0) * pow(cos(q[0 * 3 + 2]), 2.0) * + pow(sin(q[0 * 3 + 0]), 2.0) + + pow(cos(q[0 * 3 + 0]), 2.0) * pow(sin(q[0 * 3 + 1]), 2.0) * + pow(sin(q[0 * 3 + 2]), 2.0) + + pow(cos(q[0 * 3 + 1]), 2.0) * pow(sin(q[0 * 3 + 0]), 2.0) * + pow(sin(q[0 * 3 + 2]), 2.0) + + pow(cos(q[0 * 3 + 2]), 2.0) * pow(sin(q[0 * 3 + 0]), 2.0) * + pow(sin(q[0 * 3 + 1]), 2.0) + + pow(sin(q[0 * 3 + 0]), 2.0) * pow(sin(q[0 * 3 + 1]), 2.0) * + pow(sin(q[0 * 3 + 2]), 2.0) + + pow(cos(q[0 * 3 + 0]), 2.0) * pow(cos(q[0 * 3 + 1]), 2.0) * + pow(cos(q[0 * 3 + 2]), 2.0)) * + 2.0E+4) - + 1.03E+2 / 5.0E+2)) / + (pow(cos(q[0 * 3 + 0]), 2.0) * pow(cos(q[0 * 3 + 1]), 2.0) * + pow(sin(q[0 * 3 + 2]), 2.0) + + pow(cos(q[0 * 3 + 0]), 2.0) * pow(cos(q[0 * 3 + 2]), 2.0) * + pow(sin(q[0 * 3 + 1]), 2.0) + + pow(cos(q[0 * 3 + 1]), 2.0) * pow(cos(q[0 * 3 + 2]), 2.0) * + pow(sin(q[0 * 3 + 0]), 2.0) + + pow(cos(q[0 * 3 + 0]), 2.0) * pow(sin(q[0 * 3 + 1]), 2.0) * + pow(sin(q[0 * 3 + 2]), 2.0) + + pow(cos(q[0 * 3 + 1]), 2.0) * pow(sin(q[0 * 3 + 0]), 2.0) * + pow(sin(q[0 * 3 + 2]), 2.0) + + pow(cos(q[0 * 3 + 2]), 2.0) * pow(sin(q[0 * 3 + 0]), 2.0) * + pow(sin(q[0 * 3 + 1]), 2.0) + + pow(sin(q[0 * 3 + 0]), 2.0) * pow(sin(q[0 * 3 + 1]), 2.0) * + pow(sin(q[0 * 3 + 2]), 2.0) + + pow(cos(q[0 * 3 + 0]), 2.0) * pow(cos(q[0 * 3 + 1]), 2.0) * + pow(cos(q[0 * 3 + 2]), 2.0)) + + (sin(q[0 * 3 + 0]) * + (cos(q[0 * 3 + 1]) * cos(q[0 * 3 + 2]) - + sin(q[0 * 3 + 1]) * sin(q[0 * 3 + 2])) * + ((sin(q[0 * 3 + 0]) * + (pow(cos(q[0 * 3 + 1]), 2.0) * sin(q[0 * 3 + 0]) * + sin(q[0 * 3 + 2]) * 1.03E+4 + + sin(q[0 * 3 + 0]) * pow(sin(q[0 * 3 + 1]), 2.0) * + sin(q[0 * 3 + 2]) * 1.03E+4 - + cos(q[0 * 3 + 1]) * sin(q[0 * 3 + 0]) * sin(q[0 * 3 + 2]) * + 1.1315E+4 - + cos(q[0 * 3 + 2]) * sin(q[0 * 3 + 0]) * sin(q[0 * 3 + 1]) * + 1.1315E+4 - + RL * cos(q[0 * 3 + 0]) * cos(q[0 * 3 + 1]) * cos(q[0 * 3 + 2]) * + 5.049E+3 + + RL * cos(q[0 * 3 + 0]) * sin(q[0 * 3 + 1]) * sin(q[0 * 3 + 2]) * + 5.049E+3 - + RL * pow(cos(q[0 * 3 + 0]), 2.0) * cos(q[0 * 3 + 1]) * + cos(q[0 * 3 + 2]) * 3.5E+3 - + RL * cos(q[0 * 3 + 1]) * cos(q[0 * 3 + 2]) * + pow(sin(q[0 * 3 + 0]), 2.0) * 3.5E+3 + + RL * pow(cos(q[0 * 3 + 0]), 2.0) * sin(q[0 * 3 + 1]) * + sin(q[0 * 3 + 2]) * 3.5E+3 + + RL * pow(sin(q[0 * 3 + 0]), 2.0) * sin(q[0 * 3 + 1]) * + sin(q[0 * 3 + 2]) * 3.5E+3) * + (-1.0E-5)) / + ((pow(cos(q[0 * 3 + 0]), 2.0) + pow(sin(q[0 * 3 + 0]), 2.0)) * + (pow(cos(q[0 * 3 + 0]), 2.0) * pow(cos(q[0 * 3 + 1]), 2.0) * + pow(sin(q[0 * 3 + 2]), 2.0) + + pow(cos(q[0 * 3 + 0]), 2.0) * pow(cos(q[0 * 3 + 2]), 2.0) * + pow(sin(q[0 * 3 + 1]), 2.0) + + pow(cos(q[0 * 3 + 1]), 2.0) * pow(cos(q[0 * 3 + 2]), 2.0) * + pow(sin(q[0 * 3 + 0]), 2.0) + + pow(cos(q[0 * 3 + 0]), 2.0) * pow(sin(q[0 * 3 + 1]), 2.0) * + pow(sin(q[0 * 3 + 2]), 2.0) + + pow(cos(q[0 * 3 + 1]), 2.0) * pow(sin(q[0 * 3 + 0]), 2.0) * + pow(sin(q[0 * 3 + 2]), 2.0) + + pow(cos(q[0 * 3 + 2]), 2.0) * pow(sin(q[0 * 3 + 0]), 2.0) * + pow(sin(q[0 * 3 + 1]), 2.0) + + pow(sin(q[0 * 3 + 0]), 2.0) * pow(sin(q[0 * 3 + 1]), 2.0) * + pow(sin(q[0 * 3 + 2]), 2.0) + + pow(cos(q[0 * 3 + 0]), 2.0) * pow(cos(q[0 * 3 + 1]), 2.0) * + pow(cos(q[0 * 3 + 2]), 2.0))) + + ((cos(q[0 * 3 + 1]) * cos(q[0 * 3 + 2]) - + sin(q[0 * 3 + 1]) * sin(q[0 * 3 + 2])) * + (pow(cos(q[0 * 3 + 0]), 2.0) * sin(q[0 * 3 + 1]) * 1.03E+2 + + pow(sin(q[0 * 3 + 0]), 2.0) * sin(q[0 * 3 + 1]) * 1.03E+2 + + RL * pow(cos(q[0 * 3 + 1]), 2.0) * sin(q[0 * 3 + 0]) * 3.5E+1 + + RL * sin(q[0 * 3 + 0]) * pow(sin(q[0 * 3 + 1]), 2.0) * 3.5E+1)) / + ((pow(cos(q[0 * 3 + 0]), 2.0) * pow(cos(q[0 * 3 + 1]), 2.0) + + pow(cos(q[0 * 3 + 0]), 2.0) * pow(sin(q[0 * 3 + 1]), 2.0) + + pow(cos(q[0 * 3 + 1]), 2.0) * pow(sin(q[0 * 3 + 0]), 2.0) + + pow(sin(q[0 * 3 + 0]), 2.0) * pow(sin(q[0 * 3 + 1]), 2.0)) * + (pow(cos(q[0 * 3 + 1]), 2.0) * pow(cos(q[0 * 3 + 2]), 2.0) + + pow(cos(q[0 * 3 + 1]), 2.0) * pow(sin(q[0 * 3 + 2]), 2.0) + + pow(cos(q[0 * 3 + 2]), 2.0) * pow(sin(q[0 * 3 + 1]), 2.0) + + pow(sin(q[0 * 3 + 1]), 2.0) * pow(sin(q[0 * 3 + 2]), 2.0)) * + 1.0E+3) - + (cos(q[0 * 3 + 0]) * + (cos(q[0 * 3 + 0]) * pow(cos(q[0 * 3 + 1]), 2.0) * + sin(q[0 * 3 + 2]) * 1.03E+4 + + cos(q[0 * 3 + 0]) * pow(sin(q[0 * 3 + 1]), 2.0) * + sin(q[0 * 3 + 2]) * 1.03E+4 - + cos(q[0 * 3 + 0]) * cos(q[0 * 3 + 1]) * sin(q[0 * 3 + 2]) * + 1.1315E+4 - + cos(q[0 * 3 + 0]) * cos(q[0 * 3 + 2]) * sin(q[0 * 3 + 1]) * + 1.1315E+4 + + RL * cos(q[0 * 3 + 1]) * cos(q[0 * 3 + 2]) * sin(q[0 * 3 + 0]) * + 5.049E+3 - + RL * sin(q[0 * 3 + 0]) * sin(q[0 * 3 + 1]) * sin(q[0 * 3 + 2]) * + 5.049E+3)) / + ((pow(cos(q[0 * 3 + 0]), 2.0) + pow(sin(q[0 * 3 + 0]), 2.0)) * + (pow(cos(q[0 * 3 + 0]), 2.0) * pow(cos(q[0 * 3 + 1]), 2.0) * + pow(sin(q[0 * 3 + 2]), 2.0) + + pow(cos(q[0 * 3 + 0]), 2.0) * pow(cos(q[0 * 3 + 2]), 2.0) * + pow(sin(q[0 * 3 + 1]), 2.0) + + pow(cos(q[0 * 3 + 1]), 2.0) * pow(cos(q[0 * 3 + 2]), 2.0) * + pow(sin(q[0 * 3 + 0]), 2.0) + + pow(cos(q[0 * 3 + 0]), 2.0) * pow(sin(q[0 * 3 + 1]), 2.0) * + pow(sin(q[0 * 3 + 2]), 2.0) + + pow(cos(q[0 * 3 + 1]), 2.0) * pow(sin(q[0 * 3 + 0]), 2.0) * + pow(sin(q[0 * 3 + 2]), 2.0) + + pow(cos(q[0 * 3 + 2]), 2.0) * pow(sin(q[0 * 3 + 0]), 2.0) * + pow(sin(q[0 * 3 + 1]), 2.0) + + pow(sin(q[0 * 3 + 0]), 2.0) * pow(sin(q[0 * 3 + 1]), 2.0) * + pow(sin(q[0 * 3 + 2]), 2.0) + + pow(cos(q[0 * 3 + 0]), 2.0) * pow(cos(q[0 * 3 + 1]), 2.0) * + pow(cos(q[0 * 3 + 2]), 2.0)) * + 1.0E+5) + + (pow(cos(q[0 * 3 + 0]), 2.0) * + (cos(q[0 * 3 + 1]) * sin(q[0 * 3 + 2]) + + cos(q[0 * 3 + 2]) * sin(q[0 * 3 + 1])) * + (cos(q[0 * 3 + 1]) * -2.06E+3 + + pow(cos(q[0 * 3 + 1]), 2.0) * 2.263E+3 + + pow(sin(q[0 * 3 + 1]), 2.0) * 2.263E+3)) / + ((pow(cos(q[0 * 3 + 0]), 2.0) * pow(cos(q[0 * 3 + 1]), 2.0) + + pow(cos(q[0 * 3 + 0]), 2.0) * pow(sin(q[0 * 3 + 1]), 2.0) + + pow(cos(q[0 * 3 + 1]), 2.0) * pow(sin(q[0 * 3 + 0]), 2.0) + + pow(sin(q[0 * 3 + 0]), 2.0) * pow(sin(q[0 * 3 + 1]), 2.0)) * + (pow(cos(q[0 * 3 + 0]), 2.0) * pow(cos(q[0 * 3 + 1]), 2.0) * + pow(sin(q[0 * 3 + 2]), 2.0) + + pow(cos(q[0 * 3 + 0]), 2.0) * pow(cos(q[0 * 3 + 2]), 2.0) * + pow(sin(q[0 * 3 + 1]), 2.0) + + pow(cos(q[0 * 3 + 1]), 2.0) * pow(cos(q[0 * 3 + 2]), 2.0) * + pow(sin(q[0 * 3 + 0]), 2.0) + + pow(cos(q[0 * 3 + 0]), 2.0) * pow(sin(q[0 * 3 + 1]), 2.0) * + pow(sin(q[0 * 3 + 2]), 2.0) + + pow(cos(q[0 * 3 + 1]), 2.0) * pow(sin(q[0 * 3 + 0]), 2.0) * + pow(sin(q[0 * 3 + 2]), 2.0) + + pow(cos(q[0 * 3 + 2]), 2.0) * pow(sin(q[0 * 3 + 0]), 2.0) * + pow(sin(q[0 * 3 + 1]), 2.0) + + pow(sin(q[0 * 3 + 0]), 2.0) * pow(sin(q[0 * 3 + 1]), 2.0) * + pow(sin(q[0 * 3 + 2]), 2.0) + + pow(cos(q[0 * 3 + 0]), 2.0) * pow(cos(q[0 * 3 + 1]), 2.0) * + pow(cos(q[0 * 3 + 2]), 2.0)) * + 2.0E+4) + + (pow(sin(q[0 * 3 + 0]), 2.0) * + (cos(q[0 * 3 + 1]) * sin(q[0 * 3 + 2]) + + cos(q[0 * 3 + 2]) * sin(q[0 * 3 + 1])) * + (cos(q[0 * 3 + 1]) * -2.06E+3 + + pow(cos(q[0 * 3 + 1]), 2.0) * 2.263E+3 + + pow(sin(q[0 * 3 + 1]), 2.0) * 2.263E+3)) / + ((pow(cos(q[0 * 3 + 0]), 2.0) * pow(cos(q[0 * 3 + 1]), 2.0) + + pow(cos(q[0 * 3 + 0]), 2.0) * pow(sin(q[0 * 3 + 1]), 2.0) + + pow(cos(q[0 * 3 + 1]), 2.0) * pow(sin(q[0 * 3 + 0]), 2.0) + + pow(sin(q[0 * 3 + 0]), 2.0) * pow(sin(q[0 * 3 + 1]), 2.0)) * + (pow(cos(q[0 * 3 + 0]), 2.0) * pow(cos(q[0 * 3 + 1]), 2.0) * + pow(sin(q[0 * 3 + 2]), 2.0) + + pow(cos(q[0 * 3 + 0]), 2.0) * pow(cos(q[0 * 3 + 2]), 2.0) * + pow(sin(q[0 * 3 + 1]), 2.0) + + pow(cos(q[0 * 3 + 1]), 2.0) * pow(cos(q[0 * 3 + 2]), 2.0) * + pow(sin(q[0 * 3 + 0]), 2.0) + + pow(cos(q[0 * 3 + 0]), 2.0) * pow(sin(q[0 * 3 + 1]), 2.0) * + pow(sin(q[0 * 3 + 2]), 2.0) + + pow(cos(q[0 * 3 + 1]), 2.0) * pow(sin(q[0 * 3 + 0]), 2.0) * + pow(sin(q[0 * 3 + 2]), 2.0) + + pow(cos(q[0 * 3 + 2]), 2.0) * pow(sin(q[0 * 3 + 0]), 2.0) * + pow(sin(q[0 * 3 + 1]), 2.0) + + pow(sin(q[0 * 3 + 0]), 2.0) * pow(sin(q[0 * 3 + 1]), 2.0) * + pow(sin(q[0 * 3 + 2]), 2.0) + + pow(cos(q[0 * 3 + 0]), 2.0) * pow(cos(q[0 * 3 + 1]), 2.0) * + pow(cos(q[0 * 3 + 2]), 2.0)) * + 2.0E+4))) / + (pow(cos(q[0 * 3 + 0]), 2.0) * pow(cos(q[0 * 3 + 1]), 2.0) * + pow(sin(q[0 * 3 + 2]), 2.0) + + pow(cos(q[0 * 3 + 0]), 2.0) * pow(cos(q[0 * 3 + 2]), 2.0) * + pow(sin(q[0 * 3 + 1]), 2.0) + + pow(cos(q[0 * 3 + 1]), 2.0) * pow(cos(q[0 * 3 + 2]), 2.0) * + pow(sin(q[0 * 3 + 0]), 2.0) + + pow(cos(q[0 * 3 + 0]), 2.0) * pow(sin(q[0 * 3 + 1]), 2.0) * + pow(sin(q[0 * 3 + 2]), 2.0) + + pow(cos(q[0 * 3 + 1]), 2.0) * pow(sin(q[0 * 3 + 0]), 2.0) * + pow(sin(q[0 * 3 + 2]), 2.0) + + pow(cos(q[0 * 3 + 2]), 2.0) * pow(sin(q[0 * 3 + 0]), 2.0) * + pow(sin(q[0 * 3 + 1]), 2.0) + + pow(sin(q[0 * 3 + 0]), 2.0) * pow(sin(q[0 * 3 + 1]), 2.0) * + pow(sin(q[0 * 3 + 2]), 2.0) + + pow(cos(q[0 * 3 + 0]), 2.0) * pow(cos(q[0 * 3 + 1]), 2.0) * + pow(cos(q[0 * 3 + 2]), 2.0)))) / + (pow(cos(q[0 * 3 + 0]), 2.0) + pow(sin(q[0 * 3 + 0]), 2.0)) + + (cos(q[0 * 3 + 2]) * + (cos(q[0 * 3 + 1]) * sin(q[0 * 3 + 2]) + + cos(q[0 * 3 + 2]) * sin(q[0 * 3 + 1])) * + (1.03E+2 / 5.0E+2)) / + (pow(cos(q[0 * 3 + 1]), 2.0) * pow(cos(q[0 * 3 + 2]), 2.0) + + pow(cos(q[0 * 3 + 1]), 2.0) * pow(sin(q[0 * 3 + 2]), 2.0) + + pow(cos(q[0 * 3 + 2]), 2.0) * pow(sin(q[0 * 3 + 1]), 2.0) + + pow(sin(q[0 * 3 + 1]), 2.0) * pow(sin(q[0 * 3 + 2]), 2.0)) - + (sin(q[0 * 3 + 2]) * + (cos(q[0 * 3 + 1]) * cos(q[0 * 3 + 2]) - + sin(q[0 * 3 + 1]) * sin(q[0 * 3 + 2])) * + (1.03E+2 / 5.0E+2)) / + (pow(cos(q[0 * 3 + 1]), 2.0) * pow(cos(q[0 * 3 + 2]), 2.0) + + pow(cos(q[0 * 3 + 1]), 2.0) * pow(sin(q[0 * 3 + 2]), 2.0) + + pow(cos(q[0 * 3 + 2]), 2.0) * pow(sin(q[0 * 3 + 1]), 2.0) + + pow(sin(q[0 * 3 + 1]), 2.0) * pow(sin(q[0 * 3 + 2]), 2.0)); + F(0, 2) = + ((pow(cos(q[0 * 3 + 0]), 2.0) * sin(q[0 * 3 + 1]) * 1.03E+2 + + pow(sin(q[0 * 3 + 0]), 2.0) * sin(q[0 * 3 + 1]) * 1.03E+2 + + RL * pow(cos(q[0 * 3 + 1]), 2.0) * sin(q[0 * 3 + 0]) * 3.5E+1 + + RL * sin(q[0 * 3 + 0]) * pow(sin(q[0 * 3 + 1]), 2.0) * 3.5E+1) * + (-1.0 / 5.0E+2)) / + (pow(cos(q[0 * 3 + 0]), 2.0) * pow(cos(q[0 * 3 + 1]), 2.0) + + pow(cos(q[0 * 3 + 0]), 2.0) * pow(sin(q[0 * 3 + 1]), 2.0) + + pow(cos(q[0 * 3 + 1]), 2.0) * pow(sin(q[0 * 3 + 0]), 2.0) + + pow(sin(q[0 * 3 + 0]), 2.0) * pow(sin(q[0 * 3 + 1]), 2.0)) + + (cos(q[0 * 3 + 0]) * + (-(sin(q[0 * 3 + 0]) * + ((cos(q[0 * 3 + 0]) * + (cos(q[0 * 3 + 1]) * sin(q[0 * 3 + 2]) + + cos(q[0 * 3 + 2]) * sin(q[0 * 3 + 1])) * + 1.0 / + pow(pow(cos(q[0 * 3 + 0]), 2.0) * pow(cos(q[0 * 3 + 1]), 2.0) * + pow(sin(q[0 * 3 + 2]), 2.0) + + pow(cos(q[0 * 3 + 0]), 2.0) * pow(cos(q[0 * 3 + 2]), 2.0) * + pow(sin(q[0 * 3 + 1]), 2.0) + + pow(cos(q[0 * 3 + 1]), 2.0) * pow(cos(q[0 * 3 + 2]), 2.0) * + pow(sin(q[0 * 3 + 0]), 2.0) + + pow(cos(q[0 * 3 + 0]), 2.0) * pow(sin(q[0 * 3 + 1]), 2.0) * + pow(sin(q[0 * 3 + 2]), 2.0) + + pow(cos(q[0 * 3 + 1]), 2.0) * pow(sin(q[0 * 3 + 0]), 2.0) * + pow(sin(q[0 * 3 + 2]), 2.0) + + pow(cos(q[0 * 3 + 2]), 2.0) * pow(sin(q[0 * 3 + 0]), 2.0) * + pow(sin(q[0 * 3 + 1]), 2.0) + + pow(sin(q[0 * 3 + 0]), 2.0) * pow(sin(q[0 * 3 + 1]), 2.0) * + pow(sin(q[0 * 3 + 2]), 2.0) + + pow(cos(q[0 * 3 + 0]), 2.0) * pow(cos(q[0 * 3 + 1]), 2.0) * + pow(cos(q[0 * 3 + 2]), 2.0), + 2.0) * + (sin(q[0 * 3 + 0]) * sin(q[0 * 3 + 1]) * sin(q[0 * 3 + 2]) * + 1.1315E+4 + + pow(cos(q[0 * 3 + 1]), 2.0) * cos(q[0 * 3 + 2]) * + sin(q[0 * 3 + 0]) * 1.03E+4 + + cos(q[0 * 3 + 2]) * sin(q[0 * 3 + 0]) * + pow(sin(q[0 * 3 + 1]), 2.0) * 1.03E+4 - + cos(q[0 * 3 + 1]) * cos(q[0 * 3 + 2]) * sin(q[0 * 3 + 0]) * + 1.1315E+4 + + RL * cos(q[0 * 3 + 0]) * cos(q[0 * 3 + 1]) * sin(q[0 * 3 + 2]) * + 5.049E+3 + + RL * cos(q[0 * 3 + 0]) * cos(q[0 * 3 + 2]) * sin(q[0 * 3 + 1]) * + 5.049E+3 + + RL * pow(cos(q[0 * 3 + 0]), 2.0) * cos(q[0 * 3 + 1]) * + sin(q[0 * 3 + 2]) * 3.5E+3 + + RL * pow(cos(q[0 * 3 + 0]), 2.0) * cos(q[0 * 3 + 2]) * + sin(q[0 * 3 + 1]) * 3.5E+3 + + RL * cos(q[0 * 3 + 1]) * pow(sin(q[0 * 3 + 0]), 2.0) * + sin(q[0 * 3 + 2]) * 3.5E+3 + + RL * cos(q[0 * 3 + 2]) * pow(sin(q[0 * 3 + 0]), 2.0) * + sin(q[0 * 3 + 1]) * 3.5E+3)) / + 1.0E+5 - + (cos(q[0 * 3 + 0]) * + (cos(q[0 * 3 + 1]) * cos(q[0 * 3 + 2]) - + sin(q[0 * 3 + 1]) * sin(q[0 * 3 + 2])) * + 1.0 / + pow(pow(cos(q[0 * 3 + 0]), 2.0) * pow(cos(q[0 * 3 + 1]), 2.0) * + pow(sin(q[0 * 3 + 2]), 2.0) + + pow(cos(q[0 * 3 + 0]), 2.0) * pow(cos(q[0 * 3 + 2]), 2.0) * + pow(sin(q[0 * 3 + 1]), 2.0) + + pow(cos(q[0 * 3 + 1]), 2.0) * pow(cos(q[0 * 3 + 2]), 2.0) * + pow(sin(q[0 * 3 + 0]), 2.0) + + pow(cos(q[0 * 3 + 0]), 2.0) * pow(sin(q[0 * 3 + 1]), 2.0) * + pow(sin(q[0 * 3 + 2]), 2.0) + + pow(cos(q[0 * 3 + 1]), 2.0) * pow(sin(q[0 * 3 + 0]), 2.0) * + pow(sin(q[0 * 3 + 2]), 2.0) + + pow(cos(q[0 * 3 + 2]), 2.0) * pow(sin(q[0 * 3 + 0]), 2.0) * + pow(sin(q[0 * 3 + 1]), 2.0) + + pow(sin(q[0 * 3 + 0]), 2.0) * pow(sin(q[0 * 3 + 1]), 2.0) * + pow(sin(q[0 * 3 + 2]), 2.0) + + pow(cos(q[0 * 3 + 0]), 2.0) * pow(cos(q[0 * 3 + 1]), 2.0) * + pow(cos(q[0 * 3 + 2]), 2.0), + 2.0) * + (pow(cos(q[0 * 3 + 1]), 2.0) * sin(q[0 * 3 + 0]) * + sin(q[0 * 3 + 2]) * 1.03E+4 + + sin(q[0 * 3 + 0]) * pow(sin(q[0 * 3 + 1]), 2.0) * + sin(q[0 * 3 + 2]) * 1.03E+4 - + cos(q[0 * 3 + 1]) * sin(q[0 * 3 + 0]) * sin(q[0 * 3 + 2]) * + 1.1315E+4 - + cos(q[0 * 3 + 2]) * sin(q[0 * 3 + 0]) * sin(q[0 * 3 + 1]) * + 1.1315E+4 - + RL * cos(q[0 * 3 + 0]) * cos(q[0 * 3 + 1]) * cos(q[0 * 3 + 2]) * + 5.049E+3 + + RL * cos(q[0 * 3 + 0]) * sin(q[0 * 3 + 1]) * sin(q[0 * 3 + 2]) * + 5.049E+3 - + RL * pow(cos(q[0 * 3 + 0]), 2.0) * cos(q[0 * 3 + 1]) * + cos(q[0 * 3 + 2]) * 3.5E+3 - + RL * cos(q[0 * 3 + 1]) * cos(q[0 * 3 + 2]) * + pow(sin(q[0 * 3 + 0]), 2.0) * 3.5E+3 + + RL * pow(cos(q[0 * 3 + 0]), 2.0) * sin(q[0 * 3 + 1]) * + sin(q[0 * 3 + 2]) * 3.5E+3 + + RL * pow(sin(q[0 * 3 + 0]), 2.0) * sin(q[0 * 3 + 1]) * + sin(q[0 * 3 + 2]) * 3.5E+3)) / + 1.0E+5 - + (sin(q[0 * 3 + 0]) * + (cos(q[0 * 3 + 1]) * sin(q[0 * 3 + 2]) + + cos(q[0 * 3 + 2]) * sin(q[0 * 3 + 1])) * + (cos(q[0 * 3 + 0]) * pow(cos(q[0 * 3 + 1]), 2.0) * + cos(q[0 * 3 + 2]) * 1.03E+4 + + cos(q[0 * 3 + 0]) * cos(q[0 * 3 + 2]) * + pow(sin(q[0 * 3 + 1]), 2.0) * 1.03E+4 - + cos(q[0 * 3 + 0]) * cos(q[0 * 3 + 1]) * cos(q[0 * 3 + 2]) * + 1.1315E+4 + + cos(q[0 * 3 + 0]) * sin(q[0 * 3 + 1]) * sin(q[0 * 3 + 2]) * + 1.1315E+4 - + RL * cos(q[0 * 3 + 1]) * sin(q[0 * 3 + 0]) * sin(q[0 * 3 + 2]) * + 5.049E+3 - + RL * cos(q[0 * 3 + 2]) * sin(q[0 * 3 + 0]) * sin(q[0 * 3 + 1]) * + 5.049E+3) * + 1.0 / + pow(pow(cos(q[0 * 3 + 0]), 2.0) * pow(cos(q[0 * 3 + 1]), 2.0) * + pow(sin(q[0 * 3 + 2]), 2.0) + + pow(cos(q[0 * 3 + 0]), 2.0) * pow(cos(q[0 * 3 + 2]), 2.0) * + pow(sin(q[0 * 3 + 1]), 2.0) + + pow(cos(q[0 * 3 + 1]), 2.0) * pow(cos(q[0 * 3 + 2]), 2.0) * + pow(sin(q[0 * 3 + 0]), 2.0) + + pow(cos(q[0 * 3 + 0]), 2.0) * pow(sin(q[0 * 3 + 1]), 2.0) * + pow(sin(q[0 * 3 + 2]), 2.0) + + pow(cos(q[0 * 3 + 1]), 2.0) * pow(sin(q[0 * 3 + 0]), 2.0) * + pow(sin(q[0 * 3 + 2]), 2.0) + + pow(cos(q[0 * 3 + 2]), 2.0) * pow(sin(q[0 * 3 + 0]), 2.0) * + pow(sin(q[0 * 3 + 1]), 2.0) + + pow(sin(q[0 * 3 + 0]), 2.0) * pow(sin(q[0 * 3 + 1]), 2.0) * + pow(sin(q[0 * 3 + 2]), 2.0) + + pow(cos(q[0 * 3 + 0]), 2.0) * pow(cos(q[0 * 3 + 1]), 2.0) * + pow(cos(q[0 * 3 + 2]), 2.0), + 2.0)) / + 1.0E+5 + + (sin(q[0 * 3 + 0]) * + (cos(q[0 * 3 + 1]) * cos(q[0 * 3 + 2]) - + sin(q[0 * 3 + 1]) * sin(q[0 * 3 + 2])) * + (cos(q[0 * 3 + 0]) * pow(cos(q[0 * 3 + 1]), 2.0) * + sin(q[0 * 3 + 2]) * 1.03E+4 + + cos(q[0 * 3 + 0]) * pow(sin(q[0 * 3 + 1]), 2.0) * + sin(q[0 * 3 + 2]) * 1.03E+4 - + cos(q[0 * 3 + 0]) * cos(q[0 * 3 + 1]) * sin(q[0 * 3 + 2]) * + 1.1315E+4 - + cos(q[0 * 3 + 0]) * cos(q[0 * 3 + 2]) * sin(q[0 * 3 + 1]) * + 1.1315E+4 + + RL * cos(q[0 * 3 + 1]) * cos(q[0 * 3 + 2]) * sin(q[0 * 3 + 0]) * + 5.049E+3 - + RL * sin(q[0 * 3 + 0]) * sin(q[0 * 3 + 1]) * sin(q[0 * 3 + 2]) * + 5.049E+3) * + 1.0 / + pow(pow(cos(q[0 * 3 + 0]), 2.0) * pow(cos(q[0 * 3 + 1]), 2.0) * + pow(sin(q[0 * 3 + 2]), 2.0) + + pow(cos(q[0 * 3 + 0]), 2.0) * pow(cos(q[0 * 3 + 2]), 2.0) * + pow(sin(q[0 * 3 + 1]), 2.0) + + pow(cos(q[0 * 3 + 1]), 2.0) * pow(cos(q[0 * 3 + 2]), 2.0) * + pow(sin(q[0 * 3 + 0]), 2.0) + + pow(cos(q[0 * 3 + 0]), 2.0) * pow(sin(q[0 * 3 + 1]), 2.0) * + pow(sin(q[0 * 3 + 2]), 2.0) + + pow(cos(q[0 * 3 + 1]), 2.0) * pow(sin(q[0 * 3 + 0]), 2.0) * + pow(sin(q[0 * 3 + 2]), 2.0) + + pow(cos(q[0 * 3 + 2]), 2.0) * pow(sin(q[0 * 3 + 0]), 2.0) * + pow(sin(q[0 * 3 + 1]), 2.0) + + pow(sin(q[0 * 3 + 0]), 2.0) * pow(sin(q[0 * 3 + 1]), 2.0) * + pow(sin(q[0 * 3 + 2]), 2.0) + + pow(cos(q[0 * 3 + 0]), 2.0) * pow(cos(q[0 * 3 + 1]), 2.0) * + pow(cos(q[0 * 3 + 2]), 2.0), + 2.0)) / + 1.0E+5 + + ((cos(q[0 * 3 + 1]) * sin(q[0 * 3 + 2]) + + cos(q[0 * 3 + 2]) * sin(q[0 * 3 + 1])) * + (RL * cos(q[0 * 3 + 0]) * cos(q[0 * 3 + 1]) * sin(q[0 * 3 + 2]) * + 3.5E+3 + + RL * cos(q[0 * 3 + 0]) * cos(q[0 * 3 + 2]) * sin(q[0 * 3 + 1]) * + 3.5E+3 + + RL * pow(cos(q[0 * 3 + 0]), 2.0) * cos(q[0 * 3 + 1]) * + sin(q[0 * 3 + 2]) * 5.049E+3 + + RL * pow(cos(q[0 * 3 + 0]), 2.0) * cos(q[0 * 3 + 2]) * + sin(q[0 * 3 + 1]) * 5.049E+3 + + RL * cos(q[0 * 3 + 1]) * pow(sin(q[0 * 3 + 0]), 2.0) * + sin(q[0 * 3 + 2]) * 5.049E+3 + + RL * cos(q[0 * 3 + 2]) * pow(sin(q[0 * 3 + 0]), 2.0) * + sin(q[0 * 3 + 1]) * 5.049E+3)) / + ((pow(cos(q[0 * 3 + 1]), 2.0) * pow(cos(q[0 * 3 + 2]), 2.0) + + pow(cos(q[0 * 3 + 1]), 2.0) * pow(sin(q[0 * 3 + 2]), 2.0) + + pow(cos(q[0 * 3 + 2]), 2.0) * pow(sin(q[0 * 3 + 1]), 2.0) + + pow(sin(q[0 * 3 + 1]), 2.0) * pow(sin(q[0 * 3 + 2]), 2.0)) * + (pow(cos(q[0 * 3 + 0]), 2.0) * pow(cos(q[0 * 3 + 1]), 2.0) * + pow(sin(q[0 * 3 + 2]), 2.0) + + pow(cos(q[0 * 3 + 0]), 2.0) * pow(cos(q[0 * 3 + 2]), 2.0) * + pow(sin(q[0 * 3 + 1]), 2.0) + + pow(cos(q[0 * 3 + 1]), 2.0) * pow(cos(q[0 * 3 + 2]), 2.0) * + pow(sin(q[0 * 3 + 0]), 2.0) + + pow(cos(q[0 * 3 + 0]), 2.0) * pow(sin(q[0 * 3 + 1]), 2.0) * + pow(sin(q[0 * 3 + 2]), 2.0) + + pow(cos(q[0 * 3 + 1]), 2.0) * pow(sin(q[0 * 3 + 0]), 2.0) * + pow(sin(q[0 * 3 + 2]), 2.0) + + pow(cos(q[0 * 3 + 2]), 2.0) * pow(sin(q[0 * 3 + 0]), 2.0) * + pow(sin(q[0 * 3 + 1]), 2.0) + + pow(sin(q[0 * 3 + 0]), 2.0) * pow(sin(q[0 * 3 + 1]), 2.0) * + pow(sin(q[0 * 3 + 2]), 2.0) + + pow(cos(q[0 * 3 + 0]), 2.0) * pow(cos(q[0 * 3 + 1]), 2.0) * + pow(cos(q[0 * 3 + 2]), 2.0)) * + 1.0E+5) + + ((cos(q[0 * 3 + 1]) * cos(q[0 * 3 + 2]) - + sin(q[0 * 3 + 1]) * sin(q[0 * 3 + 2])) * + (RL * cos(q[0 * 3 + 0]) * cos(q[0 * 3 + 1]) * cos(q[0 * 3 + 2]) * + 3.5E+3 - + RL * cos(q[0 * 3 + 0]) * sin(q[0 * 3 + 1]) * sin(q[0 * 3 + 2]) * + 3.5E+3 + + RL * pow(cos(q[0 * 3 + 0]), 2.0) * cos(q[0 * 3 + 1]) * + cos(q[0 * 3 + 2]) * 5.049E+3 + + RL * cos(q[0 * 3 + 1]) * cos(q[0 * 3 + 2]) * + pow(sin(q[0 * 3 + 0]), 2.0) * 5.049E+3 - + RL * pow(cos(q[0 * 3 + 0]), 2.0) * sin(q[0 * 3 + 1]) * + sin(q[0 * 3 + 2]) * 5.049E+3 - + RL * pow(sin(q[0 * 3 + 0]), 2.0) * sin(q[0 * 3 + 1]) * + sin(q[0 * 3 + 2]) * 5.049E+3)) / + ((pow(cos(q[0 * 3 + 1]), 2.0) * pow(cos(q[0 * 3 + 2]), 2.0) + + pow(cos(q[0 * 3 + 1]), 2.0) * pow(sin(q[0 * 3 + 2]), 2.0) + + pow(cos(q[0 * 3 + 2]), 2.0) * pow(sin(q[0 * 3 + 1]), 2.0) + + pow(sin(q[0 * 3 + 1]), 2.0) * pow(sin(q[0 * 3 + 2]), 2.0)) * + (pow(cos(q[0 * 3 + 0]), 2.0) * pow(cos(q[0 * 3 + 1]), 2.0) * + pow(sin(q[0 * 3 + 2]), 2.0) + + pow(cos(q[0 * 3 + 0]), 2.0) * pow(cos(q[0 * 3 + 2]), 2.0) * + pow(sin(q[0 * 3 + 1]), 2.0) + + pow(cos(q[0 * 3 + 1]), 2.0) * pow(cos(q[0 * 3 + 2]), 2.0) * + pow(sin(q[0 * 3 + 0]), 2.0) + + pow(cos(q[0 * 3 + 0]), 2.0) * pow(sin(q[0 * 3 + 1]), 2.0) * + pow(sin(q[0 * 3 + 2]), 2.0) + + pow(cos(q[0 * 3 + 1]), 2.0) * pow(sin(q[0 * 3 + 0]), 2.0) * + pow(sin(q[0 * 3 + 2]), 2.0) + + pow(cos(q[0 * 3 + 2]), 2.0) * pow(sin(q[0 * 3 + 0]), 2.0) * + pow(sin(q[0 * 3 + 1]), 2.0) + + pow(sin(q[0 * 3 + 0]), 2.0) * pow(sin(q[0 * 3 + 1]), 2.0) * + pow(sin(q[0 * 3 + 2]), 2.0) + + pow(cos(q[0 * 3 + 0]), 2.0) * pow(cos(q[0 * 3 + 1]), 2.0) * + pow(cos(q[0 * 3 + 2]), 2.0)) * + 1.0E+5))) / + (pow(cos(q[0 * 3 + 0]), 2.0) + pow(sin(q[0 * 3 + 0]), 2.0)) + + (cos(q[0 * 3 + 0]) * + (cos(q[0 * 3 + 1]) * sin(q[0 * 3 + 2]) + + cos(q[0 * 3 + 2]) * sin(q[0 * 3 + 1])) * + ((cos(q[0 * 3 + 0]) * + (cos(q[0 * 3 + 0]) * pow(cos(q[0 * 3 + 1]), 2.0) * + cos(q[0 * 3 + 2]) * 1.03E+4 + + cos(q[0 * 3 + 0]) * cos(q[0 * 3 + 2]) * + pow(sin(q[0 * 3 + 1]), 2.0) * 1.03E+4 - + cos(q[0 * 3 + 0]) * cos(q[0 * 3 + 1]) * cos(q[0 * 3 + 2]) * + 1.1315E+4 + + cos(q[0 * 3 + 0]) * sin(q[0 * 3 + 1]) * sin(q[0 * 3 + 2]) * + 1.1315E+4 - + RL * cos(q[0 * 3 + 1]) * sin(q[0 * 3 + 0]) * sin(q[0 * 3 + 2]) * + 5.049E+3 - + RL * cos(q[0 * 3 + 2]) * sin(q[0 * 3 + 0]) * sin(q[0 * 3 + 1]) * + 5.049E+3)) / + ((pow(cos(q[0 * 3 + 0]), 2.0) + pow(sin(q[0 * 3 + 0]), 2.0)) * + (pow(cos(q[0 * 3 + 0]), 2.0) * pow(cos(q[0 * 3 + 1]), 2.0) * + pow(sin(q[0 * 3 + 2]), 2.0) + + pow(cos(q[0 * 3 + 0]), 2.0) * pow(cos(q[0 * 3 + 2]), 2.0) * + pow(sin(q[0 * 3 + 1]), 2.0) + + pow(cos(q[0 * 3 + 1]), 2.0) * pow(cos(q[0 * 3 + 2]), 2.0) * + pow(sin(q[0 * 3 + 0]), 2.0) + + pow(cos(q[0 * 3 + 0]), 2.0) * pow(sin(q[0 * 3 + 1]), 2.0) * + pow(sin(q[0 * 3 + 2]), 2.0) + + pow(cos(q[0 * 3 + 1]), 2.0) * pow(sin(q[0 * 3 + 0]), 2.0) * + pow(sin(q[0 * 3 + 2]), 2.0) + + pow(cos(q[0 * 3 + 2]), 2.0) * pow(sin(q[0 * 3 + 0]), 2.0) * + pow(sin(q[0 * 3 + 1]), 2.0) + + pow(sin(q[0 * 3 + 0]), 2.0) * pow(sin(q[0 * 3 + 1]), 2.0) * + pow(sin(q[0 * 3 + 2]), 2.0) + + pow(cos(q[0 * 3 + 0]), 2.0) * pow(cos(q[0 * 3 + 1]), 2.0) * + pow(cos(q[0 * 3 + 2]), 2.0)) * + 1.0E+5) + + ((cos(q[0 * 3 + 1]) * sin(q[0 * 3 + 2]) + + cos(q[0 * 3 + 2]) * sin(q[0 * 3 + 1])) * + (pow(cos(q[0 * 3 + 0]), 2.0) * sin(q[0 * 3 + 1]) * 1.03E+2 + + pow(sin(q[0 * 3 + 0]), 2.0) * sin(q[0 * 3 + 1]) * 1.03E+2 + + RL * pow(cos(q[0 * 3 + 1]), 2.0) * sin(q[0 * 3 + 0]) * 3.5E+1 + + RL * sin(q[0 * 3 + 0]) * pow(sin(q[0 * 3 + 1]), 2.0) * 3.5E+1)) / + ((pow(cos(q[0 * 3 + 0]), 2.0) * pow(cos(q[0 * 3 + 1]), 2.0) + + pow(cos(q[0 * 3 + 0]), 2.0) * pow(sin(q[0 * 3 + 1]), 2.0) + + pow(cos(q[0 * 3 + 1]), 2.0) * pow(sin(q[0 * 3 + 0]), 2.0) + + pow(sin(q[0 * 3 + 0]), 2.0) * pow(sin(q[0 * 3 + 1]), 2.0)) * + (pow(cos(q[0 * 3 + 1]), 2.0) * pow(cos(q[0 * 3 + 2]), 2.0) + + pow(cos(q[0 * 3 + 1]), 2.0) * pow(sin(q[0 * 3 + 2]), 2.0) + + pow(cos(q[0 * 3 + 2]), 2.0) * pow(sin(q[0 * 3 + 1]), 2.0) + + pow(sin(q[0 * 3 + 1]), 2.0) * pow(sin(q[0 * 3 + 2]), 2.0)) * + 1.0E+3) + + (sin(q[0 * 3 + 0]) * + (sin(q[0 * 3 + 0]) * sin(q[0 * 3 + 1]) * sin(q[0 * 3 + 2]) * + 1.1315E+4 + + pow(cos(q[0 * 3 + 1]), 2.0) * cos(q[0 * 3 + 2]) * + sin(q[0 * 3 + 0]) * 1.03E+4 + + cos(q[0 * 3 + 2]) * sin(q[0 * 3 + 0]) * + pow(sin(q[0 * 3 + 1]), 2.0) * 1.03E+4 - + cos(q[0 * 3 + 1]) * cos(q[0 * 3 + 2]) * sin(q[0 * 3 + 0]) * + 1.1315E+4 + + RL * cos(q[0 * 3 + 0]) * cos(q[0 * 3 + 1]) * sin(q[0 * 3 + 2]) * + 5.049E+3 + + RL * cos(q[0 * 3 + 0]) * cos(q[0 * 3 + 2]) * sin(q[0 * 3 + 1]) * + 5.049E+3 + + RL * pow(cos(q[0 * 3 + 0]), 2.0) * cos(q[0 * 3 + 1]) * + sin(q[0 * 3 + 2]) * 3.5E+3 + + RL * pow(cos(q[0 * 3 + 0]), 2.0) * cos(q[0 * 3 + 2]) * + sin(q[0 * 3 + 1]) * 3.5E+3 + + RL * cos(q[0 * 3 + 1]) * pow(sin(q[0 * 3 + 0]), 2.0) * + sin(q[0 * 3 + 2]) * 3.5E+3 + + RL * cos(q[0 * 3 + 2]) * pow(sin(q[0 * 3 + 0]), 2.0) * + sin(q[0 * 3 + 1]) * 3.5E+3)) / + ((pow(cos(q[0 * 3 + 0]), 2.0) + pow(sin(q[0 * 3 + 0]), 2.0)) * + (pow(cos(q[0 * 3 + 0]), 2.0) * pow(cos(q[0 * 3 + 1]), 2.0) * + pow(sin(q[0 * 3 + 2]), 2.0) + + pow(cos(q[0 * 3 + 0]), 2.0) * pow(cos(q[0 * 3 + 2]), 2.0) * + pow(sin(q[0 * 3 + 1]), 2.0) + + pow(cos(q[0 * 3 + 1]), 2.0) * pow(cos(q[0 * 3 + 2]), 2.0) * + pow(sin(q[0 * 3 + 0]), 2.0) + + pow(cos(q[0 * 3 + 0]), 2.0) * pow(sin(q[0 * 3 + 1]), 2.0) * + pow(sin(q[0 * 3 + 2]), 2.0) + + pow(cos(q[0 * 3 + 1]), 2.0) * pow(sin(q[0 * 3 + 0]), 2.0) * + pow(sin(q[0 * 3 + 2]), 2.0) + + pow(cos(q[0 * 3 + 2]), 2.0) * pow(sin(q[0 * 3 + 0]), 2.0) * + pow(sin(q[0 * 3 + 1]), 2.0) + + pow(sin(q[0 * 3 + 0]), 2.0) * pow(sin(q[0 * 3 + 1]), 2.0) * + pow(sin(q[0 * 3 + 2]), 2.0) + + pow(cos(q[0 * 3 + 0]), 2.0) * pow(cos(q[0 * 3 + 1]), 2.0) * + pow(cos(q[0 * 3 + 2]), 2.0)) * + 1.0E+5) - + (pow(cos(q[0 * 3 + 0]), 2.0) * + (cos(q[0 * 3 + 1]) * cos(q[0 * 3 + 2]) - + sin(q[0 * 3 + 1]) * sin(q[0 * 3 + 2])) * + (cos(q[0 * 3 + 1]) * -2.06E+3 + + pow(cos(q[0 * 3 + 1]), 2.0) * 2.263E+3 + + pow(sin(q[0 * 3 + 1]), 2.0) * 2.263E+3)) / + ((pow(cos(q[0 * 3 + 0]), 2.0) * pow(cos(q[0 * 3 + 1]), 2.0) + + pow(cos(q[0 * 3 + 0]), 2.0) * pow(sin(q[0 * 3 + 1]), 2.0) + + pow(cos(q[0 * 3 + 1]), 2.0) * pow(sin(q[0 * 3 + 0]), 2.0) + + pow(sin(q[0 * 3 + 0]), 2.0) * pow(sin(q[0 * 3 + 1]), 2.0)) * + (pow(cos(q[0 * 3 + 0]), 2.0) * pow(cos(q[0 * 3 + 1]), 2.0) * + pow(sin(q[0 * 3 + 2]), 2.0) + + pow(cos(q[0 * 3 + 0]), 2.0) * pow(cos(q[0 * 3 + 2]), 2.0) * + pow(sin(q[0 * 3 + 1]), 2.0) + + pow(cos(q[0 * 3 + 1]), 2.0) * pow(cos(q[0 * 3 + 2]), 2.0) * + pow(sin(q[0 * 3 + 0]), 2.0) + + pow(cos(q[0 * 3 + 0]), 2.0) * pow(sin(q[0 * 3 + 1]), 2.0) * + pow(sin(q[0 * 3 + 2]), 2.0) + + pow(cos(q[0 * 3 + 1]), 2.0) * pow(sin(q[0 * 3 + 0]), 2.0) * + pow(sin(q[0 * 3 + 2]), 2.0) + + pow(cos(q[0 * 3 + 2]), 2.0) * pow(sin(q[0 * 3 + 0]), 2.0) * + pow(sin(q[0 * 3 + 1]), 2.0) + + pow(sin(q[0 * 3 + 0]), 2.0) * pow(sin(q[0 * 3 + 1]), 2.0) * + pow(sin(q[0 * 3 + 2]), 2.0) + + pow(cos(q[0 * 3 + 0]), 2.0) * pow(cos(q[0 * 3 + 1]), 2.0) * + pow(cos(q[0 * 3 + 2]), 2.0)) * + 2.0E+4) - + (pow(sin(q[0 * 3 + 0]), 2.0) * + (cos(q[0 * 3 + 1]) * cos(q[0 * 3 + 2]) - + sin(q[0 * 3 + 1]) * sin(q[0 * 3 + 2])) * + (cos(q[0 * 3 + 1]) * -2.06E+3 + + pow(cos(q[0 * 3 + 1]), 2.0) * 2.263E+3 + + pow(sin(q[0 * 3 + 1]), 2.0) * 2.263E+3)) / + ((pow(cos(q[0 * 3 + 0]), 2.0) * pow(cos(q[0 * 3 + 1]), 2.0) + + pow(cos(q[0 * 3 + 0]), 2.0) * pow(sin(q[0 * 3 + 1]), 2.0) + + pow(cos(q[0 * 3 + 1]), 2.0) * pow(sin(q[0 * 3 + 0]), 2.0) + + pow(sin(q[0 * 3 + 0]), 2.0) * pow(sin(q[0 * 3 + 1]), 2.0)) * + (pow(cos(q[0 * 3 + 0]), 2.0) * pow(cos(q[0 * 3 + 1]), 2.0) * + pow(sin(q[0 * 3 + 2]), 2.0) + + pow(cos(q[0 * 3 + 0]), 2.0) * pow(cos(q[0 * 3 + 2]), 2.0) * + pow(sin(q[0 * 3 + 1]), 2.0) + + pow(cos(q[0 * 3 + 1]), 2.0) * pow(cos(q[0 * 3 + 2]), 2.0) * + pow(sin(q[0 * 3 + 0]), 2.0) + + pow(cos(q[0 * 3 + 0]), 2.0) * pow(sin(q[0 * 3 + 1]), 2.0) * + pow(sin(q[0 * 3 + 2]), 2.0) + + pow(cos(q[0 * 3 + 1]), 2.0) * pow(sin(q[0 * 3 + 0]), 2.0) * + pow(sin(q[0 * 3 + 2]), 2.0) + + pow(cos(q[0 * 3 + 2]), 2.0) * pow(sin(q[0 * 3 + 0]), 2.0) * + pow(sin(q[0 * 3 + 1]), 2.0) + + pow(sin(q[0 * 3 + 0]), 2.0) * pow(sin(q[0 * 3 + 1]), 2.0) * + pow(sin(q[0 * 3 + 2]), 2.0) + + pow(cos(q[0 * 3 + 0]), 2.0) * pow(cos(q[0 * 3 + 1]), 2.0) * + pow(cos(q[0 * 3 + 2]), 2.0)) * + 2.0E+4) - + 1.03E+2 / 5.0E+2)) / + (pow(cos(q[0 * 3 + 0]), 2.0) * pow(cos(q[0 * 3 + 1]), 2.0) * + pow(sin(q[0 * 3 + 2]), 2.0) + + pow(cos(q[0 * 3 + 0]), 2.0) * pow(cos(q[0 * 3 + 2]), 2.0) * + pow(sin(q[0 * 3 + 1]), 2.0) + + pow(cos(q[0 * 3 + 1]), 2.0) * pow(cos(q[0 * 3 + 2]), 2.0) * + pow(sin(q[0 * 3 + 0]), 2.0) + + pow(cos(q[0 * 3 + 0]), 2.0) * pow(sin(q[0 * 3 + 1]), 2.0) * + pow(sin(q[0 * 3 + 2]), 2.0) + + pow(cos(q[0 * 3 + 1]), 2.0) * pow(sin(q[0 * 3 + 0]), 2.0) * + pow(sin(q[0 * 3 + 2]), 2.0) + + pow(cos(q[0 * 3 + 2]), 2.0) * pow(sin(q[0 * 3 + 0]), 2.0) * + pow(sin(q[0 * 3 + 1]), 2.0) + + pow(sin(q[0 * 3 + 0]), 2.0) * pow(sin(q[0 * 3 + 1]), 2.0) * + pow(sin(q[0 * 3 + 2]), 2.0) + + pow(cos(q[0 * 3 + 0]), 2.0) * pow(cos(q[0 * 3 + 1]), 2.0) * + pow(cos(q[0 * 3 + 2]), 2.0)) + + (cos(q[0 * 3 + 0]) * + (cos(q[0 * 3 + 1]) * cos(q[0 * 3 + 2]) - + sin(q[0 * 3 + 1]) * sin(q[0 * 3 + 2])) * + ((sin(q[0 * 3 + 0]) * + (pow(cos(q[0 * 3 + 1]), 2.0) * sin(q[0 * 3 + 0]) * + sin(q[0 * 3 + 2]) * 1.03E+4 + + sin(q[0 * 3 + 0]) * pow(sin(q[0 * 3 + 1]), 2.0) * + sin(q[0 * 3 + 2]) * 1.03E+4 - + cos(q[0 * 3 + 1]) * sin(q[0 * 3 + 0]) * sin(q[0 * 3 + 2]) * + 1.1315E+4 - + cos(q[0 * 3 + 2]) * sin(q[0 * 3 + 0]) * sin(q[0 * 3 + 1]) * + 1.1315E+4 - + RL * cos(q[0 * 3 + 0]) * cos(q[0 * 3 + 1]) * cos(q[0 * 3 + 2]) * + 5.049E+3 + + RL * cos(q[0 * 3 + 0]) * sin(q[0 * 3 + 1]) * sin(q[0 * 3 + 2]) * + 5.049E+3 - + RL * pow(cos(q[0 * 3 + 0]), 2.0) * cos(q[0 * 3 + 1]) * + cos(q[0 * 3 + 2]) * 3.5E+3 - + RL * cos(q[0 * 3 + 1]) * cos(q[0 * 3 + 2]) * + pow(sin(q[0 * 3 + 0]), 2.0) * 3.5E+3 + + RL * pow(cos(q[0 * 3 + 0]), 2.0) * sin(q[0 * 3 + 1]) * + sin(q[0 * 3 + 2]) * 3.5E+3 + + RL * pow(sin(q[0 * 3 + 0]), 2.0) * sin(q[0 * 3 + 1]) * + sin(q[0 * 3 + 2]) * 3.5E+3) * + (-1.0E-5)) / + ((pow(cos(q[0 * 3 + 0]), 2.0) + pow(sin(q[0 * 3 + 0]), 2.0)) * + (pow(cos(q[0 * 3 + 0]), 2.0) * pow(cos(q[0 * 3 + 1]), 2.0) * + pow(sin(q[0 * 3 + 2]), 2.0) + + pow(cos(q[0 * 3 + 0]), 2.0) * pow(cos(q[0 * 3 + 2]), 2.0) * + pow(sin(q[0 * 3 + 1]), 2.0) + + pow(cos(q[0 * 3 + 1]), 2.0) * pow(cos(q[0 * 3 + 2]), 2.0) * + pow(sin(q[0 * 3 + 0]), 2.0) + + pow(cos(q[0 * 3 + 0]), 2.0) * pow(sin(q[0 * 3 + 1]), 2.0) * + pow(sin(q[0 * 3 + 2]), 2.0) + + pow(cos(q[0 * 3 + 1]), 2.0) * pow(sin(q[0 * 3 + 0]), 2.0) * + pow(sin(q[0 * 3 + 2]), 2.0) + + pow(cos(q[0 * 3 + 2]), 2.0) * pow(sin(q[0 * 3 + 0]), 2.0) * + pow(sin(q[0 * 3 + 1]), 2.0) + + pow(sin(q[0 * 3 + 0]), 2.0) * pow(sin(q[0 * 3 + 1]), 2.0) * + pow(sin(q[0 * 3 + 2]), 2.0) + + pow(cos(q[0 * 3 + 0]), 2.0) * pow(cos(q[0 * 3 + 1]), 2.0) * + pow(cos(q[0 * 3 + 2]), 2.0))) + + ((cos(q[0 * 3 + 1]) * cos(q[0 * 3 + 2]) - + sin(q[0 * 3 + 1]) * sin(q[0 * 3 + 2])) * + (pow(cos(q[0 * 3 + 0]), 2.0) * sin(q[0 * 3 + 1]) * 1.03E+2 + + pow(sin(q[0 * 3 + 0]), 2.0) * sin(q[0 * 3 + 1]) * 1.03E+2 + + RL * pow(cos(q[0 * 3 + 1]), 2.0) * sin(q[0 * 3 + 0]) * 3.5E+1 + + RL * sin(q[0 * 3 + 0]) * pow(sin(q[0 * 3 + 1]), 2.0) * 3.5E+1)) / + ((pow(cos(q[0 * 3 + 0]), 2.0) * pow(cos(q[0 * 3 + 1]), 2.0) + + pow(cos(q[0 * 3 + 0]), 2.0) * pow(sin(q[0 * 3 + 1]), 2.0) + + pow(cos(q[0 * 3 + 1]), 2.0) * pow(sin(q[0 * 3 + 0]), 2.0) + + pow(sin(q[0 * 3 + 0]), 2.0) * pow(sin(q[0 * 3 + 1]), 2.0)) * + (pow(cos(q[0 * 3 + 1]), 2.0) * pow(cos(q[0 * 3 + 2]), 2.0) + + pow(cos(q[0 * 3 + 1]), 2.0) * pow(sin(q[0 * 3 + 2]), 2.0) + + pow(cos(q[0 * 3 + 2]), 2.0) * pow(sin(q[0 * 3 + 1]), 2.0) + + pow(sin(q[0 * 3 + 1]), 2.0) * pow(sin(q[0 * 3 + 2]), 2.0)) * + 1.0E+3) - + (cos(q[0 * 3 + 0]) * + (cos(q[0 * 3 + 0]) * pow(cos(q[0 * 3 + 1]), 2.0) * + sin(q[0 * 3 + 2]) * 1.03E+4 + + cos(q[0 * 3 + 0]) * pow(sin(q[0 * 3 + 1]), 2.0) * + sin(q[0 * 3 + 2]) * 1.03E+4 - + cos(q[0 * 3 + 0]) * cos(q[0 * 3 + 1]) * sin(q[0 * 3 + 2]) * + 1.1315E+4 - + cos(q[0 * 3 + 0]) * cos(q[0 * 3 + 2]) * sin(q[0 * 3 + 1]) * + 1.1315E+4 + + RL * cos(q[0 * 3 + 1]) * cos(q[0 * 3 + 2]) * sin(q[0 * 3 + 0]) * + 5.049E+3 - + RL * sin(q[0 * 3 + 0]) * sin(q[0 * 3 + 1]) * sin(q[0 * 3 + 2]) * + 5.049E+3)) / + ((pow(cos(q[0 * 3 + 0]), 2.0) + pow(sin(q[0 * 3 + 0]), 2.0)) * + (pow(cos(q[0 * 3 + 0]), 2.0) * pow(cos(q[0 * 3 + 1]), 2.0) * + pow(sin(q[0 * 3 + 2]), 2.0) + + pow(cos(q[0 * 3 + 0]), 2.0) * pow(cos(q[0 * 3 + 2]), 2.0) * + pow(sin(q[0 * 3 + 1]), 2.0) + + pow(cos(q[0 * 3 + 1]), 2.0) * pow(cos(q[0 * 3 + 2]), 2.0) * + pow(sin(q[0 * 3 + 0]), 2.0) + + pow(cos(q[0 * 3 + 0]), 2.0) * pow(sin(q[0 * 3 + 1]), 2.0) * + pow(sin(q[0 * 3 + 2]), 2.0) + + pow(cos(q[0 * 3 + 1]), 2.0) * pow(sin(q[0 * 3 + 0]), 2.0) * + pow(sin(q[0 * 3 + 2]), 2.0) + + pow(cos(q[0 * 3 + 2]), 2.0) * pow(sin(q[0 * 3 + 0]), 2.0) * + pow(sin(q[0 * 3 + 1]), 2.0) + + pow(sin(q[0 * 3 + 0]), 2.0) * pow(sin(q[0 * 3 + 1]), 2.0) * + pow(sin(q[0 * 3 + 2]), 2.0) + + pow(cos(q[0 * 3 + 0]), 2.0) * pow(cos(q[0 * 3 + 1]), 2.0) * + pow(cos(q[0 * 3 + 2]), 2.0)) * + 1.0E+5) + + (pow(cos(q[0 * 3 + 0]), 2.0) * + (cos(q[0 * 3 + 1]) * sin(q[0 * 3 + 2]) + + cos(q[0 * 3 + 2]) * sin(q[0 * 3 + 1])) * + (cos(q[0 * 3 + 1]) * -2.06E+3 + + pow(cos(q[0 * 3 + 1]), 2.0) * 2.263E+3 + + pow(sin(q[0 * 3 + 1]), 2.0) * 2.263E+3)) / + ((pow(cos(q[0 * 3 + 0]), 2.0) * pow(cos(q[0 * 3 + 1]), 2.0) + + pow(cos(q[0 * 3 + 0]), 2.0) * pow(sin(q[0 * 3 + 1]), 2.0) + + pow(cos(q[0 * 3 + 1]), 2.0) * pow(sin(q[0 * 3 + 0]), 2.0) + + pow(sin(q[0 * 3 + 0]), 2.0) * pow(sin(q[0 * 3 + 1]), 2.0)) * + (pow(cos(q[0 * 3 + 0]), 2.0) * pow(cos(q[0 * 3 + 1]), 2.0) * + pow(sin(q[0 * 3 + 2]), 2.0) + + pow(cos(q[0 * 3 + 0]), 2.0) * pow(cos(q[0 * 3 + 2]), 2.0) * + pow(sin(q[0 * 3 + 1]), 2.0) + + pow(cos(q[0 * 3 + 1]), 2.0) * pow(cos(q[0 * 3 + 2]), 2.0) * + pow(sin(q[0 * 3 + 0]), 2.0) + + pow(cos(q[0 * 3 + 0]), 2.0) * pow(sin(q[0 * 3 + 1]), 2.0) * + pow(sin(q[0 * 3 + 2]), 2.0) + + pow(cos(q[0 * 3 + 1]), 2.0) * pow(sin(q[0 * 3 + 0]), 2.0) * + pow(sin(q[0 * 3 + 2]), 2.0) + + pow(cos(q[0 * 3 + 2]), 2.0) * pow(sin(q[0 * 3 + 0]), 2.0) * + pow(sin(q[0 * 3 + 1]), 2.0) + + pow(sin(q[0 * 3 + 0]), 2.0) * pow(sin(q[0 * 3 + 1]), 2.0) * + pow(sin(q[0 * 3 + 2]), 2.0) + + pow(cos(q[0 * 3 + 0]), 2.0) * pow(cos(q[0 * 3 + 1]), 2.0) * + pow(cos(q[0 * 3 + 2]), 2.0)) * + 2.0E+4) + + (pow(sin(q[0 * 3 + 0]), 2.0) * + (cos(q[0 * 3 + 1]) * sin(q[0 * 3 + 2]) + + cos(q[0 * 3 + 2]) * sin(q[0 * 3 + 1])) * + (cos(q[0 * 3 + 1]) * -2.06E+3 + + pow(cos(q[0 * 3 + 1]), 2.0) * 2.263E+3 + + pow(sin(q[0 * 3 + 1]), 2.0) * 2.263E+3)) / + ((pow(cos(q[0 * 3 + 0]), 2.0) * pow(cos(q[0 * 3 + 1]), 2.0) + + pow(cos(q[0 * 3 + 0]), 2.0) * pow(sin(q[0 * 3 + 1]), 2.0) + + pow(cos(q[0 * 3 + 1]), 2.0) * pow(sin(q[0 * 3 + 0]), 2.0) + + pow(sin(q[0 * 3 + 0]), 2.0) * pow(sin(q[0 * 3 + 1]), 2.0)) * + (pow(cos(q[0 * 3 + 0]), 2.0) * pow(cos(q[0 * 3 + 1]), 2.0) * + pow(sin(q[0 * 3 + 2]), 2.0) + + pow(cos(q[0 * 3 + 0]), 2.0) * pow(cos(q[0 * 3 + 2]), 2.0) * + pow(sin(q[0 * 3 + 1]), 2.0) + + pow(cos(q[0 * 3 + 1]), 2.0) * pow(cos(q[0 * 3 + 2]), 2.0) * + pow(sin(q[0 * 3 + 0]), 2.0) + + pow(cos(q[0 * 3 + 0]), 2.0) * pow(sin(q[0 * 3 + 1]), 2.0) * + pow(sin(q[0 * 3 + 2]), 2.0) + + pow(cos(q[0 * 3 + 1]), 2.0) * pow(sin(q[0 * 3 + 0]), 2.0) * + pow(sin(q[0 * 3 + 2]), 2.0) + + pow(cos(q[0 * 3 + 2]), 2.0) * pow(sin(q[0 * 3 + 0]), 2.0) * + pow(sin(q[0 * 3 + 1]), 2.0) + + pow(sin(q[0 * 3 + 0]), 2.0) * pow(sin(q[0 * 3 + 1]), 2.0) * + pow(sin(q[0 * 3 + 2]), 2.0) + + pow(cos(q[0 * 3 + 0]), 2.0) * pow(cos(q[0 * 3 + 1]), 2.0) * + pow(cos(q[0 * 3 + 2]), 2.0)) * + 2.0E+4))) / + (pow(cos(q[0 * 3 + 0]), 2.0) * pow(cos(q[0 * 3 + 1]), 2.0) * + pow(sin(q[0 * 3 + 2]), 2.0) + + pow(cos(q[0 * 3 + 0]), 2.0) * pow(cos(q[0 * 3 + 2]), 2.0) * + pow(sin(q[0 * 3 + 1]), 2.0) + + pow(cos(q[0 * 3 + 1]), 2.0) * pow(cos(q[0 * 3 + 2]), 2.0) * + pow(sin(q[0 * 3 + 0]), 2.0) + + pow(cos(q[0 * 3 + 0]), 2.0) * pow(sin(q[0 * 3 + 1]), 2.0) * + pow(sin(q[0 * 3 + 2]), 2.0) + + pow(cos(q[0 * 3 + 1]), 2.0) * pow(sin(q[0 * 3 + 0]), 2.0) * + pow(sin(q[0 * 3 + 2]), 2.0) + + pow(cos(q[0 * 3 + 2]), 2.0) * pow(sin(q[0 * 3 + 0]), 2.0) * + pow(sin(q[0 * 3 + 1]), 2.0) + + pow(sin(q[0 * 3 + 0]), 2.0) * pow(sin(q[0 * 3 + 1]), 2.0) * + pow(sin(q[0 * 3 + 2]), 2.0) + + pow(cos(q[0 * 3 + 0]), 2.0) * pow(cos(q[0 * 3 + 1]), 2.0) * + pow(cos(q[0 * 3 + 2]), 2.0)))) / + (pow(cos(q[0 * 3 + 0]), 2.0) + pow(sin(q[0 * 3 + 0]), 2.0)) + + (sin(q[0 * 3 + 0]) * + ((cos(q[0 * 3 + 0]) * + ((cos(q[0 * 3 + 0]) * + (cos(q[0 * 3 + 1]) * sin(q[0 * 3 + 2]) + + cos(q[0 * 3 + 2]) * sin(q[0 * 3 + 1])) * + 1.0 / + pow(pow(cos(q[0 * 3 + 0]), 2.0) * pow(cos(q[0 * 3 + 1]), 2.0) * + pow(sin(q[0 * 3 + 2]), 2.0) + + pow(cos(q[0 * 3 + 0]), 2.0) * pow(cos(q[0 * 3 + 2]), 2.0) * + pow(sin(q[0 * 3 + 1]), 2.0) + + pow(cos(q[0 * 3 + 1]), 2.0) * pow(cos(q[0 * 3 + 2]), 2.0) * + pow(sin(q[0 * 3 + 0]), 2.0) + + pow(cos(q[0 * 3 + 0]), 2.0) * pow(sin(q[0 * 3 + 1]), 2.0) * + pow(sin(q[0 * 3 + 2]), 2.0) + + pow(cos(q[0 * 3 + 1]), 2.0) * pow(sin(q[0 * 3 + 0]), 2.0) * + pow(sin(q[0 * 3 + 2]), 2.0) + + pow(cos(q[0 * 3 + 2]), 2.0) * pow(sin(q[0 * 3 + 0]), 2.0) * + pow(sin(q[0 * 3 + 1]), 2.0) + + pow(sin(q[0 * 3 + 0]), 2.0) * pow(sin(q[0 * 3 + 1]), 2.0) * + pow(sin(q[0 * 3 + 2]), 2.0) + + pow(cos(q[0 * 3 + 0]), 2.0) * pow(cos(q[0 * 3 + 1]), 2.0) * + pow(cos(q[0 * 3 + 2]), 2.0), + 2.0) * + (sin(q[0 * 3 + 0]) * sin(q[0 * 3 + 1]) * sin(q[0 * 3 + 2]) * + 1.1315E+4 + + pow(cos(q[0 * 3 + 1]), 2.0) * cos(q[0 * 3 + 2]) * + sin(q[0 * 3 + 0]) * 1.03E+4 + + cos(q[0 * 3 + 2]) * sin(q[0 * 3 + 0]) * + pow(sin(q[0 * 3 + 1]), 2.0) * 1.03E+4 - + cos(q[0 * 3 + 1]) * cos(q[0 * 3 + 2]) * sin(q[0 * 3 + 0]) * + 1.1315E+4 + + RL * cos(q[0 * 3 + 0]) * cos(q[0 * 3 + 1]) * sin(q[0 * 3 + 2]) * + 5.049E+3 + + RL * cos(q[0 * 3 + 0]) * cos(q[0 * 3 + 2]) * sin(q[0 * 3 + 1]) * + 5.049E+3 + + RL * pow(cos(q[0 * 3 + 0]), 2.0) * cos(q[0 * 3 + 1]) * + sin(q[0 * 3 + 2]) * 3.5E+3 + + RL * pow(cos(q[0 * 3 + 0]), 2.0) * cos(q[0 * 3 + 2]) * + sin(q[0 * 3 + 1]) * 3.5E+3 + + RL * cos(q[0 * 3 + 1]) * pow(sin(q[0 * 3 + 0]), 2.0) * + sin(q[0 * 3 + 2]) * 3.5E+3 + + RL * cos(q[0 * 3 + 2]) * pow(sin(q[0 * 3 + 0]), 2.0) * + sin(q[0 * 3 + 1]) * 3.5E+3)) / + 1.0E+5 - + (cos(q[0 * 3 + 0]) * + (cos(q[0 * 3 + 1]) * cos(q[0 * 3 + 2]) - + sin(q[0 * 3 + 1]) * sin(q[0 * 3 + 2])) * + 1.0 / + pow(pow(cos(q[0 * 3 + 0]), 2.0) * pow(cos(q[0 * 3 + 1]), 2.0) * + pow(sin(q[0 * 3 + 2]), 2.0) + + pow(cos(q[0 * 3 + 0]), 2.0) * pow(cos(q[0 * 3 + 2]), 2.0) * + pow(sin(q[0 * 3 + 1]), 2.0) + + pow(cos(q[0 * 3 + 1]), 2.0) * pow(cos(q[0 * 3 + 2]), 2.0) * + pow(sin(q[0 * 3 + 0]), 2.0) + + pow(cos(q[0 * 3 + 0]), 2.0) * pow(sin(q[0 * 3 + 1]), 2.0) * + pow(sin(q[0 * 3 + 2]), 2.0) + + pow(cos(q[0 * 3 + 1]), 2.0) * pow(sin(q[0 * 3 + 0]), 2.0) * + pow(sin(q[0 * 3 + 2]), 2.0) + + pow(cos(q[0 * 3 + 2]), 2.0) * pow(sin(q[0 * 3 + 0]), 2.0) * + pow(sin(q[0 * 3 + 1]), 2.0) + + pow(sin(q[0 * 3 + 0]), 2.0) * pow(sin(q[0 * 3 + 1]), 2.0) * + pow(sin(q[0 * 3 + 2]), 2.0) + + pow(cos(q[0 * 3 + 0]), 2.0) * pow(cos(q[0 * 3 + 1]), 2.0) * + pow(cos(q[0 * 3 + 2]), 2.0), + 2.0) * + (pow(cos(q[0 * 3 + 1]), 2.0) * sin(q[0 * 3 + 0]) * + sin(q[0 * 3 + 2]) * 1.03E+4 + + sin(q[0 * 3 + 0]) * pow(sin(q[0 * 3 + 1]), 2.0) * + sin(q[0 * 3 + 2]) * 1.03E+4 - + cos(q[0 * 3 + 1]) * sin(q[0 * 3 + 0]) * sin(q[0 * 3 + 2]) * + 1.1315E+4 - + cos(q[0 * 3 + 2]) * sin(q[0 * 3 + 0]) * sin(q[0 * 3 + 1]) * + 1.1315E+4 - + RL * cos(q[0 * 3 + 0]) * cos(q[0 * 3 + 1]) * cos(q[0 * 3 + 2]) * + 5.049E+3 + + RL * cos(q[0 * 3 + 0]) * sin(q[0 * 3 + 1]) * sin(q[0 * 3 + 2]) * + 5.049E+3 - + RL * pow(cos(q[0 * 3 + 0]), 2.0) * cos(q[0 * 3 + 1]) * + cos(q[0 * 3 + 2]) * 3.5E+3 - + RL * cos(q[0 * 3 + 1]) * cos(q[0 * 3 + 2]) * + pow(sin(q[0 * 3 + 0]), 2.0) * 3.5E+3 + + RL * pow(cos(q[0 * 3 + 0]), 2.0) * sin(q[0 * 3 + 1]) * + sin(q[0 * 3 + 2]) * 3.5E+3 + + RL * pow(sin(q[0 * 3 + 0]), 2.0) * sin(q[0 * 3 + 1]) * + sin(q[0 * 3 + 2]) * 3.5E+3)) / + 1.0E+5 - + (sin(q[0 * 3 + 0]) * + (cos(q[0 * 3 + 1]) * sin(q[0 * 3 + 2]) + + cos(q[0 * 3 + 2]) * sin(q[0 * 3 + 1])) * + (cos(q[0 * 3 + 0]) * pow(cos(q[0 * 3 + 1]), 2.0) * + cos(q[0 * 3 + 2]) * 1.03E+4 + + cos(q[0 * 3 + 0]) * cos(q[0 * 3 + 2]) * + pow(sin(q[0 * 3 + 1]), 2.0) * 1.03E+4 - + cos(q[0 * 3 + 0]) * cos(q[0 * 3 + 1]) * cos(q[0 * 3 + 2]) * + 1.1315E+4 + + cos(q[0 * 3 + 0]) * sin(q[0 * 3 + 1]) * sin(q[0 * 3 + 2]) * + 1.1315E+4 - + RL * cos(q[0 * 3 + 1]) * sin(q[0 * 3 + 0]) * sin(q[0 * 3 + 2]) * + 5.049E+3 - + RL * cos(q[0 * 3 + 2]) * sin(q[0 * 3 + 0]) * sin(q[0 * 3 + 1]) * + 5.049E+3) * + 1.0 / + pow(pow(cos(q[0 * 3 + 0]), 2.0) * pow(cos(q[0 * 3 + 1]), 2.0) * + pow(sin(q[0 * 3 + 2]), 2.0) + + pow(cos(q[0 * 3 + 0]), 2.0) * pow(cos(q[0 * 3 + 2]), 2.0) * + pow(sin(q[0 * 3 + 1]), 2.0) + + pow(cos(q[0 * 3 + 1]), 2.0) * pow(cos(q[0 * 3 + 2]), 2.0) * + pow(sin(q[0 * 3 + 0]), 2.0) + + pow(cos(q[0 * 3 + 0]), 2.0) * pow(sin(q[0 * 3 + 1]), 2.0) * + pow(sin(q[0 * 3 + 2]), 2.0) + + pow(cos(q[0 * 3 + 1]), 2.0) * pow(sin(q[0 * 3 + 0]), 2.0) * + pow(sin(q[0 * 3 + 2]), 2.0) + + pow(cos(q[0 * 3 + 2]), 2.0) * pow(sin(q[0 * 3 + 0]), 2.0) * + pow(sin(q[0 * 3 + 1]), 2.0) + + pow(sin(q[0 * 3 + 0]), 2.0) * pow(sin(q[0 * 3 + 1]), 2.0) * + pow(sin(q[0 * 3 + 2]), 2.0) + + pow(cos(q[0 * 3 + 0]), 2.0) * pow(cos(q[0 * 3 + 1]), 2.0) * + pow(cos(q[0 * 3 + 2]), 2.0), + 2.0)) / + 1.0E+5 + + (sin(q[0 * 3 + 0]) * + (cos(q[0 * 3 + 1]) * cos(q[0 * 3 + 2]) - + sin(q[0 * 3 + 1]) * sin(q[0 * 3 + 2])) * + (cos(q[0 * 3 + 0]) * pow(cos(q[0 * 3 + 1]), 2.0) * + sin(q[0 * 3 + 2]) * 1.03E+4 + + cos(q[0 * 3 + 0]) * pow(sin(q[0 * 3 + 1]), 2.0) * + sin(q[0 * 3 + 2]) * 1.03E+4 - + cos(q[0 * 3 + 0]) * cos(q[0 * 3 + 1]) * sin(q[0 * 3 + 2]) * + 1.1315E+4 - + cos(q[0 * 3 + 0]) * cos(q[0 * 3 + 2]) * sin(q[0 * 3 + 1]) * + 1.1315E+4 + + RL * cos(q[0 * 3 + 1]) * cos(q[0 * 3 + 2]) * sin(q[0 * 3 + 0]) * + 5.049E+3 - + RL * sin(q[0 * 3 + 0]) * sin(q[0 * 3 + 1]) * sin(q[0 * 3 + 2]) * + 5.049E+3) * + 1.0 / + pow(pow(cos(q[0 * 3 + 0]), 2.0) * pow(cos(q[0 * 3 + 1]), 2.0) * + pow(sin(q[0 * 3 + 2]), 2.0) + + pow(cos(q[0 * 3 + 0]), 2.0) * pow(cos(q[0 * 3 + 2]), 2.0) * + pow(sin(q[0 * 3 + 1]), 2.0) + + pow(cos(q[0 * 3 + 1]), 2.0) * pow(cos(q[0 * 3 + 2]), 2.0) * + pow(sin(q[0 * 3 + 0]), 2.0) + + pow(cos(q[0 * 3 + 0]), 2.0) * pow(sin(q[0 * 3 + 1]), 2.0) * + pow(sin(q[0 * 3 + 2]), 2.0) + + pow(cos(q[0 * 3 + 1]), 2.0) * pow(sin(q[0 * 3 + 0]), 2.0) * + pow(sin(q[0 * 3 + 2]), 2.0) + + pow(cos(q[0 * 3 + 2]), 2.0) * pow(sin(q[0 * 3 + 0]), 2.0) * + pow(sin(q[0 * 3 + 1]), 2.0) + + pow(sin(q[0 * 3 + 0]), 2.0) * pow(sin(q[0 * 3 + 1]), 2.0) * + pow(sin(q[0 * 3 + 2]), 2.0) + + pow(cos(q[0 * 3 + 0]), 2.0) * pow(cos(q[0 * 3 + 1]), 2.0) * + pow(cos(q[0 * 3 + 2]), 2.0), + 2.0)) / + 1.0E+5 + + ((cos(q[0 * 3 + 1]) * sin(q[0 * 3 + 2]) + + cos(q[0 * 3 + 2]) * sin(q[0 * 3 + 1])) * + (RL * cos(q[0 * 3 + 0]) * cos(q[0 * 3 + 1]) * sin(q[0 * 3 + 2]) * + 3.5E+3 + + RL * cos(q[0 * 3 + 0]) * cos(q[0 * 3 + 2]) * sin(q[0 * 3 + 1]) * + 3.5E+3 + + RL * pow(cos(q[0 * 3 + 0]), 2.0) * cos(q[0 * 3 + 1]) * + sin(q[0 * 3 + 2]) * 5.049E+3 + + RL * pow(cos(q[0 * 3 + 0]), 2.0) * cos(q[0 * 3 + 2]) * + sin(q[0 * 3 + 1]) * 5.049E+3 + + RL * cos(q[0 * 3 + 1]) * pow(sin(q[0 * 3 + 0]), 2.0) * + sin(q[0 * 3 + 2]) * 5.049E+3 + + RL * cos(q[0 * 3 + 2]) * pow(sin(q[0 * 3 + 0]), 2.0) * + sin(q[0 * 3 + 1]) * 5.049E+3)) / + ((pow(cos(q[0 * 3 + 1]), 2.0) * pow(cos(q[0 * 3 + 2]), 2.0) + + pow(cos(q[0 * 3 + 1]), 2.0) * pow(sin(q[0 * 3 + 2]), 2.0) + + pow(cos(q[0 * 3 + 2]), 2.0) * pow(sin(q[0 * 3 + 1]), 2.0) + + pow(sin(q[0 * 3 + 1]), 2.0) * pow(sin(q[0 * 3 + 2]), 2.0)) * + (pow(cos(q[0 * 3 + 0]), 2.0) * pow(cos(q[0 * 3 + 1]), 2.0) * + pow(sin(q[0 * 3 + 2]), 2.0) + + pow(cos(q[0 * 3 + 0]), 2.0) * pow(cos(q[0 * 3 + 2]), 2.0) * + pow(sin(q[0 * 3 + 1]), 2.0) + + pow(cos(q[0 * 3 + 1]), 2.0) * pow(cos(q[0 * 3 + 2]), 2.0) * + pow(sin(q[0 * 3 + 0]), 2.0) + + pow(cos(q[0 * 3 + 0]), 2.0) * pow(sin(q[0 * 3 + 1]), 2.0) * + pow(sin(q[0 * 3 + 2]), 2.0) + + pow(cos(q[0 * 3 + 1]), 2.0) * pow(sin(q[0 * 3 + 0]), 2.0) * + pow(sin(q[0 * 3 + 2]), 2.0) + + pow(cos(q[0 * 3 + 2]), 2.0) * pow(sin(q[0 * 3 + 0]), 2.0) * + pow(sin(q[0 * 3 + 1]), 2.0) + + pow(sin(q[0 * 3 + 0]), 2.0) * pow(sin(q[0 * 3 + 1]), 2.0) * + pow(sin(q[0 * 3 + 2]), 2.0) + + pow(cos(q[0 * 3 + 0]), 2.0) * pow(cos(q[0 * 3 + 1]), 2.0) * + pow(cos(q[0 * 3 + 2]), 2.0)) * + 1.0E+5) + + ((cos(q[0 * 3 + 1]) * cos(q[0 * 3 + 2]) - + sin(q[0 * 3 + 1]) * sin(q[0 * 3 + 2])) * + (RL * cos(q[0 * 3 + 0]) * cos(q[0 * 3 + 1]) * cos(q[0 * 3 + 2]) * + 3.5E+3 - + RL * cos(q[0 * 3 + 0]) * sin(q[0 * 3 + 1]) * sin(q[0 * 3 + 2]) * + 3.5E+3 + + RL * pow(cos(q[0 * 3 + 0]), 2.0) * cos(q[0 * 3 + 1]) * + cos(q[0 * 3 + 2]) * 5.049E+3 + + RL * cos(q[0 * 3 + 1]) * cos(q[0 * 3 + 2]) * + pow(sin(q[0 * 3 + 0]), 2.0) * 5.049E+3 - + RL * pow(cos(q[0 * 3 + 0]), 2.0) * sin(q[0 * 3 + 1]) * + sin(q[0 * 3 + 2]) * 5.049E+3 - + RL * pow(sin(q[0 * 3 + 0]), 2.0) * sin(q[0 * 3 + 1]) * + sin(q[0 * 3 + 2]) * 5.049E+3)) / + ((pow(cos(q[0 * 3 + 1]), 2.0) * pow(cos(q[0 * 3 + 2]), 2.0) + + pow(cos(q[0 * 3 + 1]), 2.0) * pow(sin(q[0 * 3 + 2]), 2.0) + + pow(cos(q[0 * 3 + 2]), 2.0) * pow(sin(q[0 * 3 + 1]), 2.0) + + pow(sin(q[0 * 3 + 1]), 2.0) * pow(sin(q[0 * 3 + 2]), 2.0)) * + (pow(cos(q[0 * 3 + 0]), 2.0) * pow(cos(q[0 * 3 + 1]), 2.0) * + pow(sin(q[0 * 3 + 2]), 2.0) + + pow(cos(q[0 * 3 + 0]), 2.0) * pow(cos(q[0 * 3 + 2]), 2.0) * + pow(sin(q[0 * 3 + 1]), 2.0) + + pow(cos(q[0 * 3 + 1]), 2.0) * pow(cos(q[0 * 3 + 2]), 2.0) * + pow(sin(q[0 * 3 + 0]), 2.0) + + pow(cos(q[0 * 3 + 0]), 2.0) * pow(sin(q[0 * 3 + 1]), 2.0) * + pow(sin(q[0 * 3 + 2]), 2.0) + + pow(cos(q[0 * 3 + 1]), 2.0) * pow(sin(q[0 * 3 + 0]), 2.0) * + pow(sin(q[0 * 3 + 2]), 2.0) + + pow(cos(q[0 * 3 + 2]), 2.0) * pow(sin(q[0 * 3 + 0]), 2.0) * + pow(sin(q[0 * 3 + 1]), 2.0) + + pow(sin(q[0 * 3 + 0]), 2.0) * pow(sin(q[0 * 3 + 1]), 2.0) * + pow(sin(q[0 * 3 + 2]), 2.0) + + pow(cos(q[0 * 3 + 0]), 2.0) * pow(cos(q[0 * 3 + 1]), 2.0) * + pow(cos(q[0 * 3 + 2]), 2.0)) * + 1.0E+5))) / + (pow(cos(q[0 * 3 + 0]), 2.0) + pow(sin(q[0 * 3 + 0]), 2.0)) + + (sin(q[0 * 3 + 0]) * + (cos(q[0 * 3 + 1]) * sin(q[0 * 3 + 2]) + + cos(q[0 * 3 + 2]) * sin(q[0 * 3 + 1])) * + ((cos(q[0 * 3 + 0]) * + (cos(q[0 * 3 + 0]) * pow(cos(q[0 * 3 + 1]), 2.0) * + cos(q[0 * 3 + 2]) * 1.03E+4 + + cos(q[0 * 3 + 0]) * cos(q[0 * 3 + 2]) * + pow(sin(q[0 * 3 + 1]), 2.0) * 1.03E+4 - + cos(q[0 * 3 + 0]) * cos(q[0 * 3 + 1]) * cos(q[0 * 3 + 2]) * + 1.1315E+4 + + cos(q[0 * 3 + 0]) * sin(q[0 * 3 + 1]) * sin(q[0 * 3 + 2]) * + 1.1315E+4 - + RL * cos(q[0 * 3 + 1]) * sin(q[0 * 3 + 0]) * sin(q[0 * 3 + 2]) * + 5.049E+3 - + RL * cos(q[0 * 3 + 2]) * sin(q[0 * 3 + 0]) * sin(q[0 * 3 + 1]) * + 5.049E+3)) / + ((pow(cos(q[0 * 3 + 0]), 2.0) + pow(sin(q[0 * 3 + 0]), 2.0)) * + (pow(cos(q[0 * 3 + 0]), 2.0) * pow(cos(q[0 * 3 + 1]), 2.0) * + pow(sin(q[0 * 3 + 2]), 2.0) + + pow(cos(q[0 * 3 + 0]), 2.0) * pow(cos(q[0 * 3 + 2]), 2.0) * + pow(sin(q[0 * 3 + 1]), 2.0) + + pow(cos(q[0 * 3 + 1]), 2.0) * pow(cos(q[0 * 3 + 2]), 2.0) * + pow(sin(q[0 * 3 + 0]), 2.0) + + pow(cos(q[0 * 3 + 0]), 2.0) * pow(sin(q[0 * 3 + 1]), 2.0) * + pow(sin(q[0 * 3 + 2]), 2.0) + + pow(cos(q[0 * 3 + 1]), 2.0) * pow(sin(q[0 * 3 + 0]), 2.0) * + pow(sin(q[0 * 3 + 2]), 2.0) + + pow(cos(q[0 * 3 + 2]), 2.0) * pow(sin(q[0 * 3 + 0]), 2.0) * + pow(sin(q[0 * 3 + 1]), 2.0) + + pow(sin(q[0 * 3 + 0]), 2.0) * pow(sin(q[0 * 3 + 1]), 2.0) * + pow(sin(q[0 * 3 + 2]), 2.0) + + pow(cos(q[0 * 3 + 0]), 2.0) * pow(cos(q[0 * 3 + 1]), 2.0) * + pow(cos(q[0 * 3 + 2]), 2.0)) * + 1.0E+5) + + ((cos(q[0 * 3 + 1]) * sin(q[0 * 3 + 2]) + + cos(q[0 * 3 + 2]) * sin(q[0 * 3 + 1])) * + (pow(cos(q[0 * 3 + 0]), 2.0) * sin(q[0 * 3 + 1]) * 1.03E+2 + + pow(sin(q[0 * 3 + 0]), 2.0) * sin(q[0 * 3 + 1]) * 1.03E+2 + + RL * pow(cos(q[0 * 3 + 1]), 2.0) * sin(q[0 * 3 + 0]) * 3.5E+1 + + RL * sin(q[0 * 3 + 0]) * pow(sin(q[0 * 3 + 1]), 2.0) * 3.5E+1)) / + ((pow(cos(q[0 * 3 + 0]), 2.0) * pow(cos(q[0 * 3 + 1]), 2.0) + + pow(cos(q[0 * 3 + 0]), 2.0) * pow(sin(q[0 * 3 + 1]), 2.0) + + pow(cos(q[0 * 3 + 1]), 2.0) * pow(sin(q[0 * 3 + 0]), 2.0) + + pow(sin(q[0 * 3 + 0]), 2.0) * pow(sin(q[0 * 3 + 1]), 2.0)) * + (pow(cos(q[0 * 3 + 1]), 2.0) * pow(cos(q[0 * 3 + 2]), 2.0) + + pow(cos(q[0 * 3 + 1]), 2.0) * pow(sin(q[0 * 3 + 2]), 2.0) + + pow(cos(q[0 * 3 + 2]), 2.0) * pow(sin(q[0 * 3 + 1]), 2.0) + + pow(sin(q[0 * 3 + 1]), 2.0) * pow(sin(q[0 * 3 + 2]), 2.0)) * + 1.0E+3) + + (sin(q[0 * 3 + 0]) * + (sin(q[0 * 3 + 0]) * sin(q[0 * 3 + 1]) * sin(q[0 * 3 + 2]) * + 1.1315E+4 + + pow(cos(q[0 * 3 + 1]), 2.0) * cos(q[0 * 3 + 2]) * + sin(q[0 * 3 + 0]) * 1.03E+4 + + cos(q[0 * 3 + 2]) * sin(q[0 * 3 + 0]) * + pow(sin(q[0 * 3 + 1]), 2.0) * 1.03E+4 - + cos(q[0 * 3 + 1]) * cos(q[0 * 3 + 2]) * sin(q[0 * 3 + 0]) * + 1.1315E+4 + + RL * cos(q[0 * 3 + 0]) * cos(q[0 * 3 + 1]) * sin(q[0 * 3 + 2]) * + 5.049E+3 + + RL * cos(q[0 * 3 + 0]) * cos(q[0 * 3 + 2]) * sin(q[0 * 3 + 1]) * + 5.049E+3 + + RL * pow(cos(q[0 * 3 + 0]), 2.0) * cos(q[0 * 3 + 1]) * + sin(q[0 * 3 + 2]) * 3.5E+3 + + RL * pow(cos(q[0 * 3 + 0]), 2.0) * cos(q[0 * 3 + 2]) * + sin(q[0 * 3 + 1]) * 3.5E+3 + + RL * cos(q[0 * 3 + 1]) * pow(sin(q[0 * 3 + 0]), 2.0) * + sin(q[0 * 3 + 2]) * 3.5E+3 + + RL * cos(q[0 * 3 + 2]) * pow(sin(q[0 * 3 + 0]), 2.0) * + sin(q[0 * 3 + 1]) * 3.5E+3)) / + ((pow(cos(q[0 * 3 + 0]), 2.0) + pow(sin(q[0 * 3 + 0]), 2.0)) * + (pow(cos(q[0 * 3 + 0]), 2.0) * pow(cos(q[0 * 3 + 1]), 2.0) * + pow(sin(q[0 * 3 + 2]), 2.0) + + pow(cos(q[0 * 3 + 0]), 2.0) * pow(cos(q[0 * 3 + 2]), 2.0) * + pow(sin(q[0 * 3 + 1]), 2.0) + + pow(cos(q[0 * 3 + 1]), 2.0) * pow(cos(q[0 * 3 + 2]), 2.0) * + pow(sin(q[0 * 3 + 0]), 2.0) + + pow(cos(q[0 * 3 + 0]), 2.0) * pow(sin(q[0 * 3 + 1]), 2.0) * + pow(sin(q[0 * 3 + 2]), 2.0) + + pow(cos(q[0 * 3 + 1]), 2.0) * pow(sin(q[0 * 3 + 0]), 2.0) * + pow(sin(q[0 * 3 + 2]), 2.0) + + pow(cos(q[0 * 3 + 2]), 2.0) * pow(sin(q[0 * 3 + 0]), 2.0) * + pow(sin(q[0 * 3 + 1]), 2.0) + + pow(sin(q[0 * 3 + 0]), 2.0) * pow(sin(q[0 * 3 + 1]), 2.0) * + pow(sin(q[0 * 3 + 2]), 2.0) + + pow(cos(q[0 * 3 + 0]), 2.0) * pow(cos(q[0 * 3 + 1]), 2.0) * + pow(cos(q[0 * 3 + 2]), 2.0)) * + 1.0E+5) - + (pow(cos(q[0 * 3 + 0]), 2.0) * + (cos(q[0 * 3 + 1]) * cos(q[0 * 3 + 2]) - + sin(q[0 * 3 + 1]) * sin(q[0 * 3 + 2])) * + (cos(q[0 * 3 + 1]) * -2.06E+3 + + pow(cos(q[0 * 3 + 1]), 2.0) * 2.263E+3 + + pow(sin(q[0 * 3 + 1]), 2.0) * 2.263E+3)) / + ((pow(cos(q[0 * 3 + 0]), 2.0) * pow(cos(q[0 * 3 + 1]), 2.0) + + pow(cos(q[0 * 3 + 0]), 2.0) * pow(sin(q[0 * 3 + 1]), 2.0) + + pow(cos(q[0 * 3 + 1]), 2.0) * pow(sin(q[0 * 3 + 0]), 2.0) + + pow(sin(q[0 * 3 + 0]), 2.0) * pow(sin(q[0 * 3 + 1]), 2.0)) * + (pow(cos(q[0 * 3 + 0]), 2.0) * pow(cos(q[0 * 3 + 1]), 2.0) * + pow(sin(q[0 * 3 + 2]), 2.0) + + pow(cos(q[0 * 3 + 0]), 2.0) * pow(cos(q[0 * 3 + 2]), 2.0) * + pow(sin(q[0 * 3 + 1]), 2.0) + + pow(cos(q[0 * 3 + 1]), 2.0) * pow(cos(q[0 * 3 + 2]), 2.0) * + pow(sin(q[0 * 3 + 0]), 2.0) + + pow(cos(q[0 * 3 + 0]), 2.0) * pow(sin(q[0 * 3 + 1]), 2.0) * + pow(sin(q[0 * 3 + 2]), 2.0) + + pow(cos(q[0 * 3 + 1]), 2.0) * pow(sin(q[0 * 3 + 0]), 2.0) * + pow(sin(q[0 * 3 + 2]), 2.0) + + pow(cos(q[0 * 3 + 2]), 2.0) * pow(sin(q[0 * 3 + 0]), 2.0) * + pow(sin(q[0 * 3 + 1]), 2.0) + + pow(sin(q[0 * 3 + 0]), 2.0) * pow(sin(q[0 * 3 + 1]), 2.0) * + pow(sin(q[0 * 3 + 2]), 2.0) + + pow(cos(q[0 * 3 + 0]), 2.0) * pow(cos(q[0 * 3 + 1]), 2.0) * + pow(cos(q[0 * 3 + 2]), 2.0)) * + 2.0E+4) - + (pow(sin(q[0 * 3 + 0]), 2.0) * + (cos(q[0 * 3 + 1]) * cos(q[0 * 3 + 2]) - + sin(q[0 * 3 + 1]) * sin(q[0 * 3 + 2])) * + (cos(q[0 * 3 + 1]) * -2.06E+3 + + pow(cos(q[0 * 3 + 1]), 2.0) * 2.263E+3 + + pow(sin(q[0 * 3 + 1]), 2.0) * 2.263E+3)) / + ((pow(cos(q[0 * 3 + 0]), 2.0) * pow(cos(q[0 * 3 + 1]), 2.0) + + pow(cos(q[0 * 3 + 0]), 2.0) * pow(sin(q[0 * 3 + 1]), 2.0) + + pow(cos(q[0 * 3 + 1]), 2.0) * pow(sin(q[0 * 3 + 0]), 2.0) + + pow(sin(q[0 * 3 + 0]), 2.0) * pow(sin(q[0 * 3 + 1]), 2.0)) * + (pow(cos(q[0 * 3 + 0]), 2.0) * pow(cos(q[0 * 3 + 1]), 2.0) * + pow(sin(q[0 * 3 + 2]), 2.0) + + pow(cos(q[0 * 3 + 0]), 2.0) * pow(cos(q[0 * 3 + 2]), 2.0) * + pow(sin(q[0 * 3 + 1]), 2.0) + + pow(cos(q[0 * 3 + 1]), 2.0) * pow(cos(q[0 * 3 + 2]), 2.0) * + pow(sin(q[0 * 3 + 0]), 2.0) + + pow(cos(q[0 * 3 + 0]), 2.0) * pow(sin(q[0 * 3 + 1]), 2.0) * + pow(sin(q[0 * 3 + 2]), 2.0) + + pow(cos(q[0 * 3 + 1]), 2.0) * pow(sin(q[0 * 3 + 0]), 2.0) * + pow(sin(q[0 * 3 + 2]), 2.0) + + pow(cos(q[0 * 3 + 2]), 2.0) * pow(sin(q[0 * 3 + 0]), 2.0) * + pow(sin(q[0 * 3 + 1]), 2.0) + + pow(sin(q[0 * 3 + 0]), 2.0) * pow(sin(q[0 * 3 + 1]), 2.0) * + pow(sin(q[0 * 3 + 2]), 2.0) + + pow(cos(q[0 * 3 + 0]), 2.0) * pow(cos(q[0 * 3 + 1]), 2.0) * + pow(cos(q[0 * 3 + 2]), 2.0)) * + 2.0E+4) - + 1.03E+2 / 5.0E+2)) / + (pow(cos(q[0 * 3 + 0]), 2.0) * pow(cos(q[0 * 3 + 1]), 2.0) * + pow(sin(q[0 * 3 + 2]), 2.0) + + pow(cos(q[0 * 3 + 0]), 2.0) * pow(cos(q[0 * 3 + 2]), 2.0) * + pow(sin(q[0 * 3 + 1]), 2.0) + + pow(cos(q[0 * 3 + 1]), 2.0) * pow(cos(q[0 * 3 + 2]), 2.0) * + pow(sin(q[0 * 3 + 0]), 2.0) + + pow(cos(q[0 * 3 + 0]), 2.0) * pow(sin(q[0 * 3 + 1]), 2.0) * + pow(sin(q[0 * 3 + 2]), 2.0) + + pow(cos(q[0 * 3 + 1]), 2.0) * pow(sin(q[0 * 3 + 0]), 2.0) * + pow(sin(q[0 * 3 + 2]), 2.0) + + pow(cos(q[0 * 3 + 2]), 2.0) * pow(sin(q[0 * 3 + 0]), 2.0) * + pow(sin(q[0 * 3 + 1]), 2.0) + + pow(sin(q[0 * 3 + 0]), 2.0) * pow(sin(q[0 * 3 + 1]), 2.0) * + pow(sin(q[0 * 3 + 2]), 2.0) + + pow(cos(q[0 * 3 + 0]), 2.0) * pow(cos(q[0 * 3 + 1]), 2.0) * + pow(cos(q[0 * 3 + 2]), 2.0)) + + (sin(q[0 * 3 + 0]) * + (cos(q[0 * 3 + 1]) * cos(q[0 * 3 + 2]) - + sin(q[0 * 3 + 1]) * sin(q[0 * 3 + 2])) * + ((sin(q[0 * 3 + 0]) * + (pow(cos(q[0 * 3 + 1]), 2.0) * sin(q[0 * 3 + 0]) * + sin(q[0 * 3 + 2]) * 1.03E+4 + + sin(q[0 * 3 + 0]) * pow(sin(q[0 * 3 + 1]), 2.0) * + sin(q[0 * 3 + 2]) * 1.03E+4 - + cos(q[0 * 3 + 1]) * sin(q[0 * 3 + 0]) * sin(q[0 * 3 + 2]) * + 1.1315E+4 - + cos(q[0 * 3 + 2]) * sin(q[0 * 3 + 0]) * sin(q[0 * 3 + 1]) * + 1.1315E+4 - + RL * cos(q[0 * 3 + 0]) * cos(q[0 * 3 + 1]) * cos(q[0 * 3 + 2]) * + 5.049E+3 + + RL * cos(q[0 * 3 + 0]) * sin(q[0 * 3 + 1]) * sin(q[0 * 3 + 2]) * + 5.049E+3 - + RL * pow(cos(q[0 * 3 + 0]), 2.0) * cos(q[0 * 3 + 1]) * + cos(q[0 * 3 + 2]) * 3.5E+3 - + RL * cos(q[0 * 3 + 1]) * cos(q[0 * 3 + 2]) * + pow(sin(q[0 * 3 + 0]), 2.0) * 3.5E+3 + + RL * pow(cos(q[0 * 3 + 0]), 2.0) * sin(q[0 * 3 + 1]) * + sin(q[0 * 3 + 2]) * 3.5E+3 + + RL * pow(sin(q[0 * 3 + 0]), 2.0) * sin(q[0 * 3 + 1]) * + sin(q[0 * 3 + 2]) * 3.5E+3) * + (-1.0E-5)) / + ((pow(cos(q[0 * 3 + 0]), 2.0) + pow(sin(q[0 * 3 + 0]), 2.0)) * + (pow(cos(q[0 * 3 + 0]), 2.0) * pow(cos(q[0 * 3 + 1]), 2.0) * + pow(sin(q[0 * 3 + 2]), 2.0) + + pow(cos(q[0 * 3 + 0]), 2.0) * pow(cos(q[0 * 3 + 2]), 2.0) * + pow(sin(q[0 * 3 + 1]), 2.0) + + pow(cos(q[0 * 3 + 1]), 2.0) * pow(cos(q[0 * 3 + 2]), 2.0) * + pow(sin(q[0 * 3 + 0]), 2.0) + + pow(cos(q[0 * 3 + 0]), 2.0) * pow(sin(q[0 * 3 + 1]), 2.0) * + pow(sin(q[0 * 3 + 2]), 2.0) + + pow(cos(q[0 * 3 + 1]), 2.0) * pow(sin(q[0 * 3 + 0]), 2.0) * + pow(sin(q[0 * 3 + 2]), 2.0) + + pow(cos(q[0 * 3 + 2]), 2.0) * pow(sin(q[0 * 3 + 0]), 2.0) * + pow(sin(q[0 * 3 + 1]), 2.0) + + pow(sin(q[0 * 3 + 0]), 2.0) * pow(sin(q[0 * 3 + 1]), 2.0) * + pow(sin(q[0 * 3 + 2]), 2.0) + + pow(cos(q[0 * 3 + 0]), 2.0) * pow(cos(q[0 * 3 + 1]), 2.0) * + pow(cos(q[0 * 3 + 2]), 2.0))) + + ((cos(q[0 * 3 + 1]) * cos(q[0 * 3 + 2]) - + sin(q[0 * 3 + 1]) * sin(q[0 * 3 + 2])) * + (pow(cos(q[0 * 3 + 0]), 2.0) * sin(q[0 * 3 + 1]) * 1.03E+2 + + pow(sin(q[0 * 3 + 0]), 2.0) * sin(q[0 * 3 + 1]) * 1.03E+2 + + RL * pow(cos(q[0 * 3 + 1]), 2.0) * sin(q[0 * 3 + 0]) * 3.5E+1 + + RL * sin(q[0 * 3 + 0]) * pow(sin(q[0 * 3 + 1]), 2.0) * 3.5E+1)) / + ((pow(cos(q[0 * 3 + 0]), 2.0) * pow(cos(q[0 * 3 + 1]), 2.0) + + pow(cos(q[0 * 3 + 0]), 2.0) * pow(sin(q[0 * 3 + 1]), 2.0) + + pow(cos(q[0 * 3 + 1]), 2.0) * pow(sin(q[0 * 3 + 0]), 2.0) + + pow(sin(q[0 * 3 + 0]), 2.0) * pow(sin(q[0 * 3 + 1]), 2.0)) * + (pow(cos(q[0 * 3 + 1]), 2.0) * pow(cos(q[0 * 3 + 2]), 2.0) + + pow(cos(q[0 * 3 + 1]), 2.0) * pow(sin(q[0 * 3 + 2]), 2.0) + + pow(cos(q[0 * 3 + 2]), 2.0) * pow(sin(q[0 * 3 + 1]), 2.0) + + pow(sin(q[0 * 3 + 1]), 2.0) * pow(sin(q[0 * 3 + 2]), 2.0)) * + 1.0E+3) - + (cos(q[0 * 3 + 0]) * + (cos(q[0 * 3 + 0]) * pow(cos(q[0 * 3 + 1]), 2.0) * + sin(q[0 * 3 + 2]) * 1.03E+4 + + cos(q[0 * 3 + 0]) * pow(sin(q[0 * 3 + 1]), 2.0) * + sin(q[0 * 3 + 2]) * 1.03E+4 - + cos(q[0 * 3 + 0]) * cos(q[0 * 3 + 1]) * sin(q[0 * 3 + 2]) * + 1.1315E+4 - + cos(q[0 * 3 + 0]) * cos(q[0 * 3 + 2]) * sin(q[0 * 3 + 1]) * + 1.1315E+4 + + RL * cos(q[0 * 3 + 1]) * cos(q[0 * 3 + 2]) * sin(q[0 * 3 + 0]) * + 5.049E+3 - + RL * sin(q[0 * 3 + 0]) * sin(q[0 * 3 + 1]) * sin(q[0 * 3 + 2]) * + 5.049E+3)) / + ((pow(cos(q[0 * 3 + 0]), 2.0) + pow(sin(q[0 * 3 + 0]), 2.0)) * + (pow(cos(q[0 * 3 + 0]), 2.0) * pow(cos(q[0 * 3 + 1]), 2.0) * + pow(sin(q[0 * 3 + 2]), 2.0) + + pow(cos(q[0 * 3 + 0]), 2.0) * pow(cos(q[0 * 3 + 2]), 2.0) * + pow(sin(q[0 * 3 + 1]), 2.0) + + pow(cos(q[0 * 3 + 1]), 2.0) * pow(cos(q[0 * 3 + 2]), 2.0) * + pow(sin(q[0 * 3 + 0]), 2.0) + + pow(cos(q[0 * 3 + 0]), 2.0) * pow(sin(q[0 * 3 + 1]), 2.0) * + pow(sin(q[0 * 3 + 2]), 2.0) + + pow(cos(q[0 * 3 + 1]), 2.0) * pow(sin(q[0 * 3 + 0]), 2.0) * + pow(sin(q[0 * 3 + 2]), 2.0) + + pow(cos(q[0 * 3 + 2]), 2.0) * pow(sin(q[0 * 3 + 0]), 2.0) * + pow(sin(q[0 * 3 + 1]), 2.0) + + pow(sin(q[0 * 3 + 0]), 2.0) * pow(sin(q[0 * 3 + 1]), 2.0) * + pow(sin(q[0 * 3 + 2]), 2.0) + + pow(cos(q[0 * 3 + 0]), 2.0) * pow(cos(q[0 * 3 + 1]), 2.0) * + pow(cos(q[0 * 3 + 2]), 2.0)) * + 1.0E+5) + + (pow(cos(q[0 * 3 + 0]), 2.0) * + (cos(q[0 * 3 + 1]) * sin(q[0 * 3 + 2]) + + cos(q[0 * 3 + 2]) * sin(q[0 * 3 + 1])) * + (cos(q[0 * 3 + 1]) * -2.06E+3 + + pow(cos(q[0 * 3 + 1]), 2.0) * 2.263E+3 + + pow(sin(q[0 * 3 + 1]), 2.0) * 2.263E+3)) / + ((pow(cos(q[0 * 3 + 0]), 2.0) * pow(cos(q[0 * 3 + 1]), 2.0) + + pow(cos(q[0 * 3 + 0]), 2.0) * pow(sin(q[0 * 3 + 1]), 2.0) + + pow(cos(q[0 * 3 + 1]), 2.0) * pow(sin(q[0 * 3 + 0]), 2.0) + + pow(sin(q[0 * 3 + 0]), 2.0) * pow(sin(q[0 * 3 + 1]), 2.0)) * + (pow(cos(q[0 * 3 + 0]), 2.0) * pow(cos(q[0 * 3 + 1]), 2.0) * + pow(sin(q[0 * 3 + 2]), 2.0) + + pow(cos(q[0 * 3 + 0]), 2.0) * pow(cos(q[0 * 3 + 2]), 2.0) * + pow(sin(q[0 * 3 + 1]), 2.0) + + pow(cos(q[0 * 3 + 1]), 2.0) * pow(cos(q[0 * 3 + 2]), 2.0) * + pow(sin(q[0 * 3 + 0]), 2.0) + + pow(cos(q[0 * 3 + 0]), 2.0) * pow(sin(q[0 * 3 + 1]), 2.0) * + pow(sin(q[0 * 3 + 2]), 2.0) + + pow(cos(q[0 * 3 + 1]), 2.0) * pow(sin(q[0 * 3 + 0]), 2.0) * + pow(sin(q[0 * 3 + 2]), 2.0) + + pow(cos(q[0 * 3 + 2]), 2.0) * pow(sin(q[0 * 3 + 0]), 2.0) * + pow(sin(q[0 * 3 + 1]), 2.0) + + pow(sin(q[0 * 3 + 0]), 2.0) * pow(sin(q[0 * 3 + 1]), 2.0) * + pow(sin(q[0 * 3 + 2]), 2.0) + + pow(cos(q[0 * 3 + 0]), 2.0) * pow(cos(q[0 * 3 + 1]), 2.0) * + pow(cos(q[0 * 3 + 2]), 2.0)) * + 2.0E+4) + + (pow(sin(q[0 * 3 + 0]), 2.0) * + (cos(q[0 * 3 + 1]) * sin(q[0 * 3 + 2]) + + cos(q[0 * 3 + 2]) * sin(q[0 * 3 + 1])) * + (cos(q[0 * 3 + 1]) * -2.06E+3 + + pow(cos(q[0 * 3 + 1]), 2.0) * 2.263E+3 + + pow(sin(q[0 * 3 + 1]), 2.0) * 2.263E+3)) / + ((pow(cos(q[0 * 3 + 0]), 2.0) * pow(cos(q[0 * 3 + 1]), 2.0) + + pow(cos(q[0 * 3 + 0]), 2.0) * pow(sin(q[0 * 3 + 1]), 2.0) + + pow(cos(q[0 * 3 + 1]), 2.0) * pow(sin(q[0 * 3 + 0]), 2.0) + + pow(sin(q[0 * 3 + 0]), 2.0) * pow(sin(q[0 * 3 + 1]), 2.0)) * + (pow(cos(q[0 * 3 + 0]), 2.0) * pow(cos(q[0 * 3 + 1]), 2.0) * + pow(sin(q[0 * 3 + 2]), 2.0) + + pow(cos(q[0 * 3 + 0]), 2.0) * pow(cos(q[0 * 3 + 2]), 2.0) * + pow(sin(q[0 * 3 + 1]), 2.0) + + pow(cos(q[0 * 3 + 1]), 2.0) * pow(cos(q[0 * 3 + 2]), 2.0) * + pow(sin(q[0 * 3 + 0]), 2.0) + + pow(cos(q[0 * 3 + 0]), 2.0) * pow(sin(q[0 * 3 + 1]), 2.0) * + pow(sin(q[0 * 3 + 2]), 2.0) + + pow(cos(q[0 * 3 + 1]), 2.0) * pow(sin(q[0 * 3 + 0]), 2.0) * + pow(sin(q[0 * 3 + 2]), 2.0) + + pow(cos(q[0 * 3 + 2]), 2.0) * pow(sin(q[0 * 3 + 0]), 2.0) * + pow(sin(q[0 * 3 + 1]), 2.0) + + pow(sin(q[0 * 3 + 0]), 2.0) * pow(sin(q[0 * 3 + 1]), 2.0) * + pow(sin(q[0 * 3 + 2]), 2.0) + + pow(cos(q[0 * 3 + 0]), 2.0) * pow(cos(q[0 * 3 + 1]), 2.0) * + pow(cos(q[0 * 3 + 2]), 2.0)) * + 2.0E+4))) / + (pow(cos(q[0 * 3 + 0]), 2.0) * pow(cos(q[0 * 3 + 1]), 2.0) * + pow(sin(q[0 * 3 + 2]), 2.0) + + pow(cos(q[0 * 3 + 0]), 2.0) * pow(cos(q[0 * 3 + 2]), 2.0) * + pow(sin(q[0 * 3 + 1]), 2.0) + + pow(cos(q[0 * 3 + 1]), 2.0) * pow(cos(q[0 * 3 + 2]), 2.0) * + pow(sin(q[0 * 3 + 0]), 2.0) + + pow(cos(q[0 * 3 + 0]), 2.0) * pow(sin(q[0 * 3 + 1]), 2.0) * + pow(sin(q[0 * 3 + 2]), 2.0) + + pow(cos(q[0 * 3 + 1]), 2.0) * pow(sin(q[0 * 3 + 0]), 2.0) * + pow(sin(q[0 * 3 + 2]), 2.0) + + pow(cos(q[0 * 3 + 2]), 2.0) * pow(sin(q[0 * 3 + 0]), 2.0) * + pow(sin(q[0 * 3 + 1]), 2.0) + + pow(sin(q[0 * 3 + 0]), 2.0) * pow(sin(q[0 * 3 + 1]), 2.0) * + pow(sin(q[0 * 3 + 2]), 2.0) + + pow(cos(q[0 * 3 + 0]), 2.0) * pow(cos(q[0 * 3 + 1]), 2.0) * + pow(cos(q[0 * 3 + 2]), 2.0)))) / + (pow(cos(q[0 * 3 + 0]), 2.0) + pow(sin(q[0 * 3 + 0]), 2.0)); + F(1, 0) = + -(pow(cos(q[0 * 3 + 1]) * sin(q[0 * 3 + 2]) + + cos(q[0 * 3 + 2]) * sin(q[0 * 3 + 1]), + 2.0) / + (pow(cos(q[0 * 3 + 1]), 2.0) * pow(cos(q[0 * 3 + 2]), 2.0) + + pow(cos(q[0 * 3 + 1]), 2.0) * pow(sin(q[0 * 3 + 2]), 2.0) + + pow(cos(q[0 * 3 + 2]), 2.0) * pow(sin(q[0 * 3 + 1]), 2.0) + + pow(sin(q[0 * 3 + 1]), 2.0) * pow(sin(q[0 * 3 + 2]), 2.0)) + + pow(cos(q[0 * 3 + 1]) * cos(q[0 * 3 + 2]) - + sin(q[0 * 3 + 1]) * sin(q[0 * 3 + 2]), + 2.0) / + (pow(cos(q[0 * 3 + 1]), 2.0) * pow(cos(q[0 * 3 + 2]), 2.0) + + pow(cos(q[0 * 3 + 1]), 2.0) * pow(sin(q[0 * 3 + 2]), 2.0) + + pow(cos(q[0 * 3 + 2]), 2.0) * pow(sin(q[0 * 3 + 1]), 2.0) + + pow(sin(q[0 * 3 + 1]), 2.0) * pow(sin(q[0 * 3 + 2]), 2.0))) * + (-(sin(q[0 * 3 + 0]) * + ((cos(q[0 * 3 + 0]) * + (cos(q[0 * 3 + 1]) * sin(q[0 * 3 + 2]) + + cos(q[0 * 3 + 2]) * sin(q[0 * 3 + 1])) * + 1.0 / + pow(pow(cos(q[0 * 3 + 0]), 2.0) * pow(cos(q[0 * 3 + 1]), 2.0) * + pow(sin(q[0 * 3 + 2]), 2.0) + + pow(cos(q[0 * 3 + 0]), 2.0) * + pow(cos(q[0 * 3 + 2]), 2.0) * + pow(sin(q[0 * 3 + 1]), 2.0) + + pow(cos(q[0 * 3 + 1]), 2.0) * + pow(cos(q[0 * 3 + 2]), 2.0) * + pow(sin(q[0 * 3 + 0]), 2.0) + + pow(cos(q[0 * 3 + 0]), 2.0) * + pow(sin(q[0 * 3 + 1]), 2.0) * + pow(sin(q[0 * 3 + 2]), 2.0) + + pow(cos(q[0 * 3 + 1]), 2.0) * + pow(sin(q[0 * 3 + 0]), 2.0) * + pow(sin(q[0 * 3 + 2]), 2.0) + + pow(cos(q[0 * 3 + 2]), 2.0) * + pow(sin(q[0 * 3 + 0]), 2.0) * + pow(sin(q[0 * 3 + 1]), 2.0) + + pow(sin(q[0 * 3 + 0]), 2.0) * + pow(sin(q[0 * 3 + 1]), 2.0) * + pow(sin(q[0 * 3 + 2]), 2.0) + + pow(cos(q[0 * 3 + 0]), 2.0) * + pow(cos(q[0 * 3 + 1]), 2.0) * + pow(cos(q[0 * 3 + 2]), 2.0), + 2.0) * + (sin(q[0 * 3 + 0]) * sin(q[0 * 3 + 1]) * sin(q[0 * 3 + 2]) * + 1.1315E+4 + + pow(cos(q[0 * 3 + 1]), 2.0) * cos(q[0 * 3 + 2]) * + sin(q[0 * 3 + 0]) * 1.03E+4 + + cos(q[0 * 3 + 2]) * sin(q[0 * 3 + 0]) * + pow(sin(q[0 * 3 + 1]), 2.0) * 1.03E+4 - + cos(q[0 * 3 + 1]) * cos(q[0 * 3 + 2]) * sin(q[0 * 3 + 0]) * + 1.1315E+4 + + RL * cos(q[0 * 3 + 0]) * cos(q[0 * 3 + 1]) * sin(q[0 * 3 + 2]) * + 5.049E+3 + + RL * cos(q[0 * 3 + 0]) * cos(q[0 * 3 + 2]) * sin(q[0 * 3 + 1]) * + 5.049E+3 + + RL * pow(cos(q[0 * 3 + 0]), 2.0) * cos(q[0 * 3 + 1]) * + sin(q[0 * 3 + 2]) * 3.5E+3 + + RL * pow(cos(q[0 * 3 + 0]), 2.0) * cos(q[0 * 3 + 2]) * + sin(q[0 * 3 + 1]) * 3.5E+3 + + RL * cos(q[0 * 3 + 1]) * pow(sin(q[0 * 3 + 0]), 2.0) * + sin(q[0 * 3 + 2]) * 3.5E+3 + + RL * cos(q[0 * 3 + 2]) * pow(sin(q[0 * 3 + 0]), 2.0) * + sin(q[0 * 3 + 1]) * 3.5E+3)) / + 1.0E+5 - + (cos(q[0 * 3 + 0]) * + (cos(q[0 * 3 + 1]) * cos(q[0 * 3 + 2]) - + sin(q[0 * 3 + 1]) * sin(q[0 * 3 + 2])) * + 1.0 / + pow(pow(cos(q[0 * 3 + 0]), 2.0) * pow(cos(q[0 * 3 + 1]), 2.0) * + pow(sin(q[0 * 3 + 2]), 2.0) + + pow(cos(q[0 * 3 + 0]), 2.0) * + pow(cos(q[0 * 3 + 2]), 2.0) * + pow(sin(q[0 * 3 + 1]), 2.0) + + pow(cos(q[0 * 3 + 1]), 2.0) * + pow(cos(q[0 * 3 + 2]), 2.0) * + pow(sin(q[0 * 3 + 0]), 2.0) + + pow(cos(q[0 * 3 + 0]), 2.0) * + pow(sin(q[0 * 3 + 1]), 2.0) * + pow(sin(q[0 * 3 + 2]), 2.0) + + pow(cos(q[0 * 3 + 1]), 2.0) * + pow(sin(q[0 * 3 + 0]), 2.0) * + pow(sin(q[0 * 3 + 2]), 2.0) + + pow(cos(q[0 * 3 + 2]), 2.0) * + pow(sin(q[0 * 3 + 0]), 2.0) * + pow(sin(q[0 * 3 + 1]), 2.0) + + pow(sin(q[0 * 3 + 0]), 2.0) * + pow(sin(q[0 * 3 + 1]), 2.0) * + pow(sin(q[0 * 3 + 2]), 2.0) + + pow(cos(q[0 * 3 + 0]), 2.0) * + pow(cos(q[0 * 3 + 1]), 2.0) * + pow(cos(q[0 * 3 + 2]), 2.0), + 2.0) * + (pow(cos(q[0 * 3 + 1]), 2.0) * sin(q[0 * 3 + 0]) * + sin(q[0 * 3 + 2]) * 1.03E+4 + + sin(q[0 * 3 + 0]) * pow(sin(q[0 * 3 + 1]), 2.0) * + sin(q[0 * 3 + 2]) * 1.03E+4 - + cos(q[0 * 3 + 1]) * sin(q[0 * 3 + 0]) * sin(q[0 * 3 + 2]) * + 1.1315E+4 - + cos(q[0 * 3 + 2]) * sin(q[0 * 3 + 0]) * sin(q[0 * 3 + 1]) * + 1.1315E+4 - + RL * cos(q[0 * 3 + 0]) * cos(q[0 * 3 + 1]) * cos(q[0 * 3 + 2]) * + 5.049E+3 + + RL * cos(q[0 * 3 + 0]) * sin(q[0 * 3 + 1]) * sin(q[0 * 3 + 2]) * + 5.049E+3 - + RL * pow(cos(q[0 * 3 + 0]), 2.0) * cos(q[0 * 3 + 1]) * + cos(q[0 * 3 + 2]) * 3.5E+3 - + RL * cos(q[0 * 3 + 1]) * cos(q[0 * 3 + 2]) * + pow(sin(q[0 * 3 + 0]), 2.0) * 3.5E+3 + + RL * pow(cos(q[0 * 3 + 0]), 2.0) * sin(q[0 * 3 + 1]) * + sin(q[0 * 3 + 2]) * 3.5E+3 + + RL * pow(sin(q[0 * 3 + 0]), 2.0) * sin(q[0 * 3 + 1]) * + sin(q[0 * 3 + 2]) * 3.5E+3)) / + 1.0E+5 - + (sin(q[0 * 3 + 0]) * + (cos(q[0 * 3 + 1]) * sin(q[0 * 3 + 2]) + + cos(q[0 * 3 + 2]) * sin(q[0 * 3 + 1])) * + (cos(q[0 * 3 + 0]) * pow(cos(q[0 * 3 + 1]), 2.0) * + cos(q[0 * 3 + 2]) * 1.03E+4 + + cos(q[0 * 3 + 0]) * cos(q[0 * 3 + 2]) * + pow(sin(q[0 * 3 + 1]), 2.0) * 1.03E+4 - + cos(q[0 * 3 + 0]) * cos(q[0 * 3 + 1]) * cos(q[0 * 3 + 2]) * + 1.1315E+4 + + cos(q[0 * 3 + 0]) * sin(q[0 * 3 + 1]) * sin(q[0 * 3 + 2]) * + 1.1315E+4 - + RL * cos(q[0 * 3 + 1]) * sin(q[0 * 3 + 0]) * sin(q[0 * 3 + 2]) * + 5.049E+3 - + RL * cos(q[0 * 3 + 2]) * sin(q[0 * 3 + 0]) * sin(q[0 * 3 + 1]) * + 5.049E+3) * + 1.0 / + pow(pow(cos(q[0 * 3 + 0]), 2.0) * pow(cos(q[0 * 3 + 1]), 2.0) * + pow(sin(q[0 * 3 + 2]), 2.0) + + pow(cos(q[0 * 3 + 0]), 2.0) * + pow(cos(q[0 * 3 + 2]), 2.0) * + pow(sin(q[0 * 3 + 1]), 2.0) + + pow(cos(q[0 * 3 + 1]), 2.0) * + pow(cos(q[0 * 3 + 2]), 2.0) * + pow(sin(q[0 * 3 + 0]), 2.0) + + pow(cos(q[0 * 3 + 0]), 2.0) * + pow(sin(q[0 * 3 + 1]), 2.0) * + pow(sin(q[0 * 3 + 2]), 2.0) + + pow(cos(q[0 * 3 + 1]), 2.0) * + pow(sin(q[0 * 3 + 0]), 2.0) * + pow(sin(q[0 * 3 + 2]), 2.0) + + pow(cos(q[0 * 3 + 2]), 2.0) * + pow(sin(q[0 * 3 + 0]), 2.0) * + pow(sin(q[0 * 3 + 1]), 2.0) + + pow(sin(q[0 * 3 + 0]), 2.0) * + pow(sin(q[0 * 3 + 1]), 2.0) * + pow(sin(q[0 * 3 + 2]), 2.0) + + pow(cos(q[0 * 3 + 0]), 2.0) * + pow(cos(q[0 * 3 + 1]), 2.0) * + pow(cos(q[0 * 3 + 2]), 2.0), + 2.0)) / + 1.0E+5 + + (sin(q[0 * 3 + 0]) * + (cos(q[0 * 3 + 1]) * cos(q[0 * 3 + 2]) - + sin(q[0 * 3 + 1]) * sin(q[0 * 3 + 2])) * + (cos(q[0 * 3 + 0]) * pow(cos(q[0 * 3 + 1]), 2.0) * + sin(q[0 * 3 + 2]) * 1.03E+4 + + cos(q[0 * 3 + 0]) * pow(sin(q[0 * 3 + 1]), 2.0) * + sin(q[0 * 3 + 2]) * 1.03E+4 - + cos(q[0 * 3 + 0]) * cos(q[0 * 3 + 1]) * sin(q[0 * 3 + 2]) * + 1.1315E+4 - + cos(q[0 * 3 + 0]) * cos(q[0 * 3 + 2]) * sin(q[0 * 3 + 1]) * + 1.1315E+4 + + RL * cos(q[0 * 3 + 1]) * cos(q[0 * 3 + 2]) * sin(q[0 * 3 + 0]) * + 5.049E+3 - + RL * sin(q[0 * 3 + 0]) * sin(q[0 * 3 + 1]) * sin(q[0 * 3 + 2]) * + 5.049E+3) * + 1.0 / + pow(pow(cos(q[0 * 3 + 0]), 2.0) * pow(cos(q[0 * 3 + 1]), 2.0) * + pow(sin(q[0 * 3 + 2]), 2.0) + + pow(cos(q[0 * 3 + 0]), 2.0) * + pow(cos(q[0 * 3 + 2]), 2.0) * + pow(sin(q[0 * 3 + 1]), 2.0) + + pow(cos(q[0 * 3 + 1]), 2.0) * + pow(cos(q[0 * 3 + 2]), 2.0) * + pow(sin(q[0 * 3 + 0]), 2.0) + + pow(cos(q[0 * 3 + 0]), 2.0) * + pow(sin(q[0 * 3 + 1]), 2.0) * + pow(sin(q[0 * 3 + 2]), 2.0) + + pow(cos(q[0 * 3 + 1]), 2.0) * + pow(sin(q[0 * 3 + 0]), 2.0) * + pow(sin(q[0 * 3 + 2]), 2.0) + + pow(cos(q[0 * 3 + 2]), 2.0) * + pow(sin(q[0 * 3 + 0]), 2.0) * + pow(sin(q[0 * 3 + 1]), 2.0) + + pow(sin(q[0 * 3 + 0]), 2.0) * + pow(sin(q[0 * 3 + 1]), 2.0) * + pow(sin(q[0 * 3 + 2]), 2.0) + + pow(cos(q[0 * 3 + 0]), 2.0) * + pow(cos(q[0 * 3 + 1]), 2.0) * + pow(cos(q[0 * 3 + 2]), 2.0), + 2.0)) / + 1.0E+5 + + ((cos(q[0 * 3 + 1]) * sin(q[0 * 3 + 2]) + + cos(q[0 * 3 + 2]) * sin(q[0 * 3 + 1])) * + (RL * cos(q[0 * 3 + 0]) * cos(q[0 * 3 + 1]) * sin(q[0 * 3 + 2]) * + 3.5E+3 + + RL * cos(q[0 * 3 + 0]) * cos(q[0 * 3 + 2]) * sin(q[0 * 3 + 1]) * + 3.5E+3 + + RL * pow(cos(q[0 * 3 + 0]), 2.0) * cos(q[0 * 3 + 1]) * + sin(q[0 * 3 + 2]) * 5.049E+3 + + RL * pow(cos(q[0 * 3 + 0]), 2.0) * cos(q[0 * 3 + 2]) * + sin(q[0 * 3 + 1]) * 5.049E+3 + + RL * cos(q[0 * 3 + 1]) * pow(sin(q[0 * 3 + 0]), 2.0) * + sin(q[0 * 3 + 2]) * 5.049E+3 + + RL * cos(q[0 * 3 + 2]) * pow(sin(q[0 * 3 + 0]), 2.0) * + sin(q[0 * 3 + 1]) * 5.049E+3)) / + ((pow(cos(q[0 * 3 + 1]), 2.0) * pow(cos(q[0 * 3 + 2]), 2.0) + + pow(cos(q[0 * 3 + 1]), 2.0) * pow(sin(q[0 * 3 + 2]), 2.0) + + pow(cos(q[0 * 3 + 2]), 2.0) * pow(sin(q[0 * 3 + 1]), 2.0) + + pow(sin(q[0 * 3 + 1]), 2.0) * pow(sin(q[0 * 3 + 2]), 2.0)) * + (pow(cos(q[0 * 3 + 0]), 2.0) * pow(cos(q[0 * 3 + 1]), 2.0) * + pow(sin(q[0 * 3 + 2]), 2.0) + + pow(cos(q[0 * 3 + 0]), 2.0) * pow(cos(q[0 * 3 + 2]), 2.0) * + pow(sin(q[0 * 3 + 1]), 2.0) + + pow(cos(q[0 * 3 + 1]), 2.0) * pow(cos(q[0 * 3 + 2]), 2.0) * + pow(sin(q[0 * 3 + 0]), 2.0) + + pow(cos(q[0 * 3 + 0]), 2.0) * pow(sin(q[0 * 3 + 1]), 2.0) * + pow(sin(q[0 * 3 + 2]), 2.0) + + pow(cos(q[0 * 3 + 1]), 2.0) * pow(sin(q[0 * 3 + 0]), 2.0) * + pow(sin(q[0 * 3 + 2]), 2.0) + + pow(cos(q[0 * 3 + 2]), 2.0) * pow(sin(q[0 * 3 + 0]), 2.0) * + pow(sin(q[0 * 3 + 1]), 2.0) + + pow(sin(q[0 * 3 + 0]), 2.0) * pow(sin(q[0 * 3 + 1]), 2.0) * + pow(sin(q[0 * 3 + 2]), 2.0) + + pow(cos(q[0 * 3 + 0]), 2.0) * pow(cos(q[0 * 3 + 1]), 2.0) * + pow(cos(q[0 * 3 + 2]), 2.0)) * + 1.0E+5) + + ((cos(q[0 * 3 + 1]) * cos(q[0 * 3 + 2]) - + sin(q[0 * 3 + 1]) * sin(q[0 * 3 + 2])) * + (RL * cos(q[0 * 3 + 0]) * cos(q[0 * 3 + 1]) * cos(q[0 * 3 + 2]) * + 3.5E+3 - + RL * cos(q[0 * 3 + 0]) * sin(q[0 * 3 + 1]) * sin(q[0 * 3 + 2]) * + 3.5E+3 + + RL * pow(cos(q[0 * 3 + 0]), 2.0) * cos(q[0 * 3 + 1]) * + cos(q[0 * 3 + 2]) * 5.049E+3 + + RL * cos(q[0 * 3 + 1]) * cos(q[0 * 3 + 2]) * + pow(sin(q[0 * 3 + 0]), 2.0) * 5.049E+3 - + RL * pow(cos(q[0 * 3 + 0]), 2.0) * sin(q[0 * 3 + 1]) * + sin(q[0 * 3 + 2]) * 5.049E+3 - + RL * pow(sin(q[0 * 3 + 0]), 2.0) * sin(q[0 * 3 + 1]) * + sin(q[0 * 3 + 2]) * 5.049E+3)) / + ((pow(cos(q[0 * 3 + 1]), 2.0) * pow(cos(q[0 * 3 + 2]), 2.0) + + pow(cos(q[0 * 3 + 1]), 2.0) * pow(sin(q[0 * 3 + 2]), 2.0) + + pow(cos(q[0 * 3 + 2]), 2.0) * pow(sin(q[0 * 3 + 1]), 2.0) + + pow(sin(q[0 * 3 + 1]), 2.0) * pow(sin(q[0 * 3 + 2]), 2.0)) * + (pow(cos(q[0 * 3 + 0]), 2.0) * pow(cos(q[0 * 3 + 1]), 2.0) * + pow(sin(q[0 * 3 + 2]), 2.0) + + pow(cos(q[0 * 3 + 0]), 2.0) * pow(cos(q[0 * 3 + 2]), 2.0) * + pow(sin(q[0 * 3 + 1]), 2.0) + + pow(cos(q[0 * 3 + 1]), 2.0) * pow(cos(q[0 * 3 + 2]), 2.0) * + pow(sin(q[0 * 3 + 0]), 2.0) + + pow(cos(q[0 * 3 + 0]), 2.0) * pow(sin(q[0 * 3 + 1]), 2.0) * + pow(sin(q[0 * 3 + 2]), 2.0) + + pow(cos(q[0 * 3 + 1]), 2.0) * pow(sin(q[0 * 3 + 0]), 2.0) * + pow(sin(q[0 * 3 + 2]), 2.0) + + pow(cos(q[0 * 3 + 2]), 2.0) * pow(sin(q[0 * 3 + 0]), 2.0) * + pow(sin(q[0 * 3 + 1]), 2.0) + + pow(sin(q[0 * 3 + 0]), 2.0) * pow(sin(q[0 * 3 + 1]), 2.0) * + pow(sin(q[0 * 3 + 2]), 2.0) + + pow(cos(q[0 * 3 + 0]), 2.0) * pow(cos(q[0 * 3 + 1]), 2.0) * + pow(cos(q[0 * 3 + 2]), 2.0)) * + 1.0E+5))) / + (pow(cos(q[0 * 3 + 0]), 2.0) + pow(sin(q[0 * 3 + 0]), 2.0)) + + (cos(q[0 * 3 + 0]) * + (cos(q[0 * 3 + 1]) * sin(q[0 * 3 + 2]) + + cos(q[0 * 3 + 2]) * sin(q[0 * 3 + 1])) * + ((cos(q[0 * 3 + 0]) * + (cos(q[0 * 3 + 0]) * pow(cos(q[0 * 3 + 1]), 2.0) * + cos(q[0 * 3 + 2]) * 1.03E+4 + + cos(q[0 * 3 + 0]) * cos(q[0 * 3 + 2]) * + pow(sin(q[0 * 3 + 1]), 2.0) * 1.03E+4 - + cos(q[0 * 3 + 0]) * cos(q[0 * 3 + 1]) * cos(q[0 * 3 + 2]) * + 1.1315E+4 + + cos(q[0 * 3 + 0]) * sin(q[0 * 3 + 1]) * sin(q[0 * 3 + 2]) * + 1.1315E+4 - + RL * cos(q[0 * 3 + 1]) * sin(q[0 * 3 + 0]) * sin(q[0 * 3 + 2]) * + 5.049E+3 - + RL * cos(q[0 * 3 + 2]) * sin(q[0 * 3 + 0]) * sin(q[0 * 3 + 1]) * + 5.049E+3)) / + ((pow(cos(q[0 * 3 + 0]), 2.0) + pow(sin(q[0 * 3 + 0]), 2.0)) * + (pow(cos(q[0 * 3 + 0]), 2.0) * pow(cos(q[0 * 3 + 1]), 2.0) * + pow(sin(q[0 * 3 + 2]), 2.0) + + pow(cos(q[0 * 3 + 0]), 2.0) * pow(cos(q[0 * 3 + 2]), 2.0) * + pow(sin(q[0 * 3 + 1]), 2.0) + + pow(cos(q[0 * 3 + 1]), 2.0) * pow(cos(q[0 * 3 + 2]), 2.0) * + pow(sin(q[0 * 3 + 0]), 2.0) + + pow(cos(q[0 * 3 + 0]), 2.0) * pow(sin(q[0 * 3 + 1]), 2.0) * + pow(sin(q[0 * 3 + 2]), 2.0) + + pow(cos(q[0 * 3 + 1]), 2.0) * pow(sin(q[0 * 3 + 0]), 2.0) * + pow(sin(q[0 * 3 + 2]), 2.0) + + pow(cos(q[0 * 3 + 2]), 2.0) * pow(sin(q[0 * 3 + 0]), 2.0) * + pow(sin(q[0 * 3 + 1]), 2.0) + + pow(sin(q[0 * 3 + 0]), 2.0) * pow(sin(q[0 * 3 + 1]), 2.0) * + pow(sin(q[0 * 3 + 2]), 2.0) + + pow(cos(q[0 * 3 + 0]), 2.0) * pow(cos(q[0 * 3 + 1]), 2.0) * + pow(cos(q[0 * 3 + 2]), 2.0)) * + 1.0E+5) + + ((cos(q[0 * 3 + 1]) * sin(q[0 * 3 + 2]) + + cos(q[0 * 3 + 2]) * sin(q[0 * 3 + 1])) * + (pow(cos(q[0 * 3 + 0]), 2.0) * sin(q[0 * 3 + 1]) * 1.03E+2 + + pow(sin(q[0 * 3 + 0]), 2.0) * sin(q[0 * 3 + 1]) * 1.03E+2 + + RL * pow(cos(q[0 * 3 + 1]), 2.0) * sin(q[0 * 3 + 0]) * 3.5E+1 + + RL * sin(q[0 * 3 + 0]) * pow(sin(q[0 * 3 + 1]), 2.0) * 3.5E+1)) / + ((pow(cos(q[0 * 3 + 0]), 2.0) * pow(cos(q[0 * 3 + 1]), 2.0) + + pow(cos(q[0 * 3 + 0]), 2.0) * pow(sin(q[0 * 3 + 1]), 2.0) + + pow(cos(q[0 * 3 + 1]), 2.0) * pow(sin(q[0 * 3 + 0]), 2.0) + + pow(sin(q[0 * 3 + 0]), 2.0) * pow(sin(q[0 * 3 + 1]), 2.0)) * + (pow(cos(q[0 * 3 + 1]), 2.0) * pow(cos(q[0 * 3 + 2]), 2.0) + + pow(cos(q[0 * 3 + 1]), 2.0) * pow(sin(q[0 * 3 + 2]), 2.0) + + pow(cos(q[0 * 3 + 2]), 2.0) * pow(sin(q[0 * 3 + 1]), 2.0) + + pow(sin(q[0 * 3 + 1]), 2.0) * pow(sin(q[0 * 3 + 2]), 2.0)) * + 1.0E+3) + + (sin(q[0 * 3 + 0]) * + (sin(q[0 * 3 + 0]) * sin(q[0 * 3 + 1]) * sin(q[0 * 3 + 2]) * + 1.1315E+4 + + pow(cos(q[0 * 3 + 1]), 2.0) * cos(q[0 * 3 + 2]) * + sin(q[0 * 3 + 0]) * 1.03E+4 + + cos(q[0 * 3 + 2]) * sin(q[0 * 3 + 0]) * + pow(sin(q[0 * 3 + 1]), 2.0) * 1.03E+4 - + cos(q[0 * 3 + 1]) * cos(q[0 * 3 + 2]) * sin(q[0 * 3 + 0]) * + 1.1315E+4 + + RL * cos(q[0 * 3 + 0]) * cos(q[0 * 3 + 1]) * sin(q[0 * 3 + 2]) * + 5.049E+3 + + RL * cos(q[0 * 3 + 0]) * cos(q[0 * 3 + 2]) * sin(q[0 * 3 + 1]) * + 5.049E+3 + + RL * pow(cos(q[0 * 3 + 0]), 2.0) * cos(q[0 * 3 + 1]) * + sin(q[0 * 3 + 2]) * 3.5E+3 + + RL * pow(cos(q[0 * 3 + 0]), 2.0) * cos(q[0 * 3 + 2]) * + sin(q[0 * 3 + 1]) * 3.5E+3 + + RL * cos(q[0 * 3 + 1]) * pow(sin(q[0 * 3 + 0]), 2.0) * + sin(q[0 * 3 + 2]) * 3.5E+3 + + RL * cos(q[0 * 3 + 2]) * pow(sin(q[0 * 3 + 0]), 2.0) * + sin(q[0 * 3 + 1]) * 3.5E+3)) / + ((pow(cos(q[0 * 3 + 0]), 2.0) + pow(sin(q[0 * 3 + 0]), 2.0)) * + (pow(cos(q[0 * 3 + 0]), 2.0) * pow(cos(q[0 * 3 + 1]), 2.0) * + pow(sin(q[0 * 3 + 2]), 2.0) + + pow(cos(q[0 * 3 + 0]), 2.0) * pow(cos(q[0 * 3 + 2]), 2.0) * + pow(sin(q[0 * 3 + 1]), 2.0) + + pow(cos(q[0 * 3 + 1]), 2.0) * pow(cos(q[0 * 3 + 2]), 2.0) * + pow(sin(q[0 * 3 + 0]), 2.0) + + pow(cos(q[0 * 3 + 0]), 2.0) * pow(sin(q[0 * 3 + 1]), 2.0) * + pow(sin(q[0 * 3 + 2]), 2.0) + + pow(cos(q[0 * 3 + 1]), 2.0) * pow(sin(q[0 * 3 + 0]), 2.0) * + pow(sin(q[0 * 3 + 2]), 2.0) + + pow(cos(q[0 * 3 + 2]), 2.0) * pow(sin(q[0 * 3 + 0]), 2.0) * + pow(sin(q[0 * 3 + 1]), 2.0) + + pow(sin(q[0 * 3 + 0]), 2.0) * pow(sin(q[0 * 3 + 1]), 2.0) * + pow(sin(q[0 * 3 + 2]), 2.0) + + pow(cos(q[0 * 3 + 0]), 2.0) * pow(cos(q[0 * 3 + 1]), 2.0) * + pow(cos(q[0 * 3 + 2]), 2.0)) * + 1.0E+5) - + (pow(cos(q[0 * 3 + 0]), 2.0) * + (cos(q[0 * 3 + 1]) * cos(q[0 * 3 + 2]) - + sin(q[0 * 3 + 1]) * sin(q[0 * 3 + 2])) * + (cos(q[0 * 3 + 1]) * -2.06E+3 + + pow(cos(q[0 * 3 + 1]), 2.0) * 2.263E+3 + + pow(sin(q[0 * 3 + 1]), 2.0) * 2.263E+3)) / + ((pow(cos(q[0 * 3 + 0]), 2.0) * pow(cos(q[0 * 3 + 1]), 2.0) + + pow(cos(q[0 * 3 + 0]), 2.0) * pow(sin(q[0 * 3 + 1]), 2.0) + + pow(cos(q[0 * 3 + 1]), 2.0) * pow(sin(q[0 * 3 + 0]), 2.0) + + pow(sin(q[0 * 3 + 0]), 2.0) * pow(sin(q[0 * 3 + 1]), 2.0)) * + (pow(cos(q[0 * 3 + 0]), 2.0) * pow(cos(q[0 * 3 + 1]), 2.0) * + pow(sin(q[0 * 3 + 2]), 2.0) + + pow(cos(q[0 * 3 + 0]), 2.0) * pow(cos(q[0 * 3 + 2]), 2.0) * + pow(sin(q[0 * 3 + 1]), 2.0) + + pow(cos(q[0 * 3 + 1]), 2.0) * pow(cos(q[0 * 3 + 2]), 2.0) * + pow(sin(q[0 * 3 + 0]), 2.0) + + pow(cos(q[0 * 3 + 0]), 2.0) * pow(sin(q[0 * 3 + 1]), 2.0) * + pow(sin(q[0 * 3 + 2]), 2.0) + + pow(cos(q[0 * 3 + 1]), 2.0) * pow(sin(q[0 * 3 + 0]), 2.0) * + pow(sin(q[0 * 3 + 2]), 2.0) + + pow(cos(q[0 * 3 + 2]), 2.0) * pow(sin(q[0 * 3 + 0]), 2.0) * + pow(sin(q[0 * 3 + 1]), 2.0) + + pow(sin(q[0 * 3 + 0]), 2.0) * pow(sin(q[0 * 3 + 1]), 2.0) * + pow(sin(q[0 * 3 + 2]), 2.0) + + pow(cos(q[0 * 3 + 0]), 2.0) * pow(cos(q[0 * 3 + 1]), 2.0) * + pow(cos(q[0 * 3 + 2]), 2.0)) * + 2.0E+4) - + (pow(sin(q[0 * 3 + 0]), 2.0) * + (cos(q[0 * 3 + 1]) * cos(q[0 * 3 + 2]) - + sin(q[0 * 3 + 1]) * sin(q[0 * 3 + 2])) * + (cos(q[0 * 3 + 1]) * -2.06E+3 + + pow(cos(q[0 * 3 + 1]), 2.0) * 2.263E+3 + + pow(sin(q[0 * 3 + 1]), 2.0) * 2.263E+3)) / + ((pow(cos(q[0 * 3 + 0]), 2.0) * pow(cos(q[0 * 3 + 1]), 2.0) + + pow(cos(q[0 * 3 + 0]), 2.0) * pow(sin(q[0 * 3 + 1]), 2.0) + + pow(cos(q[0 * 3 + 1]), 2.0) * pow(sin(q[0 * 3 + 0]), 2.0) + + pow(sin(q[0 * 3 + 0]), 2.0) * pow(sin(q[0 * 3 + 1]), 2.0)) * + (pow(cos(q[0 * 3 + 0]), 2.0) * pow(cos(q[0 * 3 + 1]), 2.0) * + pow(sin(q[0 * 3 + 2]), 2.0) + + pow(cos(q[0 * 3 + 0]), 2.0) * pow(cos(q[0 * 3 + 2]), 2.0) * + pow(sin(q[0 * 3 + 1]), 2.0) + + pow(cos(q[0 * 3 + 1]), 2.0) * pow(cos(q[0 * 3 + 2]), 2.0) * + pow(sin(q[0 * 3 + 0]), 2.0) + + pow(cos(q[0 * 3 + 0]), 2.0) * pow(sin(q[0 * 3 + 1]), 2.0) * + pow(sin(q[0 * 3 + 2]), 2.0) + + pow(cos(q[0 * 3 + 1]), 2.0) * pow(sin(q[0 * 3 + 0]), 2.0) * + pow(sin(q[0 * 3 + 2]), 2.0) + + pow(cos(q[0 * 3 + 2]), 2.0) * pow(sin(q[0 * 3 + 0]), 2.0) * + pow(sin(q[0 * 3 + 1]), 2.0) + + pow(sin(q[0 * 3 + 0]), 2.0) * pow(sin(q[0 * 3 + 1]), 2.0) * + pow(sin(q[0 * 3 + 2]), 2.0) + + pow(cos(q[0 * 3 + 0]), 2.0) * pow(cos(q[0 * 3 + 1]), 2.0) * + pow(cos(q[0 * 3 + 2]), 2.0)) * + 2.0E+4) - + 1.03E+2 / 5.0E+2)) / + (pow(cos(q[0 * 3 + 0]), 2.0) * pow(cos(q[0 * 3 + 1]), 2.0) * + pow(sin(q[0 * 3 + 2]), 2.0) + + pow(cos(q[0 * 3 + 0]), 2.0) * pow(cos(q[0 * 3 + 2]), 2.0) * + pow(sin(q[0 * 3 + 1]), 2.0) + + pow(cos(q[0 * 3 + 1]), 2.0) * pow(cos(q[0 * 3 + 2]), 2.0) * + pow(sin(q[0 * 3 + 0]), 2.0) + + pow(cos(q[0 * 3 + 0]), 2.0) * pow(sin(q[0 * 3 + 1]), 2.0) * + pow(sin(q[0 * 3 + 2]), 2.0) + + pow(cos(q[0 * 3 + 1]), 2.0) * pow(sin(q[0 * 3 + 0]), 2.0) * + pow(sin(q[0 * 3 + 2]), 2.0) + + pow(cos(q[0 * 3 + 2]), 2.0) * pow(sin(q[0 * 3 + 0]), 2.0) * + pow(sin(q[0 * 3 + 1]), 2.0) + + pow(sin(q[0 * 3 + 0]), 2.0) * pow(sin(q[0 * 3 + 1]), 2.0) * + pow(sin(q[0 * 3 + 2]), 2.0) + + pow(cos(q[0 * 3 + 0]), 2.0) * pow(cos(q[0 * 3 + 1]), 2.0) * + pow(cos(q[0 * 3 + 2]), 2.0)) + + (cos(q[0 * 3 + 0]) * + (cos(q[0 * 3 + 1]) * cos(q[0 * 3 + 2]) - + sin(q[0 * 3 + 1]) * sin(q[0 * 3 + 2])) * + ((sin(q[0 * 3 + 0]) * + (pow(cos(q[0 * 3 + 1]), 2.0) * sin(q[0 * 3 + 0]) * + sin(q[0 * 3 + 2]) * 1.03E+4 + + sin(q[0 * 3 + 0]) * pow(sin(q[0 * 3 + 1]), 2.0) * + sin(q[0 * 3 + 2]) * 1.03E+4 - + cos(q[0 * 3 + 1]) * sin(q[0 * 3 + 0]) * sin(q[0 * 3 + 2]) * + 1.1315E+4 - + cos(q[0 * 3 + 2]) * sin(q[0 * 3 + 0]) * sin(q[0 * 3 + 1]) * + 1.1315E+4 - + RL * cos(q[0 * 3 + 0]) * cos(q[0 * 3 + 1]) * cos(q[0 * 3 + 2]) * + 5.049E+3 + + RL * cos(q[0 * 3 + 0]) * sin(q[0 * 3 + 1]) * sin(q[0 * 3 + 2]) * + 5.049E+3 - + RL * pow(cos(q[0 * 3 + 0]), 2.0) * cos(q[0 * 3 + 1]) * + cos(q[0 * 3 + 2]) * 3.5E+3 - + RL * cos(q[0 * 3 + 1]) * cos(q[0 * 3 + 2]) * + pow(sin(q[0 * 3 + 0]), 2.0) * 3.5E+3 + + RL * pow(cos(q[0 * 3 + 0]), 2.0) * sin(q[0 * 3 + 1]) * + sin(q[0 * 3 + 2]) * 3.5E+3 + + RL * pow(sin(q[0 * 3 + 0]), 2.0) * sin(q[0 * 3 + 1]) * + sin(q[0 * 3 + 2]) * 3.5E+3) * + (-1.0E-5)) / + ((pow(cos(q[0 * 3 + 0]), 2.0) + pow(sin(q[0 * 3 + 0]), 2.0)) * + (pow(cos(q[0 * 3 + 0]), 2.0) * pow(cos(q[0 * 3 + 1]), 2.0) * + pow(sin(q[0 * 3 + 2]), 2.0) + + pow(cos(q[0 * 3 + 0]), 2.0) * pow(cos(q[0 * 3 + 2]), 2.0) * + pow(sin(q[0 * 3 + 1]), 2.0) + + pow(cos(q[0 * 3 + 1]), 2.0) * pow(cos(q[0 * 3 + 2]), 2.0) * + pow(sin(q[0 * 3 + 0]), 2.0) + + pow(cos(q[0 * 3 + 0]), 2.0) * pow(sin(q[0 * 3 + 1]), 2.0) * + pow(sin(q[0 * 3 + 2]), 2.0) + + pow(cos(q[0 * 3 + 1]), 2.0) * pow(sin(q[0 * 3 + 0]), 2.0) * + pow(sin(q[0 * 3 + 2]), 2.0) + + pow(cos(q[0 * 3 + 2]), 2.0) * pow(sin(q[0 * 3 + 0]), 2.0) * + pow(sin(q[0 * 3 + 1]), 2.0) + + pow(sin(q[0 * 3 + 0]), 2.0) * pow(sin(q[0 * 3 + 1]), 2.0) * + pow(sin(q[0 * 3 + 2]), 2.0) + + pow(cos(q[0 * 3 + 0]), 2.0) * pow(cos(q[0 * 3 + 1]), 2.0) * + pow(cos(q[0 * 3 + 2]), 2.0))) + + ((cos(q[0 * 3 + 1]) * cos(q[0 * 3 + 2]) - + sin(q[0 * 3 + 1]) * sin(q[0 * 3 + 2])) * + (pow(cos(q[0 * 3 + 0]), 2.0) * sin(q[0 * 3 + 1]) * 1.03E+2 + + pow(sin(q[0 * 3 + 0]), 2.0) * sin(q[0 * 3 + 1]) * 1.03E+2 + + RL * pow(cos(q[0 * 3 + 1]), 2.0) * sin(q[0 * 3 + 0]) * 3.5E+1 + + RL * sin(q[0 * 3 + 0]) * pow(sin(q[0 * 3 + 1]), 2.0) * 3.5E+1)) / + ((pow(cos(q[0 * 3 + 0]), 2.0) * pow(cos(q[0 * 3 + 1]), 2.0) + + pow(cos(q[0 * 3 + 0]), 2.0) * pow(sin(q[0 * 3 + 1]), 2.0) + + pow(cos(q[0 * 3 + 1]), 2.0) * pow(sin(q[0 * 3 + 0]), 2.0) + + pow(sin(q[0 * 3 + 0]), 2.0) * pow(sin(q[0 * 3 + 1]), 2.0)) * + (pow(cos(q[0 * 3 + 1]), 2.0) * pow(cos(q[0 * 3 + 2]), 2.0) + + pow(cos(q[0 * 3 + 1]), 2.0) * pow(sin(q[0 * 3 + 2]), 2.0) + + pow(cos(q[0 * 3 + 2]), 2.0) * pow(sin(q[0 * 3 + 1]), 2.0) + + pow(sin(q[0 * 3 + 1]), 2.0) * pow(sin(q[0 * 3 + 2]), 2.0)) * + 1.0E+3) - + (cos(q[0 * 3 + 0]) * + (cos(q[0 * 3 + 0]) * pow(cos(q[0 * 3 + 1]), 2.0) * + sin(q[0 * 3 + 2]) * 1.03E+4 + + cos(q[0 * 3 + 0]) * pow(sin(q[0 * 3 + 1]), 2.0) * + sin(q[0 * 3 + 2]) * 1.03E+4 - + cos(q[0 * 3 + 0]) * cos(q[0 * 3 + 1]) * sin(q[0 * 3 + 2]) * + 1.1315E+4 - + cos(q[0 * 3 + 0]) * cos(q[0 * 3 + 2]) * sin(q[0 * 3 + 1]) * + 1.1315E+4 + + RL * cos(q[0 * 3 + 1]) * cos(q[0 * 3 + 2]) * sin(q[0 * 3 + 0]) * + 5.049E+3 - + RL * sin(q[0 * 3 + 0]) * sin(q[0 * 3 + 1]) * sin(q[0 * 3 + 2]) * + 5.049E+3)) / + ((pow(cos(q[0 * 3 + 0]), 2.0) + pow(sin(q[0 * 3 + 0]), 2.0)) * + (pow(cos(q[0 * 3 + 0]), 2.0) * pow(cos(q[0 * 3 + 1]), 2.0) * + pow(sin(q[0 * 3 + 2]), 2.0) + + pow(cos(q[0 * 3 + 0]), 2.0) * pow(cos(q[0 * 3 + 2]), 2.0) * + pow(sin(q[0 * 3 + 1]), 2.0) + + pow(cos(q[0 * 3 + 1]), 2.0) * pow(cos(q[0 * 3 + 2]), 2.0) * + pow(sin(q[0 * 3 + 0]), 2.0) + + pow(cos(q[0 * 3 + 0]), 2.0) * pow(sin(q[0 * 3 + 1]), 2.0) * + pow(sin(q[0 * 3 + 2]), 2.0) + + pow(cos(q[0 * 3 + 1]), 2.0) * pow(sin(q[0 * 3 + 0]), 2.0) * + pow(sin(q[0 * 3 + 2]), 2.0) + + pow(cos(q[0 * 3 + 2]), 2.0) * pow(sin(q[0 * 3 + 0]), 2.0) * + pow(sin(q[0 * 3 + 1]), 2.0) + + pow(sin(q[0 * 3 + 0]), 2.0) * pow(sin(q[0 * 3 + 1]), 2.0) * + pow(sin(q[0 * 3 + 2]), 2.0) + + pow(cos(q[0 * 3 + 0]), 2.0) * pow(cos(q[0 * 3 + 1]), 2.0) * + pow(cos(q[0 * 3 + 2]), 2.0)) * + 1.0E+5) + + (pow(cos(q[0 * 3 + 0]), 2.0) * + (cos(q[0 * 3 + 1]) * sin(q[0 * 3 + 2]) + + cos(q[0 * 3 + 2]) * sin(q[0 * 3 + 1])) * + (cos(q[0 * 3 + 1]) * -2.06E+3 + + pow(cos(q[0 * 3 + 1]), 2.0) * 2.263E+3 + + pow(sin(q[0 * 3 + 1]), 2.0) * 2.263E+3)) / + ((pow(cos(q[0 * 3 + 0]), 2.0) * pow(cos(q[0 * 3 + 1]), 2.0) + + pow(cos(q[0 * 3 + 0]), 2.0) * pow(sin(q[0 * 3 + 1]), 2.0) + + pow(cos(q[0 * 3 + 1]), 2.0) * pow(sin(q[0 * 3 + 0]), 2.0) + + pow(sin(q[0 * 3 + 0]), 2.0) * pow(sin(q[0 * 3 + 1]), 2.0)) * + (pow(cos(q[0 * 3 + 0]), 2.0) * pow(cos(q[0 * 3 + 1]), 2.0) * + pow(sin(q[0 * 3 + 2]), 2.0) + + pow(cos(q[0 * 3 + 0]), 2.0) * pow(cos(q[0 * 3 + 2]), 2.0) * + pow(sin(q[0 * 3 + 1]), 2.0) + + pow(cos(q[0 * 3 + 1]), 2.0) * pow(cos(q[0 * 3 + 2]), 2.0) * + pow(sin(q[0 * 3 + 0]), 2.0) + + pow(cos(q[0 * 3 + 0]), 2.0) * pow(sin(q[0 * 3 + 1]), 2.0) * + pow(sin(q[0 * 3 + 2]), 2.0) + + pow(cos(q[0 * 3 + 1]), 2.0) * pow(sin(q[0 * 3 + 0]), 2.0) * + pow(sin(q[0 * 3 + 2]), 2.0) + + pow(cos(q[0 * 3 + 2]), 2.0) * pow(sin(q[0 * 3 + 0]), 2.0) * + pow(sin(q[0 * 3 + 1]), 2.0) + + pow(sin(q[0 * 3 + 0]), 2.0) * pow(sin(q[0 * 3 + 1]), 2.0) * + pow(sin(q[0 * 3 + 2]), 2.0) + + pow(cos(q[0 * 3 + 0]), 2.0) * pow(cos(q[0 * 3 + 1]), 2.0) * + pow(cos(q[0 * 3 + 2]), 2.0)) * + 2.0E+4) + + (pow(sin(q[0 * 3 + 0]), 2.0) * + (cos(q[0 * 3 + 1]) * sin(q[0 * 3 + 2]) + + cos(q[0 * 3 + 2]) * sin(q[0 * 3 + 1])) * + (cos(q[0 * 3 + 1]) * -2.06E+3 + + pow(cos(q[0 * 3 + 1]), 2.0) * 2.263E+3 + + pow(sin(q[0 * 3 + 1]), 2.0) * 2.263E+3)) / + ((pow(cos(q[0 * 3 + 0]), 2.0) * pow(cos(q[0 * 3 + 1]), 2.0) + + pow(cos(q[0 * 3 + 0]), 2.0) * pow(sin(q[0 * 3 + 1]), 2.0) + + pow(cos(q[0 * 3 + 1]), 2.0) * pow(sin(q[0 * 3 + 0]), 2.0) + + pow(sin(q[0 * 3 + 0]), 2.0) * pow(sin(q[0 * 3 + 1]), 2.0)) * + (pow(cos(q[0 * 3 + 0]), 2.0) * pow(cos(q[0 * 3 + 1]), 2.0) * + pow(sin(q[0 * 3 + 2]), 2.0) + + pow(cos(q[0 * 3 + 0]), 2.0) * pow(cos(q[0 * 3 + 2]), 2.0) * + pow(sin(q[0 * 3 + 1]), 2.0) + + pow(cos(q[0 * 3 + 1]), 2.0) * pow(cos(q[0 * 3 + 2]), 2.0) * + pow(sin(q[0 * 3 + 0]), 2.0) + + pow(cos(q[0 * 3 + 0]), 2.0) * pow(sin(q[0 * 3 + 1]), 2.0) * + pow(sin(q[0 * 3 + 2]), 2.0) + + pow(cos(q[0 * 3 + 1]), 2.0) * pow(sin(q[0 * 3 + 0]), 2.0) * + pow(sin(q[0 * 3 + 2]), 2.0) + + pow(cos(q[0 * 3 + 2]), 2.0) * pow(sin(q[0 * 3 + 0]), 2.0) * + pow(sin(q[0 * 3 + 1]), 2.0) + + pow(sin(q[0 * 3 + 0]), 2.0) * pow(sin(q[0 * 3 + 1]), 2.0) * + pow(sin(q[0 * 3 + 2]), 2.0) + + pow(cos(q[0 * 3 + 0]), 2.0) * pow(cos(q[0 * 3 + 1]), 2.0) * + pow(cos(q[0 * 3 + 2]), 2.0)) * + 2.0E+4))) / + (pow(cos(q[0 * 3 + 0]), 2.0) * pow(cos(q[0 * 3 + 1]), 2.0) * + pow(sin(q[0 * 3 + 2]), 2.0) + + pow(cos(q[0 * 3 + 0]), 2.0) * pow(cos(q[0 * 3 + 2]), 2.0) * + pow(sin(q[0 * 3 + 1]), 2.0) + + pow(cos(q[0 * 3 + 1]), 2.0) * pow(cos(q[0 * 3 + 2]), 2.0) * + pow(sin(q[0 * 3 + 0]), 2.0) + + pow(cos(q[0 * 3 + 0]), 2.0) * pow(sin(q[0 * 3 + 1]), 2.0) * + pow(sin(q[0 * 3 + 2]), 2.0) + + pow(cos(q[0 * 3 + 1]), 2.0) * pow(sin(q[0 * 3 + 0]), 2.0) * + pow(sin(q[0 * 3 + 2]), 2.0) + + pow(cos(q[0 * 3 + 2]), 2.0) * pow(sin(q[0 * 3 + 0]), 2.0) * + pow(sin(q[0 * 3 + 1]), 2.0) + + pow(sin(q[0 * 3 + 0]), 2.0) * pow(sin(q[0 * 3 + 1]), 2.0) * + pow(sin(q[0 * 3 + 2]), 2.0) + + pow(cos(q[0 * 3 + 0]), 2.0) * pow(cos(q[0 * 3 + 1]), 2.0) * + pow(cos(q[0 * 3 + 2]), 2.0))) - + (cos(q[0 * 3 + 0]) * sin(q[0 * 3 + 1]) * (1.03E+2 / 5.0E+2)) / + (pow(cos(q[0 * 3 + 0]), 2.0) + pow(sin(q[0 * 3 + 0]), 2.0)) + + ((cos(q[0 * 3 + 1]) * sin(q[0 * 3 + 2]) + + cos(q[0 * 3 + 2]) * sin(q[0 * 3 + 1])) * + (cos(q[0 * 3 + 0]) * pow(cos(q[0 * 3 + 1]), 2.0) * cos(q[0 * 3 + 2]) * + 1.03E+4 + + cos(q[0 * 3 + 0]) * cos(q[0 * 3 + 2]) * pow(sin(q[0 * 3 + 1]), 2.0) * + 1.03E+4 - + cos(q[0 * 3 + 0]) * cos(q[0 * 3 + 1]) * cos(q[0 * 3 + 2]) * 1.1315E+4 + + cos(q[0 * 3 + 0]) * sin(q[0 * 3 + 1]) * sin(q[0 * 3 + 2]) * 1.1315E+4 - + RL * cos(q[0 * 3 + 1]) * sin(q[0 * 3 + 0]) * sin(q[0 * 3 + 2]) * + 5.049E+3 - + RL * cos(q[0 * 3 + 2]) * sin(q[0 * 3 + 0]) * sin(q[0 * 3 + 1]) * + 5.049E+3)) / + (pow(cos(q[0 * 3 + 0]), 2.0) * pow(cos(q[0 * 3 + 1]), 2.0) * + pow(sin(q[0 * 3 + 2]), 2.0) * 5.0E+4 + + pow(cos(q[0 * 3 + 0]), 2.0) * pow(cos(q[0 * 3 + 2]), 2.0) * + pow(sin(q[0 * 3 + 1]), 2.0) * 5.0E+4 + + pow(cos(q[0 * 3 + 1]), 2.0) * pow(cos(q[0 * 3 + 2]), 2.0) * + pow(sin(q[0 * 3 + 0]), 2.0) * 5.0E+4 + + pow(cos(q[0 * 3 + 0]), 2.0) * pow(sin(q[0 * 3 + 1]), 2.0) * + pow(sin(q[0 * 3 + 2]), 2.0) * 5.0E+4 + + pow(cos(q[0 * 3 + 1]), 2.0) * pow(sin(q[0 * 3 + 0]), 2.0) * + pow(sin(q[0 * 3 + 2]), 2.0) * 5.0E+4 + + pow(cos(q[0 * 3 + 2]), 2.0) * pow(sin(q[0 * 3 + 0]), 2.0) * + pow(sin(q[0 * 3 + 1]), 2.0) * 5.0E+4 + + pow(sin(q[0 * 3 + 0]), 2.0) * pow(sin(q[0 * 3 + 1]), 2.0) * + pow(sin(q[0 * 3 + 2]), 2.0) * 5.0E+4 + + pow(cos(q[0 * 3 + 0]), 2.0) * pow(cos(q[0 * 3 + 1]), 2.0) * + pow(cos(q[0 * 3 + 2]), 2.0) * 5.0E+4) - + ((cos(q[0 * 3 + 1]) * cos(q[0 * 3 + 2]) - + sin(q[0 * 3 + 1]) * sin(q[0 * 3 + 2])) * + (cos(q[0 * 3 + 0]) * pow(cos(q[0 * 3 + 1]), 2.0) * sin(q[0 * 3 + 2]) * + 1.03E+4 + + cos(q[0 * 3 + 0]) * pow(sin(q[0 * 3 + 1]), 2.0) * sin(q[0 * 3 + 2]) * + 1.03E+4 - + cos(q[0 * 3 + 0]) * cos(q[0 * 3 + 1]) * sin(q[0 * 3 + 2]) * 1.1315E+4 - + cos(q[0 * 3 + 0]) * cos(q[0 * 3 + 2]) * sin(q[0 * 3 + 1]) * 1.1315E+4 + + RL * cos(q[0 * 3 + 1]) * cos(q[0 * 3 + 2]) * sin(q[0 * 3 + 0]) * + 5.049E+3 - + RL * sin(q[0 * 3 + 0]) * sin(q[0 * 3 + 1]) * sin(q[0 * 3 + 2]) * + 5.049E+3)) / + (pow(cos(q[0 * 3 + 0]), 2.0) * pow(cos(q[0 * 3 + 1]), 2.0) * + pow(sin(q[0 * 3 + 2]), 2.0) * 5.0E+4 + + pow(cos(q[0 * 3 + 0]), 2.0) * pow(cos(q[0 * 3 + 2]), 2.0) * + pow(sin(q[0 * 3 + 1]), 2.0) * 5.0E+4 + + pow(cos(q[0 * 3 + 1]), 2.0) * pow(cos(q[0 * 3 + 2]), 2.0) * + pow(sin(q[0 * 3 + 0]), 2.0) * 5.0E+4 + + pow(cos(q[0 * 3 + 0]), 2.0) * pow(sin(q[0 * 3 + 1]), 2.0) * + pow(sin(q[0 * 3 + 2]), 2.0) * 5.0E+4 + + pow(cos(q[0 * 3 + 1]), 2.0) * pow(sin(q[0 * 3 + 0]), 2.0) * + pow(sin(q[0 * 3 + 2]), 2.0) * 5.0E+4 + + pow(cos(q[0 * 3 + 2]), 2.0) * pow(sin(q[0 * 3 + 0]), 2.0) * + pow(sin(q[0 * 3 + 1]), 2.0) * 5.0E+4 + + pow(sin(q[0 * 3 + 0]), 2.0) * pow(sin(q[0 * 3 + 1]), 2.0) * + pow(sin(q[0 * 3 + 2]), 2.0) * 5.0E+4 + + pow(cos(q[0 * 3 + 0]), 2.0) * pow(cos(q[0 * 3 + 1]), 2.0) * + pow(cos(q[0 * 3 + 2]), 2.0) * 5.0E+4) + + (sin(q[0 * 3 + 0]) * + (RL * cos(q[0 * 3 + 1]) * sin(q[0 * 3 + 2]) * 1.0098E-1 + + RL * cos(q[0 * 3 + 2]) * sin(q[0 * 3 + 1]) * 1.0098E-1) * + (cos(q[0 * 3 + 1]) * sin(q[0 * 3 + 2]) + + cos(q[0 * 3 + 2]) * sin(q[0 * 3 + 1]))) / + (pow(cos(q[0 * 3 + 0]), 2.0) * pow(cos(q[0 * 3 + 1]), 2.0) * + pow(sin(q[0 * 3 + 2]), 2.0) + + pow(cos(q[0 * 3 + 0]), 2.0) * pow(cos(q[0 * 3 + 2]), 2.0) * + pow(sin(q[0 * 3 + 1]), 2.0) + + pow(cos(q[0 * 3 + 1]), 2.0) * pow(cos(q[0 * 3 + 2]), 2.0) * + pow(sin(q[0 * 3 + 0]), 2.0) + + pow(cos(q[0 * 3 + 0]), 2.0) * pow(sin(q[0 * 3 + 1]), 2.0) * + pow(sin(q[0 * 3 + 2]), 2.0) + + pow(cos(q[0 * 3 + 1]), 2.0) * pow(sin(q[0 * 3 + 0]), 2.0) * + pow(sin(q[0 * 3 + 2]), 2.0) + + pow(cos(q[0 * 3 + 2]), 2.0) * pow(sin(q[0 * 3 + 0]), 2.0) * + pow(sin(q[0 * 3 + 1]), 2.0) + + pow(sin(q[0 * 3 + 0]), 2.0) * pow(sin(q[0 * 3 + 1]), 2.0) * + pow(sin(q[0 * 3 + 2]), 2.0) + + pow(cos(q[0 * 3 + 0]), 2.0) * pow(cos(q[0 * 3 + 1]), 2.0) * + pow(cos(q[0 * 3 + 2]), 2.0)) + + (sin(q[0 * 3 + 0]) * + (RL * cos(q[0 * 3 + 1]) * cos(q[0 * 3 + 2]) * 1.0098E-1 - + RL * sin(q[0 * 3 + 1]) * sin(q[0 * 3 + 2]) * 1.0098E-1) * + (cos(q[0 * 3 + 1]) * cos(q[0 * 3 + 2]) - + sin(q[0 * 3 + 1]) * sin(q[0 * 3 + 2]))) / + (pow(cos(q[0 * 3 + 0]), 2.0) * pow(cos(q[0 * 3 + 1]), 2.0) * + pow(sin(q[0 * 3 + 2]), 2.0) + + pow(cos(q[0 * 3 + 0]), 2.0) * pow(cos(q[0 * 3 + 2]), 2.0) * + pow(sin(q[0 * 3 + 1]), 2.0) + + pow(cos(q[0 * 3 + 1]), 2.0) * pow(cos(q[0 * 3 + 2]), 2.0) * + pow(sin(q[0 * 3 + 0]), 2.0) + + pow(cos(q[0 * 3 + 0]), 2.0) * pow(sin(q[0 * 3 + 1]), 2.0) * + pow(sin(q[0 * 3 + 2]), 2.0) + + pow(cos(q[0 * 3 + 1]), 2.0) * pow(sin(q[0 * 3 + 0]), 2.0) * + pow(sin(q[0 * 3 + 2]), 2.0) + + pow(cos(q[0 * 3 + 2]), 2.0) * pow(sin(q[0 * 3 + 0]), 2.0) * + pow(sin(q[0 * 3 + 1]), 2.0) + + pow(sin(q[0 * 3 + 0]), 2.0) * pow(sin(q[0 * 3 + 1]), 2.0) * + pow(sin(q[0 * 3 + 2]), 2.0) + + pow(cos(q[0 * 3 + 0]), 2.0) * pow(cos(q[0 * 3 + 1]), 2.0) * + pow(cos(q[0 * 3 + 2]), 2.0)); + F(1, 1) = + (sin(q[0 * 3 + 0]) * + (cos(q[0 * 3 + 1]) * -2.06E+3 + pow(cos(q[0 * 3 + 1]), 2.0) * 2.263E+3 + + pow(sin(q[0 * 3 + 1]), 2.0) * 2.263E+3) * + (-1.0E-4)) / + (pow(cos(q[0 * 3 + 0]), 2.0) * pow(cos(q[0 * 3 + 1]), 2.0) + + pow(cos(q[0 * 3 + 0]), 2.0) * pow(sin(q[0 * 3 + 1]), 2.0) + + pow(cos(q[0 * 3 + 1]), 2.0) * pow(sin(q[0 * 3 + 0]), 2.0) + + pow(sin(q[0 * 3 + 0]), 2.0) * pow(sin(q[0 * 3 + 1]), 2.0)) - + (sin(q[0 * 3 + 0]) * + (((cos(q[0 * 3 + 1]) * cos(q[0 * 3 + 2]) - + sin(q[0 * 3 + 1]) * sin(q[0 * 3 + 2])) * + ((cos(q[0 * 3 + 0]) * + (cos(q[0 * 3 + 0]) * pow(cos(q[0 * 3 + 1]), 2.0) * + cos(q[0 * 3 + 2]) * 1.03E+4 + + cos(q[0 * 3 + 0]) * cos(q[0 * 3 + 2]) * + pow(sin(q[0 * 3 + 1]), 2.0) * 1.03E+4 - + cos(q[0 * 3 + 0]) * cos(q[0 * 3 + 1]) * cos(q[0 * 3 + 2]) * + 1.1315E+4 + + cos(q[0 * 3 + 0]) * sin(q[0 * 3 + 1]) * sin(q[0 * 3 + 2]) * + 1.1315E+4 - + RL * cos(q[0 * 3 + 1]) * sin(q[0 * 3 + 0]) * sin(q[0 * 3 + 2]) * + 5.049E+3 - + RL * cos(q[0 * 3 + 2]) * sin(q[0 * 3 + 0]) * sin(q[0 * 3 + 1]) * + 5.049E+3)) / + ((pow(cos(q[0 * 3 + 0]), 2.0) + pow(sin(q[0 * 3 + 0]), 2.0)) * + (pow(cos(q[0 * 3 + 0]), 2.0) * pow(cos(q[0 * 3 + 1]), 2.0) * + pow(sin(q[0 * 3 + 2]), 2.0) + + pow(cos(q[0 * 3 + 0]), 2.0) * pow(cos(q[0 * 3 + 2]), 2.0) * + pow(sin(q[0 * 3 + 1]), 2.0) + + pow(cos(q[0 * 3 + 1]), 2.0) * pow(cos(q[0 * 3 + 2]), 2.0) * + pow(sin(q[0 * 3 + 0]), 2.0) + + pow(cos(q[0 * 3 + 0]), 2.0) * pow(sin(q[0 * 3 + 1]), 2.0) * + pow(sin(q[0 * 3 + 2]), 2.0) + + pow(cos(q[0 * 3 + 1]), 2.0) * pow(sin(q[0 * 3 + 0]), 2.0) * + pow(sin(q[0 * 3 + 2]), 2.0) + + pow(cos(q[0 * 3 + 2]), 2.0) * pow(sin(q[0 * 3 + 0]), 2.0) * + pow(sin(q[0 * 3 + 1]), 2.0) + + pow(sin(q[0 * 3 + 0]), 2.0) * pow(sin(q[0 * 3 + 1]), 2.0) * + pow(sin(q[0 * 3 + 2]), 2.0) + + pow(cos(q[0 * 3 + 0]), 2.0) * pow(cos(q[0 * 3 + 1]), 2.0) * + pow(cos(q[0 * 3 + 2]), 2.0)) * + 1.0E+5) + + ((cos(q[0 * 3 + 1]) * sin(q[0 * 3 + 2]) + + cos(q[0 * 3 + 2]) * sin(q[0 * 3 + 1])) * + (pow(cos(q[0 * 3 + 0]), 2.0) * sin(q[0 * 3 + 1]) * 1.03E+2 + + pow(sin(q[0 * 3 + 0]), 2.0) * sin(q[0 * 3 + 1]) * 1.03E+2 + + RL * pow(cos(q[0 * 3 + 1]), 2.0) * sin(q[0 * 3 + 0]) * 3.5E+1 + + RL * sin(q[0 * 3 + 0]) * pow(sin(q[0 * 3 + 1]), 2.0) * 3.5E+1)) / + ((pow(cos(q[0 * 3 + 0]), 2.0) * pow(cos(q[0 * 3 + 1]), 2.0) + + pow(cos(q[0 * 3 + 0]), 2.0) * pow(sin(q[0 * 3 + 1]), 2.0) + + pow(cos(q[0 * 3 + 1]), 2.0) * pow(sin(q[0 * 3 + 0]), 2.0) + + pow(sin(q[0 * 3 + 0]), 2.0) * pow(sin(q[0 * 3 + 1]), 2.0)) * + (pow(cos(q[0 * 3 + 1]), 2.0) * pow(cos(q[0 * 3 + 2]), 2.0) + + pow(cos(q[0 * 3 + 1]), 2.0) * pow(sin(q[0 * 3 + 2]), 2.0) + + pow(cos(q[0 * 3 + 2]), 2.0) * pow(sin(q[0 * 3 + 1]), 2.0) + + pow(sin(q[0 * 3 + 1]), 2.0) * pow(sin(q[0 * 3 + 2]), 2.0)) * + 1.0E+3) + + (sin(q[0 * 3 + 0]) * + (sin(q[0 * 3 + 0]) * sin(q[0 * 3 + 1]) * sin(q[0 * 3 + 2]) * + 1.1315E+4 + + pow(cos(q[0 * 3 + 1]), 2.0) * cos(q[0 * 3 + 2]) * + sin(q[0 * 3 + 0]) * 1.03E+4 + + cos(q[0 * 3 + 2]) * sin(q[0 * 3 + 0]) * + pow(sin(q[0 * 3 + 1]), 2.0) * 1.03E+4 - + cos(q[0 * 3 + 1]) * cos(q[0 * 3 + 2]) * sin(q[0 * 3 + 0]) * + 1.1315E+4 + + RL * cos(q[0 * 3 + 0]) * cos(q[0 * 3 + 1]) * sin(q[0 * 3 + 2]) * + 5.049E+3 + + RL * cos(q[0 * 3 + 0]) * cos(q[0 * 3 + 2]) * sin(q[0 * 3 + 1]) * + 5.049E+3 + + RL * pow(cos(q[0 * 3 + 0]), 2.0) * cos(q[0 * 3 + 1]) * + sin(q[0 * 3 + 2]) * 3.5E+3 + + RL * pow(cos(q[0 * 3 + 0]), 2.0) * cos(q[0 * 3 + 2]) * + sin(q[0 * 3 + 1]) * 3.5E+3 + + RL * cos(q[0 * 3 + 1]) * pow(sin(q[0 * 3 + 0]), 2.0) * + sin(q[0 * 3 + 2]) * 3.5E+3 + + RL * cos(q[0 * 3 + 2]) * pow(sin(q[0 * 3 + 0]), 2.0) * + sin(q[0 * 3 + 1]) * 3.5E+3)) / + ((pow(cos(q[0 * 3 + 0]), 2.0) + pow(sin(q[0 * 3 + 0]), 2.0)) * + (pow(cos(q[0 * 3 + 0]), 2.0) * pow(cos(q[0 * 3 + 1]), 2.0) * + pow(sin(q[0 * 3 + 2]), 2.0) + + pow(cos(q[0 * 3 + 0]), 2.0) * pow(cos(q[0 * 3 + 2]), 2.0) * + pow(sin(q[0 * 3 + 1]), 2.0) + + pow(cos(q[0 * 3 + 1]), 2.0) * pow(cos(q[0 * 3 + 2]), 2.0) * + pow(sin(q[0 * 3 + 0]), 2.0) + + pow(cos(q[0 * 3 + 0]), 2.0) * pow(sin(q[0 * 3 + 1]), 2.0) * + pow(sin(q[0 * 3 + 2]), 2.0) + + pow(cos(q[0 * 3 + 1]), 2.0) * pow(sin(q[0 * 3 + 0]), 2.0) * + pow(sin(q[0 * 3 + 2]), 2.0) + + pow(cos(q[0 * 3 + 2]), 2.0) * pow(sin(q[0 * 3 + 0]), 2.0) * + pow(sin(q[0 * 3 + 1]), 2.0) + + pow(sin(q[0 * 3 + 0]), 2.0) * pow(sin(q[0 * 3 + 1]), 2.0) * + pow(sin(q[0 * 3 + 2]), 2.0) + + pow(cos(q[0 * 3 + 0]), 2.0) * pow(cos(q[0 * 3 + 1]), 2.0) * + pow(cos(q[0 * 3 + 2]), 2.0)) * + 1.0E+5) - + (pow(cos(q[0 * 3 + 0]), 2.0) * + (cos(q[0 * 3 + 1]) * cos(q[0 * 3 + 2]) - + sin(q[0 * 3 + 1]) * sin(q[0 * 3 + 2])) * + (cos(q[0 * 3 + 1]) * -2.06E+3 + + pow(cos(q[0 * 3 + 1]), 2.0) * 2.263E+3 + + pow(sin(q[0 * 3 + 1]), 2.0) * 2.263E+3)) / + ((pow(cos(q[0 * 3 + 0]), 2.0) * pow(cos(q[0 * 3 + 1]), 2.0) + + pow(cos(q[0 * 3 + 0]), 2.0) * pow(sin(q[0 * 3 + 1]), 2.0) + + pow(cos(q[0 * 3 + 1]), 2.0) * pow(sin(q[0 * 3 + 0]), 2.0) + + pow(sin(q[0 * 3 + 0]), 2.0) * pow(sin(q[0 * 3 + 1]), 2.0)) * + (pow(cos(q[0 * 3 + 0]), 2.0) * pow(cos(q[0 * 3 + 1]), 2.0) * + pow(sin(q[0 * 3 + 2]), 2.0) + + pow(cos(q[0 * 3 + 0]), 2.0) * pow(cos(q[0 * 3 + 2]), 2.0) * + pow(sin(q[0 * 3 + 1]), 2.0) + + pow(cos(q[0 * 3 + 1]), 2.0) * pow(cos(q[0 * 3 + 2]), 2.0) * + pow(sin(q[0 * 3 + 0]), 2.0) + + pow(cos(q[0 * 3 + 0]), 2.0) * pow(sin(q[0 * 3 + 1]), 2.0) * + pow(sin(q[0 * 3 + 2]), 2.0) + + pow(cos(q[0 * 3 + 1]), 2.0) * pow(sin(q[0 * 3 + 0]), 2.0) * + pow(sin(q[0 * 3 + 2]), 2.0) + + pow(cos(q[0 * 3 + 2]), 2.0) * pow(sin(q[0 * 3 + 0]), 2.0) * + pow(sin(q[0 * 3 + 1]), 2.0) + + pow(sin(q[0 * 3 + 0]), 2.0) * pow(sin(q[0 * 3 + 1]), 2.0) * + pow(sin(q[0 * 3 + 2]), 2.0) + + pow(cos(q[0 * 3 + 0]), 2.0) * pow(cos(q[0 * 3 + 1]), 2.0) * + pow(cos(q[0 * 3 + 2]), 2.0)) * + 2.0E+4) - + (pow(sin(q[0 * 3 + 0]), 2.0) * + (cos(q[0 * 3 + 1]) * cos(q[0 * 3 + 2]) - + sin(q[0 * 3 + 1]) * sin(q[0 * 3 + 2])) * + (cos(q[0 * 3 + 1]) * -2.06E+3 + + pow(cos(q[0 * 3 + 1]), 2.0) * 2.263E+3 + + pow(sin(q[0 * 3 + 1]), 2.0) * 2.263E+3)) / + ((pow(cos(q[0 * 3 + 0]), 2.0) * pow(cos(q[0 * 3 + 1]), 2.0) + + pow(cos(q[0 * 3 + 0]), 2.0) * pow(sin(q[0 * 3 + 1]), 2.0) + + pow(cos(q[0 * 3 + 1]), 2.0) * pow(sin(q[0 * 3 + 0]), 2.0) + + pow(sin(q[0 * 3 + 0]), 2.0) * pow(sin(q[0 * 3 + 1]), 2.0)) * + (pow(cos(q[0 * 3 + 0]), 2.0) * pow(cos(q[0 * 3 + 1]), 2.0) * + pow(sin(q[0 * 3 + 2]), 2.0) + + pow(cos(q[0 * 3 + 0]), 2.0) * pow(cos(q[0 * 3 + 2]), 2.0) * + pow(sin(q[0 * 3 + 1]), 2.0) + + pow(cos(q[0 * 3 + 1]), 2.0) * pow(cos(q[0 * 3 + 2]), 2.0) * + pow(sin(q[0 * 3 + 0]), 2.0) + + pow(cos(q[0 * 3 + 0]), 2.0) * pow(sin(q[0 * 3 + 1]), 2.0) * + pow(sin(q[0 * 3 + 2]), 2.0) + + pow(cos(q[0 * 3 + 1]), 2.0) * pow(sin(q[0 * 3 + 0]), 2.0) * + pow(sin(q[0 * 3 + 2]), 2.0) + + pow(cos(q[0 * 3 + 2]), 2.0) * pow(sin(q[0 * 3 + 0]), 2.0) * + pow(sin(q[0 * 3 + 1]), 2.0) + + pow(sin(q[0 * 3 + 0]), 2.0) * pow(sin(q[0 * 3 + 1]), 2.0) * + pow(sin(q[0 * 3 + 2]), 2.0) + + pow(cos(q[0 * 3 + 0]), 2.0) * pow(cos(q[0 * 3 + 1]), 2.0) * + pow(cos(q[0 * 3 + 2]), 2.0)) * + 2.0E+4) - + 1.03E+2 / 5.0E+2)) / + (pow(cos(q[0 * 3 + 1]), 2.0) * pow(cos(q[0 * 3 + 2]), 2.0) + + pow(cos(q[0 * 3 + 1]), 2.0) * pow(sin(q[0 * 3 + 2]), 2.0) + + pow(cos(q[0 * 3 + 2]), 2.0) * pow(sin(q[0 * 3 + 1]), 2.0) + + pow(sin(q[0 * 3 + 1]), 2.0) * pow(sin(q[0 * 3 + 2]), 2.0)) - + ((cos(q[0 * 3 + 1]) * sin(q[0 * 3 + 2]) + + cos(q[0 * 3 + 2]) * sin(q[0 * 3 + 1])) * + ((sin(q[0 * 3 + 0]) * + (pow(cos(q[0 * 3 + 1]), 2.0) * sin(q[0 * 3 + 0]) * + sin(q[0 * 3 + 2]) * 1.03E+4 + + sin(q[0 * 3 + 0]) * pow(sin(q[0 * 3 + 1]), 2.0) * + sin(q[0 * 3 + 2]) * 1.03E+4 - + cos(q[0 * 3 + 1]) * sin(q[0 * 3 + 0]) * sin(q[0 * 3 + 2]) * + 1.1315E+4 - + cos(q[0 * 3 + 2]) * sin(q[0 * 3 + 0]) * sin(q[0 * 3 + 1]) * + 1.1315E+4 - + RL * cos(q[0 * 3 + 0]) * cos(q[0 * 3 + 1]) * cos(q[0 * 3 + 2]) * + 5.049E+3 + + RL * cos(q[0 * 3 + 0]) * sin(q[0 * 3 + 1]) * sin(q[0 * 3 + 2]) * + 5.049E+3 - + RL * pow(cos(q[0 * 3 + 0]), 2.0) * cos(q[0 * 3 + 1]) * + cos(q[0 * 3 + 2]) * 3.5E+3 - + RL * cos(q[0 * 3 + 1]) * cos(q[0 * 3 + 2]) * + pow(sin(q[0 * 3 + 0]), 2.0) * 3.5E+3 + + RL * pow(cos(q[0 * 3 + 0]), 2.0) * sin(q[0 * 3 + 1]) * + sin(q[0 * 3 + 2]) * 3.5E+3 + + RL * pow(sin(q[0 * 3 + 0]), 2.0) * sin(q[0 * 3 + 1]) * + sin(q[0 * 3 + 2]) * 3.5E+3) * + (-1.0E-5)) / + ((pow(cos(q[0 * 3 + 0]), 2.0) + pow(sin(q[0 * 3 + 0]), 2.0)) * + (pow(cos(q[0 * 3 + 0]), 2.0) * pow(cos(q[0 * 3 + 1]), 2.0) * + pow(sin(q[0 * 3 + 2]), 2.0) + + pow(cos(q[0 * 3 + 0]), 2.0) * pow(cos(q[0 * 3 + 2]), 2.0) * + pow(sin(q[0 * 3 + 1]), 2.0) + + pow(cos(q[0 * 3 + 1]), 2.0) * pow(cos(q[0 * 3 + 2]), 2.0) * + pow(sin(q[0 * 3 + 0]), 2.0) + + pow(cos(q[0 * 3 + 0]), 2.0) * pow(sin(q[0 * 3 + 1]), 2.0) * + pow(sin(q[0 * 3 + 2]), 2.0) + + pow(cos(q[0 * 3 + 1]), 2.0) * pow(sin(q[0 * 3 + 0]), 2.0) * + pow(sin(q[0 * 3 + 2]), 2.0) + + pow(cos(q[0 * 3 + 2]), 2.0) * pow(sin(q[0 * 3 + 0]), 2.0) * + pow(sin(q[0 * 3 + 1]), 2.0) + + pow(sin(q[0 * 3 + 0]), 2.0) * pow(sin(q[0 * 3 + 1]), 2.0) * + pow(sin(q[0 * 3 + 2]), 2.0) + + pow(cos(q[0 * 3 + 0]), 2.0) * pow(cos(q[0 * 3 + 1]), 2.0) * + pow(cos(q[0 * 3 + 2]), 2.0))) + + ((cos(q[0 * 3 + 1]) * cos(q[0 * 3 + 2]) - + sin(q[0 * 3 + 1]) * sin(q[0 * 3 + 2])) * + (pow(cos(q[0 * 3 + 0]), 2.0) * sin(q[0 * 3 + 1]) * 1.03E+2 + + pow(sin(q[0 * 3 + 0]), 2.0) * sin(q[0 * 3 + 1]) * 1.03E+2 + + RL * pow(cos(q[0 * 3 + 1]), 2.0) * sin(q[0 * 3 + 0]) * 3.5E+1 + + RL * sin(q[0 * 3 + 0]) * pow(sin(q[0 * 3 + 1]), 2.0) * 3.5E+1)) / + ((pow(cos(q[0 * 3 + 0]), 2.0) * pow(cos(q[0 * 3 + 1]), 2.0) + + pow(cos(q[0 * 3 + 0]), 2.0) * pow(sin(q[0 * 3 + 1]), 2.0) + + pow(cos(q[0 * 3 + 1]), 2.0) * pow(sin(q[0 * 3 + 0]), 2.0) + + pow(sin(q[0 * 3 + 0]), 2.0) * pow(sin(q[0 * 3 + 1]), 2.0)) * + (pow(cos(q[0 * 3 + 1]), 2.0) * pow(cos(q[0 * 3 + 2]), 2.0) + + pow(cos(q[0 * 3 + 1]), 2.0) * pow(sin(q[0 * 3 + 2]), 2.0) + + pow(cos(q[0 * 3 + 2]), 2.0) * pow(sin(q[0 * 3 + 1]), 2.0) + + pow(sin(q[0 * 3 + 1]), 2.0) * pow(sin(q[0 * 3 + 2]), 2.0)) * + 1.0E+3) - + (cos(q[0 * 3 + 0]) * + (cos(q[0 * 3 + 0]) * pow(cos(q[0 * 3 + 1]), 2.0) * + sin(q[0 * 3 + 2]) * 1.03E+4 + + cos(q[0 * 3 + 0]) * pow(sin(q[0 * 3 + 1]), 2.0) * + sin(q[0 * 3 + 2]) * 1.03E+4 - + cos(q[0 * 3 + 0]) * cos(q[0 * 3 + 1]) * sin(q[0 * 3 + 2]) * + 1.1315E+4 - + cos(q[0 * 3 + 0]) * cos(q[0 * 3 + 2]) * sin(q[0 * 3 + 1]) * + 1.1315E+4 + + RL * cos(q[0 * 3 + 1]) * cos(q[0 * 3 + 2]) * sin(q[0 * 3 + 0]) * + 5.049E+3 - + RL * sin(q[0 * 3 + 0]) * sin(q[0 * 3 + 1]) * sin(q[0 * 3 + 2]) * + 5.049E+3)) / + ((pow(cos(q[0 * 3 + 0]), 2.0) + pow(sin(q[0 * 3 + 0]), 2.0)) * + (pow(cos(q[0 * 3 + 0]), 2.0) * pow(cos(q[0 * 3 + 1]), 2.0) * + pow(sin(q[0 * 3 + 2]), 2.0) + + pow(cos(q[0 * 3 + 0]), 2.0) * pow(cos(q[0 * 3 + 2]), 2.0) * + pow(sin(q[0 * 3 + 1]), 2.0) + + pow(cos(q[0 * 3 + 1]), 2.0) * pow(cos(q[0 * 3 + 2]), 2.0) * + pow(sin(q[0 * 3 + 0]), 2.0) + + pow(cos(q[0 * 3 + 0]), 2.0) * pow(sin(q[0 * 3 + 1]), 2.0) * + pow(sin(q[0 * 3 + 2]), 2.0) + + pow(cos(q[0 * 3 + 1]), 2.0) * pow(sin(q[0 * 3 + 0]), 2.0) * + pow(sin(q[0 * 3 + 2]), 2.0) + + pow(cos(q[0 * 3 + 2]), 2.0) * pow(sin(q[0 * 3 + 0]), 2.0) * + pow(sin(q[0 * 3 + 1]), 2.0) + + pow(sin(q[0 * 3 + 0]), 2.0) * pow(sin(q[0 * 3 + 1]), 2.0) * + pow(sin(q[0 * 3 + 2]), 2.0) + + pow(cos(q[0 * 3 + 0]), 2.0) * pow(cos(q[0 * 3 + 1]), 2.0) * + pow(cos(q[0 * 3 + 2]), 2.0)) * + 1.0E+5) + + (pow(cos(q[0 * 3 + 0]), 2.0) * + (cos(q[0 * 3 + 1]) * sin(q[0 * 3 + 2]) + + cos(q[0 * 3 + 2]) * sin(q[0 * 3 + 1])) * + (cos(q[0 * 3 + 1]) * -2.06E+3 + + pow(cos(q[0 * 3 + 1]), 2.0) * 2.263E+3 + + pow(sin(q[0 * 3 + 1]), 2.0) * 2.263E+3)) / + ((pow(cos(q[0 * 3 + 0]), 2.0) * pow(cos(q[0 * 3 + 1]), 2.0) + + pow(cos(q[0 * 3 + 0]), 2.0) * pow(sin(q[0 * 3 + 1]), 2.0) + + pow(cos(q[0 * 3 + 1]), 2.0) * pow(sin(q[0 * 3 + 0]), 2.0) + + pow(sin(q[0 * 3 + 0]), 2.0) * pow(sin(q[0 * 3 + 1]), 2.0)) * + (pow(cos(q[0 * 3 + 0]), 2.0) * pow(cos(q[0 * 3 + 1]), 2.0) * + pow(sin(q[0 * 3 + 2]), 2.0) + + pow(cos(q[0 * 3 + 0]), 2.0) * pow(cos(q[0 * 3 + 2]), 2.0) * + pow(sin(q[0 * 3 + 1]), 2.0) + + pow(cos(q[0 * 3 + 1]), 2.0) * pow(cos(q[0 * 3 + 2]), 2.0) * + pow(sin(q[0 * 3 + 0]), 2.0) + + pow(cos(q[0 * 3 + 0]), 2.0) * pow(sin(q[0 * 3 + 1]), 2.0) * + pow(sin(q[0 * 3 + 2]), 2.0) + + pow(cos(q[0 * 3 + 1]), 2.0) * pow(sin(q[0 * 3 + 0]), 2.0) * + pow(sin(q[0 * 3 + 2]), 2.0) + + pow(cos(q[0 * 3 + 2]), 2.0) * pow(sin(q[0 * 3 + 0]), 2.0) * + pow(sin(q[0 * 3 + 1]), 2.0) + + pow(sin(q[0 * 3 + 0]), 2.0) * pow(sin(q[0 * 3 + 1]), 2.0) * + pow(sin(q[0 * 3 + 2]), 2.0) + + pow(cos(q[0 * 3 + 0]), 2.0) * pow(cos(q[0 * 3 + 1]), 2.0) * + pow(cos(q[0 * 3 + 2]), 2.0)) * + 2.0E+4) + + (pow(sin(q[0 * 3 + 0]), 2.0) * + (cos(q[0 * 3 + 1]) * sin(q[0 * 3 + 2]) + + cos(q[0 * 3 + 2]) * sin(q[0 * 3 + 1])) * + (cos(q[0 * 3 + 1]) * -2.06E+3 + + pow(cos(q[0 * 3 + 1]), 2.0) * 2.263E+3 + + pow(sin(q[0 * 3 + 1]), 2.0) * 2.263E+3)) / + ((pow(cos(q[0 * 3 + 0]), 2.0) * pow(cos(q[0 * 3 + 1]), 2.0) + + pow(cos(q[0 * 3 + 0]), 2.0) * pow(sin(q[0 * 3 + 1]), 2.0) + + pow(cos(q[0 * 3 + 1]), 2.0) * pow(sin(q[0 * 3 + 0]), 2.0) + + pow(sin(q[0 * 3 + 0]), 2.0) * pow(sin(q[0 * 3 + 1]), 2.0)) * + (pow(cos(q[0 * 3 + 0]), 2.0) * pow(cos(q[0 * 3 + 1]), 2.0) * + pow(sin(q[0 * 3 + 2]), 2.0) + + pow(cos(q[0 * 3 + 0]), 2.0) * pow(cos(q[0 * 3 + 2]), 2.0) * + pow(sin(q[0 * 3 + 1]), 2.0) + + pow(cos(q[0 * 3 + 1]), 2.0) * pow(cos(q[0 * 3 + 2]), 2.0) * + pow(sin(q[0 * 3 + 0]), 2.0) + + pow(cos(q[0 * 3 + 0]), 2.0) * pow(sin(q[0 * 3 + 1]), 2.0) * + pow(sin(q[0 * 3 + 2]), 2.0) + + pow(cos(q[0 * 3 + 1]), 2.0) * pow(sin(q[0 * 3 + 0]), 2.0) * + pow(sin(q[0 * 3 + 2]), 2.0) + + pow(cos(q[0 * 3 + 2]), 2.0) * pow(sin(q[0 * 3 + 0]), 2.0) * + pow(sin(q[0 * 3 + 1]), 2.0) + + pow(sin(q[0 * 3 + 0]), 2.0) * pow(sin(q[0 * 3 + 1]), 2.0) * + pow(sin(q[0 * 3 + 2]), 2.0) + + pow(cos(q[0 * 3 + 0]), 2.0) * pow(cos(q[0 * 3 + 1]), 2.0) * + pow(cos(q[0 * 3 + 2]), 2.0)) * + 2.0E+4))) / + (pow(cos(q[0 * 3 + 1]), 2.0) * pow(cos(q[0 * 3 + 2]), 2.0) + + pow(cos(q[0 * 3 + 1]), 2.0) * pow(sin(q[0 * 3 + 2]), 2.0) + + pow(cos(q[0 * 3 + 2]), 2.0) * pow(sin(q[0 * 3 + 1]), 2.0) + + pow(sin(q[0 * 3 + 1]), 2.0) * pow(sin(q[0 * 3 + 2]), 2.0)))) / + (pow(cos(q[0 * 3 + 0]), 2.0) + pow(sin(q[0 * 3 + 0]), 2.0)) - + (sin(q[0 * 3 + 0]) * sin(q[0 * 3 + 2]) * + (cos(q[0 * 3 + 1]) * sin(q[0 * 3 + 2]) + + cos(q[0 * 3 + 2]) * sin(q[0 * 3 + 1])) * + (1.03E+2 / 5.0E+2)) / + (pow(cos(q[0 * 3 + 0]), 2.0) * pow(cos(q[0 * 3 + 1]), 2.0) * + pow(sin(q[0 * 3 + 2]), 2.0) + + pow(cos(q[0 * 3 + 0]), 2.0) * pow(cos(q[0 * 3 + 2]), 2.0) * + pow(sin(q[0 * 3 + 1]), 2.0) + + pow(cos(q[0 * 3 + 1]), 2.0) * pow(cos(q[0 * 3 + 2]), 2.0) * + pow(sin(q[0 * 3 + 0]), 2.0) + + pow(cos(q[0 * 3 + 0]), 2.0) * pow(sin(q[0 * 3 + 1]), 2.0) * + pow(sin(q[0 * 3 + 2]), 2.0) + + pow(cos(q[0 * 3 + 1]), 2.0) * pow(sin(q[0 * 3 + 0]), 2.0) * + pow(sin(q[0 * 3 + 2]), 2.0) + + pow(cos(q[0 * 3 + 2]), 2.0) * pow(sin(q[0 * 3 + 0]), 2.0) * + pow(sin(q[0 * 3 + 1]), 2.0) + + pow(sin(q[0 * 3 + 0]), 2.0) * pow(sin(q[0 * 3 + 1]), 2.0) * + pow(sin(q[0 * 3 + 2]), 2.0) + + pow(cos(q[0 * 3 + 0]), 2.0) * pow(cos(q[0 * 3 + 1]), 2.0) * + pow(cos(q[0 * 3 + 2]), 2.0)) - + (cos(q[0 * 3 + 2]) * sin(q[0 * 3 + 0]) * + (cos(q[0 * 3 + 1]) * cos(q[0 * 3 + 2]) - + sin(q[0 * 3 + 1]) * sin(q[0 * 3 + 2])) * + (1.03E+2 / 5.0E+2)) / + (pow(cos(q[0 * 3 + 0]), 2.0) * pow(cos(q[0 * 3 + 1]), 2.0) * + pow(sin(q[0 * 3 + 2]), 2.0) + + pow(cos(q[0 * 3 + 0]), 2.0) * pow(cos(q[0 * 3 + 2]), 2.0) * + pow(sin(q[0 * 3 + 1]), 2.0) + + pow(cos(q[0 * 3 + 1]), 2.0) * pow(cos(q[0 * 3 + 2]), 2.0) * + pow(sin(q[0 * 3 + 0]), 2.0) + + pow(cos(q[0 * 3 + 0]), 2.0) * pow(sin(q[0 * 3 + 1]), 2.0) * + pow(sin(q[0 * 3 + 2]), 2.0) + + pow(cos(q[0 * 3 + 1]), 2.0) * pow(sin(q[0 * 3 + 0]), 2.0) * + pow(sin(q[0 * 3 + 2]), 2.0) + + pow(cos(q[0 * 3 + 2]), 2.0) * pow(sin(q[0 * 3 + 0]), 2.0) * + pow(sin(q[0 * 3 + 1]), 2.0) + + pow(sin(q[0 * 3 + 0]), 2.0) * pow(sin(q[0 * 3 + 1]), 2.0) * + pow(sin(q[0 * 3 + 2]), 2.0) + + pow(cos(q[0 * 3 + 0]), 2.0) * pow(cos(q[0 * 3 + 1]), 2.0) * + pow(cos(q[0 * 3 + 2]), 2.0)); + F(1, 2) = + (sin(q[0 * 3 + 0]) * + (cos(q[0 * 3 + 1]) * -2.06E+3 + pow(cos(q[0 * 3 + 1]), 2.0) * 2.263E+3 + + pow(sin(q[0 * 3 + 1]), 2.0) * 2.263E+3) * + (-1.0E-4)) / + (pow(cos(q[0 * 3 + 0]), 2.0) * pow(cos(q[0 * 3 + 1]), 2.0) + + pow(cos(q[0 * 3 + 0]), 2.0) * pow(sin(q[0 * 3 + 1]), 2.0) + + pow(cos(q[0 * 3 + 1]), 2.0) * pow(sin(q[0 * 3 + 0]), 2.0) + + pow(sin(q[0 * 3 + 0]), 2.0) * pow(sin(q[0 * 3 + 1]), 2.0)) - + (sin(q[0 * 3 + 0]) * + (((cos(q[0 * 3 + 1]) * cos(q[0 * 3 + 2]) - + sin(q[0 * 3 + 1]) * sin(q[0 * 3 + 2])) * + ((cos(q[0 * 3 + 0]) * + (cos(q[0 * 3 + 0]) * pow(cos(q[0 * 3 + 1]), 2.0) * + cos(q[0 * 3 + 2]) * 1.03E+4 + + cos(q[0 * 3 + 0]) * cos(q[0 * 3 + 2]) * + pow(sin(q[0 * 3 + 1]), 2.0) * 1.03E+4 - + cos(q[0 * 3 + 0]) * cos(q[0 * 3 + 1]) * cos(q[0 * 3 + 2]) * + 1.1315E+4 + + cos(q[0 * 3 + 0]) * sin(q[0 * 3 + 1]) * sin(q[0 * 3 + 2]) * + 1.1315E+4 - + RL * cos(q[0 * 3 + 1]) * sin(q[0 * 3 + 0]) * sin(q[0 * 3 + 2]) * + 5.049E+3 - + RL * cos(q[0 * 3 + 2]) * sin(q[0 * 3 + 0]) * sin(q[0 * 3 + 1]) * + 5.049E+3)) / + ((pow(cos(q[0 * 3 + 0]), 2.0) + pow(sin(q[0 * 3 + 0]), 2.0)) * + (pow(cos(q[0 * 3 + 0]), 2.0) * pow(cos(q[0 * 3 + 1]), 2.0) * + pow(sin(q[0 * 3 + 2]), 2.0) + + pow(cos(q[0 * 3 + 0]), 2.0) * pow(cos(q[0 * 3 + 2]), 2.0) * + pow(sin(q[0 * 3 + 1]), 2.0) + + pow(cos(q[0 * 3 + 1]), 2.0) * pow(cos(q[0 * 3 + 2]), 2.0) * + pow(sin(q[0 * 3 + 0]), 2.0) + + pow(cos(q[0 * 3 + 0]), 2.0) * pow(sin(q[0 * 3 + 1]), 2.0) * + pow(sin(q[0 * 3 + 2]), 2.0) + + pow(cos(q[0 * 3 + 1]), 2.0) * pow(sin(q[0 * 3 + 0]), 2.0) * + pow(sin(q[0 * 3 + 2]), 2.0) + + pow(cos(q[0 * 3 + 2]), 2.0) * pow(sin(q[0 * 3 + 0]), 2.0) * + pow(sin(q[0 * 3 + 1]), 2.0) + + pow(sin(q[0 * 3 + 0]), 2.0) * pow(sin(q[0 * 3 + 1]), 2.0) * + pow(sin(q[0 * 3 + 2]), 2.0) + + pow(cos(q[0 * 3 + 0]), 2.0) * pow(cos(q[0 * 3 + 1]), 2.0) * + pow(cos(q[0 * 3 + 2]), 2.0)) * + 1.0E+5) + + ((cos(q[0 * 3 + 1]) * sin(q[0 * 3 + 2]) + + cos(q[0 * 3 + 2]) * sin(q[0 * 3 + 1])) * + (pow(cos(q[0 * 3 + 0]), 2.0) * sin(q[0 * 3 + 1]) * 1.03E+2 + + pow(sin(q[0 * 3 + 0]), 2.0) * sin(q[0 * 3 + 1]) * 1.03E+2 + + RL * pow(cos(q[0 * 3 + 1]), 2.0) * sin(q[0 * 3 + 0]) * 3.5E+1 + + RL * sin(q[0 * 3 + 0]) * pow(sin(q[0 * 3 + 1]), 2.0) * 3.5E+1)) / + ((pow(cos(q[0 * 3 + 0]), 2.0) * pow(cos(q[0 * 3 + 1]), 2.0) + + pow(cos(q[0 * 3 + 0]), 2.0) * pow(sin(q[0 * 3 + 1]), 2.0) + + pow(cos(q[0 * 3 + 1]), 2.0) * pow(sin(q[0 * 3 + 0]), 2.0) + + pow(sin(q[0 * 3 + 0]), 2.0) * pow(sin(q[0 * 3 + 1]), 2.0)) * + (pow(cos(q[0 * 3 + 1]), 2.0) * pow(cos(q[0 * 3 + 2]), 2.0) + + pow(cos(q[0 * 3 + 1]), 2.0) * pow(sin(q[0 * 3 + 2]), 2.0) + + pow(cos(q[0 * 3 + 2]), 2.0) * pow(sin(q[0 * 3 + 1]), 2.0) + + pow(sin(q[0 * 3 + 1]), 2.0) * pow(sin(q[0 * 3 + 2]), 2.0)) * + 1.0E+3) + + (sin(q[0 * 3 + 0]) * + (sin(q[0 * 3 + 0]) * sin(q[0 * 3 + 1]) * sin(q[0 * 3 + 2]) * + 1.1315E+4 + + pow(cos(q[0 * 3 + 1]), 2.0) * cos(q[0 * 3 + 2]) * + sin(q[0 * 3 + 0]) * 1.03E+4 + + cos(q[0 * 3 + 2]) * sin(q[0 * 3 + 0]) * + pow(sin(q[0 * 3 + 1]), 2.0) * 1.03E+4 - + cos(q[0 * 3 + 1]) * cos(q[0 * 3 + 2]) * sin(q[0 * 3 + 0]) * + 1.1315E+4 + + RL * cos(q[0 * 3 + 0]) * cos(q[0 * 3 + 1]) * sin(q[0 * 3 + 2]) * + 5.049E+3 + + RL * cos(q[0 * 3 + 0]) * cos(q[0 * 3 + 2]) * sin(q[0 * 3 + 1]) * + 5.049E+3 + + RL * pow(cos(q[0 * 3 + 0]), 2.0) * cos(q[0 * 3 + 1]) * + sin(q[0 * 3 + 2]) * 3.5E+3 + + RL * pow(cos(q[0 * 3 + 0]), 2.0) * cos(q[0 * 3 + 2]) * + sin(q[0 * 3 + 1]) * 3.5E+3 + + RL * cos(q[0 * 3 + 1]) * pow(sin(q[0 * 3 + 0]), 2.0) * + sin(q[0 * 3 + 2]) * 3.5E+3 + + RL * cos(q[0 * 3 + 2]) * pow(sin(q[0 * 3 + 0]), 2.0) * + sin(q[0 * 3 + 1]) * 3.5E+3)) / + ((pow(cos(q[0 * 3 + 0]), 2.0) + pow(sin(q[0 * 3 + 0]), 2.0)) * + (pow(cos(q[0 * 3 + 0]), 2.0) * pow(cos(q[0 * 3 + 1]), 2.0) * + pow(sin(q[0 * 3 + 2]), 2.0) + + pow(cos(q[0 * 3 + 0]), 2.0) * pow(cos(q[0 * 3 + 2]), 2.0) * + pow(sin(q[0 * 3 + 1]), 2.0) + + pow(cos(q[0 * 3 + 1]), 2.0) * pow(cos(q[0 * 3 + 2]), 2.0) * + pow(sin(q[0 * 3 + 0]), 2.0) + + pow(cos(q[0 * 3 + 0]), 2.0) * pow(sin(q[0 * 3 + 1]), 2.0) * + pow(sin(q[0 * 3 + 2]), 2.0) + + pow(cos(q[0 * 3 + 1]), 2.0) * pow(sin(q[0 * 3 + 0]), 2.0) * + pow(sin(q[0 * 3 + 2]), 2.0) + + pow(cos(q[0 * 3 + 2]), 2.0) * pow(sin(q[0 * 3 + 0]), 2.0) * + pow(sin(q[0 * 3 + 1]), 2.0) + + pow(sin(q[0 * 3 + 0]), 2.0) * pow(sin(q[0 * 3 + 1]), 2.0) * + pow(sin(q[0 * 3 + 2]), 2.0) + + pow(cos(q[0 * 3 + 0]), 2.0) * pow(cos(q[0 * 3 + 1]), 2.0) * + pow(cos(q[0 * 3 + 2]), 2.0)) * + 1.0E+5) - + (pow(cos(q[0 * 3 + 0]), 2.0) * + (cos(q[0 * 3 + 1]) * cos(q[0 * 3 + 2]) - + sin(q[0 * 3 + 1]) * sin(q[0 * 3 + 2])) * + (cos(q[0 * 3 + 1]) * -2.06E+3 + + pow(cos(q[0 * 3 + 1]), 2.0) * 2.263E+3 + + pow(sin(q[0 * 3 + 1]), 2.0) * 2.263E+3)) / + ((pow(cos(q[0 * 3 + 0]), 2.0) * pow(cos(q[0 * 3 + 1]), 2.0) + + pow(cos(q[0 * 3 + 0]), 2.0) * pow(sin(q[0 * 3 + 1]), 2.0) + + pow(cos(q[0 * 3 + 1]), 2.0) * pow(sin(q[0 * 3 + 0]), 2.0) + + pow(sin(q[0 * 3 + 0]), 2.0) * pow(sin(q[0 * 3 + 1]), 2.0)) * + (pow(cos(q[0 * 3 + 0]), 2.0) * pow(cos(q[0 * 3 + 1]), 2.0) * + pow(sin(q[0 * 3 + 2]), 2.0) + + pow(cos(q[0 * 3 + 0]), 2.0) * pow(cos(q[0 * 3 + 2]), 2.0) * + pow(sin(q[0 * 3 + 1]), 2.0) + + pow(cos(q[0 * 3 + 1]), 2.0) * pow(cos(q[0 * 3 + 2]), 2.0) * + pow(sin(q[0 * 3 + 0]), 2.0) + + pow(cos(q[0 * 3 + 0]), 2.0) * pow(sin(q[0 * 3 + 1]), 2.0) * + pow(sin(q[0 * 3 + 2]), 2.0) + + pow(cos(q[0 * 3 + 1]), 2.0) * pow(sin(q[0 * 3 + 0]), 2.0) * + pow(sin(q[0 * 3 + 2]), 2.0) + + pow(cos(q[0 * 3 + 2]), 2.0) * pow(sin(q[0 * 3 + 0]), 2.0) * + pow(sin(q[0 * 3 + 1]), 2.0) + + pow(sin(q[0 * 3 + 0]), 2.0) * pow(sin(q[0 * 3 + 1]), 2.0) * + pow(sin(q[0 * 3 + 2]), 2.0) + + pow(cos(q[0 * 3 + 0]), 2.0) * pow(cos(q[0 * 3 + 1]), 2.0) * + pow(cos(q[0 * 3 + 2]), 2.0)) * + 2.0E+4) - + (pow(sin(q[0 * 3 + 0]), 2.0) * + (cos(q[0 * 3 + 1]) * cos(q[0 * 3 + 2]) - + sin(q[0 * 3 + 1]) * sin(q[0 * 3 + 2])) * + (cos(q[0 * 3 + 1]) * -2.06E+3 + + pow(cos(q[0 * 3 + 1]), 2.0) * 2.263E+3 + + pow(sin(q[0 * 3 + 1]), 2.0) * 2.263E+3)) / + ((pow(cos(q[0 * 3 + 0]), 2.0) * pow(cos(q[0 * 3 + 1]), 2.0) + + pow(cos(q[0 * 3 + 0]), 2.0) * pow(sin(q[0 * 3 + 1]), 2.0) + + pow(cos(q[0 * 3 + 1]), 2.0) * pow(sin(q[0 * 3 + 0]), 2.0) + + pow(sin(q[0 * 3 + 0]), 2.0) * pow(sin(q[0 * 3 + 1]), 2.0)) * + (pow(cos(q[0 * 3 + 0]), 2.0) * pow(cos(q[0 * 3 + 1]), 2.0) * + pow(sin(q[0 * 3 + 2]), 2.0) + + pow(cos(q[0 * 3 + 0]), 2.0) * pow(cos(q[0 * 3 + 2]), 2.0) * + pow(sin(q[0 * 3 + 1]), 2.0) + + pow(cos(q[0 * 3 + 1]), 2.0) * pow(cos(q[0 * 3 + 2]), 2.0) * + pow(sin(q[0 * 3 + 0]), 2.0) + + pow(cos(q[0 * 3 + 0]), 2.0) * pow(sin(q[0 * 3 + 1]), 2.0) * + pow(sin(q[0 * 3 + 2]), 2.0) + + pow(cos(q[0 * 3 + 1]), 2.0) * pow(sin(q[0 * 3 + 0]), 2.0) * + pow(sin(q[0 * 3 + 2]), 2.0) + + pow(cos(q[0 * 3 + 2]), 2.0) * pow(sin(q[0 * 3 + 0]), 2.0) * + pow(sin(q[0 * 3 + 1]), 2.0) + + pow(sin(q[0 * 3 + 0]), 2.0) * pow(sin(q[0 * 3 + 1]), 2.0) * + pow(sin(q[0 * 3 + 2]), 2.0) + + pow(cos(q[0 * 3 + 0]), 2.0) * pow(cos(q[0 * 3 + 1]), 2.0) * + pow(cos(q[0 * 3 + 2]), 2.0)) * + 2.0E+4) - + 1.03E+2 / 5.0E+2)) / + (pow(cos(q[0 * 3 + 1]), 2.0) * pow(cos(q[0 * 3 + 2]), 2.0) + + pow(cos(q[0 * 3 + 1]), 2.0) * pow(sin(q[0 * 3 + 2]), 2.0) + + pow(cos(q[0 * 3 + 2]), 2.0) * pow(sin(q[0 * 3 + 1]), 2.0) + + pow(sin(q[0 * 3 + 1]), 2.0) * pow(sin(q[0 * 3 + 2]), 2.0)) - + ((cos(q[0 * 3 + 1]) * sin(q[0 * 3 + 2]) + + cos(q[0 * 3 + 2]) * sin(q[0 * 3 + 1])) * + ((sin(q[0 * 3 + 0]) * + (pow(cos(q[0 * 3 + 1]), 2.0) * sin(q[0 * 3 + 0]) * + sin(q[0 * 3 + 2]) * 1.03E+4 + + sin(q[0 * 3 + 0]) * pow(sin(q[0 * 3 + 1]), 2.0) * + sin(q[0 * 3 + 2]) * 1.03E+4 - + cos(q[0 * 3 + 1]) * sin(q[0 * 3 + 0]) * sin(q[0 * 3 + 2]) * + 1.1315E+4 - + cos(q[0 * 3 + 2]) * sin(q[0 * 3 + 0]) * sin(q[0 * 3 + 1]) * + 1.1315E+4 - + RL * cos(q[0 * 3 + 0]) * cos(q[0 * 3 + 1]) * cos(q[0 * 3 + 2]) * + 5.049E+3 + + RL * cos(q[0 * 3 + 0]) * sin(q[0 * 3 + 1]) * sin(q[0 * 3 + 2]) * + 5.049E+3 - + RL * pow(cos(q[0 * 3 + 0]), 2.0) * cos(q[0 * 3 + 1]) * + cos(q[0 * 3 + 2]) * 3.5E+3 - + RL * cos(q[0 * 3 + 1]) * cos(q[0 * 3 + 2]) * + pow(sin(q[0 * 3 + 0]), 2.0) * 3.5E+3 + + RL * pow(cos(q[0 * 3 + 0]), 2.0) * sin(q[0 * 3 + 1]) * + sin(q[0 * 3 + 2]) * 3.5E+3 + + RL * pow(sin(q[0 * 3 + 0]), 2.0) * sin(q[0 * 3 + 1]) * + sin(q[0 * 3 + 2]) * 3.5E+3) * + (-1.0E-5)) / + ((pow(cos(q[0 * 3 + 0]), 2.0) + pow(sin(q[0 * 3 + 0]), 2.0)) * + (pow(cos(q[0 * 3 + 0]), 2.0) * pow(cos(q[0 * 3 + 1]), 2.0) * + pow(sin(q[0 * 3 + 2]), 2.0) + + pow(cos(q[0 * 3 + 0]), 2.0) * pow(cos(q[0 * 3 + 2]), 2.0) * + pow(sin(q[0 * 3 + 1]), 2.0) + + pow(cos(q[0 * 3 + 1]), 2.0) * pow(cos(q[0 * 3 + 2]), 2.0) * + pow(sin(q[0 * 3 + 0]), 2.0) + + pow(cos(q[0 * 3 + 0]), 2.0) * pow(sin(q[0 * 3 + 1]), 2.0) * + pow(sin(q[0 * 3 + 2]), 2.0) + + pow(cos(q[0 * 3 + 1]), 2.0) * pow(sin(q[0 * 3 + 0]), 2.0) * + pow(sin(q[0 * 3 + 2]), 2.0) + + pow(cos(q[0 * 3 + 2]), 2.0) * pow(sin(q[0 * 3 + 0]), 2.0) * + pow(sin(q[0 * 3 + 1]), 2.0) + + pow(sin(q[0 * 3 + 0]), 2.0) * pow(sin(q[0 * 3 + 1]), 2.0) * + pow(sin(q[0 * 3 + 2]), 2.0) + + pow(cos(q[0 * 3 + 0]), 2.0) * pow(cos(q[0 * 3 + 1]), 2.0) * + pow(cos(q[0 * 3 + 2]), 2.0))) + + ((cos(q[0 * 3 + 1]) * cos(q[0 * 3 + 2]) - + sin(q[0 * 3 + 1]) * sin(q[0 * 3 + 2])) * + (pow(cos(q[0 * 3 + 0]), 2.0) * sin(q[0 * 3 + 1]) * 1.03E+2 + + pow(sin(q[0 * 3 + 0]), 2.0) * sin(q[0 * 3 + 1]) * 1.03E+2 + + RL * pow(cos(q[0 * 3 + 1]), 2.0) * sin(q[0 * 3 + 0]) * 3.5E+1 + + RL * sin(q[0 * 3 + 0]) * pow(sin(q[0 * 3 + 1]), 2.0) * 3.5E+1)) / + ((pow(cos(q[0 * 3 + 0]), 2.0) * pow(cos(q[0 * 3 + 1]), 2.0) + + pow(cos(q[0 * 3 + 0]), 2.0) * pow(sin(q[0 * 3 + 1]), 2.0) + + pow(cos(q[0 * 3 + 1]), 2.0) * pow(sin(q[0 * 3 + 0]), 2.0) + + pow(sin(q[0 * 3 + 0]), 2.0) * pow(sin(q[0 * 3 + 1]), 2.0)) * + (pow(cos(q[0 * 3 + 1]), 2.0) * pow(cos(q[0 * 3 + 2]), 2.0) + + pow(cos(q[0 * 3 + 1]), 2.0) * pow(sin(q[0 * 3 + 2]), 2.0) + + pow(cos(q[0 * 3 + 2]), 2.0) * pow(sin(q[0 * 3 + 1]), 2.0) + + pow(sin(q[0 * 3 + 1]), 2.0) * pow(sin(q[0 * 3 + 2]), 2.0)) * + 1.0E+3) - + (cos(q[0 * 3 + 0]) * + (cos(q[0 * 3 + 0]) * pow(cos(q[0 * 3 + 1]), 2.0) * + sin(q[0 * 3 + 2]) * 1.03E+4 + + cos(q[0 * 3 + 0]) * pow(sin(q[0 * 3 + 1]), 2.0) * + sin(q[0 * 3 + 2]) * 1.03E+4 - + cos(q[0 * 3 + 0]) * cos(q[0 * 3 + 1]) * sin(q[0 * 3 + 2]) * + 1.1315E+4 - + cos(q[0 * 3 + 0]) * cos(q[0 * 3 + 2]) * sin(q[0 * 3 + 1]) * + 1.1315E+4 + + RL * cos(q[0 * 3 + 1]) * cos(q[0 * 3 + 2]) * sin(q[0 * 3 + 0]) * + 5.049E+3 - + RL * sin(q[0 * 3 + 0]) * sin(q[0 * 3 + 1]) * sin(q[0 * 3 + 2]) * + 5.049E+3)) / + ((pow(cos(q[0 * 3 + 0]), 2.0) + pow(sin(q[0 * 3 + 0]), 2.0)) * + (pow(cos(q[0 * 3 + 0]), 2.0) * pow(cos(q[0 * 3 + 1]), 2.0) * + pow(sin(q[0 * 3 + 2]), 2.0) + + pow(cos(q[0 * 3 + 0]), 2.0) * pow(cos(q[0 * 3 + 2]), 2.0) * + pow(sin(q[0 * 3 + 1]), 2.0) + + pow(cos(q[0 * 3 + 1]), 2.0) * pow(cos(q[0 * 3 + 2]), 2.0) * + pow(sin(q[0 * 3 + 0]), 2.0) + + pow(cos(q[0 * 3 + 0]), 2.0) * pow(sin(q[0 * 3 + 1]), 2.0) * + pow(sin(q[0 * 3 + 2]), 2.0) + + pow(cos(q[0 * 3 + 1]), 2.0) * pow(sin(q[0 * 3 + 0]), 2.0) * + pow(sin(q[0 * 3 + 2]), 2.0) + + pow(cos(q[0 * 3 + 2]), 2.0) * pow(sin(q[0 * 3 + 0]), 2.0) * + pow(sin(q[0 * 3 + 1]), 2.0) + + pow(sin(q[0 * 3 + 0]), 2.0) * pow(sin(q[0 * 3 + 1]), 2.0) * + pow(sin(q[0 * 3 + 2]), 2.0) + + pow(cos(q[0 * 3 + 0]), 2.0) * pow(cos(q[0 * 3 + 1]), 2.0) * + pow(cos(q[0 * 3 + 2]), 2.0)) * + 1.0E+5) + + (pow(cos(q[0 * 3 + 0]), 2.0) * + (cos(q[0 * 3 + 1]) * sin(q[0 * 3 + 2]) + + cos(q[0 * 3 + 2]) * sin(q[0 * 3 + 1])) * + (cos(q[0 * 3 + 1]) * -2.06E+3 + + pow(cos(q[0 * 3 + 1]), 2.0) * 2.263E+3 + + pow(sin(q[0 * 3 + 1]), 2.0) * 2.263E+3)) / + ((pow(cos(q[0 * 3 + 0]), 2.0) * pow(cos(q[0 * 3 + 1]), 2.0) + + pow(cos(q[0 * 3 + 0]), 2.0) * pow(sin(q[0 * 3 + 1]), 2.0) + + pow(cos(q[0 * 3 + 1]), 2.0) * pow(sin(q[0 * 3 + 0]), 2.0) + + pow(sin(q[0 * 3 + 0]), 2.0) * pow(sin(q[0 * 3 + 1]), 2.0)) * + (pow(cos(q[0 * 3 + 0]), 2.0) * pow(cos(q[0 * 3 + 1]), 2.0) * + pow(sin(q[0 * 3 + 2]), 2.0) + + pow(cos(q[0 * 3 + 0]), 2.0) * pow(cos(q[0 * 3 + 2]), 2.0) * + pow(sin(q[0 * 3 + 1]), 2.0) + + pow(cos(q[0 * 3 + 1]), 2.0) * pow(cos(q[0 * 3 + 2]), 2.0) * + pow(sin(q[0 * 3 + 0]), 2.0) + + pow(cos(q[0 * 3 + 0]), 2.0) * pow(sin(q[0 * 3 + 1]), 2.0) * + pow(sin(q[0 * 3 + 2]), 2.0) + + pow(cos(q[0 * 3 + 1]), 2.0) * pow(sin(q[0 * 3 + 0]), 2.0) * + pow(sin(q[0 * 3 + 2]), 2.0) + + pow(cos(q[0 * 3 + 2]), 2.0) * pow(sin(q[0 * 3 + 0]), 2.0) * + pow(sin(q[0 * 3 + 1]), 2.0) + + pow(sin(q[0 * 3 + 0]), 2.0) * pow(sin(q[0 * 3 + 1]), 2.0) * + pow(sin(q[0 * 3 + 2]), 2.0) + + pow(cos(q[0 * 3 + 0]), 2.0) * pow(cos(q[0 * 3 + 1]), 2.0) * + pow(cos(q[0 * 3 + 2]), 2.0)) * + 2.0E+4) + + (pow(sin(q[0 * 3 + 0]), 2.0) * + (cos(q[0 * 3 + 1]) * sin(q[0 * 3 + 2]) + + cos(q[0 * 3 + 2]) * sin(q[0 * 3 + 1])) * + (cos(q[0 * 3 + 1]) * -2.06E+3 + + pow(cos(q[0 * 3 + 1]), 2.0) * 2.263E+3 + + pow(sin(q[0 * 3 + 1]), 2.0) * 2.263E+3)) / + ((pow(cos(q[0 * 3 + 0]), 2.0) * pow(cos(q[0 * 3 + 1]), 2.0) + + pow(cos(q[0 * 3 + 0]), 2.0) * pow(sin(q[0 * 3 + 1]), 2.0) + + pow(cos(q[0 * 3 + 1]), 2.0) * pow(sin(q[0 * 3 + 0]), 2.0) + + pow(sin(q[0 * 3 + 0]), 2.0) * pow(sin(q[0 * 3 + 1]), 2.0)) * + (pow(cos(q[0 * 3 + 0]), 2.0) * pow(cos(q[0 * 3 + 1]), 2.0) * + pow(sin(q[0 * 3 + 2]), 2.0) + + pow(cos(q[0 * 3 + 0]), 2.0) * pow(cos(q[0 * 3 + 2]), 2.0) * + pow(sin(q[0 * 3 + 1]), 2.0) + + pow(cos(q[0 * 3 + 1]), 2.0) * pow(cos(q[0 * 3 + 2]), 2.0) * + pow(sin(q[0 * 3 + 0]), 2.0) + + pow(cos(q[0 * 3 + 0]), 2.0) * pow(sin(q[0 * 3 + 1]), 2.0) * + pow(sin(q[0 * 3 + 2]), 2.0) + + pow(cos(q[0 * 3 + 1]), 2.0) * pow(sin(q[0 * 3 + 0]), 2.0) * + pow(sin(q[0 * 3 + 2]), 2.0) + + pow(cos(q[0 * 3 + 2]), 2.0) * pow(sin(q[0 * 3 + 0]), 2.0) * + pow(sin(q[0 * 3 + 1]), 2.0) + + pow(sin(q[0 * 3 + 0]), 2.0) * pow(sin(q[0 * 3 + 1]), 2.0) * + pow(sin(q[0 * 3 + 2]), 2.0) + + pow(cos(q[0 * 3 + 0]), 2.0) * pow(cos(q[0 * 3 + 1]), 2.0) * + pow(cos(q[0 * 3 + 2]), 2.0)) * + 2.0E+4))) / + (pow(cos(q[0 * 3 + 1]), 2.0) * pow(cos(q[0 * 3 + 2]), 2.0) + + pow(cos(q[0 * 3 + 1]), 2.0) * pow(sin(q[0 * 3 + 2]), 2.0) + + pow(cos(q[0 * 3 + 2]), 2.0) * pow(sin(q[0 * 3 + 1]), 2.0) + + pow(sin(q[0 * 3 + 1]), 2.0) * pow(sin(q[0 * 3 + 2]), 2.0)))) / + (pow(cos(q[0 * 3 + 0]), 2.0) + pow(sin(q[0 * 3 + 0]), 2.0)); + F(2, 0) = + -(pow(cos(q[0 * 3 + 1]) * sin(q[0 * 3 + 2]) + + cos(q[0 * 3 + 2]) * sin(q[0 * 3 + 1]), + 2.0) / + (pow(cos(q[0 * 3 + 1]), 2.0) * pow(cos(q[0 * 3 + 2]), 2.0) + + pow(cos(q[0 * 3 + 1]), 2.0) * pow(sin(q[0 * 3 + 2]), 2.0) + + pow(cos(q[0 * 3 + 2]), 2.0) * pow(sin(q[0 * 3 + 1]), 2.0) + + pow(sin(q[0 * 3 + 1]), 2.0) * pow(sin(q[0 * 3 + 2]), 2.0)) + + pow(cos(q[0 * 3 + 1]) * cos(q[0 * 3 + 2]) - + sin(q[0 * 3 + 1]) * sin(q[0 * 3 + 2]), + 2.0) / + (pow(cos(q[0 * 3 + 1]), 2.0) * pow(cos(q[0 * 3 + 2]), 2.0) + + pow(cos(q[0 * 3 + 1]), 2.0) * pow(sin(q[0 * 3 + 2]), 2.0) + + pow(cos(q[0 * 3 + 2]), 2.0) * pow(sin(q[0 * 3 + 1]), 2.0) + + pow(sin(q[0 * 3 + 1]), 2.0) * pow(sin(q[0 * 3 + 2]), 2.0))) * + ((cos(q[0 * 3 + 0]) * + ((cos(q[0 * 3 + 0]) * + (cos(q[0 * 3 + 1]) * sin(q[0 * 3 + 2]) + + cos(q[0 * 3 + 2]) * sin(q[0 * 3 + 1])) * + 1.0 / + pow(pow(cos(q[0 * 3 + 0]), 2.0) * pow(cos(q[0 * 3 + 1]), 2.0) * + pow(sin(q[0 * 3 + 2]), 2.0) + + pow(cos(q[0 * 3 + 0]), 2.0) * + pow(cos(q[0 * 3 + 2]), 2.0) * + pow(sin(q[0 * 3 + 1]), 2.0) + + pow(cos(q[0 * 3 + 1]), 2.0) * + pow(cos(q[0 * 3 + 2]), 2.0) * + pow(sin(q[0 * 3 + 0]), 2.0) + + pow(cos(q[0 * 3 + 0]), 2.0) * + pow(sin(q[0 * 3 + 1]), 2.0) * + pow(sin(q[0 * 3 + 2]), 2.0) + + pow(cos(q[0 * 3 + 1]), 2.0) * + pow(sin(q[0 * 3 + 0]), 2.0) * + pow(sin(q[0 * 3 + 2]), 2.0) + + pow(cos(q[0 * 3 + 2]), 2.0) * + pow(sin(q[0 * 3 + 0]), 2.0) * + pow(sin(q[0 * 3 + 1]), 2.0) + + pow(sin(q[0 * 3 + 0]), 2.0) * + pow(sin(q[0 * 3 + 1]), 2.0) * + pow(sin(q[0 * 3 + 2]), 2.0) + + pow(cos(q[0 * 3 + 0]), 2.0) * + pow(cos(q[0 * 3 + 1]), 2.0) * + pow(cos(q[0 * 3 + 2]), 2.0), + 2.0) * + (sin(q[0 * 3 + 0]) * sin(q[0 * 3 + 1]) * sin(q[0 * 3 + 2]) * + 1.1315E+4 + + pow(cos(q[0 * 3 + 1]), 2.0) * cos(q[0 * 3 + 2]) * + sin(q[0 * 3 + 0]) * 1.03E+4 + + cos(q[0 * 3 + 2]) * sin(q[0 * 3 + 0]) * + pow(sin(q[0 * 3 + 1]), 2.0) * 1.03E+4 - + cos(q[0 * 3 + 1]) * cos(q[0 * 3 + 2]) * sin(q[0 * 3 + 0]) * + 1.1315E+4 + + RL * cos(q[0 * 3 + 0]) * cos(q[0 * 3 + 1]) * sin(q[0 * 3 + 2]) * + 5.049E+3 + + RL * cos(q[0 * 3 + 0]) * cos(q[0 * 3 + 2]) * sin(q[0 * 3 + 1]) * + 5.049E+3 + + RL * pow(cos(q[0 * 3 + 0]), 2.0) * cos(q[0 * 3 + 1]) * + sin(q[0 * 3 + 2]) * 3.5E+3 + + RL * pow(cos(q[0 * 3 + 0]), 2.0) * cos(q[0 * 3 + 2]) * + sin(q[0 * 3 + 1]) * 3.5E+3 + + RL * cos(q[0 * 3 + 1]) * pow(sin(q[0 * 3 + 0]), 2.0) * + sin(q[0 * 3 + 2]) * 3.5E+3 + + RL * cos(q[0 * 3 + 2]) * pow(sin(q[0 * 3 + 0]), 2.0) * + sin(q[0 * 3 + 1]) * 3.5E+3)) / + 1.0E+5 - + (cos(q[0 * 3 + 0]) * + (cos(q[0 * 3 + 1]) * cos(q[0 * 3 + 2]) - + sin(q[0 * 3 + 1]) * sin(q[0 * 3 + 2])) * + 1.0 / + pow(pow(cos(q[0 * 3 + 0]), 2.0) * pow(cos(q[0 * 3 + 1]), 2.0) * + pow(sin(q[0 * 3 + 2]), 2.0) + + pow(cos(q[0 * 3 + 0]), 2.0) * + pow(cos(q[0 * 3 + 2]), 2.0) * + pow(sin(q[0 * 3 + 1]), 2.0) + + pow(cos(q[0 * 3 + 1]), 2.0) * + pow(cos(q[0 * 3 + 2]), 2.0) * + pow(sin(q[0 * 3 + 0]), 2.0) + + pow(cos(q[0 * 3 + 0]), 2.0) * + pow(sin(q[0 * 3 + 1]), 2.0) * + pow(sin(q[0 * 3 + 2]), 2.0) + + pow(cos(q[0 * 3 + 1]), 2.0) * + pow(sin(q[0 * 3 + 0]), 2.0) * + pow(sin(q[0 * 3 + 2]), 2.0) + + pow(cos(q[0 * 3 + 2]), 2.0) * + pow(sin(q[0 * 3 + 0]), 2.0) * + pow(sin(q[0 * 3 + 1]), 2.0) + + pow(sin(q[0 * 3 + 0]), 2.0) * + pow(sin(q[0 * 3 + 1]), 2.0) * + pow(sin(q[0 * 3 + 2]), 2.0) + + pow(cos(q[0 * 3 + 0]), 2.0) * + pow(cos(q[0 * 3 + 1]), 2.0) * + pow(cos(q[0 * 3 + 2]), 2.0), + 2.0) * + (pow(cos(q[0 * 3 + 1]), 2.0) * sin(q[0 * 3 + 0]) * + sin(q[0 * 3 + 2]) * 1.03E+4 + + sin(q[0 * 3 + 0]) * pow(sin(q[0 * 3 + 1]), 2.0) * + sin(q[0 * 3 + 2]) * 1.03E+4 - + cos(q[0 * 3 + 1]) * sin(q[0 * 3 + 0]) * sin(q[0 * 3 + 2]) * + 1.1315E+4 - + cos(q[0 * 3 + 2]) * sin(q[0 * 3 + 0]) * sin(q[0 * 3 + 1]) * + 1.1315E+4 - + RL * cos(q[0 * 3 + 0]) * cos(q[0 * 3 + 1]) * cos(q[0 * 3 + 2]) * + 5.049E+3 + + RL * cos(q[0 * 3 + 0]) * sin(q[0 * 3 + 1]) * sin(q[0 * 3 + 2]) * + 5.049E+3 - + RL * pow(cos(q[0 * 3 + 0]), 2.0) * cos(q[0 * 3 + 1]) * + cos(q[0 * 3 + 2]) * 3.5E+3 - + RL * cos(q[0 * 3 + 1]) * cos(q[0 * 3 + 2]) * + pow(sin(q[0 * 3 + 0]), 2.0) * 3.5E+3 + + RL * pow(cos(q[0 * 3 + 0]), 2.0) * sin(q[0 * 3 + 1]) * + sin(q[0 * 3 + 2]) * 3.5E+3 + + RL * pow(sin(q[0 * 3 + 0]), 2.0) * sin(q[0 * 3 + 1]) * + sin(q[0 * 3 + 2]) * 3.5E+3)) / + 1.0E+5 - + (sin(q[0 * 3 + 0]) * + (cos(q[0 * 3 + 1]) * sin(q[0 * 3 + 2]) + + cos(q[0 * 3 + 2]) * sin(q[0 * 3 + 1])) * + (cos(q[0 * 3 + 0]) * pow(cos(q[0 * 3 + 1]), 2.0) * + cos(q[0 * 3 + 2]) * 1.03E+4 + + cos(q[0 * 3 + 0]) * cos(q[0 * 3 + 2]) * + pow(sin(q[0 * 3 + 1]), 2.0) * 1.03E+4 - + cos(q[0 * 3 + 0]) * cos(q[0 * 3 + 1]) * cos(q[0 * 3 + 2]) * + 1.1315E+4 + + cos(q[0 * 3 + 0]) * sin(q[0 * 3 + 1]) * sin(q[0 * 3 + 2]) * + 1.1315E+4 - + RL * cos(q[0 * 3 + 1]) * sin(q[0 * 3 + 0]) * sin(q[0 * 3 + 2]) * + 5.049E+3 - + RL * cos(q[0 * 3 + 2]) * sin(q[0 * 3 + 0]) * sin(q[0 * 3 + 1]) * + 5.049E+3) * + 1.0 / + pow(pow(cos(q[0 * 3 + 0]), 2.0) * pow(cos(q[0 * 3 + 1]), 2.0) * + pow(sin(q[0 * 3 + 2]), 2.0) + + pow(cos(q[0 * 3 + 0]), 2.0) * + pow(cos(q[0 * 3 + 2]), 2.0) * + pow(sin(q[0 * 3 + 1]), 2.0) + + pow(cos(q[0 * 3 + 1]), 2.0) * + pow(cos(q[0 * 3 + 2]), 2.0) * + pow(sin(q[0 * 3 + 0]), 2.0) + + pow(cos(q[0 * 3 + 0]), 2.0) * + pow(sin(q[0 * 3 + 1]), 2.0) * + pow(sin(q[0 * 3 + 2]), 2.0) + + pow(cos(q[0 * 3 + 1]), 2.0) * + pow(sin(q[0 * 3 + 0]), 2.0) * + pow(sin(q[0 * 3 + 2]), 2.0) + + pow(cos(q[0 * 3 + 2]), 2.0) * + pow(sin(q[0 * 3 + 0]), 2.0) * + pow(sin(q[0 * 3 + 1]), 2.0) + + pow(sin(q[0 * 3 + 0]), 2.0) * + pow(sin(q[0 * 3 + 1]), 2.0) * + pow(sin(q[0 * 3 + 2]), 2.0) + + pow(cos(q[0 * 3 + 0]), 2.0) * + pow(cos(q[0 * 3 + 1]), 2.0) * + pow(cos(q[0 * 3 + 2]), 2.0), + 2.0)) / + 1.0E+5 + + (sin(q[0 * 3 + 0]) * + (cos(q[0 * 3 + 1]) * cos(q[0 * 3 + 2]) - + sin(q[0 * 3 + 1]) * sin(q[0 * 3 + 2])) * + (cos(q[0 * 3 + 0]) * pow(cos(q[0 * 3 + 1]), 2.0) * + sin(q[0 * 3 + 2]) * 1.03E+4 + + cos(q[0 * 3 + 0]) * pow(sin(q[0 * 3 + 1]), 2.0) * + sin(q[0 * 3 + 2]) * 1.03E+4 - + cos(q[0 * 3 + 0]) * cos(q[0 * 3 + 1]) * sin(q[0 * 3 + 2]) * + 1.1315E+4 - + cos(q[0 * 3 + 0]) * cos(q[0 * 3 + 2]) * sin(q[0 * 3 + 1]) * + 1.1315E+4 + + RL * cos(q[0 * 3 + 1]) * cos(q[0 * 3 + 2]) * sin(q[0 * 3 + 0]) * + 5.049E+3 - + RL * sin(q[0 * 3 + 0]) * sin(q[0 * 3 + 1]) * sin(q[0 * 3 + 2]) * + 5.049E+3) * + 1.0 / + pow(pow(cos(q[0 * 3 + 0]), 2.0) * pow(cos(q[0 * 3 + 1]), 2.0) * + pow(sin(q[0 * 3 + 2]), 2.0) + + pow(cos(q[0 * 3 + 0]), 2.0) * + pow(cos(q[0 * 3 + 2]), 2.0) * + pow(sin(q[0 * 3 + 1]), 2.0) + + pow(cos(q[0 * 3 + 1]), 2.0) * + pow(cos(q[0 * 3 + 2]), 2.0) * + pow(sin(q[0 * 3 + 0]), 2.0) + + pow(cos(q[0 * 3 + 0]), 2.0) * + pow(sin(q[0 * 3 + 1]), 2.0) * + pow(sin(q[0 * 3 + 2]), 2.0) + + pow(cos(q[0 * 3 + 1]), 2.0) * + pow(sin(q[0 * 3 + 0]), 2.0) * + pow(sin(q[0 * 3 + 2]), 2.0) + + pow(cos(q[0 * 3 + 2]), 2.0) * + pow(sin(q[0 * 3 + 0]), 2.0) * + pow(sin(q[0 * 3 + 1]), 2.0) + + pow(sin(q[0 * 3 + 0]), 2.0) * + pow(sin(q[0 * 3 + 1]), 2.0) * + pow(sin(q[0 * 3 + 2]), 2.0) + + pow(cos(q[0 * 3 + 0]), 2.0) * + pow(cos(q[0 * 3 + 1]), 2.0) * + pow(cos(q[0 * 3 + 2]), 2.0), + 2.0)) / + 1.0E+5 + + ((cos(q[0 * 3 + 1]) * sin(q[0 * 3 + 2]) + + cos(q[0 * 3 + 2]) * sin(q[0 * 3 + 1])) * + (RL * cos(q[0 * 3 + 0]) * cos(q[0 * 3 + 1]) * sin(q[0 * 3 + 2]) * + 3.5E+3 + + RL * cos(q[0 * 3 + 0]) * cos(q[0 * 3 + 2]) * sin(q[0 * 3 + 1]) * + 3.5E+3 + + RL * pow(cos(q[0 * 3 + 0]), 2.0) * cos(q[0 * 3 + 1]) * + sin(q[0 * 3 + 2]) * 5.049E+3 + + RL * pow(cos(q[0 * 3 + 0]), 2.0) * cos(q[0 * 3 + 2]) * + sin(q[0 * 3 + 1]) * 5.049E+3 + + RL * cos(q[0 * 3 + 1]) * pow(sin(q[0 * 3 + 0]), 2.0) * + sin(q[0 * 3 + 2]) * 5.049E+3 + + RL * cos(q[0 * 3 + 2]) * pow(sin(q[0 * 3 + 0]), 2.0) * + sin(q[0 * 3 + 1]) * 5.049E+3)) / + ((pow(cos(q[0 * 3 + 1]), 2.0) * pow(cos(q[0 * 3 + 2]), 2.0) + + pow(cos(q[0 * 3 + 1]), 2.0) * pow(sin(q[0 * 3 + 2]), 2.0) + + pow(cos(q[0 * 3 + 2]), 2.0) * pow(sin(q[0 * 3 + 1]), 2.0) + + pow(sin(q[0 * 3 + 1]), 2.0) * pow(sin(q[0 * 3 + 2]), 2.0)) * + (pow(cos(q[0 * 3 + 0]), 2.0) * pow(cos(q[0 * 3 + 1]), 2.0) * + pow(sin(q[0 * 3 + 2]), 2.0) + + pow(cos(q[0 * 3 + 0]), 2.0) * pow(cos(q[0 * 3 + 2]), 2.0) * + pow(sin(q[0 * 3 + 1]), 2.0) + + pow(cos(q[0 * 3 + 1]), 2.0) * pow(cos(q[0 * 3 + 2]), 2.0) * + pow(sin(q[0 * 3 + 0]), 2.0) + + pow(cos(q[0 * 3 + 0]), 2.0) * pow(sin(q[0 * 3 + 1]), 2.0) * + pow(sin(q[0 * 3 + 2]), 2.0) + + pow(cos(q[0 * 3 + 1]), 2.0) * pow(sin(q[0 * 3 + 0]), 2.0) * + pow(sin(q[0 * 3 + 2]), 2.0) + + pow(cos(q[0 * 3 + 2]), 2.0) * pow(sin(q[0 * 3 + 0]), 2.0) * + pow(sin(q[0 * 3 + 1]), 2.0) + + pow(sin(q[0 * 3 + 0]), 2.0) * pow(sin(q[0 * 3 + 1]), 2.0) * + pow(sin(q[0 * 3 + 2]), 2.0) + + pow(cos(q[0 * 3 + 0]), 2.0) * pow(cos(q[0 * 3 + 1]), 2.0) * + pow(cos(q[0 * 3 + 2]), 2.0)) * + 1.0E+5) + + ((cos(q[0 * 3 + 1]) * cos(q[0 * 3 + 2]) - + sin(q[0 * 3 + 1]) * sin(q[0 * 3 + 2])) * + (RL * cos(q[0 * 3 + 0]) * cos(q[0 * 3 + 1]) * cos(q[0 * 3 + 2]) * + 3.5E+3 - + RL * cos(q[0 * 3 + 0]) * sin(q[0 * 3 + 1]) * sin(q[0 * 3 + 2]) * + 3.5E+3 + + RL * pow(cos(q[0 * 3 + 0]), 2.0) * cos(q[0 * 3 + 1]) * + cos(q[0 * 3 + 2]) * 5.049E+3 + + RL * cos(q[0 * 3 + 1]) * cos(q[0 * 3 + 2]) * + pow(sin(q[0 * 3 + 0]), 2.0) * 5.049E+3 - + RL * pow(cos(q[0 * 3 + 0]), 2.0) * sin(q[0 * 3 + 1]) * + sin(q[0 * 3 + 2]) * 5.049E+3 - + RL * pow(sin(q[0 * 3 + 0]), 2.0) * sin(q[0 * 3 + 1]) * + sin(q[0 * 3 + 2]) * 5.049E+3)) / + ((pow(cos(q[0 * 3 + 1]), 2.0) * pow(cos(q[0 * 3 + 2]), 2.0) + + pow(cos(q[0 * 3 + 1]), 2.0) * pow(sin(q[0 * 3 + 2]), 2.0) + + pow(cos(q[0 * 3 + 2]), 2.0) * pow(sin(q[0 * 3 + 1]), 2.0) + + pow(sin(q[0 * 3 + 1]), 2.0) * pow(sin(q[0 * 3 + 2]), 2.0)) * + (pow(cos(q[0 * 3 + 0]), 2.0) * pow(cos(q[0 * 3 + 1]), 2.0) * + pow(sin(q[0 * 3 + 2]), 2.0) + + pow(cos(q[0 * 3 + 0]), 2.0) * pow(cos(q[0 * 3 + 2]), 2.0) * + pow(sin(q[0 * 3 + 1]), 2.0) + + pow(cos(q[0 * 3 + 1]), 2.0) * pow(cos(q[0 * 3 + 2]), 2.0) * + pow(sin(q[0 * 3 + 0]), 2.0) + + pow(cos(q[0 * 3 + 0]), 2.0) * pow(sin(q[0 * 3 + 1]), 2.0) * + pow(sin(q[0 * 3 + 2]), 2.0) + + pow(cos(q[0 * 3 + 1]), 2.0) * pow(sin(q[0 * 3 + 0]), 2.0) * + pow(sin(q[0 * 3 + 2]), 2.0) + + pow(cos(q[0 * 3 + 2]), 2.0) * pow(sin(q[0 * 3 + 0]), 2.0) * + pow(sin(q[0 * 3 + 1]), 2.0) + + pow(sin(q[0 * 3 + 0]), 2.0) * pow(sin(q[0 * 3 + 1]), 2.0) * + pow(sin(q[0 * 3 + 2]), 2.0) + + pow(cos(q[0 * 3 + 0]), 2.0) * pow(cos(q[0 * 3 + 1]), 2.0) * + pow(cos(q[0 * 3 + 2]), 2.0)) * + 1.0E+5))) / + (pow(cos(q[0 * 3 + 0]), 2.0) + pow(sin(q[0 * 3 + 0]), 2.0)) + + (sin(q[0 * 3 + 0]) * + (cos(q[0 * 3 + 1]) * sin(q[0 * 3 + 2]) + + cos(q[0 * 3 + 2]) * sin(q[0 * 3 + 1])) * + ((cos(q[0 * 3 + 0]) * + (cos(q[0 * 3 + 0]) * pow(cos(q[0 * 3 + 1]), 2.0) * + cos(q[0 * 3 + 2]) * 1.03E+4 + + cos(q[0 * 3 + 0]) * cos(q[0 * 3 + 2]) * + pow(sin(q[0 * 3 + 1]), 2.0) * 1.03E+4 - + cos(q[0 * 3 + 0]) * cos(q[0 * 3 + 1]) * cos(q[0 * 3 + 2]) * + 1.1315E+4 + + cos(q[0 * 3 + 0]) * sin(q[0 * 3 + 1]) * sin(q[0 * 3 + 2]) * + 1.1315E+4 - + RL * cos(q[0 * 3 + 1]) * sin(q[0 * 3 + 0]) * sin(q[0 * 3 + 2]) * + 5.049E+3 - + RL * cos(q[0 * 3 + 2]) * sin(q[0 * 3 + 0]) * sin(q[0 * 3 + 1]) * + 5.049E+3)) / + ((pow(cos(q[0 * 3 + 0]), 2.0) + pow(sin(q[0 * 3 + 0]), 2.0)) * + (pow(cos(q[0 * 3 + 0]), 2.0) * pow(cos(q[0 * 3 + 1]), 2.0) * + pow(sin(q[0 * 3 + 2]), 2.0) + + pow(cos(q[0 * 3 + 0]), 2.0) * pow(cos(q[0 * 3 + 2]), 2.0) * + pow(sin(q[0 * 3 + 1]), 2.0) + + pow(cos(q[0 * 3 + 1]), 2.0) * pow(cos(q[0 * 3 + 2]), 2.0) * + pow(sin(q[0 * 3 + 0]), 2.0) + + pow(cos(q[0 * 3 + 0]), 2.0) * pow(sin(q[0 * 3 + 1]), 2.0) * + pow(sin(q[0 * 3 + 2]), 2.0) + + pow(cos(q[0 * 3 + 1]), 2.0) * pow(sin(q[0 * 3 + 0]), 2.0) * + pow(sin(q[0 * 3 + 2]), 2.0) + + pow(cos(q[0 * 3 + 2]), 2.0) * pow(sin(q[0 * 3 + 0]), 2.0) * + pow(sin(q[0 * 3 + 1]), 2.0) + + pow(sin(q[0 * 3 + 0]), 2.0) * pow(sin(q[0 * 3 + 1]), 2.0) * + pow(sin(q[0 * 3 + 2]), 2.0) + + pow(cos(q[0 * 3 + 0]), 2.0) * pow(cos(q[0 * 3 + 1]), 2.0) * + pow(cos(q[0 * 3 + 2]), 2.0)) * + 1.0E+5) + + ((cos(q[0 * 3 + 1]) * sin(q[0 * 3 + 2]) + + cos(q[0 * 3 + 2]) * sin(q[0 * 3 + 1])) * + (pow(cos(q[0 * 3 + 0]), 2.0) * sin(q[0 * 3 + 1]) * 1.03E+2 + + pow(sin(q[0 * 3 + 0]), 2.0) * sin(q[0 * 3 + 1]) * 1.03E+2 + + RL * pow(cos(q[0 * 3 + 1]), 2.0) * sin(q[0 * 3 + 0]) * 3.5E+1 + + RL * sin(q[0 * 3 + 0]) * pow(sin(q[0 * 3 + 1]), 2.0) * 3.5E+1)) / + ((pow(cos(q[0 * 3 + 0]), 2.0) * pow(cos(q[0 * 3 + 1]), 2.0) + + pow(cos(q[0 * 3 + 0]), 2.0) * pow(sin(q[0 * 3 + 1]), 2.0) + + pow(cos(q[0 * 3 + 1]), 2.0) * pow(sin(q[0 * 3 + 0]), 2.0) + + pow(sin(q[0 * 3 + 0]), 2.0) * pow(sin(q[0 * 3 + 1]), 2.0)) * + (pow(cos(q[0 * 3 + 1]), 2.0) * pow(cos(q[0 * 3 + 2]), 2.0) + + pow(cos(q[0 * 3 + 1]), 2.0) * pow(sin(q[0 * 3 + 2]), 2.0) + + pow(cos(q[0 * 3 + 2]), 2.0) * pow(sin(q[0 * 3 + 1]), 2.0) + + pow(sin(q[0 * 3 + 1]), 2.0) * pow(sin(q[0 * 3 + 2]), 2.0)) * + 1.0E+3) + + (sin(q[0 * 3 + 0]) * + (sin(q[0 * 3 + 0]) * sin(q[0 * 3 + 1]) * sin(q[0 * 3 + 2]) * + 1.1315E+4 + + pow(cos(q[0 * 3 + 1]), 2.0) * cos(q[0 * 3 + 2]) * + sin(q[0 * 3 + 0]) * 1.03E+4 + + cos(q[0 * 3 + 2]) * sin(q[0 * 3 + 0]) * + pow(sin(q[0 * 3 + 1]), 2.0) * 1.03E+4 - + cos(q[0 * 3 + 1]) * cos(q[0 * 3 + 2]) * sin(q[0 * 3 + 0]) * + 1.1315E+4 + + RL * cos(q[0 * 3 + 0]) * cos(q[0 * 3 + 1]) * sin(q[0 * 3 + 2]) * + 5.049E+3 + + RL * cos(q[0 * 3 + 0]) * cos(q[0 * 3 + 2]) * sin(q[0 * 3 + 1]) * + 5.049E+3 + + RL * pow(cos(q[0 * 3 + 0]), 2.0) * cos(q[0 * 3 + 1]) * + sin(q[0 * 3 + 2]) * 3.5E+3 + + RL * pow(cos(q[0 * 3 + 0]), 2.0) * cos(q[0 * 3 + 2]) * + sin(q[0 * 3 + 1]) * 3.5E+3 + + RL * cos(q[0 * 3 + 1]) * pow(sin(q[0 * 3 + 0]), 2.0) * + sin(q[0 * 3 + 2]) * 3.5E+3 + + RL * cos(q[0 * 3 + 2]) * pow(sin(q[0 * 3 + 0]), 2.0) * + sin(q[0 * 3 + 1]) * 3.5E+3)) / + ((pow(cos(q[0 * 3 + 0]), 2.0) + pow(sin(q[0 * 3 + 0]), 2.0)) * + (pow(cos(q[0 * 3 + 0]), 2.0) * pow(cos(q[0 * 3 + 1]), 2.0) * + pow(sin(q[0 * 3 + 2]), 2.0) + + pow(cos(q[0 * 3 + 0]), 2.0) * pow(cos(q[0 * 3 + 2]), 2.0) * + pow(sin(q[0 * 3 + 1]), 2.0) + + pow(cos(q[0 * 3 + 1]), 2.0) * pow(cos(q[0 * 3 + 2]), 2.0) * + pow(sin(q[0 * 3 + 0]), 2.0) + + pow(cos(q[0 * 3 + 0]), 2.0) * pow(sin(q[0 * 3 + 1]), 2.0) * + pow(sin(q[0 * 3 + 2]), 2.0) + + pow(cos(q[0 * 3 + 1]), 2.0) * pow(sin(q[0 * 3 + 0]), 2.0) * + pow(sin(q[0 * 3 + 2]), 2.0) + + pow(cos(q[0 * 3 + 2]), 2.0) * pow(sin(q[0 * 3 + 0]), 2.0) * + pow(sin(q[0 * 3 + 1]), 2.0) + + pow(sin(q[0 * 3 + 0]), 2.0) * pow(sin(q[0 * 3 + 1]), 2.0) * + pow(sin(q[0 * 3 + 2]), 2.0) + + pow(cos(q[0 * 3 + 0]), 2.0) * pow(cos(q[0 * 3 + 1]), 2.0) * + pow(cos(q[0 * 3 + 2]), 2.0)) * + 1.0E+5) - + (pow(cos(q[0 * 3 + 0]), 2.0) * + (cos(q[0 * 3 + 1]) * cos(q[0 * 3 + 2]) - + sin(q[0 * 3 + 1]) * sin(q[0 * 3 + 2])) * + (cos(q[0 * 3 + 1]) * -2.06E+3 + + pow(cos(q[0 * 3 + 1]), 2.0) * 2.263E+3 + + pow(sin(q[0 * 3 + 1]), 2.0) * 2.263E+3)) / + ((pow(cos(q[0 * 3 + 0]), 2.0) * pow(cos(q[0 * 3 + 1]), 2.0) + + pow(cos(q[0 * 3 + 0]), 2.0) * pow(sin(q[0 * 3 + 1]), 2.0) + + pow(cos(q[0 * 3 + 1]), 2.0) * pow(sin(q[0 * 3 + 0]), 2.0) + + pow(sin(q[0 * 3 + 0]), 2.0) * pow(sin(q[0 * 3 + 1]), 2.0)) * + (pow(cos(q[0 * 3 + 0]), 2.0) * pow(cos(q[0 * 3 + 1]), 2.0) * + pow(sin(q[0 * 3 + 2]), 2.0) + + pow(cos(q[0 * 3 + 0]), 2.0) * pow(cos(q[0 * 3 + 2]), 2.0) * + pow(sin(q[0 * 3 + 1]), 2.0) + + pow(cos(q[0 * 3 + 1]), 2.0) * pow(cos(q[0 * 3 + 2]), 2.0) * + pow(sin(q[0 * 3 + 0]), 2.0) + + pow(cos(q[0 * 3 + 0]), 2.0) * pow(sin(q[0 * 3 + 1]), 2.0) * + pow(sin(q[0 * 3 + 2]), 2.0) + + pow(cos(q[0 * 3 + 1]), 2.0) * pow(sin(q[0 * 3 + 0]), 2.0) * + pow(sin(q[0 * 3 + 2]), 2.0) + + pow(cos(q[0 * 3 + 2]), 2.0) * pow(sin(q[0 * 3 + 0]), 2.0) * + pow(sin(q[0 * 3 + 1]), 2.0) + + pow(sin(q[0 * 3 + 0]), 2.0) * pow(sin(q[0 * 3 + 1]), 2.0) * + pow(sin(q[0 * 3 + 2]), 2.0) + + pow(cos(q[0 * 3 + 0]), 2.0) * pow(cos(q[0 * 3 + 1]), 2.0) * + pow(cos(q[0 * 3 + 2]), 2.0)) * + 2.0E+4) - + (pow(sin(q[0 * 3 + 0]), 2.0) * + (cos(q[0 * 3 + 1]) * cos(q[0 * 3 + 2]) - + sin(q[0 * 3 + 1]) * sin(q[0 * 3 + 2])) * + (cos(q[0 * 3 + 1]) * -2.06E+3 + + pow(cos(q[0 * 3 + 1]), 2.0) * 2.263E+3 + + pow(sin(q[0 * 3 + 1]), 2.0) * 2.263E+3)) / + ((pow(cos(q[0 * 3 + 0]), 2.0) * pow(cos(q[0 * 3 + 1]), 2.0) + + pow(cos(q[0 * 3 + 0]), 2.0) * pow(sin(q[0 * 3 + 1]), 2.0) + + pow(cos(q[0 * 3 + 1]), 2.0) * pow(sin(q[0 * 3 + 0]), 2.0) + + pow(sin(q[0 * 3 + 0]), 2.0) * pow(sin(q[0 * 3 + 1]), 2.0)) * + (pow(cos(q[0 * 3 + 0]), 2.0) * pow(cos(q[0 * 3 + 1]), 2.0) * + pow(sin(q[0 * 3 + 2]), 2.0) + + pow(cos(q[0 * 3 + 0]), 2.0) * pow(cos(q[0 * 3 + 2]), 2.0) * + pow(sin(q[0 * 3 + 1]), 2.0) + + pow(cos(q[0 * 3 + 1]), 2.0) * pow(cos(q[0 * 3 + 2]), 2.0) * + pow(sin(q[0 * 3 + 0]), 2.0) + + pow(cos(q[0 * 3 + 0]), 2.0) * pow(sin(q[0 * 3 + 1]), 2.0) * + pow(sin(q[0 * 3 + 2]), 2.0) + + pow(cos(q[0 * 3 + 1]), 2.0) * pow(sin(q[0 * 3 + 0]), 2.0) * + pow(sin(q[0 * 3 + 2]), 2.0) + + pow(cos(q[0 * 3 + 2]), 2.0) * pow(sin(q[0 * 3 + 0]), 2.0) * + pow(sin(q[0 * 3 + 1]), 2.0) + + pow(sin(q[0 * 3 + 0]), 2.0) * pow(sin(q[0 * 3 + 1]), 2.0) * + pow(sin(q[0 * 3 + 2]), 2.0) + + pow(cos(q[0 * 3 + 0]), 2.0) * pow(cos(q[0 * 3 + 1]), 2.0) * + pow(cos(q[0 * 3 + 2]), 2.0)) * + 2.0E+4) - + 1.03E+2 / 5.0E+2)) / + (pow(cos(q[0 * 3 + 0]), 2.0) * pow(cos(q[0 * 3 + 1]), 2.0) * + pow(sin(q[0 * 3 + 2]), 2.0) + + pow(cos(q[0 * 3 + 0]), 2.0) * pow(cos(q[0 * 3 + 2]), 2.0) * + pow(sin(q[0 * 3 + 1]), 2.0) + + pow(cos(q[0 * 3 + 1]), 2.0) * pow(cos(q[0 * 3 + 2]), 2.0) * + pow(sin(q[0 * 3 + 0]), 2.0) + + pow(cos(q[0 * 3 + 0]), 2.0) * pow(sin(q[0 * 3 + 1]), 2.0) * + pow(sin(q[0 * 3 + 2]), 2.0) + + pow(cos(q[0 * 3 + 1]), 2.0) * pow(sin(q[0 * 3 + 0]), 2.0) * + pow(sin(q[0 * 3 + 2]), 2.0) + + pow(cos(q[0 * 3 + 2]), 2.0) * pow(sin(q[0 * 3 + 0]), 2.0) * + pow(sin(q[0 * 3 + 1]), 2.0) + + pow(sin(q[0 * 3 + 0]), 2.0) * pow(sin(q[0 * 3 + 1]), 2.0) * + pow(sin(q[0 * 3 + 2]), 2.0) + + pow(cos(q[0 * 3 + 0]), 2.0) * pow(cos(q[0 * 3 + 1]), 2.0) * + pow(cos(q[0 * 3 + 2]), 2.0)) + + (sin(q[0 * 3 + 0]) * + (cos(q[0 * 3 + 1]) * cos(q[0 * 3 + 2]) - + sin(q[0 * 3 + 1]) * sin(q[0 * 3 + 2])) * + ((sin(q[0 * 3 + 0]) * + (pow(cos(q[0 * 3 + 1]), 2.0) * sin(q[0 * 3 + 0]) * + sin(q[0 * 3 + 2]) * 1.03E+4 + + sin(q[0 * 3 + 0]) * pow(sin(q[0 * 3 + 1]), 2.0) * + sin(q[0 * 3 + 2]) * 1.03E+4 - + cos(q[0 * 3 + 1]) * sin(q[0 * 3 + 0]) * sin(q[0 * 3 + 2]) * + 1.1315E+4 - + cos(q[0 * 3 + 2]) * sin(q[0 * 3 + 0]) * sin(q[0 * 3 + 1]) * + 1.1315E+4 - + RL * cos(q[0 * 3 + 0]) * cos(q[0 * 3 + 1]) * cos(q[0 * 3 + 2]) * + 5.049E+3 + + RL * cos(q[0 * 3 + 0]) * sin(q[0 * 3 + 1]) * sin(q[0 * 3 + 2]) * + 5.049E+3 - + RL * pow(cos(q[0 * 3 + 0]), 2.0) * cos(q[0 * 3 + 1]) * + cos(q[0 * 3 + 2]) * 3.5E+3 - + RL * cos(q[0 * 3 + 1]) * cos(q[0 * 3 + 2]) * + pow(sin(q[0 * 3 + 0]), 2.0) * 3.5E+3 + + RL * pow(cos(q[0 * 3 + 0]), 2.0) * sin(q[0 * 3 + 1]) * + sin(q[0 * 3 + 2]) * 3.5E+3 + + RL * pow(sin(q[0 * 3 + 0]), 2.0) * sin(q[0 * 3 + 1]) * + sin(q[0 * 3 + 2]) * 3.5E+3) * + (-1.0E-5)) / + ((pow(cos(q[0 * 3 + 0]), 2.0) + pow(sin(q[0 * 3 + 0]), 2.0)) * + (pow(cos(q[0 * 3 + 0]), 2.0) * pow(cos(q[0 * 3 + 1]), 2.0) * + pow(sin(q[0 * 3 + 2]), 2.0) + + pow(cos(q[0 * 3 + 0]), 2.0) * pow(cos(q[0 * 3 + 2]), 2.0) * + pow(sin(q[0 * 3 + 1]), 2.0) + + pow(cos(q[0 * 3 + 1]), 2.0) * pow(cos(q[0 * 3 + 2]), 2.0) * + pow(sin(q[0 * 3 + 0]), 2.0) + + pow(cos(q[0 * 3 + 0]), 2.0) * pow(sin(q[0 * 3 + 1]), 2.0) * + pow(sin(q[0 * 3 + 2]), 2.0) + + pow(cos(q[0 * 3 + 1]), 2.0) * pow(sin(q[0 * 3 + 0]), 2.0) * + pow(sin(q[0 * 3 + 2]), 2.0) + + pow(cos(q[0 * 3 + 2]), 2.0) * pow(sin(q[0 * 3 + 0]), 2.0) * + pow(sin(q[0 * 3 + 1]), 2.0) + + pow(sin(q[0 * 3 + 0]), 2.0) * pow(sin(q[0 * 3 + 1]), 2.0) * + pow(sin(q[0 * 3 + 2]), 2.0) + + pow(cos(q[0 * 3 + 0]), 2.0) * pow(cos(q[0 * 3 + 1]), 2.0) * + pow(cos(q[0 * 3 + 2]), 2.0))) + + ((cos(q[0 * 3 + 1]) * cos(q[0 * 3 + 2]) - + sin(q[0 * 3 + 1]) * sin(q[0 * 3 + 2])) * + (pow(cos(q[0 * 3 + 0]), 2.0) * sin(q[0 * 3 + 1]) * 1.03E+2 + + pow(sin(q[0 * 3 + 0]), 2.0) * sin(q[0 * 3 + 1]) * 1.03E+2 + + RL * pow(cos(q[0 * 3 + 1]), 2.0) * sin(q[0 * 3 + 0]) * 3.5E+1 + + RL * sin(q[0 * 3 + 0]) * pow(sin(q[0 * 3 + 1]), 2.0) * 3.5E+1)) / + ((pow(cos(q[0 * 3 + 0]), 2.0) * pow(cos(q[0 * 3 + 1]), 2.0) + + pow(cos(q[0 * 3 + 0]), 2.0) * pow(sin(q[0 * 3 + 1]), 2.0) + + pow(cos(q[0 * 3 + 1]), 2.0) * pow(sin(q[0 * 3 + 0]), 2.0) + + pow(sin(q[0 * 3 + 0]), 2.0) * pow(sin(q[0 * 3 + 1]), 2.0)) * + (pow(cos(q[0 * 3 + 1]), 2.0) * pow(cos(q[0 * 3 + 2]), 2.0) + + pow(cos(q[0 * 3 + 1]), 2.0) * pow(sin(q[0 * 3 + 2]), 2.0) + + pow(cos(q[0 * 3 + 2]), 2.0) * pow(sin(q[0 * 3 + 1]), 2.0) + + pow(sin(q[0 * 3 + 1]), 2.0) * pow(sin(q[0 * 3 + 2]), 2.0)) * + 1.0E+3) - + (cos(q[0 * 3 + 0]) * + (cos(q[0 * 3 + 0]) * pow(cos(q[0 * 3 + 1]), 2.0) * + sin(q[0 * 3 + 2]) * 1.03E+4 + + cos(q[0 * 3 + 0]) * pow(sin(q[0 * 3 + 1]), 2.0) * + sin(q[0 * 3 + 2]) * 1.03E+4 - + cos(q[0 * 3 + 0]) * cos(q[0 * 3 + 1]) * sin(q[0 * 3 + 2]) * + 1.1315E+4 - + cos(q[0 * 3 + 0]) * cos(q[0 * 3 + 2]) * sin(q[0 * 3 + 1]) * + 1.1315E+4 + + RL * cos(q[0 * 3 + 1]) * cos(q[0 * 3 + 2]) * sin(q[0 * 3 + 0]) * + 5.049E+3 - + RL * sin(q[0 * 3 + 0]) * sin(q[0 * 3 + 1]) * sin(q[0 * 3 + 2]) * + 5.049E+3)) / + ((pow(cos(q[0 * 3 + 0]), 2.0) + pow(sin(q[0 * 3 + 0]), 2.0)) * + (pow(cos(q[0 * 3 + 0]), 2.0) * pow(cos(q[0 * 3 + 1]), 2.0) * + pow(sin(q[0 * 3 + 2]), 2.0) + + pow(cos(q[0 * 3 + 0]), 2.0) * pow(cos(q[0 * 3 + 2]), 2.0) * + pow(sin(q[0 * 3 + 1]), 2.0) + + pow(cos(q[0 * 3 + 1]), 2.0) * pow(cos(q[0 * 3 + 2]), 2.0) * + pow(sin(q[0 * 3 + 0]), 2.0) + + pow(cos(q[0 * 3 + 0]), 2.0) * pow(sin(q[0 * 3 + 1]), 2.0) * + pow(sin(q[0 * 3 + 2]), 2.0) + + pow(cos(q[0 * 3 + 1]), 2.0) * pow(sin(q[0 * 3 + 0]), 2.0) * + pow(sin(q[0 * 3 + 2]), 2.0) + + pow(cos(q[0 * 3 + 2]), 2.0) * pow(sin(q[0 * 3 + 0]), 2.0) * + pow(sin(q[0 * 3 + 1]), 2.0) + + pow(sin(q[0 * 3 + 0]), 2.0) * pow(sin(q[0 * 3 + 1]), 2.0) * + pow(sin(q[0 * 3 + 2]), 2.0) + + pow(cos(q[0 * 3 + 0]), 2.0) * pow(cos(q[0 * 3 + 1]), 2.0) * + pow(cos(q[0 * 3 + 2]), 2.0)) * + 1.0E+5) + + (pow(cos(q[0 * 3 + 0]), 2.0) * + (cos(q[0 * 3 + 1]) * sin(q[0 * 3 + 2]) + + cos(q[0 * 3 + 2]) * sin(q[0 * 3 + 1])) * + (cos(q[0 * 3 + 1]) * -2.06E+3 + + pow(cos(q[0 * 3 + 1]), 2.0) * 2.263E+3 + + pow(sin(q[0 * 3 + 1]), 2.0) * 2.263E+3)) / + ((pow(cos(q[0 * 3 + 0]), 2.0) * pow(cos(q[0 * 3 + 1]), 2.0) + + pow(cos(q[0 * 3 + 0]), 2.0) * pow(sin(q[0 * 3 + 1]), 2.0) + + pow(cos(q[0 * 3 + 1]), 2.0) * pow(sin(q[0 * 3 + 0]), 2.0) + + pow(sin(q[0 * 3 + 0]), 2.0) * pow(sin(q[0 * 3 + 1]), 2.0)) * + (pow(cos(q[0 * 3 + 0]), 2.0) * pow(cos(q[0 * 3 + 1]), 2.0) * + pow(sin(q[0 * 3 + 2]), 2.0) + + pow(cos(q[0 * 3 + 0]), 2.0) * pow(cos(q[0 * 3 + 2]), 2.0) * + pow(sin(q[0 * 3 + 1]), 2.0) + + pow(cos(q[0 * 3 + 1]), 2.0) * pow(cos(q[0 * 3 + 2]), 2.0) * + pow(sin(q[0 * 3 + 0]), 2.0) + + pow(cos(q[0 * 3 + 0]), 2.0) * pow(sin(q[0 * 3 + 1]), 2.0) * + pow(sin(q[0 * 3 + 2]), 2.0) + + pow(cos(q[0 * 3 + 1]), 2.0) * pow(sin(q[0 * 3 + 0]), 2.0) * + pow(sin(q[0 * 3 + 2]), 2.0) + + pow(cos(q[0 * 3 + 2]), 2.0) * pow(sin(q[0 * 3 + 0]), 2.0) * + pow(sin(q[0 * 3 + 1]), 2.0) + + pow(sin(q[0 * 3 + 0]), 2.0) * pow(sin(q[0 * 3 + 1]), 2.0) * + pow(sin(q[0 * 3 + 2]), 2.0) + + pow(cos(q[0 * 3 + 0]), 2.0) * pow(cos(q[0 * 3 + 1]), 2.0) * + pow(cos(q[0 * 3 + 2]), 2.0)) * + 2.0E+4) + + (pow(sin(q[0 * 3 + 0]), 2.0) * + (cos(q[0 * 3 + 1]) * sin(q[0 * 3 + 2]) + + cos(q[0 * 3 + 2]) * sin(q[0 * 3 + 1])) * + (cos(q[0 * 3 + 1]) * -2.06E+3 + + pow(cos(q[0 * 3 + 1]), 2.0) * 2.263E+3 + + pow(sin(q[0 * 3 + 1]), 2.0) * 2.263E+3)) / + ((pow(cos(q[0 * 3 + 0]), 2.0) * pow(cos(q[0 * 3 + 1]), 2.0) + + pow(cos(q[0 * 3 + 0]), 2.0) * pow(sin(q[0 * 3 + 1]), 2.0) + + pow(cos(q[0 * 3 + 1]), 2.0) * pow(sin(q[0 * 3 + 0]), 2.0) + + pow(sin(q[0 * 3 + 0]), 2.0) * pow(sin(q[0 * 3 + 1]), 2.0)) * + (pow(cos(q[0 * 3 + 0]), 2.0) * pow(cos(q[0 * 3 + 1]), 2.0) * + pow(sin(q[0 * 3 + 2]), 2.0) + + pow(cos(q[0 * 3 + 0]), 2.0) * pow(cos(q[0 * 3 + 2]), 2.0) * + pow(sin(q[0 * 3 + 1]), 2.0) + + pow(cos(q[0 * 3 + 1]), 2.0) * pow(cos(q[0 * 3 + 2]), 2.0) * + pow(sin(q[0 * 3 + 0]), 2.0) + + pow(cos(q[0 * 3 + 0]), 2.0) * pow(sin(q[0 * 3 + 1]), 2.0) * + pow(sin(q[0 * 3 + 2]), 2.0) + + pow(cos(q[0 * 3 + 1]), 2.0) * pow(sin(q[0 * 3 + 0]), 2.0) * + pow(sin(q[0 * 3 + 2]), 2.0) + + pow(cos(q[0 * 3 + 2]), 2.0) * pow(sin(q[0 * 3 + 0]), 2.0) * + pow(sin(q[0 * 3 + 1]), 2.0) + + pow(sin(q[0 * 3 + 0]), 2.0) * pow(sin(q[0 * 3 + 1]), 2.0) * + pow(sin(q[0 * 3 + 2]), 2.0) + + pow(cos(q[0 * 3 + 0]), 2.0) * pow(cos(q[0 * 3 + 1]), 2.0) * + pow(cos(q[0 * 3 + 2]), 2.0)) * + 2.0E+4))) / + (pow(cos(q[0 * 3 + 0]), 2.0) * pow(cos(q[0 * 3 + 1]), 2.0) * + pow(sin(q[0 * 3 + 2]), 2.0) + + pow(cos(q[0 * 3 + 0]), 2.0) * pow(cos(q[0 * 3 + 2]), 2.0) * + pow(sin(q[0 * 3 + 1]), 2.0) + + pow(cos(q[0 * 3 + 1]), 2.0) * pow(cos(q[0 * 3 + 2]), 2.0) * + pow(sin(q[0 * 3 + 0]), 2.0) + + pow(cos(q[0 * 3 + 0]), 2.0) * pow(sin(q[0 * 3 + 1]), 2.0) * + pow(sin(q[0 * 3 + 2]), 2.0) + + pow(cos(q[0 * 3 + 1]), 2.0) * pow(sin(q[0 * 3 + 0]), 2.0) * + pow(sin(q[0 * 3 + 2]), 2.0) + + pow(cos(q[0 * 3 + 2]), 2.0) * pow(sin(q[0 * 3 + 0]), 2.0) * + pow(sin(q[0 * 3 + 1]), 2.0) + + pow(sin(q[0 * 3 + 0]), 2.0) * pow(sin(q[0 * 3 + 1]), 2.0) * + pow(sin(q[0 * 3 + 2]), 2.0) + + pow(cos(q[0 * 3 + 0]), 2.0) * pow(cos(q[0 * 3 + 1]), 2.0) * + pow(cos(q[0 * 3 + 2]), 2.0))) - + (sin(q[0 * 3 + 0]) * sin(q[0 * 3 + 1]) * (1.03E+2 / 5.0E+2)) / + (pow(cos(q[0 * 3 + 0]), 2.0) + pow(sin(q[0 * 3 + 0]), 2.0)) + + ((cos(q[0 * 3 + 1]) * sin(q[0 * 3 + 2]) + + cos(q[0 * 3 + 2]) * sin(q[0 * 3 + 1])) * + (sin(q[0 * 3 + 0]) * sin(q[0 * 3 + 1]) * sin(q[0 * 3 + 2]) * 1.1315E+4 + + pow(cos(q[0 * 3 + 1]), 2.0) * cos(q[0 * 3 + 2]) * sin(q[0 * 3 + 0]) * + 1.03E+4 + + cos(q[0 * 3 + 2]) * sin(q[0 * 3 + 0]) * pow(sin(q[0 * 3 + 1]), 2.0) * + 1.03E+4 - + cos(q[0 * 3 + 1]) * cos(q[0 * 3 + 2]) * sin(q[0 * 3 + 0]) * 1.1315E+4 + + RL * cos(q[0 * 3 + 0]) * cos(q[0 * 3 + 1]) * sin(q[0 * 3 + 2]) * + 5.049E+3 + + RL * cos(q[0 * 3 + 0]) * cos(q[0 * 3 + 2]) * sin(q[0 * 3 + 1]) * + 5.049E+3 + + RL * pow(cos(q[0 * 3 + 0]), 2.0) * cos(q[0 * 3 + 1]) * + sin(q[0 * 3 + 2]) * 3.5E+3 + + RL * pow(cos(q[0 * 3 + 0]), 2.0) * cos(q[0 * 3 + 2]) * + sin(q[0 * 3 + 1]) * 3.5E+3 + + RL * cos(q[0 * 3 + 1]) * pow(sin(q[0 * 3 + 0]), 2.0) * + sin(q[0 * 3 + 2]) * 3.5E+3 + + RL * cos(q[0 * 3 + 2]) * pow(sin(q[0 * 3 + 0]), 2.0) * + sin(q[0 * 3 + 1]) * 3.5E+3)) / + (pow(cos(q[0 * 3 + 0]), 2.0) * pow(cos(q[0 * 3 + 1]), 2.0) * + pow(sin(q[0 * 3 + 2]), 2.0) * 5.0E+4 + + pow(cos(q[0 * 3 + 0]), 2.0) * pow(cos(q[0 * 3 + 2]), 2.0) * + pow(sin(q[0 * 3 + 1]), 2.0) * 5.0E+4 + + pow(cos(q[0 * 3 + 1]), 2.0) * pow(cos(q[0 * 3 + 2]), 2.0) * + pow(sin(q[0 * 3 + 0]), 2.0) * 5.0E+4 + + pow(cos(q[0 * 3 + 0]), 2.0) * pow(sin(q[0 * 3 + 1]), 2.0) * + pow(sin(q[0 * 3 + 2]), 2.0) * 5.0E+4 + + pow(cos(q[0 * 3 + 1]), 2.0) * pow(sin(q[0 * 3 + 0]), 2.0) * + pow(sin(q[0 * 3 + 2]), 2.0) * 5.0E+4 + + pow(cos(q[0 * 3 + 2]), 2.0) * pow(sin(q[0 * 3 + 0]), 2.0) * + pow(sin(q[0 * 3 + 1]), 2.0) * 5.0E+4 + + pow(sin(q[0 * 3 + 0]), 2.0) * pow(sin(q[0 * 3 + 1]), 2.0) * + pow(sin(q[0 * 3 + 2]), 2.0) * 5.0E+4 + + pow(cos(q[0 * 3 + 0]), 2.0) * pow(cos(q[0 * 3 + 1]), 2.0) * + pow(cos(q[0 * 3 + 2]), 2.0) * 5.0E+4) - + ((cos(q[0 * 3 + 1]) * cos(q[0 * 3 + 2]) - + sin(q[0 * 3 + 1]) * sin(q[0 * 3 + 2])) * + (pow(cos(q[0 * 3 + 1]), 2.0) * sin(q[0 * 3 + 0]) * sin(q[0 * 3 + 2]) * + 1.03E+4 + + sin(q[0 * 3 + 0]) * pow(sin(q[0 * 3 + 1]), 2.0) * sin(q[0 * 3 + 2]) * + 1.03E+4 - + cos(q[0 * 3 + 1]) * sin(q[0 * 3 + 0]) * sin(q[0 * 3 + 2]) * 1.1315E+4 - + cos(q[0 * 3 + 2]) * sin(q[0 * 3 + 0]) * sin(q[0 * 3 + 1]) * 1.1315E+4 - + RL * cos(q[0 * 3 + 0]) * cos(q[0 * 3 + 1]) * cos(q[0 * 3 + 2]) * + 5.049E+3 + + RL * cos(q[0 * 3 + 0]) * sin(q[0 * 3 + 1]) * sin(q[0 * 3 + 2]) * + 5.049E+3 - + RL * pow(cos(q[0 * 3 + 0]), 2.0) * cos(q[0 * 3 + 1]) * + cos(q[0 * 3 + 2]) * 3.5E+3 - + RL * cos(q[0 * 3 + 1]) * cos(q[0 * 3 + 2]) * + pow(sin(q[0 * 3 + 0]), 2.0) * 3.5E+3 + + RL * pow(cos(q[0 * 3 + 0]), 2.0) * sin(q[0 * 3 + 1]) * + sin(q[0 * 3 + 2]) * 3.5E+3 + + RL * pow(sin(q[0 * 3 + 0]), 2.0) * sin(q[0 * 3 + 1]) * + sin(q[0 * 3 + 2]) * 3.5E+3)) / + (pow(cos(q[0 * 3 + 0]), 2.0) * pow(cos(q[0 * 3 + 1]), 2.0) * + pow(sin(q[0 * 3 + 2]), 2.0) * 5.0E+4 + + pow(cos(q[0 * 3 + 0]), 2.0) * pow(cos(q[0 * 3 + 2]), 2.0) * + pow(sin(q[0 * 3 + 1]), 2.0) * 5.0E+4 + + pow(cos(q[0 * 3 + 1]), 2.0) * pow(cos(q[0 * 3 + 2]), 2.0) * + pow(sin(q[0 * 3 + 0]), 2.0) * 5.0E+4 + + pow(cos(q[0 * 3 + 0]), 2.0) * pow(sin(q[0 * 3 + 1]), 2.0) * + pow(sin(q[0 * 3 + 2]), 2.0) * 5.0E+4 + + pow(cos(q[0 * 3 + 1]), 2.0) * pow(sin(q[0 * 3 + 0]), 2.0) * + pow(sin(q[0 * 3 + 2]), 2.0) * 5.0E+4 + + pow(cos(q[0 * 3 + 2]), 2.0) * pow(sin(q[0 * 3 + 0]), 2.0) * + pow(sin(q[0 * 3 + 1]), 2.0) * 5.0E+4 + + pow(sin(q[0 * 3 + 0]), 2.0) * pow(sin(q[0 * 3 + 1]), 2.0) * + pow(sin(q[0 * 3 + 2]), 2.0) * 5.0E+4 + + pow(cos(q[0 * 3 + 0]), 2.0) * pow(cos(q[0 * 3 + 1]), 2.0) * + pow(cos(q[0 * 3 + 2]), 2.0) * 5.0E+4) - + (cos(q[0 * 3 + 0]) * + (RL * cos(q[0 * 3 + 1]) * sin(q[0 * 3 + 2]) * 1.0098E-1 + + RL * cos(q[0 * 3 + 2]) * sin(q[0 * 3 + 1]) * 1.0098E-1) * + (cos(q[0 * 3 + 1]) * sin(q[0 * 3 + 2]) + + cos(q[0 * 3 + 2]) * sin(q[0 * 3 + 1]))) / + (pow(cos(q[0 * 3 + 0]), 2.0) * pow(cos(q[0 * 3 + 1]), 2.0) * + pow(sin(q[0 * 3 + 2]), 2.0) + + pow(cos(q[0 * 3 + 0]), 2.0) * pow(cos(q[0 * 3 + 2]), 2.0) * + pow(sin(q[0 * 3 + 1]), 2.0) + + pow(cos(q[0 * 3 + 1]), 2.0) * pow(cos(q[0 * 3 + 2]), 2.0) * + pow(sin(q[0 * 3 + 0]), 2.0) + + pow(cos(q[0 * 3 + 0]), 2.0) * pow(sin(q[0 * 3 + 1]), 2.0) * + pow(sin(q[0 * 3 + 2]), 2.0) + + pow(cos(q[0 * 3 + 1]), 2.0) * pow(sin(q[0 * 3 + 0]), 2.0) * + pow(sin(q[0 * 3 + 2]), 2.0) + + pow(cos(q[0 * 3 + 2]), 2.0) * pow(sin(q[0 * 3 + 0]), 2.0) * + pow(sin(q[0 * 3 + 1]), 2.0) + + pow(sin(q[0 * 3 + 0]), 2.0) * pow(sin(q[0 * 3 + 1]), 2.0) * + pow(sin(q[0 * 3 + 2]), 2.0) + + pow(cos(q[0 * 3 + 0]), 2.0) * pow(cos(q[0 * 3 + 1]), 2.0) * + pow(cos(q[0 * 3 + 2]), 2.0)) - + (cos(q[0 * 3 + 0]) * + (RL * cos(q[0 * 3 + 1]) * cos(q[0 * 3 + 2]) * 1.0098E-1 - + RL * sin(q[0 * 3 + 1]) * sin(q[0 * 3 + 2]) * 1.0098E-1) * + (cos(q[0 * 3 + 1]) * cos(q[0 * 3 + 2]) - + sin(q[0 * 3 + 1]) * sin(q[0 * 3 + 2]))) / + (pow(cos(q[0 * 3 + 0]), 2.0) * pow(cos(q[0 * 3 + 1]), 2.0) * + pow(sin(q[0 * 3 + 2]), 2.0) + + pow(cos(q[0 * 3 + 0]), 2.0) * pow(cos(q[0 * 3 + 2]), 2.0) * + pow(sin(q[0 * 3 + 1]), 2.0) + + pow(cos(q[0 * 3 + 1]), 2.0) * pow(cos(q[0 * 3 + 2]), 2.0) * + pow(sin(q[0 * 3 + 0]), 2.0) + + pow(cos(q[0 * 3 + 0]), 2.0) * pow(sin(q[0 * 3 + 1]), 2.0) * + pow(sin(q[0 * 3 + 2]), 2.0) + + pow(cos(q[0 * 3 + 1]), 2.0) * pow(sin(q[0 * 3 + 0]), 2.0) * + pow(sin(q[0 * 3 + 2]), 2.0) + + pow(cos(q[0 * 3 + 2]), 2.0) * pow(sin(q[0 * 3 + 0]), 2.0) * + pow(sin(q[0 * 3 + 1]), 2.0) + + pow(sin(q[0 * 3 + 0]), 2.0) * pow(sin(q[0 * 3 + 1]), 2.0) * + pow(sin(q[0 * 3 + 2]), 2.0) + + pow(cos(q[0 * 3 + 0]), 2.0) * pow(cos(q[0 * 3 + 1]), 2.0) * + pow(cos(q[0 * 3 + 2]), 2.0)); + F(2, 1) = + (cos(q[0 * 3 + 0]) * + (cos(q[0 * 3 + 1]) * -2.06E+3 + pow(cos(q[0 * 3 + 1]), 2.0) * 2.263E+3 + + pow(sin(q[0 * 3 + 1]), 2.0) * 2.263E+3)) / + (pow(cos(q[0 * 3 + 0]), 2.0) * pow(cos(q[0 * 3 + 1]), 2.0) * 1.0E+4 + + pow(cos(q[0 * 3 + 0]), 2.0) * pow(sin(q[0 * 3 + 1]), 2.0) * 1.0E+4 + + pow(cos(q[0 * 3 + 1]), 2.0) * pow(sin(q[0 * 3 + 0]), 2.0) * 1.0E+4 + + pow(sin(q[0 * 3 + 0]), 2.0) * pow(sin(q[0 * 3 + 1]), 2.0) * 1.0E+4) + + (cos(q[0 * 3 + 0]) * + (((cos(q[0 * 3 + 1]) * cos(q[0 * 3 + 2]) - + sin(q[0 * 3 + 1]) * sin(q[0 * 3 + 2])) * + ((cos(q[0 * 3 + 0]) * + (cos(q[0 * 3 + 0]) * pow(cos(q[0 * 3 + 1]), 2.0) * + cos(q[0 * 3 + 2]) * 1.03E+4 + + cos(q[0 * 3 + 0]) * cos(q[0 * 3 + 2]) * + pow(sin(q[0 * 3 + 1]), 2.0) * 1.03E+4 - + cos(q[0 * 3 + 0]) * cos(q[0 * 3 + 1]) * cos(q[0 * 3 + 2]) * + 1.1315E+4 + + cos(q[0 * 3 + 0]) * sin(q[0 * 3 + 1]) * sin(q[0 * 3 + 2]) * + 1.1315E+4 - + RL * cos(q[0 * 3 + 1]) * sin(q[0 * 3 + 0]) * sin(q[0 * 3 + 2]) * + 5.049E+3 - + RL * cos(q[0 * 3 + 2]) * sin(q[0 * 3 + 0]) * sin(q[0 * 3 + 1]) * + 5.049E+3)) / + ((pow(cos(q[0 * 3 + 0]), 2.0) + pow(sin(q[0 * 3 + 0]), 2.0)) * + (pow(cos(q[0 * 3 + 0]), 2.0) * pow(cos(q[0 * 3 + 1]), 2.0) * + pow(sin(q[0 * 3 + 2]), 2.0) + + pow(cos(q[0 * 3 + 0]), 2.0) * pow(cos(q[0 * 3 + 2]), 2.0) * + pow(sin(q[0 * 3 + 1]), 2.0) + + pow(cos(q[0 * 3 + 1]), 2.0) * pow(cos(q[0 * 3 + 2]), 2.0) * + pow(sin(q[0 * 3 + 0]), 2.0) + + pow(cos(q[0 * 3 + 0]), 2.0) * pow(sin(q[0 * 3 + 1]), 2.0) * + pow(sin(q[0 * 3 + 2]), 2.0) + + pow(cos(q[0 * 3 + 1]), 2.0) * pow(sin(q[0 * 3 + 0]), 2.0) * + pow(sin(q[0 * 3 + 2]), 2.0) + + pow(cos(q[0 * 3 + 2]), 2.0) * pow(sin(q[0 * 3 + 0]), 2.0) * + pow(sin(q[0 * 3 + 1]), 2.0) + + pow(sin(q[0 * 3 + 0]), 2.0) * pow(sin(q[0 * 3 + 1]), 2.0) * + pow(sin(q[0 * 3 + 2]), 2.0) + + pow(cos(q[0 * 3 + 0]), 2.0) * pow(cos(q[0 * 3 + 1]), 2.0) * + pow(cos(q[0 * 3 + 2]), 2.0)) * + 1.0E+5) + + ((cos(q[0 * 3 + 1]) * sin(q[0 * 3 + 2]) + + cos(q[0 * 3 + 2]) * sin(q[0 * 3 + 1])) * + (pow(cos(q[0 * 3 + 0]), 2.0) * sin(q[0 * 3 + 1]) * 1.03E+2 + + pow(sin(q[0 * 3 + 0]), 2.0) * sin(q[0 * 3 + 1]) * 1.03E+2 + + RL * pow(cos(q[0 * 3 + 1]), 2.0) * sin(q[0 * 3 + 0]) * 3.5E+1 + + RL * sin(q[0 * 3 + 0]) * pow(sin(q[0 * 3 + 1]), 2.0) * 3.5E+1)) / + ((pow(cos(q[0 * 3 + 0]), 2.0) * pow(cos(q[0 * 3 + 1]), 2.0) + + pow(cos(q[0 * 3 + 0]), 2.0) * pow(sin(q[0 * 3 + 1]), 2.0) + + pow(cos(q[0 * 3 + 1]), 2.0) * pow(sin(q[0 * 3 + 0]), 2.0) + + pow(sin(q[0 * 3 + 0]), 2.0) * pow(sin(q[0 * 3 + 1]), 2.0)) * + (pow(cos(q[0 * 3 + 1]), 2.0) * pow(cos(q[0 * 3 + 2]), 2.0) + + pow(cos(q[0 * 3 + 1]), 2.0) * pow(sin(q[0 * 3 + 2]), 2.0) + + pow(cos(q[0 * 3 + 2]), 2.0) * pow(sin(q[0 * 3 + 1]), 2.0) + + pow(sin(q[0 * 3 + 1]), 2.0) * pow(sin(q[0 * 3 + 2]), 2.0)) * + 1.0E+3) + + (sin(q[0 * 3 + 0]) * + (sin(q[0 * 3 + 0]) * sin(q[0 * 3 + 1]) * sin(q[0 * 3 + 2]) * + 1.1315E+4 + + pow(cos(q[0 * 3 + 1]), 2.0) * cos(q[0 * 3 + 2]) * + sin(q[0 * 3 + 0]) * 1.03E+4 + + cos(q[0 * 3 + 2]) * sin(q[0 * 3 + 0]) * + pow(sin(q[0 * 3 + 1]), 2.0) * 1.03E+4 - + cos(q[0 * 3 + 1]) * cos(q[0 * 3 + 2]) * sin(q[0 * 3 + 0]) * + 1.1315E+4 + + RL * cos(q[0 * 3 + 0]) * cos(q[0 * 3 + 1]) * sin(q[0 * 3 + 2]) * + 5.049E+3 + + RL * cos(q[0 * 3 + 0]) * cos(q[0 * 3 + 2]) * sin(q[0 * 3 + 1]) * + 5.049E+3 + + RL * pow(cos(q[0 * 3 + 0]), 2.0) * cos(q[0 * 3 + 1]) * + sin(q[0 * 3 + 2]) * 3.5E+3 + + RL * pow(cos(q[0 * 3 + 0]), 2.0) * cos(q[0 * 3 + 2]) * + sin(q[0 * 3 + 1]) * 3.5E+3 + + RL * cos(q[0 * 3 + 1]) * pow(sin(q[0 * 3 + 0]), 2.0) * + sin(q[0 * 3 + 2]) * 3.5E+3 + + RL * cos(q[0 * 3 + 2]) * pow(sin(q[0 * 3 + 0]), 2.0) * + sin(q[0 * 3 + 1]) * 3.5E+3)) / + ((pow(cos(q[0 * 3 + 0]), 2.0) + pow(sin(q[0 * 3 + 0]), 2.0)) * + (pow(cos(q[0 * 3 + 0]), 2.0) * pow(cos(q[0 * 3 + 1]), 2.0) * + pow(sin(q[0 * 3 + 2]), 2.0) + + pow(cos(q[0 * 3 + 0]), 2.0) * pow(cos(q[0 * 3 + 2]), 2.0) * + pow(sin(q[0 * 3 + 1]), 2.0) + + pow(cos(q[0 * 3 + 1]), 2.0) * pow(cos(q[0 * 3 + 2]), 2.0) * + pow(sin(q[0 * 3 + 0]), 2.0) + + pow(cos(q[0 * 3 + 0]), 2.0) * pow(sin(q[0 * 3 + 1]), 2.0) * + pow(sin(q[0 * 3 + 2]), 2.0) + + pow(cos(q[0 * 3 + 1]), 2.0) * pow(sin(q[0 * 3 + 0]), 2.0) * + pow(sin(q[0 * 3 + 2]), 2.0) + + pow(cos(q[0 * 3 + 2]), 2.0) * pow(sin(q[0 * 3 + 0]), 2.0) * + pow(sin(q[0 * 3 + 1]), 2.0) + + pow(sin(q[0 * 3 + 0]), 2.0) * pow(sin(q[0 * 3 + 1]), 2.0) * + pow(sin(q[0 * 3 + 2]), 2.0) + + pow(cos(q[0 * 3 + 0]), 2.0) * pow(cos(q[0 * 3 + 1]), 2.0) * + pow(cos(q[0 * 3 + 2]), 2.0)) * + 1.0E+5) - + (pow(cos(q[0 * 3 + 0]), 2.0) * + (cos(q[0 * 3 + 1]) * cos(q[0 * 3 + 2]) - + sin(q[0 * 3 + 1]) * sin(q[0 * 3 + 2])) * + (cos(q[0 * 3 + 1]) * -2.06E+3 + + pow(cos(q[0 * 3 + 1]), 2.0) * 2.263E+3 + + pow(sin(q[0 * 3 + 1]), 2.0) * 2.263E+3)) / + ((pow(cos(q[0 * 3 + 0]), 2.0) * pow(cos(q[0 * 3 + 1]), 2.0) + + pow(cos(q[0 * 3 + 0]), 2.0) * pow(sin(q[0 * 3 + 1]), 2.0) + + pow(cos(q[0 * 3 + 1]), 2.0) * pow(sin(q[0 * 3 + 0]), 2.0) + + pow(sin(q[0 * 3 + 0]), 2.0) * pow(sin(q[0 * 3 + 1]), 2.0)) * + (pow(cos(q[0 * 3 + 0]), 2.0) * pow(cos(q[0 * 3 + 1]), 2.0) * + pow(sin(q[0 * 3 + 2]), 2.0) + + pow(cos(q[0 * 3 + 0]), 2.0) * pow(cos(q[0 * 3 + 2]), 2.0) * + pow(sin(q[0 * 3 + 1]), 2.0) + + pow(cos(q[0 * 3 + 1]), 2.0) * pow(cos(q[0 * 3 + 2]), 2.0) * + pow(sin(q[0 * 3 + 0]), 2.0) + + pow(cos(q[0 * 3 + 0]), 2.0) * pow(sin(q[0 * 3 + 1]), 2.0) * + pow(sin(q[0 * 3 + 2]), 2.0) + + pow(cos(q[0 * 3 + 1]), 2.0) * pow(sin(q[0 * 3 + 0]), 2.0) * + pow(sin(q[0 * 3 + 2]), 2.0) + + pow(cos(q[0 * 3 + 2]), 2.0) * pow(sin(q[0 * 3 + 0]), 2.0) * + pow(sin(q[0 * 3 + 1]), 2.0) + + pow(sin(q[0 * 3 + 0]), 2.0) * pow(sin(q[0 * 3 + 1]), 2.0) * + pow(sin(q[0 * 3 + 2]), 2.0) + + pow(cos(q[0 * 3 + 0]), 2.0) * pow(cos(q[0 * 3 + 1]), 2.0) * + pow(cos(q[0 * 3 + 2]), 2.0)) * + 2.0E+4) - + (pow(sin(q[0 * 3 + 0]), 2.0) * + (cos(q[0 * 3 + 1]) * cos(q[0 * 3 + 2]) - + sin(q[0 * 3 + 1]) * sin(q[0 * 3 + 2])) * + (cos(q[0 * 3 + 1]) * -2.06E+3 + + pow(cos(q[0 * 3 + 1]), 2.0) * 2.263E+3 + + pow(sin(q[0 * 3 + 1]), 2.0) * 2.263E+3)) / + ((pow(cos(q[0 * 3 + 0]), 2.0) * pow(cos(q[0 * 3 + 1]), 2.0) + + pow(cos(q[0 * 3 + 0]), 2.0) * pow(sin(q[0 * 3 + 1]), 2.0) + + pow(cos(q[0 * 3 + 1]), 2.0) * pow(sin(q[0 * 3 + 0]), 2.0) + + pow(sin(q[0 * 3 + 0]), 2.0) * pow(sin(q[0 * 3 + 1]), 2.0)) * + (pow(cos(q[0 * 3 + 0]), 2.0) * pow(cos(q[0 * 3 + 1]), 2.0) * + pow(sin(q[0 * 3 + 2]), 2.0) + + pow(cos(q[0 * 3 + 0]), 2.0) * pow(cos(q[0 * 3 + 2]), 2.0) * + pow(sin(q[0 * 3 + 1]), 2.0) + + pow(cos(q[0 * 3 + 1]), 2.0) * pow(cos(q[0 * 3 + 2]), 2.0) * + pow(sin(q[0 * 3 + 0]), 2.0) + + pow(cos(q[0 * 3 + 0]), 2.0) * pow(sin(q[0 * 3 + 1]), 2.0) * + pow(sin(q[0 * 3 + 2]), 2.0) + + pow(cos(q[0 * 3 + 1]), 2.0) * pow(sin(q[0 * 3 + 0]), 2.0) * + pow(sin(q[0 * 3 + 2]), 2.0) + + pow(cos(q[0 * 3 + 2]), 2.0) * pow(sin(q[0 * 3 + 0]), 2.0) * + pow(sin(q[0 * 3 + 1]), 2.0) + + pow(sin(q[0 * 3 + 0]), 2.0) * pow(sin(q[0 * 3 + 1]), 2.0) * + pow(sin(q[0 * 3 + 2]), 2.0) + + pow(cos(q[0 * 3 + 0]), 2.0) * pow(cos(q[0 * 3 + 1]), 2.0) * + pow(cos(q[0 * 3 + 2]), 2.0)) * + 2.0E+4) - + 1.03E+2 / 5.0E+2)) / + (pow(cos(q[0 * 3 + 1]), 2.0) * pow(cos(q[0 * 3 + 2]), 2.0) + + pow(cos(q[0 * 3 + 1]), 2.0) * pow(sin(q[0 * 3 + 2]), 2.0) + + pow(cos(q[0 * 3 + 2]), 2.0) * pow(sin(q[0 * 3 + 1]), 2.0) + + pow(sin(q[0 * 3 + 1]), 2.0) * pow(sin(q[0 * 3 + 2]), 2.0)) - + ((cos(q[0 * 3 + 1]) * sin(q[0 * 3 + 2]) + + cos(q[0 * 3 + 2]) * sin(q[0 * 3 + 1])) * + ((sin(q[0 * 3 + 0]) * + (pow(cos(q[0 * 3 + 1]), 2.0) * sin(q[0 * 3 + 0]) * + sin(q[0 * 3 + 2]) * 1.03E+4 + + sin(q[0 * 3 + 0]) * pow(sin(q[0 * 3 + 1]), 2.0) * + sin(q[0 * 3 + 2]) * 1.03E+4 - + cos(q[0 * 3 + 1]) * sin(q[0 * 3 + 0]) * sin(q[0 * 3 + 2]) * + 1.1315E+4 - + cos(q[0 * 3 + 2]) * sin(q[0 * 3 + 0]) * sin(q[0 * 3 + 1]) * + 1.1315E+4 - + RL * cos(q[0 * 3 + 0]) * cos(q[0 * 3 + 1]) * cos(q[0 * 3 + 2]) * + 5.049E+3 + + RL * cos(q[0 * 3 + 0]) * sin(q[0 * 3 + 1]) * sin(q[0 * 3 + 2]) * + 5.049E+3 - + RL * pow(cos(q[0 * 3 + 0]), 2.0) * cos(q[0 * 3 + 1]) * + cos(q[0 * 3 + 2]) * 3.5E+3 - + RL * cos(q[0 * 3 + 1]) * cos(q[0 * 3 + 2]) * + pow(sin(q[0 * 3 + 0]), 2.0) * 3.5E+3 + + RL * pow(cos(q[0 * 3 + 0]), 2.0) * sin(q[0 * 3 + 1]) * + sin(q[0 * 3 + 2]) * 3.5E+3 + + RL * pow(sin(q[0 * 3 + 0]), 2.0) * sin(q[0 * 3 + 1]) * + sin(q[0 * 3 + 2]) * 3.5E+3) * + (-1.0E-5)) / + ((pow(cos(q[0 * 3 + 0]), 2.0) + pow(sin(q[0 * 3 + 0]), 2.0)) * + (pow(cos(q[0 * 3 + 0]), 2.0) * pow(cos(q[0 * 3 + 1]), 2.0) * + pow(sin(q[0 * 3 + 2]), 2.0) + + pow(cos(q[0 * 3 + 0]), 2.0) * pow(cos(q[0 * 3 + 2]), 2.0) * + pow(sin(q[0 * 3 + 1]), 2.0) + + pow(cos(q[0 * 3 + 1]), 2.0) * pow(cos(q[0 * 3 + 2]), 2.0) * + pow(sin(q[0 * 3 + 0]), 2.0) + + pow(cos(q[0 * 3 + 0]), 2.0) * pow(sin(q[0 * 3 + 1]), 2.0) * + pow(sin(q[0 * 3 + 2]), 2.0) + + pow(cos(q[0 * 3 + 1]), 2.0) * pow(sin(q[0 * 3 + 0]), 2.0) * + pow(sin(q[0 * 3 + 2]), 2.0) + + pow(cos(q[0 * 3 + 2]), 2.0) * pow(sin(q[0 * 3 + 0]), 2.0) * + pow(sin(q[0 * 3 + 1]), 2.0) + + pow(sin(q[0 * 3 + 0]), 2.0) * pow(sin(q[0 * 3 + 1]), 2.0) * + pow(sin(q[0 * 3 + 2]), 2.0) + + pow(cos(q[0 * 3 + 0]), 2.0) * pow(cos(q[0 * 3 + 1]), 2.0) * + pow(cos(q[0 * 3 + 2]), 2.0))) + + ((cos(q[0 * 3 + 1]) * cos(q[0 * 3 + 2]) - + sin(q[0 * 3 + 1]) * sin(q[0 * 3 + 2])) * + (pow(cos(q[0 * 3 + 0]), 2.0) * sin(q[0 * 3 + 1]) * 1.03E+2 + + pow(sin(q[0 * 3 + 0]), 2.0) * sin(q[0 * 3 + 1]) * 1.03E+2 + + RL * pow(cos(q[0 * 3 + 1]), 2.0) * sin(q[0 * 3 + 0]) * 3.5E+1 + + RL * sin(q[0 * 3 + 0]) * pow(sin(q[0 * 3 + 1]), 2.0) * 3.5E+1)) / + ((pow(cos(q[0 * 3 + 0]), 2.0) * pow(cos(q[0 * 3 + 1]), 2.0) + + pow(cos(q[0 * 3 + 0]), 2.0) * pow(sin(q[0 * 3 + 1]), 2.0) + + pow(cos(q[0 * 3 + 1]), 2.0) * pow(sin(q[0 * 3 + 0]), 2.0) + + pow(sin(q[0 * 3 + 0]), 2.0) * pow(sin(q[0 * 3 + 1]), 2.0)) * + (pow(cos(q[0 * 3 + 1]), 2.0) * pow(cos(q[0 * 3 + 2]), 2.0) + + pow(cos(q[0 * 3 + 1]), 2.0) * pow(sin(q[0 * 3 + 2]), 2.0) + + pow(cos(q[0 * 3 + 2]), 2.0) * pow(sin(q[0 * 3 + 1]), 2.0) + + pow(sin(q[0 * 3 + 1]), 2.0) * pow(sin(q[0 * 3 + 2]), 2.0)) * + 1.0E+3) - + (cos(q[0 * 3 + 0]) * + (cos(q[0 * 3 + 0]) * pow(cos(q[0 * 3 + 1]), 2.0) * + sin(q[0 * 3 + 2]) * 1.03E+4 + + cos(q[0 * 3 + 0]) * pow(sin(q[0 * 3 + 1]), 2.0) * + sin(q[0 * 3 + 2]) * 1.03E+4 - + cos(q[0 * 3 + 0]) * cos(q[0 * 3 + 1]) * sin(q[0 * 3 + 2]) * + 1.1315E+4 - + cos(q[0 * 3 + 0]) * cos(q[0 * 3 + 2]) * sin(q[0 * 3 + 1]) * + 1.1315E+4 + + RL * cos(q[0 * 3 + 1]) * cos(q[0 * 3 + 2]) * sin(q[0 * 3 + 0]) * + 5.049E+3 - + RL * sin(q[0 * 3 + 0]) * sin(q[0 * 3 + 1]) * sin(q[0 * 3 + 2]) * + 5.049E+3)) / + ((pow(cos(q[0 * 3 + 0]), 2.0) + pow(sin(q[0 * 3 + 0]), 2.0)) * + (pow(cos(q[0 * 3 + 0]), 2.0) * pow(cos(q[0 * 3 + 1]), 2.0) * + pow(sin(q[0 * 3 + 2]), 2.0) + + pow(cos(q[0 * 3 + 0]), 2.0) * pow(cos(q[0 * 3 + 2]), 2.0) * + pow(sin(q[0 * 3 + 1]), 2.0) + + pow(cos(q[0 * 3 + 1]), 2.0) * pow(cos(q[0 * 3 + 2]), 2.0) * + pow(sin(q[0 * 3 + 0]), 2.0) + + pow(cos(q[0 * 3 + 0]), 2.0) * pow(sin(q[0 * 3 + 1]), 2.0) * + pow(sin(q[0 * 3 + 2]), 2.0) + + pow(cos(q[0 * 3 + 1]), 2.0) * pow(sin(q[0 * 3 + 0]), 2.0) * + pow(sin(q[0 * 3 + 2]), 2.0) + + pow(cos(q[0 * 3 + 2]), 2.0) * pow(sin(q[0 * 3 + 0]), 2.0) * + pow(sin(q[0 * 3 + 1]), 2.0) + + pow(sin(q[0 * 3 + 0]), 2.0) * pow(sin(q[0 * 3 + 1]), 2.0) * + pow(sin(q[0 * 3 + 2]), 2.0) + + pow(cos(q[0 * 3 + 0]), 2.0) * pow(cos(q[0 * 3 + 1]), 2.0) * + pow(cos(q[0 * 3 + 2]), 2.0)) * + 1.0E+5) + + (pow(cos(q[0 * 3 + 0]), 2.0) * + (cos(q[0 * 3 + 1]) * sin(q[0 * 3 + 2]) + + cos(q[0 * 3 + 2]) * sin(q[0 * 3 + 1])) * + (cos(q[0 * 3 + 1]) * -2.06E+3 + + pow(cos(q[0 * 3 + 1]), 2.0) * 2.263E+3 + + pow(sin(q[0 * 3 + 1]), 2.0) * 2.263E+3)) / + ((pow(cos(q[0 * 3 + 0]), 2.0) * pow(cos(q[0 * 3 + 1]), 2.0) + + pow(cos(q[0 * 3 + 0]), 2.0) * pow(sin(q[0 * 3 + 1]), 2.0) + + pow(cos(q[0 * 3 + 1]), 2.0) * pow(sin(q[0 * 3 + 0]), 2.0) + + pow(sin(q[0 * 3 + 0]), 2.0) * pow(sin(q[0 * 3 + 1]), 2.0)) * + (pow(cos(q[0 * 3 + 0]), 2.0) * pow(cos(q[0 * 3 + 1]), 2.0) * + pow(sin(q[0 * 3 + 2]), 2.0) + + pow(cos(q[0 * 3 + 0]), 2.0) * pow(cos(q[0 * 3 + 2]), 2.0) * + pow(sin(q[0 * 3 + 1]), 2.0) + + pow(cos(q[0 * 3 + 1]), 2.0) * pow(cos(q[0 * 3 + 2]), 2.0) * + pow(sin(q[0 * 3 + 0]), 2.0) + + pow(cos(q[0 * 3 + 0]), 2.0) * pow(sin(q[0 * 3 + 1]), 2.0) * + pow(sin(q[0 * 3 + 2]), 2.0) + + pow(cos(q[0 * 3 + 1]), 2.0) * pow(sin(q[0 * 3 + 0]), 2.0) * + pow(sin(q[0 * 3 + 2]), 2.0) + + pow(cos(q[0 * 3 + 2]), 2.0) * pow(sin(q[0 * 3 + 0]), 2.0) * + pow(sin(q[0 * 3 + 1]), 2.0) + + pow(sin(q[0 * 3 + 0]), 2.0) * pow(sin(q[0 * 3 + 1]), 2.0) * + pow(sin(q[0 * 3 + 2]), 2.0) + + pow(cos(q[0 * 3 + 0]), 2.0) * pow(cos(q[0 * 3 + 1]), 2.0) * + pow(cos(q[0 * 3 + 2]), 2.0)) * + 2.0E+4) + + (pow(sin(q[0 * 3 + 0]), 2.0) * + (cos(q[0 * 3 + 1]) * sin(q[0 * 3 + 2]) + + cos(q[0 * 3 + 2]) * sin(q[0 * 3 + 1])) * + (cos(q[0 * 3 + 1]) * -2.06E+3 + + pow(cos(q[0 * 3 + 1]), 2.0) * 2.263E+3 + + pow(sin(q[0 * 3 + 1]), 2.0) * 2.263E+3)) / + ((pow(cos(q[0 * 3 + 0]), 2.0) * pow(cos(q[0 * 3 + 1]), 2.0) + + pow(cos(q[0 * 3 + 0]), 2.0) * pow(sin(q[0 * 3 + 1]), 2.0) + + pow(cos(q[0 * 3 + 1]), 2.0) * pow(sin(q[0 * 3 + 0]), 2.0) + + pow(sin(q[0 * 3 + 0]), 2.0) * pow(sin(q[0 * 3 + 1]), 2.0)) * + (pow(cos(q[0 * 3 + 0]), 2.0) * pow(cos(q[0 * 3 + 1]), 2.0) * + pow(sin(q[0 * 3 + 2]), 2.0) + + pow(cos(q[0 * 3 + 0]), 2.0) * pow(cos(q[0 * 3 + 2]), 2.0) * + pow(sin(q[0 * 3 + 1]), 2.0) + + pow(cos(q[0 * 3 + 1]), 2.0) * pow(cos(q[0 * 3 + 2]), 2.0) * + pow(sin(q[0 * 3 + 0]), 2.0) + + pow(cos(q[0 * 3 + 0]), 2.0) * pow(sin(q[0 * 3 + 1]), 2.0) * + pow(sin(q[0 * 3 + 2]), 2.0) + + pow(cos(q[0 * 3 + 1]), 2.0) * pow(sin(q[0 * 3 + 0]), 2.0) * + pow(sin(q[0 * 3 + 2]), 2.0) + + pow(cos(q[0 * 3 + 2]), 2.0) * pow(sin(q[0 * 3 + 0]), 2.0) * + pow(sin(q[0 * 3 + 1]), 2.0) + + pow(sin(q[0 * 3 + 0]), 2.0) * pow(sin(q[0 * 3 + 1]), 2.0) * + pow(sin(q[0 * 3 + 2]), 2.0) + + pow(cos(q[0 * 3 + 0]), 2.0) * pow(cos(q[0 * 3 + 1]), 2.0) * + pow(cos(q[0 * 3 + 2]), 2.0)) * + 2.0E+4))) / + (pow(cos(q[0 * 3 + 1]), 2.0) * pow(cos(q[0 * 3 + 2]), 2.0) + + pow(cos(q[0 * 3 + 1]), 2.0) * pow(sin(q[0 * 3 + 2]), 2.0) + + pow(cos(q[0 * 3 + 2]), 2.0) * pow(sin(q[0 * 3 + 1]), 2.0) + + pow(sin(q[0 * 3 + 1]), 2.0) * pow(sin(q[0 * 3 + 2]), 2.0)))) / + (pow(cos(q[0 * 3 + 0]), 2.0) + pow(sin(q[0 * 3 + 0]), 2.0)) + + (cos(q[0 * 3 + 0]) * cos(q[0 * 3 + 2]) * + (cos(q[0 * 3 + 1]) * cos(q[0 * 3 + 2]) - + sin(q[0 * 3 + 1]) * sin(q[0 * 3 + 2])) * + (1.03E+2 / 5.0E+2)) / + (pow(cos(q[0 * 3 + 0]), 2.0) * pow(cos(q[0 * 3 + 1]), 2.0) * + pow(sin(q[0 * 3 + 2]), 2.0) + + pow(cos(q[0 * 3 + 0]), 2.0) * pow(cos(q[0 * 3 + 2]), 2.0) * + pow(sin(q[0 * 3 + 1]), 2.0) + + pow(cos(q[0 * 3 + 1]), 2.0) * pow(cos(q[0 * 3 + 2]), 2.0) * + pow(sin(q[0 * 3 + 0]), 2.0) + + pow(cos(q[0 * 3 + 0]), 2.0) * pow(sin(q[0 * 3 + 1]), 2.0) * + pow(sin(q[0 * 3 + 2]), 2.0) + + pow(cos(q[0 * 3 + 1]), 2.0) * pow(sin(q[0 * 3 + 0]), 2.0) * + pow(sin(q[0 * 3 + 2]), 2.0) + + pow(cos(q[0 * 3 + 2]), 2.0) * pow(sin(q[0 * 3 + 0]), 2.0) * + pow(sin(q[0 * 3 + 1]), 2.0) + + pow(sin(q[0 * 3 + 0]), 2.0) * pow(sin(q[0 * 3 + 1]), 2.0) * + pow(sin(q[0 * 3 + 2]), 2.0) + + pow(cos(q[0 * 3 + 0]), 2.0) * pow(cos(q[0 * 3 + 1]), 2.0) * + pow(cos(q[0 * 3 + 2]), 2.0)) + + (cos(q[0 * 3 + 0]) * sin(q[0 * 3 + 2]) * + (cos(q[0 * 3 + 1]) * sin(q[0 * 3 + 2]) + + cos(q[0 * 3 + 2]) * sin(q[0 * 3 + 1])) * + (1.03E+2 / 5.0E+2)) / + (pow(cos(q[0 * 3 + 0]), 2.0) * pow(cos(q[0 * 3 + 1]), 2.0) * + pow(sin(q[0 * 3 + 2]), 2.0) + + pow(cos(q[0 * 3 + 0]), 2.0) * pow(cos(q[0 * 3 + 2]), 2.0) * + pow(sin(q[0 * 3 + 1]), 2.0) + + pow(cos(q[0 * 3 + 1]), 2.0) * pow(cos(q[0 * 3 + 2]), 2.0) * + pow(sin(q[0 * 3 + 0]), 2.0) + + pow(cos(q[0 * 3 + 0]), 2.0) * pow(sin(q[0 * 3 + 1]), 2.0) * + pow(sin(q[0 * 3 + 2]), 2.0) + + pow(cos(q[0 * 3 + 1]), 2.0) * pow(sin(q[0 * 3 + 0]), 2.0) * + pow(sin(q[0 * 3 + 2]), 2.0) + + pow(cos(q[0 * 3 + 2]), 2.0) * pow(sin(q[0 * 3 + 0]), 2.0) * + pow(sin(q[0 * 3 + 1]), 2.0) + + pow(sin(q[0 * 3 + 0]), 2.0) * pow(sin(q[0 * 3 + 1]), 2.0) * + pow(sin(q[0 * 3 + 2]), 2.0) + + pow(cos(q[0 * 3 + 0]), 2.0) * pow(cos(q[0 * 3 + 1]), 2.0) * + pow(cos(q[0 * 3 + 2]), 2.0)); + F(2, 2) = + (cos(q[0 * 3 + 0]) * + (cos(q[0 * 3 + 1]) * -2.06E+3 + pow(cos(q[0 * 3 + 1]), 2.0) * 2.263E+3 + + pow(sin(q[0 * 3 + 1]), 2.0) * 2.263E+3)) / + (pow(cos(q[0 * 3 + 0]), 2.0) * pow(cos(q[0 * 3 + 1]), 2.0) * 1.0E+4 + + pow(cos(q[0 * 3 + 0]), 2.0) * pow(sin(q[0 * 3 + 1]), 2.0) * 1.0E+4 + + pow(cos(q[0 * 3 + 1]), 2.0) * pow(sin(q[0 * 3 + 0]), 2.0) * 1.0E+4 + + pow(sin(q[0 * 3 + 0]), 2.0) * pow(sin(q[0 * 3 + 1]), 2.0) * 1.0E+4) + + (cos(q[0 * 3 + 0]) * + (((cos(q[0 * 3 + 1]) * cos(q[0 * 3 + 2]) - + sin(q[0 * 3 + 1]) * sin(q[0 * 3 + 2])) * + ((cos(q[0 * 3 + 0]) * + (cos(q[0 * 3 + 0]) * pow(cos(q[0 * 3 + 1]), 2.0) * + cos(q[0 * 3 + 2]) * 1.03E+4 + + cos(q[0 * 3 + 0]) * cos(q[0 * 3 + 2]) * + pow(sin(q[0 * 3 + 1]), 2.0) * 1.03E+4 - + cos(q[0 * 3 + 0]) * cos(q[0 * 3 + 1]) * cos(q[0 * 3 + 2]) * + 1.1315E+4 + + cos(q[0 * 3 + 0]) * sin(q[0 * 3 + 1]) * sin(q[0 * 3 + 2]) * + 1.1315E+4 - + RL * cos(q[0 * 3 + 1]) * sin(q[0 * 3 + 0]) * sin(q[0 * 3 + 2]) * + 5.049E+3 - + RL * cos(q[0 * 3 + 2]) * sin(q[0 * 3 + 0]) * sin(q[0 * 3 + 1]) * + 5.049E+3)) / + ((pow(cos(q[0 * 3 + 0]), 2.0) + pow(sin(q[0 * 3 + 0]), 2.0)) * + (pow(cos(q[0 * 3 + 0]), 2.0) * pow(cos(q[0 * 3 + 1]), 2.0) * + pow(sin(q[0 * 3 + 2]), 2.0) + + pow(cos(q[0 * 3 + 0]), 2.0) * pow(cos(q[0 * 3 + 2]), 2.0) * + pow(sin(q[0 * 3 + 1]), 2.0) + + pow(cos(q[0 * 3 + 1]), 2.0) * pow(cos(q[0 * 3 + 2]), 2.0) * + pow(sin(q[0 * 3 + 0]), 2.0) + + pow(cos(q[0 * 3 + 0]), 2.0) * pow(sin(q[0 * 3 + 1]), 2.0) * + pow(sin(q[0 * 3 + 2]), 2.0) + + pow(cos(q[0 * 3 + 1]), 2.0) * pow(sin(q[0 * 3 + 0]), 2.0) * + pow(sin(q[0 * 3 + 2]), 2.0) + + pow(cos(q[0 * 3 + 2]), 2.0) * pow(sin(q[0 * 3 + 0]), 2.0) * + pow(sin(q[0 * 3 + 1]), 2.0) + + pow(sin(q[0 * 3 + 0]), 2.0) * pow(sin(q[0 * 3 + 1]), 2.0) * + pow(sin(q[0 * 3 + 2]), 2.0) + + pow(cos(q[0 * 3 + 0]), 2.0) * pow(cos(q[0 * 3 + 1]), 2.0) * + pow(cos(q[0 * 3 + 2]), 2.0)) * + 1.0E+5) + + ((cos(q[0 * 3 + 1]) * sin(q[0 * 3 + 2]) + + cos(q[0 * 3 + 2]) * sin(q[0 * 3 + 1])) * + (pow(cos(q[0 * 3 + 0]), 2.0) * sin(q[0 * 3 + 1]) * 1.03E+2 + + pow(sin(q[0 * 3 + 0]), 2.0) * sin(q[0 * 3 + 1]) * 1.03E+2 + + RL * pow(cos(q[0 * 3 + 1]), 2.0) * sin(q[0 * 3 + 0]) * 3.5E+1 + + RL * sin(q[0 * 3 + 0]) * pow(sin(q[0 * 3 + 1]), 2.0) * 3.5E+1)) / + ((pow(cos(q[0 * 3 + 0]), 2.0) * pow(cos(q[0 * 3 + 1]), 2.0) + + pow(cos(q[0 * 3 + 0]), 2.0) * pow(sin(q[0 * 3 + 1]), 2.0) + + pow(cos(q[0 * 3 + 1]), 2.0) * pow(sin(q[0 * 3 + 0]), 2.0) + + pow(sin(q[0 * 3 + 0]), 2.0) * pow(sin(q[0 * 3 + 1]), 2.0)) * + (pow(cos(q[0 * 3 + 1]), 2.0) * pow(cos(q[0 * 3 + 2]), 2.0) + + pow(cos(q[0 * 3 + 1]), 2.0) * pow(sin(q[0 * 3 + 2]), 2.0) + + pow(cos(q[0 * 3 + 2]), 2.0) * pow(sin(q[0 * 3 + 1]), 2.0) + + pow(sin(q[0 * 3 + 1]), 2.0) * pow(sin(q[0 * 3 + 2]), 2.0)) * + 1.0E+3) + + (sin(q[0 * 3 + 0]) * + (sin(q[0 * 3 + 0]) * sin(q[0 * 3 + 1]) * sin(q[0 * 3 + 2]) * + 1.1315E+4 + + pow(cos(q[0 * 3 + 1]), 2.0) * cos(q[0 * 3 + 2]) * + sin(q[0 * 3 + 0]) * 1.03E+4 + + cos(q[0 * 3 + 2]) * sin(q[0 * 3 + 0]) * + pow(sin(q[0 * 3 + 1]), 2.0) * 1.03E+4 - + cos(q[0 * 3 + 1]) * cos(q[0 * 3 + 2]) * sin(q[0 * 3 + 0]) * + 1.1315E+4 + + RL * cos(q[0 * 3 + 0]) * cos(q[0 * 3 + 1]) * sin(q[0 * 3 + 2]) * + 5.049E+3 + + RL * cos(q[0 * 3 + 0]) * cos(q[0 * 3 + 2]) * sin(q[0 * 3 + 1]) * + 5.049E+3 + + RL * pow(cos(q[0 * 3 + 0]), 2.0) * cos(q[0 * 3 + 1]) * + sin(q[0 * 3 + 2]) * 3.5E+3 + + RL * pow(cos(q[0 * 3 + 0]), 2.0) * cos(q[0 * 3 + 2]) * + sin(q[0 * 3 + 1]) * 3.5E+3 + + RL * cos(q[0 * 3 + 1]) * pow(sin(q[0 * 3 + 0]), 2.0) * + sin(q[0 * 3 + 2]) * 3.5E+3 + + RL * cos(q[0 * 3 + 2]) * pow(sin(q[0 * 3 + 0]), 2.0) * + sin(q[0 * 3 + 1]) * 3.5E+3)) / + ((pow(cos(q[0 * 3 + 0]), 2.0) + pow(sin(q[0 * 3 + 0]), 2.0)) * + (pow(cos(q[0 * 3 + 0]), 2.0) * pow(cos(q[0 * 3 + 1]), 2.0) * + pow(sin(q[0 * 3 + 2]), 2.0) + + pow(cos(q[0 * 3 + 0]), 2.0) * pow(cos(q[0 * 3 + 2]), 2.0) * + pow(sin(q[0 * 3 + 1]), 2.0) + + pow(cos(q[0 * 3 + 1]), 2.0) * pow(cos(q[0 * 3 + 2]), 2.0) * + pow(sin(q[0 * 3 + 0]), 2.0) + + pow(cos(q[0 * 3 + 0]), 2.0) * pow(sin(q[0 * 3 + 1]), 2.0) * + pow(sin(q[0 * 3 + 2]), 2.0) + + pow(cos(q[0 * 3 + 1]), 2.0) * pow(sin(q[0 * 3 + 0]), 2.0) * + pow(sin(q[0 * 3 + 2]), 2.0) + + pow(cos(q[0 * 3 + 2]), 2.0) * pow(sin(q[0 * 3 + 0]), 2.0) * + pow(sin(q[0 * 3 + 1]), 2.0) + + pow(sin(q[0 * 3 + 0]), 2.0) * pow(sin(q[0 * 3 + 1]), 2.0) * + pow(sin(q[0 * 3 + 2]), 2.0) + + pow(cos(q[0 * 3 + 0]), 2.0) * pow(cos(q[0 * 3 + 1]), 2.0) * + pow(cos(q[0 * 3 + 2]), 2.0)) * + 1.0E+5) - + (pow(cos(q[0 * 3 + 0]), 2.0) * + (cos(q[0 * 3 + 1]) * cos(q[0 * 3 + 2]) - + sin(q[0 * 3 + 1]) * sin(q[0 * 3 + 2])) * + (cos(q[0 * 3 + 1]) * -2.06E+3 + + pow(cos(q[0 * 3 + 1]), 2.0) * 2.263E+3 + + pow(sin(q[0 * 3 + 1]), 2.0) * 2.263E+3)) / + ((pow(cos(q[0 * 3 + 0]), 2.0) * pow(cos(q[0 * 3 + 1]), 2.0) + + pow(cos(q[0 * 3 + 0]), 2.0) * pow(sin(q[0 * 3 + 1]), 2.0) + + pow(cos(q[0 * 3 + 1]), 2.0) * pow(sin(q[0 * 3 + 0]), 2.0) + + pow(sin(q[0 * 3 + 0]), 2.0) * pow(sin(q[0 * 3 + 1]), 2.0)) * + (pow(cos(q[0 * 3 + 0]), 2.0) * pow(cos(q[0 * 3 + 1]), 2.0) * + pow(sin(q[0 * 3 + 2]), 2.0) + + pow(cos(q[0 * 3 + 0]), 2.0) * pow(cos(q[0 * 3 + 2]), 2.0) * + pow(sin(q[0 * 3 + 1]), 2.0) + + pow(cos(q[0 * 3 + 1]), 2.0) * pow(cos(q[0 * 3 + 2]), 2.0) * + pow(sin(q[0 * 3 + 0]), 2.0) + + pow(cos(q[0 * 3 + 0]), 2.0) * pow(sin(q[0 * 3 + 1]), 2.0) * + pow(sin(q[0 * 3 + 2]), 2.0) + + pow(cos(q[0 * 3 + 1]), 2.0) * pow(sin(q[0 * 3 + 0]), 2.0) * + pow(sin(q[0 * 3 + 2]), 2.0) + + pow(cos(q[0 * 3 + 2]), 2.0) * pow(sin(q[0 * 3 + 0]), 2.0) * + pow(sin(q[0 * 3 + 1]), 2.0) + + pow(sin(q[0 * 3 + 0]), 2.0) * pow(sin(q[0 * 3 + 1]), 2.0) * + pow(sin(q[0 * 3 + 2]), 2.0) + + pow(cos(q[0 * 3 + 0]), 2.0) * pow(cos(q[0 * 3 + 1]), 2.0) * + pow(cos(q[0 * 3 + 2]), 2.0)) * + 2.0E+4) - + (pow(sin(q[0 * 3 + 0]), 2.0) * + (cos(q[0 * 3 + 1]) * cos(q[0 * 3 + 2]) - + sin(q[0 * 3 + 1]) * sin(q[0 * 3 + 2])) * + (cos(q[0 * 3 + 1]) * -2.06E+3 + + pow(cos(q[0 * 3 + 1]), 2.0) * 2.263E+3 + + pow(sin(q[0 * 3 + 1]), 2.0) * 2.263E+3)) / + ((pow(cos(q[0 * 3 + 0]), 2.0) * pow(cos(q[0 * 3 + 1]), 2.0) + + pow(cos(q[0 * 3 + 0]), 2.0) * pow(sin(q[0 * 3 + 1]), 2.0) + + pow(cos(q[0 * 3 + 1]), 2.0) * pow(sin(q[0 * 3 + 0]), 2.0) + + pow(sin(q[0 * 3 + 0]), 2.0) * pow(sin(q[0 * 3 + 1]), 2.0)) * + (pow(cos(q[0 * 3 + 0]), 2.0) * pow(cos(q[0 * 3 + 1]), 2.0) * + pow(sin(q[0 * 3 + 2]), 2.0) + + pow(cos(q[0 * 3 + 0]), 2.0) * pow(cos(q[0 * 3 + 2]), 2.0) * + pow(sin(q[0 * 3 + 1]), 2.0) + + pow(cos(q[0 * 3 + 1]), 2.0) * pow(cos(q[0 * 3 + 2]), 2.0) * + pow(sin(q[0 * 3 + 0]), 2.0) + + pow(cos(q[0 * 3 + 0]), 2.0) * pow(sin(q[0 * 3 + 1]), 2.0) * + pow(sin(q[0 * 3 + 2]), 2.0) + + pow(cos(q[0 * 3 + 1]), 2.0) * pow(sin(q[0 * 3 + 0]), 2.0) * + pow(sin(q[0 * 3 + 2]), 2.0) + + pow(cos(q[0 * 3 + 2]), 2.0) * pow(sin(q[0 * 3 + 0]), 2.0) * + pow(sin(q[0 * 3 + 1]), 2.0) + + pow(sin(q[0 * 3 + 0]), 2.0) * pow(sin(q[0 * 3 + 1]), 2.0) * + pow(sin(q[0 * 3 + 2]), 2.0) + + pow(cos(q[0 * 3 + 0]), 2.0) * pow(cos(q[0 * 3 + 1]), 2.0) * + pow(cos(q[0 * 3 + 2]), 2.0)) * + 2.0E+4) - + 1.03E+2 / 5.0E+2)) / + (pow(cos(q[0 * 3 + 1]), 2.0) * pow(cos(q[0 * 3 + 2]), 2.0) + + pow(cos(q[0 * 3 + 1]), 2.0) * pow(sin(q[0 * 3 + 2]), 2.0) + + pow(cos(q[0 * 3 + 2]), 2.0) * pow(sin(q[0 * 3 + 1]), 2.0) + + pow(sin(q[0 * 3 + 1]), 2.0) * pow(sin(q[0 * 3 + 2]), 2.0)) - + ((cos(q[0 * 3 + 1]) * sin(q[0 * 3 + 2]) + + cos(q[0 * 3 + 2]) * sin(q[0 * 3 + 1])) * + ((sin(q[0 * 3 + 0]) * + (pow(cos(q[0 * 3 + 1]), 2.0) * sin(q[0 * 3 + 0]) * + sin(q[0 * 3 + 2]) * 1.03E+4 + + sin(q[0 * 3 + 0]) * pow(sin(q[0 * 3 + 1]), 2.0) * + sin(q[0 * 3 + 2]) * 1.03E+4 - + cos(q[0 * 3 + 1]) * sin(q[0 * 3 + 0]) * sin(q[0 * 3 + 2]) * + 1.1315E+4 - + cos(q[0 * 3 + 2]) * sin(q[0 * 3 + 0]) * sin(q[0 * 3 + 1]) * + 1.1315E+4 - + RL * cos(q[0 * 3 + 0]) * cos(q[0 * 3 + 1]) * cos(q[0 * 3 + 2]) * + 5.049E+3 + + RL * cos(q[0 * 3 + 0]) * sin(q[0 * 3 + 1]) * sin(q[0 * 3 + 2]) * + 5.049E+3 - + RL * pow(cos(q[0 * 3 + 0]), 2.0) * cos(q[0 * 3 + 1]) * + cos(q[0 * 3 + 2]) * 3.5E+3 - + RL * cos(q[0 * 3 + 1]) * cos(q[0 * 3 + 2]) * + pow(sin(q[0 * 3 + 0]), 2.0) * 3.5E+3 + + RL * pow(cos(q[0 * 3 + 0]), 2.0) * sin(q[0 * 3 + 1]) * + sin(q[0 * 3 + 2]) * 3.5E+3 + + RL * pow(sin(q[0 * 3 + 0]), 2.0) * sin(q[0 * 3 + 1]) * + sin(q[0 * 3 + 2]) * 3.5E+3) * + (-1.0E-5)) / + ((pow(cos(q[0 * 3 + 0]), 2.0) + pow(sin(q[0 * 3 + 0]), 2.0)) * + (pow(cos(q[0 * 3 + 0]), 2.0) * pow(cos(q[0 * 3 + 1]), 2.0) * + pow(sin(q[0 * 3 + 2]), 2.0) + + pow(cos(q[0 * 3 + 0]), 2.0) * pow(cos(q[0 * 3 + 2]), 2.0) * + pow(sin(q[0 * 3 + 1]), 2.0) + + pow(cos(q[0 * 3 + 1]), 2.0) * pow(cos(q[0 * 3 + 2]), 2.0) * + pow(sin(q[0 * 3 + 0]), 2.0) + + pow(cos(q[0 * 3 + 0]), 2.0) * pow(sin(q[0 * 3 + 1]), 2.0) * + pow(sin(q[0 * 3 + 2]), 2.0) + + pow(cos(q[0 * 3 + 1]), 2.0) * pow(sin(q[0 * 3 + 0]), 2.0) * + pow(sin(q[0 * 3 + 2]), 2.0) + + pow(cos(q[0 * 3 + 2]), 2.0) * pow(sin(q[0 * 3 + 0]), 2.0) * + pow(sin(q[0 * 3 + 1]), 2.0) + + pow(sin(q[0 * 3 + 0]), 2.0) * pow(sin(q[0 * 3 + 1]), 2.0) * + pow(sin(q[0 * 3 + 2]), 2.0) + + pow(cos(q[0 * 3 + 0]), 2.0) * pow(cos(q[0 * 3 + 1]), 2.0) * + pow(cos(q[0 * 3 + 2]), 2.0))) + + ((cos(q[0 * 3 + 1]) * cos(q[0 * 3 + 2]) - + sin(q[0 * 3 + 1]) * sin(q[0 * 3 + 2])) * + (pow(cos(q[0 * 3 + 0]), 2.0) * sin(q[0 * 3 + 1]) * 1.03E+2 + + pow(sin(q[0 * 3 + 0]), 2.0) * sin(q[0 * 3 + 1]) * 1.03E+2 + + RL * pow(cos(q[0 * 3 + 1]), 2.0) * sin(q[0 * 3 + 0]) * 3.5E+1 + + RL * sin(q[0 * 3 + 0]) * pow(sin(q[0 * 3 + 1]), 2.0) * 3.5E+1)) / + ((pow(cos(q[0 * 3 + 0]), 2.0) * pow(cos(q[0 * 3 + 1]), 2.0) + + pow(cos(q[0 * 3 + 0]), 2.0) * pow(sin(q[0 * 3 + 1]), 2.0) + + pow(cos(q[0 * 3 + 1]), 2.0) * pow(sin(q[0 * 3 + 0]), 2.0) + + pow(sin(q[0 * 3 + 0]), 2.0) * pow(sin(q[0 * 3 + 1]), 2.0)) * + (pow(cos(q[0 * 3 + 1]), 2.0) * pow(cos(q[0 * 3 + 2]), 2.0) + + pow(cos(q[0 * 3 + 1]), 2.0) * pow(sin(q[0 * 3 + 2]), 2.0) + + pow(cos(q[0 * 3 + 2]), 2.0) * pow(sin(q[0 * 3 + 1]), 2.0) + + pow(sin(q[0 * 3 + 1]), 2.0) * pow(sin(q[0 * 3 + 2]), 2.0)) * + 1.0E+3) - + (cos(q[0 * 3 + 0]) * + (cos(q[0 * 3 + 0]) * pow(cos(q[0 * 3 + 1]), 2.0) * + sin(q[0 * 3 + 2]) * 1.03E+4 + + cos(q[0 * 3 + 0]) * pow(sin(q[0 * 3 + 1]), 2.0) * + sin(q[0 * 3 + 2]) * 1.03E+4 - + cos(q[0 * 3 + 0]) * cos(q[0 * 3 + 1]) * sin(q[0 * 3 + 2]) * + 1.1315E+4 - + cos(q[0 * 3 + 0]) * cos(q[0 * 3 + 2]) * sin(q[0 * 3 + 1]) * + 1.1315E+4 + + RL * cos(q[0 * 3 + 1]) * cos(q[0 * 3 + 2]) * sin(q[0 * 3 + 0]) * + 5.049E+3 - + RL * sin(q[0 * 3 + 0]) * sin(q[0 * 3 + 1]) * sin(q[0 * 3 + 2]) * + 5.049E+3)) / + ((pow(cos(q[0 * 3 + 0]), 2.0) + pow(sin(q[0 * 3 + 0]), 2.0)) * + (pow(cos(q[0 * 3 + 0]), 2.0) * pow(cos(q[0 * 3 + 1]), 2.0) * + pow(sin(q[0 * 3 + 2]), 2.0) + + pow(cos(q[0 * 3 + 0]), 2.0) * pow(cos(q[0 * 3 + 2]), 2.0) * + pow(sin(q[0 * 3 + 1]), 2.0) + + pow(cos(q[0 * 3 + 1]), 2.0) * pow(cos(q[0 * 3 + 2]), 2.0) * + pow(sin(q[0 * 3 + 0]), 2.0) + + pow(cos(q[0 * 3 + 0]), 2.0) * pow(sin(q[0 * 3 + 1]), 2.0) * + pow(sin(q[0 * 3 + 2]), 2.0) + + pow(cos(q[0 * 3 + 1]), 2.0) * pow(sin(q[0 * 3 + 0]), 2.0) * + pow(sin(q[0 * 3 + 2]), 2.0) + + pow(cos(q[0 * 3 + 2]), 2.0) * pow(sin(q[0 * 3 + 0]), 2.0) * + pow(sin(q[0 * 3 + 1]), 2.0) + + pow(sin(q[0 * 3 + 0]), 2.0) * pow(sin(q[0 * 3 + 1]), 2.0) * + pow(sin(q[0 * 3 + 2]), 2.0) + + pow(cos(q[0 * 3 + 0]), 2.0) * pow(cos(q[0 * 3 + 1]), 2.0) * + pow(cos(q[0 * 3 + 2]), 2.0)) * + 1.0E+5) + + (pow(cos(q[0 * 3 + 0]), 2.0) * + (cos(q[0 * 3 + 1]) * sin(q[0 * 3 + 2]) + + cos(q[0 * 3 + 2]) * sin(q[0 * 3 + 1])) * + (cos(q[0 * 3 + 1]) * -2.06E+3 + + pow(cos(q[0 * 3 + 1]), 2.0) * 2.263E+3 + + pow(sin(q[0 * 3 + 1]), 2.0) * 2.263E+3)) / + ((pow(cos(q[0 * 3 + 0]), 2.0) * pow(cos(q[0 * 3 + 1]), 2.0) + + pow(cos(q[0 * 3 + 0]), 2.0) * pow(sin(q[0 * 3 + 1]), 2.0) + + pow(cos(q[0 * 3 + 1]), 2.0) * pow(sin(q[0 * 3 + 0]), 2.0) + + pow(sin(q[0 * 3 + 0]), 2.0) * pow(sin(q[0 * 3 + 1]), 2.0)) * + (pow(cos(q[0 * 3 + 0]), 2.0) * pow(cos(q[0 * 3 + 1]), 2.0) * + pow(sin(q[0 * 3 + 2]), 2.0) + + pow(cos(q[0 * 3 + 0]), 2.0) * pow(cos(q[0 * 3 + 2]), 2.0) * + pow(sin(q[0 * 3 + 1]), 2.0) + + pow(cos(q[0 * 3 + 1]), 2.0) * pow(cos(q[0 * 3 + 2]), 2.0) * + pow(sin(q[0 * 3 + 0]), 2.0) + + pow(cos(q[0 * 3 + 0]), 2.0) * pow(sin(q[0 * 3 + 1]), 2.0) * + pow(sin(q[0 * 3 + 2]), 2.0) + + pow(cos(q[0 * 3 + 1]), 2.0) * pow(sin(q[0 * 3 + 0]), 2.0) * + pow(sin(q[0 * 3 + 2]), 2.0) + + pow(cos(q[0 * 3 + 2]), 2.0) * pow(sin(q[0 * 3 + 0]), 2.0) * + pow(sin(q[0 * 3 + 1]), 2.0) + + pow(sin(q[0 * 3 + 0]), 2.0) * pow(sin(q[0 * 3 + 1]), 2.0) * + pow(sin(q[0 * 3 + 2]), 2.0) + + pow(cos(q[0 * 3 + 0]), 2.0) * pow(cos(q[0 * 3 + 1]), 2.0) * + pow(cos(q[0 * 3 + 2]), 2.0)) * + 2.0E+4) + + (pow(sin(q[0 * 3 + 0]), 2.0) * + (cos(q[0 * 3 + 1]) * sin(q[0 * 3 + 2]) + + cos(q[0 * 3 + 2]) * sin(q[0 * 3 + 1])) * + (cos(q[0 * 3 + 1]) * -2.06E+3 + + pow(cos(q[0 * 3 + 1]), 2.0) * 2.263E+3 + + pow(sin(q[0 * 3 + 1]), 2.0) * 2.263E+3)) / + ((pow(cos(q[0 * 3 + 0]), 2.0) * pow(cos(q[0 * 3 + 1]), 2.0) + + pow(cos(q[0 * 3 + 0]), 2.0) * pow(sin(q[0 * 3 + 1]), 2.0) + + pow(cos(q[0 * 3 + 1]), 2.0) * pow(sin(q[0 * 3 + 0]), 2.0) + + pow(sin(q[0 * 3 + 0]), 2.0) * pow(sin(q[0 * 3 + 1]), 2.0)) * + (pow(cos(q[0 * 3 + 0]), 2.0) * pow(cos(q[0 * 3 + 1]), 2.0) * + pow(sin(q[0 * 3 + 2]), 2.0) + + pow(cos(q[0 * 3 + 0]), 2.0) * pow(cos(q[0 * 3 + 2]), 2.0) * + pow(sin(q[0 * 3 + 1]), 2.0) + + pow(cos(q[0 * 3 + 1]), 2.0) * pow(cos(q[0 * 3 + 2]), 2.0) * + pow(sin(q[0 * 3 + 0]), 2.0) + + pow(cos(q[0 * 3 + 0]), 2.0) * pow(sin(q[0 * 3 + 1]), 2.0) * + pow(sin(q[0 * 3 + 2]), 2.0) + + pow(cos(q[0 * 3 + 1]), 2.0) * pow(sin(q[0 * 3 + 0]), 2.0) * + pow(sin(q[0 * 3 + 2]), 2.0) + + pow(cos(q[0 * 3 + 2]), 2.0) * pow(sin(q[0 * 3 + 0]), 2.0) * + pow(sin(q[0 * 3 + 1]), 2.0) + + pow(sin(q[0 * 3 + 0]), 2.0) * pow(sin(q[0 * 3 + 1]), 2.0) * + pow(sin(q[0 * 3 + 2]), 2.0) + + pow(cos(q[0 * 3 + 0]), 2.0) * pow(cos(q[0 * 3 + 1]), 2.0) * + pow(cos(q[0 * 3 + 2]), 2.0)) * + 2.0E+4))) / + (pow(cos(q[0 * 3 + 1]), 2.0) * pow(cos(q[0 * 3 + 2]), 2.0) + + pow(cos(q[0 * 3 + 1]), 2.0) * pow(sin(q[0 * 3 + 2]), 2.0) + + pow(cos(q[0 * 3 + 2]), 2.0) * pow(sin(q[0 * 3 + 1]), 2.0) + + pow(sin(q[0 * 3 + 1]), 2.0) * pow(sin(q[0 * 3 + 2]), 2.0)))) / + (pow(cos(q[0 * 3 + 0]), 2.0) + pow(sin(q[0 * 3 + 0]), 2.0)); +} +#endif +} // namespace force_estimation_dynamics diff --git a/body_force_estimator/src/body_force_estimator_node.cpp b/body_force_estimator/src/body_force_estimator_node.cpp new file mode 100644 index 000000000..251a276fb --- /dev/null +++ b/body_force_estimator/src/body_force_estimator_node.cpp @@ -0,0 +1,13 @@ +#include + +#include "body_force_estimator/body_force_estimator.h" + +int main(int argc, char** argv) { + ros::init(argc, argv, "body_force_estimator_node"); + ros::NodeHandle nh; + + BodyForceEstimator body_force_estimator(nh); + body_force_estimator.spin(); + + return 0; +} diff --git a/body_force_estimator/test/test_body_force_estimator.cpp b/body_force_estimator/test/test_body_force_estimator.cpp new file mode 100644 index 000000000..b663bb2a6 --- /dev/null +++ b/body_force_estimator/test/test_body_force_estimator.cpp @@ -0,0 +1,17 @@ +#include +#include + +#include "body_force_estimator/body_force_estimator.h" + +TEST(BodyForceEstimator, testTrue) { + ros::NodeHandle nh; + BodyForceEstimator body_force_estimator(nh); + EXPECT_EQ(1 + 1, 2); +} + +int main(int argc, char** argv) { + testing::InitGoogleTest(&argc, argv); + ros::init(argc, argv, "body_force_estimator_tester"); + + return RUN_ALL_TESTS(); +} diff --git a/docker/Dockerfile b/docker/Dockerfile new file mode 100644 index 000000000..ad50dc6ae --- /dev/null +++ b/docker/Dockerfile @@ -0,0 +1,131 @@ +############################################# +# Created from template ros.dockerfile.jinja +############################################# + +########################################### +# Base image +########################################### +FROM ubuntu:20.04 AS base + +ENV DEBIAN_FRONTEND=noninteractive + +# Install language +RUN apt-get update && apt-get install -y \ + locales \ + && locale-gen en_US.UTF-8 \ + && update-locale LC_ALL=en_US.UTF-8 LANG=en_US.UTF-8 \ + && rm -rf /var/lib/apt/lists/* +ENV LANG en_US.UTF-8 + +# Install timezone +RUN ln -fs /usr/share/zoneinfo/UTC /etc/localtime \ + && export DEBIAN_FRONTEND=noninteractive \ + && apt-get update \ + && apt-get install -y tzdata \ + && dpkg-reconfigure --frontend noninteractive tzdata \ + && rm -rf /var/lib/apt/lists/* + +########################################### +# ROS image +########################################### +FROM base AS ros + +# Install ROS +RUN apt-get update && apt-get install -y \ + curl \ + dirmngr \ + gnupg2 \ + lsb-release \ + sudo \ + && apt-get upgrade -y \ + && sh -c 'echo "deb http://packages.ros.org/ros/ubuntu $(lsb_release -sc) main" > /etc/apt/sources.list.d/ros-latest.list' \ + && curl -s https://raw.githubusercontent.com/ros/rosdistro/master/ros.asc | apt-key add - \ + && apt-get update && apt-get install -y \ + ros-noetic-desktop-full \ + && apt-get upgrade libignition-math4 -y \ + && rm -rf /var/lib/apt/lists/* + +# Setup environment +ENV LD_LIBRARY_PATH=/opt/ros/noetic/lib +ENV ROS_DISTRO=noetic +ENV ROS_ROOT=/opt/ros/noetic/share/ros +ENV ROS_PACKAGE_PATH=/opt/ros/noetic/share +ENV ROS_MASTER_URI=http://localhost:11311 +ENV ROS_PYTHON_VERSION= +ENV ROS_VERSION=1 +ENV PATH=/opt/ros/noetic/bin:/usr/local/sbin:/usr/local/bin:/usr/sbin:/usr/bin:/sbin:/bin +ENV ROSLISP_PACKAGE_DIRECTORIES= +ENV PYTHONPATH=/opt/ros/noetic/lib/python3.8/dist-packages +ENV PKG_CONFIG_PATH=/opt/ros/noetic/lib/pkgconfig +ENV ROS_ETC_DIR=/opt/ros/noetic/etc/ros +ENV CMAKE_PREFIX_PATH=/opt/ros/noetic +ENV DEBIAN_FRONTEND= + +########################################### +# Develop image +########################################### +FROM ros AS dev + +ENV DEBIAN_FRONTEND=noninteractive +# Install dev tools +RUN apt-get update && apt-get install -y \ + python3-rosdep \ + python3-rosinstall \ + python3-rosinstall-generator \ + python3-wstool \ + python3-pip \ + python3-pep8 \ + python3-autopep8 \ + pylint \ + build-essential \ + bash-completion \ + git \ + vim \ + python3-catkin-tools \ + && rm -rf /var/lib/apt/lists/* \ + && rosdep init || echo "rosdep already initialized" + +ARG USERNAME=ros +ARG USER_UID=1000 +ARG USER_GID=$USER_UID + +# Create a non-root user +RUN groupadd --gid $USER_GID $USERNAME \ + && useradd -s /bin/bash --uid $USER_UID --gid $USER_GID -m $USERNAME \ + # [Optional] Add sudo support for the non-root user + && apt-get update \ + && apt-get install -y sudo git-core bash-completion \ + && echo $USERNAME ALL=\(root\) NOPASSWD:ALL > /etc/sudoers.d/$USERNAME\ + && chmod 0440 /etc/sudoers.d/$USERNAME \ + # Cleanup + && rm -rf /var/lib/apt/lists/* \ + && echo "if [ -f /opt/ros/${ROS_DISTRO}/setup.bash ]; then source /opt/ros/${ROS_DISTRO}/setup.bash; fi" >> /home/$USERNAME/.bashrc +ENV DEBIAN_FRONTEND= + +########################################### +# ADD SSH into Docker Container +########################################### + + +######################################### +# Set up auto-source of workspace for ros user +ARG WORKSPACE +RUN echo "if [ -f ${WORKSPACE}/devel/setup.bash ]; then source ${WORKSPACE}/devel/setup.bash; fi" >> /home/ros/.bashrc + + +ENV CATKIN_WS = /home/catkin_ws +ENV SDK = /home/catkin_ws/src +RUN mkdir -p $CATKIN_WS/src + +RUN /bin/bash -c "source /opt/ros/${ROS_DISTRO}/setup.bash; catkin init;" +WORKDIR $SDK +RUN git clone https://github.com/robomechanics/quad-sdk.git + +# ENV QUAD_SDK = /root/catkin_ws/src/quad-sdk +# WORKDIR $QUAD_SDK +# RUN git checkout devel + +# ADD coinhsl /home/catkin_ws/src/quad-sdk/external/ipopt/coinhsl + + + diff --git a/external/setup_deps.sh b/external/setup_deps.sh index dc787fe29..8cc2e89be 100755 --- a/external/setup_deps.sh +++ b/external/setup_deps.sh @@ -14,9 +14,9 @@ then fi mkdir coinbrew cd coinbrew -wget https://raw.githubusercontent.com/coin-or/coinbrew/v1.0/coinbrew +wget https://raw.githubusercontent.com/coin-or/coinbrew/v2.0/coinbrew chmod u+x coinbrew -./coinbrew fetch Ipopt --latest-release --no-prompt +./coinbrew fetch Ipopt --no-prompt cd .. if [ -d "./coinhsl" ] then @@ -26,11 +26,11 @@ else echo "Warning: HSL not found." fi cd coinbrew -./coinbrew build Ipopt --latest-release --tests none --prefix=/usr/local --no-prompt --parallel-jobs=8 +./coinbrew build Ipopt --latest-release --tests none --prefix=/usr/local --no-prompt cd ../.. # Setup and build for rbdl -sudo apt install -y ros-melodic-urdf +sudo apt install -y ros-noetic-urdf cd rbdl-orb quiet_mkdir build cd build @@ -40,4 +40,4 @@ sudo make install cd ../.. # Setup for teleop_twist_joy to get dependencies installed -sudo apt install -y ros-melodic-teleop-twist-joy +sudo apt install -y ros-noetic-teleop-twist-joy diff --git a/external/teleop_twist_keyboard/CHANGELOG.rst b/external/teleop_twist_keyboard/CHANGELOG.rst new file mode 100644 index 000000000..8d8d204ce --- /dev/null +++ b/external/teleop_twist_keyboard/CHANGELOG.rst @@ -0,0 +1,40 @@ +1.0.0 (2020-06-26) +------------------ +* Implement repeat rate and key timeout +* Update readme about publishing to another topic. +* Contributors: Austin, trainman419 + +0.6.2 (2018-10-21) +------------------ +* Replace tabs with spaces, fixes `#15 `_ +* Merge pull request `#13 `_ from asukiaaa/patch-3 + Add rosrun command to specify values +* Add rosrun command to specify values +* Contributors: Asuki Kono, Austin, trainman419 + +0.6.1 (2018-05-02) +------------------ +* Merge pull request `#11 `_ from MatthijsBurgh/patch-1 + Correct exception handling; Python3 print compatible +* import print from future +* Print python3 compatible +* correct Exception handling +* Merge pull request `#7 `_ from lucasw/speed_params + set linear and turn speed via rosparams +* Using tabs instead of spaces to match rest of file +* set linear and turn speed via rosparams +* Contributors: Austin, Lucas Walter, Matthijs van der Burgh + +0.6.0 (2016-03-21) +------------------ +* Better instruction formatting +* support holonomic base, send commmand with keyboard down +* Fixing queue_size warning +* Create README.md +* Update the description string in package.xml +* Contributors: Austin, Kei Okada, LiohAu, Mike Purvis, kk6axq, trainman419 + +0.5.0 (2014-02-11) +------------------ +* Initial import from brown_remotelab +* Convert to catkin diff --git a/external/teleop_twist_keyboard/CMakeLists.txt b/external/teleop_twist_keyboard/CMakeLists.txt new file mode 100644 index 000000000..86d90cb1b --- /dev/null +++ b/external/teleop_twist_keyboard/CMakeLists.txt @@ -0,0 +1,14 @@ +cmake_minimum_required(VERSION 2.8.3) +project(teleop_twist_keyboard) + +find_package(catkin REQUIRED) + +catkin_package() + +## Mark executable scripts (Python etc.) for installation +## in contrast to setup.py, you can choose the destination +catkin_install_python(PROGRAMS + teleop_twist_keyboard.py + DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION} +) + diff --git a/external/teleop_twist_keyboard/README.md b/external/teleop_twist_keyboard/README.md new file mode 100644 index 000000000..000d05090 --- /dev/null +++ b/external/teleop_twist_keyboard/README.md @@ -0,0 +1,74 @@ +# teleop_twist_keyboard +Generic Keyboard Teleop for ROS + +# Launch +Run. +``` +rosrun teleop_twist_keyboard teleop_twist_keyboard.py +``` + +With custom values. +``` +rosrun teleop_twist_keyboard teleop_twist_keyboard.py _speed:=0.9 _turn:=0.8 +``` + +Publishing to a different topic (in this case `my_cmd_vel`). +``` +rosrun teleop_twist_keyboard teleop_twist_keyboard.py cmd_vel:=my_cmd_vel +``` + +# Usage +``` +Reading from the keyboard and Publishing to Twist! +--------------------------- +Moving around: + u i o + j k l + m , . + +For Holonomic mode (strafing), hold down the shift key: +--------------------------- + U I O + J K L + M < > + +t : up (+z) +b : down (-z) + +anything else : stop + +q/z : increase/decrease max speeds by 10% +w/x : increase/decrease only linear speed by 10% +e/c : increase/decrease only angular speed by 10% + +CTRL-C to quit +``` + +# Repeat Rate + +If your mobile base requires constant updates on the cmd\_vel topic, teleop\_twist\_keyboard can be configured to repeat the last command at a fixed interval, using the `repeat_rate` private parameter. + +For example, to repeat the last command at 10Hz: + +``` +rosrun teleop_twist_keyboard teleop_twist_keyboard.py _repeat_rate:=10.0 +``` + +It is _highly_ recommened that the repeat rate be used in conjunction with the key timeout, to prevent runaway robots. + +# Key Timeout + +Teleop\_twist\_keyboard can be configured to stop your robot if it does not receive any key presses in a configured time period, using the `key_timeout` private parameter. + +For example, to stop your robot if a keypress has not been received in 0.6 seconds: +``` +rosrun teleop_twist_keyboard teleop_twist_keyboard.py _key_timeout:=0.6 +``` + +It is recommended that you set `key_timeout` higher than the initial key repeat delay on your system (This delay is 0.5 seconds by default on Ubuntu, but can be adjusted). + +# Twist with header +Publishing a `TwistStamped` message instead of `Twist` can be enabled with the `stamped` private parameter. Additionally the `frame_id` of the `TwistStamped` message can be set with the `frame_id` private parameter. +``` +rosrun teleop_twist_keyboard teleop_twist_keyboard.py _stamped:=True _frame_id:=base_link +``` \ No newline at end of file diff --git a/external/teleop_twist_keyboard/package.xml b/external/teleop_twist_keyboard/package.xml new file mode 100644 index 000000000..84add0221 --- /dev/null +++ b/external/teleop_twist_keyboard/package.xml @@ -0,0 +1,18 @@ + + + teleop_twist_keyboard + 1.0.0 + Generic keyboard teleop for twist robots. + + Austin Hendrix + + BSD + + http://wiki.ros.org/teleop_twist_keyboard + + Graylin Trevor Jay + + catkin + geometry_msgs + rospy + diff --git a/external/teleop_twist_keyboard/teleop_twist_keyboard.py b/external/teleop_twist_keyboard/teleop_twist_keyboard.py new file mode 100755 index 000000000..a85e1cc5c --- /dev/null +++ b/external/teleop_twist_keyboard/teleop_twist_keyboard.py @@ -0,0 +1,265 @@ +#!/usr/bin/env python + +from __future__ import print_function + +import threading + +import roslib; roslib.load_manifest('teleop_twist_keyboard') +import rospy + +from geometry_msgs.msg import Twist +from geometry_msgs.msg import TwistStamped + +import sys +from select import select + +if sys.platform == 'win32': + import msvcrt +else: + import termios + import tty + + +TwistMsg = Twist + +msg = """ +Reading from the keyboard and Publishing to Twist! +--------------------------- +Moving around: + u i o + j k l + m , . + +For Holonomic mode (strafing), hold down the shift key: +--------------------------- + U I O + J K L + M < > + +t : up (+z) +b : down (-z) + +anything else : stop + +q/z : increase/decrease max speeds by 10% +w/x : increase/decrease only linear speed by 10% +e/c : increase/decrease only angular speed by 10% + +CTRL-C to quit +""" + +moveBindings = { + 'i':(1,0,0,0), + 'o':(1,0,0,-1), + 'j':(0,0,0,1), + 'l':(0,0,0,-1), + 'u':(1,0,0,1), + ',':(-1,0,0,0), + '.':(-1,0,0,1), + 'm':(-1,0,0,-1), + 'O':(1,-1,0,0), + 'I':(1,0,0,0), + 'J':(0,1,0,0), + 'L':(0,-1,0,0), + 'U':(1,1,0,0), + '<':(-1,0,0,0), + '>':(-1,-1,0,0), + 'M':(-1,1,0,0), + 't':(0,0,1,0), + 'b':(0,0,-1,0), + } + +speedBindings={ + 'q':(1.1,1.1), + 'z':(.9,.9), + 'w':(1.1,1), + 'x':(.9,1), + 'e':(1,1.1), + 'c':(1,.9), + } + +class PublishThread(threading.Thread): + def __init__(self, rate): + super(PublishThread, self).__init__() + self.publisher = rospy.Publisher('cmd_vel', TwistMsg, queue_size = 1) + self.x = 0.0 + self.y = 0.0 + self.z = 0.0 + self.th = 0.0 + self.speed = 0.0 + self.turn = 0.0 + self.condition = threading.Condition() + self.done = False + + # Set timeout to None if rate is 0 (causes new_message to wait forever + # for new data to publish) + if rate != 0.0: + self.timeout = 1.0 / rate + else: + self.timeout = None + + self.start() + + def wait_for_subscribers(self): + i = 0 + while not rospy.is_shutdown() and self.publisher.get_num_connections() == 0: + if i == 4: + print("Waiting for subscriber to connect to {}".format(self.publisher.name)) + rospy.sleep(0.5) + i += 1 + i = i % 5 + if rospy.is_shutdown(): + raise Exception("Got shutdown request before subscribers connected") + + def update(self, x, y, z, th, speed, turn): + self.condition.acquire() + self.x = x + self.y = y + self.z = z + self.th = th + self.speed = speed + self.turn = turn + # Notify publish thread that we have a new message. + self.condition.notify() + self.condition.release() + + def stop(self): + self.done = True + self.update(0, 0, 0, 0, 0, 0) + self.join() + + def run(self): + twist_msg = TwistMsg() + + if stamped: + twist = twist_msg.twist + twist_msg.header.stamp = rospy.Time.now() + twist_msg.header.frame_id = twist_frame + else: + twist = twist_msg + while not self.done: + if stamped: + twist_msg.header.stamp = rospy.Time.now() + self.condition.acquire() + # Wait for a new message or timeout. + self.condition.wait(self.timeout) + + # Copy state into twist message. + twist.linear.x = self.x * self.speed + twist.linear.y = self.y * self.speed + twist.linear.z = self.z * self.speed + twist.angular.x = 0 + twist.angular.y = 0 + twist.angular.z = self.th * self.turn + + self.condition.release() + + # Publish. + self.publisher.publish(twist_msg) + + # Publish stop message when thread exits. + twist.linear.x = 0 + twist.linear.y = 0 + twist.linear.z = 0 + twist.angular.x = 0 + twist.angular.y = 0 + twist.angular.z = 0 + self.publisher.publish(twist_msg) + + +def getKey(settings, timeout): + if sys.platform == 'win32': + # getwch() returns a string on Windows + key = msvcrt.getwch() + else: + tty.setraw(sys.stdin.fileno()) + # sys.stdin.read() returns a string on Linux + rlist, _, _ = select([sys.stdin], [], [], timeout) + if rlist: + key = sys.stdin.read(1) + else: + key = '' + termios.tcsetattr(sys.stdin, termios.TCSADRAIN, settings) + return key + +def saveTerminalSettings(): + if sys.platform == 'win32': + return None + return termios.tcgetattr(sys.stdin) + +def restoreTerminalSettings(old_settings): + if sys.platform == 'win32': + return + termios.tcsetattr(sys.stdin, termios.TCSADRAIN, old_settings) + +def vels(speed, turn): + return "currently:\tspeed %s\tturn %s " % (speed,turn) + +if __name__=="__main__": + settings = saveTerminalSettings() + + rospy.init_node('teleop_twist_keyboard') + + speed = rospy.get_param("~speed", 0.5) + turn = rospy.get_param("~turn", 1.0) + speed_limit = rospy.get_param("~speed_limit", 1000) + turn_limit = rospy.get_param("~turn_limit", 1000) + repeat = rospy.get_param("~repeat_rate", 0.0) + key_timeout = rospy.get_param("~key_timeout", 0.5) + stamped = rospy.get_param("~stamped", False) + twist_frame = rospy.get_param("~frame_id", '') + if stamped: + TwistMsg = TwistStamped + + pub_thread = PublishThread(repeat) + + x = 0 + y = 0 + z = 0 + th = 0 + status = 0 + + try: + pub_thread.wait_for_subscribers() + pub_thread.update(x, y, z, th, speed, turn) + + print(msg) + print(vels(speed,turn)) + while(1): + key = getKey(settings, key_timeout) + if key in moveBindings.keys(): + x = moveBindings[key][0] + y = moveBindings[key][1] + z = moveBindings[key][2] + th = moveBindings[key][3] + elif key in speedBindings.keys(): + speed = min(speed_limit, speed * speedBindings[key][0]) + turn = min(turn_limit, turn * speedBindings[key][1]) + if speed == speed_limit: + print("Linear speed limit reached!") + if turn == turn_limit: + print("Angular speed limit reached!") + print(vels(speed,turn)) + if (status == 14): + print(msg) + status = (status + 1) % 15 + else: + # Skip updating cmd_vel if key timeout and robot already + # stopped. + if key == '' and x == 0 and y == 0 and z == 0 and th == 0: + continue + x = 0 + y = 0 + z = 0 + th = 0 + if (key == '\x03'): + break + + pub_thread.update(x, y, z, th, speed, turn) + + except Exception as e: + print(e) + + finally: + pub_thread.stop() + restoreTerminalSettings(settings) diff --git a/global_body_planner/include/global_body_planner/global_body_plan.h b/global_body_planner/include/global_body_planner/global_body_plan.h index 2b1884a1b..c7ddf8f84 100644 --- a/global_body_planner/include/global_body_planner/global_body_plan.h +++ b/global_body_planner/include/global_body_planner/global_body_plan.h @@ -112,7 +112,7 @@ class GlobalBodyPlan { * @brief Set the timestamp at which this plan was published * @param[in] timestamp Timestamp at which the plan was published */ - inline double setPublishedTimestamp(ros::Time timestamp) { + inline void setPublishedTimestamp(ros::Time timestamp) { published_timestamp_ = timestamp; } diff --git a/global_body_planner/package.xml b/global_body_planner/package.xml index 12a7f7703..c320e58fe 100644 --- a/global_body_planner/package.xml +++ b/global_body_planner/package.xml @@ -68,6 +68,10 @@ grid_map_core grid_map_ros + tf + tf2 + tf2_geometry_msgs + grid_map diff --git a/global_body_planner/setup_deps.sh b/global_body_planner/setup_deps.sh index 31d9db6dd..b031d9405 100755 --- a/global_body_planner/setup_deps.sh +++ b/global_body_planner/setup_deps.sh @@ -1,4 +1,4 @@ -sudo apt install -y ros-melodic-tf -sudo apt install -y ros-melodic-tf2 -sudo apt install -y ros-melodic-tf2-geometry-msgs -sudo apt install -y ros-melodic-grid-map \ No newline at end of file +sudo apt install -y ros-noetic-tf +sudo apt install -y ros-noetic-tf2 +sudo apt install -y ros-noetic-tf2-geometry-msgs +sudo apt install -y ros-noetic-grid-map diff --git a/global_body_planner/src/planning_utils.cpp b/global_body_planner/src/planning_utils.cpp index bd83d41ea..d5a1a0f49 100644 --- a/global_body_planner/src/planning_utils.cpp +++ b/global_body_planner/src/planning_utils.cpp @@ -204,7 +204,7 @@ void addFullStates(const FullState &start_state, } // Unwrap yaw for filtering - unwrapped_yaw = math_utils::unwrap(wrapped_yaw); + unwrapped_yaw = math_utils::getUnwrappedVector(wrapped_yaw); // Compute pitch and height to align with the terrain, add first order filter // on init state diff --git a/local_planner/CMakeLists.txt b/local_planner/CMakeLists.txt index dac5ea825..a5d0850c7 100644 --- a/local_planner/CMakeLists.txt +++ b/local_planner/CMakeLists.txt @@ -28,11 +28,6 @@ find_package(Eigen3 REQUIRED) # find_package(OsqpEigen REQUIRED) find_package(PythonLibs 2.7) -set(QPOASES_DIR "${CMAKE_CURRENT_SOURCE_DIR}/../external/qpOASES") -set(QPOASES_INCLUDE_DIRS "${QPOASES_DIR}/include") -set(QPOASES_LIBRARIES "${QPOASES_DIR}/build/libs") -find_library(QPOASES NAMES qpOASES PATHS ${QPOASES_LIBRARIES} REQUIRED) - catkin_package( INCLUDE_DIRS include CATKIN_DEPENDS roscpp std_msgs quad_msgs quad_utils grid_map_core grid_map_ros eigen_conversions nmpc_controller @@ -50,7 +45,6 @@ include_directories( include ${catkin_INCLUDE_DIRS} ${EIGEN3_INCLUDE_DIRS} - ${QPOASES_INCLUDE_DIRS} ${PYTHON_INCLUDE_DIRS} /usr/local/include/coin-or ) diff --git a/local_planner/include/local_planner/local_planner.h b/local_planner/include/local_planner/local_planner.h index 3f6c8817d..8fd881818 100644 --- a/local_planner/include/local_planner/local_planner.h +++ b/local_planner/include/local_planner/local_planner.h @@ -79,6 +79,11 @@ class LocalPlanner { */ void getReference(); + /** + * @brief Unwrap the yaw signal in the reference body plan + */ + void unwrapYawReference(); + /** * @brief Function to compute the local plan * @return Boolean if local plan was found successfully diff --git a/local_planner/local_planner.yaml b/local_planner/local_planner.yaml index 29408d3e4..15e56d73a 100644 --- a/local_planner/local_planner.yaml +++ b/local_planner/local_planner.yaml @@ -1,7 +1,7 @@ local_planner: update_rate: 333 # Hz timestep: 0.03 # s - horizon_length: 26 # timesteps + horizon_length: 26 # timesteps # Try Increasing to 32 desired_height: 0.27 # m toe_radius: 0.02 # m cmd_vel_filter_const: 0.10 @@ -13,7 +13,7 @@ local_planner: local_footstep_planner: grf_weight: 0.5 - period: 0.36 # s + period: 0.54 # s ground_clearance: 0.07 standing_error_threshold: 0.03 # m foothold_obj_threshold: 0.6 # Note: For now must match /grid_map_filters/traversability_mask_lower_threshold/lower_threshold diff --git a/local_planner/setup_deps.sh b/local_planner/setup_deps.sh index 961455b6b..f7ef0248c 100755 --- a/local_planner/setup_deps.sh +++ b/local_planner/setup_deps.sh @@ -1 +1 @@ -sudo apt install -y ros-melodic-grid-map \ No newline at end of file +sudo apt install -y ros-noetic-grid-map diff --git a/local_planner/src/.local_planner.cpp.swp b/local_planner/src/.local_planner.cpp.swp new file mode 100644 index 000000000..06b3bb187 Binary files /dev/null and b/local_planner/src/.local_planner.cpp.swp differ diff --git a/local_planner/src/local_footstep_planner.cpp b/local_planner/src/local_footstep_planner.cpp index 5558c3028..b50398425 100644 --- a/local_planner/src/local_footstep_planner.cpp +++ b/local_planner/src/local_footstep_planner.cpp @@ -107,19 +107,17 @@ void LocalFootstepPlanner::computeContactSchedule( for (int i = 0; i < horizon_length_; i++) { // Leaping and landing if (ref_primitive_plan(i) == LEAP_STANCE) { - contact_schedule.at(i) = {true, true, true, true}; - } else if (ref_primitive_plan(i) == FLIGHT) { - // Flight, check that min landing height is exceeded - double min_landing_height = 0.3; - double current_height = - body_plan(i, 2) - terrain_grid_.atPosition( - "z_inpainted", body_plan.row(i).head<2>(), - grid_map::InterpolationMethods::INTER_NEAREST); - if (current_height < min_landing_height && body_plan(i, 8) < 0) { - contact_schedule.at(i) = {true, true, true, true}; + int leading_leg_liftoff_idx = std::min(i, horizon_length_ - 1); + + if (ref_primitive_plan(leading_leg_liftoff_idx) == FLIGHT) { + contact_schedule.at(i) = {false, true, false, true}; } else { - contact_schedule.at(i) = {false, false, false, false}; + contact_schedule.at(i) = {true, true, true, true}; } + } else if (ref_primitive_plan(i) == FLIGHT) { + // Flight + std::fill(contact_schedule.at(i).begin(), contact_schedule.at(i).end(), + false); } else if (ref_primitive_plan(i) == LAND_STANCE) { contact_schedule.at(i) = {true, true, true, true}; } diff --git a/local_planner/src/local_planner.cpp b/local_planner/src/local_planner.cpp index 98116e246..eb8cc1b4e 100644 --- a/local_planner/src/local_planner.cpp +++ b/local_planner/src/local_planner.cpp @@ -4,114 +4,117 @@ Eigen::IOFormat CleanFmt(4, 0, ", ", "\n", "[", "]"); LocalPlanner::LocalPlanner(ros::NodeHandle nh) : local_body_planner_nonlinear_(), local_footstep_planner_() { - nh_ = nh; - - // Load rosparams from parameter server - std::string terrain_map_topic, body_plan_topic, robot_state_topic, - local_plan_topic, foot_plan_discrete_topic, foot_plan_continuous_topic, - cmd_vel_topic, control_mode_topic; - - // Load system parameters from launch file (not in config file) - quad_utils::loadROSParamDefault(nh_, "robot_type", robot_name_, - std::string("spirit")); - quad_utils::loadROSParam(nh_, "/topics/terrain_map", terrain_map_topic); - quad_utils::loadROSParam(nh_, "topics/global_plan", body_plan_topic); - quad_utils::loadROSParam(nh_, "topics/state/ground_truth", robot_state_topic); - quad_utils::loadROSParam(nh_, "topics/local_plan", local_plan_topic); - quad_utils::loadROSParam(nh_, "topics/foot_plan_discrete", - foot_plan_discrete_topic); - quad_utils::loadROSParam(nh_, "topics/foot_plan_continuous", - foot_plan_continuous_topic); - quad_utils::loadROSParam(nh_, "topics/cmd_vel", cmd_vel_topic); - quad_utils::loadROSParam(nh_, "/map_frame", map_frame_); - quad_utils::loadROSParam(nh_, "topics/control/mode", control_mode_topic); - - // Setup pubs and subs - terrain_map_sub_ = nh_.subscribe(terrain_map_topic, 1, - &LocalPlanner::terrainMapCallback, this); - body_plan_sub_ = - nh_.subscribe(body_plan_topic, 1, &LocalPlanner::robotPlanCallback, this); - robot_state_sub_ = - nh_.subscribe(robot_state_topic, 1, &LocalPlanner::robotStateCallback, - this, ros::TransportHints().tcpNoDelay(true)); - cmd_vel_sub_ = - nh_.subscribe(cmd_vel_topic, 1, &LocalPlanner::cmdVelCallback, this); - - local_plan_pub_ = nh_.advertise(local_plan_topic, 1); - foot_plan_discrete_pub_ = nh_.advertise( - foot_plan_discrete_topic, 1); - foot_plan_continuous_pub_ = nh_.advertise( - foot_plan_continuous_topic, 1); - - // Load system parameters from parameter server - quad_utils::loadROSParam(nh_, "/local_planner/update_rate", update_rate_); - quad_utils::loadROSParam(nh_, "/local_planner/timestep", dt_); - quad_utils::loadROSParam(nh_, "/local_planner/horizon_length", N_); - quad_utils::loadROSParam(nh_, "/local_planner/desired_height", z_des_); - quad_utils::loadROSParam(nh_, "/local_planner/toe_radius", toe_radius_); - quad_utils::loadROSParam(nh_, "/local_planner/cmd_vel_scale", cmd_vel_scale_); - quad_utils::loadROSParam(nh_, "/local_planner/last_cmd_vel_msg_time_max", - last_cmd_vel_msg_time_max_); - quad_utils::loadROSParam(nh_, "/local_planner/cmd_vel_filter_const", - cmd_vel_filter_const_); - quad_utils::loadROSParam(nh_, "/local_planner/stand_vel_threshold", - stand_vel_threshold_); - quad_utils::loadROSParam(nh_, "/local_planner/stand_cmd_vel_threshold", - stand_cmd_vel_threshold_); - quad_utils::loadROSParam(nh_, "/local_planner/stand_pos_error_threshold", - stand_pos_error_threshold_); - - // Load system parameters from launch file (not in config file) - nh.param("local_planner/use_twist_input", use_twist_input_, false); - - // Convert kinematics - quadKD_ = std::make_shared(); - - // Initialize body and foot position arrays (grf_plan horizon is one index - // shorter since control after last state is not in the horizon) - ref_body_plan_ = Eigen::MatrixXd::Zero(N_, Nx_); - foot_positions_world_ = Eigen::MatrixXd::Zero(N_, num_feet_ * 3); - foot_velocities_world_ = Eigen::MatrixXd::Zero(N_, num_feet_ * 3); - foot_accelerations_world_ = Eigen::MatrixXd::Zero(N_, num_feet_ * 3); - foot_positions_body_ = Eigen::MatrixXd::Zero(N_, num_feet_ * 3); - current_foot_positions_body_ = Eigen::VectorXd::Zero(num_feet_ * 3); - current_foot_positions_world_ = Eigen::VectorXd::Zero(num_feet_ * 3); - current_foot_velocities_world_ = Eigen::VectorXd::Zero(num_feet_ * 3); - ref_primitive_plan_ = Eigen::VectorXi::Zero(N_); - ref_ground_height_ = Eigen::VectorXd::Zero(N_); - grf_plan_ = Eigen::MatrixXd::Zero(N_ - 1, 12); - for (int i = 0; i < num_feet_; i++) { - grf_plan_.col(3 * i + 2).fill(13.3 * 9.81 / num_feet_); - } + nh_ = nh; + + // Load rosparams from parameter server + std::string terrain_map_topic, body_plan_topic, robot_state_topic, + local_plan_topic, foot_plan_discrete_topic, foot_plan_continuous_topic, + cmd_vel_topic, control_mode_topic; + + // Load system parameters from launch file (not in config file) + quad_utils::loadROSParamDefault(nh_, "robot_type", robot_name_, + std::string("spirit")); + quad_utils::loadROSParam(nh_, "/topics/terrain_map", terrain_map_topic); + quad_utils::loadROSParam(nh_, "topics/global_plan", body_plan_topic); + quad_utils::loadROSParam(nh_, "topics/state/ground_truth", + robot_state_topic); + quad_utils::loadROSParam(nh_, "topics/local_plan", local_plan_topic); + quad_utils::loadROSParam(nh_, "topics/foot_plan_discrete", + foot_plan_discrete_topic); + quad_utils::loadROSParam(nh_, "topics/foot_plan_continuous", + foot_plan_continuous_topic); + quad_utils::loadROSParam(nh_, "topics/cmd_vel", cmd_vel_topic); + quad_utils::loadROSParam(nh_, "/map_frame", map_frame_); + quad_utils::loadROSParam(nh_, "topics/control/mode", control_mode_topic); + + // Setup pubs and subs + terrain_map_sub_ = nh_.subscribe(terrain_map_topic, 1, + &LocalPlanner::terrainMapCallback, this); + body_plan_sub_ = nh_.subscribe(body_plan_topic, 1, + &LocalPlanner::robotPlanCallback, this); + robot_state_sub_ = + nh_.subscribe(robot_state_topic, 1, &LocalPlanner::robotStateCallback, + this, ros::TransportHints().tcpNoDelay(true)); + cmd_vel_sub_ = + nh_.subscribe(cmd_vel_topic, 1, &LocalPlanner::cmdVelCallback, this); + + local_plan_pub_ = nh_.advertise(local_plan_topic, 1); + foot_plan_discrete_pub_ = nh_.advertise( + foot_plan_discrete_topic, 1); + foot_plan_continuous_pub_ = + nh_.advertise( + foot_plan_continuous_topic, 1); + + // Load system parameters from parameter server + quad_utils::loadROSParam(nh_, "/local_planner/update_rate", update_rate_); + quad_utils::loadROSParam(nh_, "/local_planner/timestep", dt_); + quad_utils::loadROSParam(nh_, "/local_planner/horizon_length", N_); + quad_utils::loadROSParam(nh_, "/local_planner/desired_height", z_des_); + quad_utils::loadROSParam(nh_, "/local_planner/toe_radius", toe_radius_); + quad_utils::loadROSParam(nh_, "/local_planner/cmd_vel_scale", + cmd_vel_scale_); + quad_utils::loadROSParam(nh_, "/local_planner/last_cmd_vel_msg_time_max", + last_cmd_vel_msg_time_max_); + quad_utils::loadROSParam(nh_, "/local_planner/cmd_vel_filter_const", + cmd_vel_filter_const_); + quad_utils::loadROSParam(nh_, "/local_planner/stand_vel_threshold", + stand_vel_threshold_); + quad_utils::loadROSParam(nh_, "/local_planner/stand_cmd_vel_threshold", + stand_cmd_vel_threshold_); + quad_utils::loadROSParam(nh_, "/local_planner/stand_pos_error_threshold", + stand_pos_error_threshold_); + + // Load system parameters from launch file (not in config file) + nh.param("local_planner/use_twist_input", use_twist_input_, false); + + // Convert kinematics + quadKD_ = std::make_shared(); + + // Initialize body and foot position arrays (grf_plan horizon is one index + // shorter since control after last state is not in the horizon) + ref_body_plan_ = Eigen::MatrixXd::Zero(N_, Nx_); + foot_positions_world_ = Eigen::MatrixXd::Zero(N_, num_feet_ * 3); + foot_velocities_world_ = Eigen::MatrixXd::Zero(N_, num_feet_ * 3); + foot_accelerations_world_ = Eigen::MatrixXd::Zero(N_, num_feet_ * 3); + foot_positions_body_ = Eigen::MatrixXd::Zero(N_, num_feet_ * 3); + current_foot_positions_body_ = Eigen::VectorXd::Zero(num_feet_ * 3); + current_foot_positions_world_ = Eigen::VectorXd::Zero(num_feet_ * 3); + current_foot_velocities_world_ = Eigen::VectorXd::Zero(num_feet_ * 3); + ref_primitive_plan_ = Eigen::VectorXi::Zero(N_); + ref_ground_height_ = Eigen::VectorXd::Zero(N_); + grf_plan_ = Eigen::MatrixXd::Zero(N_ - 1, 12); + for (int i = 0; i < num_feet_; i++) { + grf_plan_.col(3 * i + 2).fill(13.3 * 9.81 / num_feet_); + } - // Initialize body and footstep planners - initLocalBodyPlanner(); - initLocalFootstepPlanner(); + // Initialize body and footstep planners + initLocalBodyPlanner(); + initLocalFootstepPlanner(); - // Initialize twist input variables - cmd_vel_.resize(6); - cmd_vel_.setZero(); - initial_timestamp_ = ros::Time::now(); - first_plan_ = true; + // Initialize twist input variables + cmd_vel_.resize(6); + cmd_vel_.setZero(); + initial_timestamp_ = ros::Time::now(); + first_plan_ = true; - // Initialize stand pose - stand_pose_.fill(std::numeric_limits::max()); - control_mode_ = STAND; + // Initialize stand pose + stand_pose_.fill(std::numeric_limits::max()); + control_mode_ = STAND; - // Initialize the time duration to the next plan index - first_element_duration_ = dt_; + // Initialize the time duration to the next plan index + first_element_duration_ = dt_; - // Initialize the plan index diff - plan_index_diff_ = 0; + // Initialize the plan index diff + plan_index_diff_ = 0; - // Initialize the plan index - current_plan_index_ = 0; + // Initialize the plan index + current_plan_index_ = 0; } void LocalPlanner::initLocalBodyPlanner() { // Create nmpc wrapper class SystemID type; - if (robot_name_ == "spirit") { + if (robot_name_ == "spirit" || robot_name_ == "spirit_rotors") { type = SPIRIT; } else if (robot_name_ == "a1") { type = A1; @@ -122,475 +125,506 @@ void LocalPlanner::initLocalBodyPlanner() { } void LocalPlanner::initLocalFootstepPlanner() { - // Load parameters from server - double grf_weight, ground_clearance, hip_clearance, standing_error_threshold, - period_d, foothold_search_radius, foothold_obj_threshold; - std::string obj_fun_layer; - int period; - std::vector duty_cycles, phase_offsets; - quad_utils::loadROSParam(nh_, "/local_footstep_planner/grf_weight", - grf_weight); - quad_utils::loadROSParam(nh_, "/local_footstep_planner/ground_clearance", - ground_clearance); - quad_utils::loadROSParam(nh_, "local_footstep_planner/hip_clearance", - hip_clearance); - quad_utils::loadROSParam(nh_, - "/local_footstep_planner/standing_error_threshold", - standing_error_threshold); - quad_utils::loadROSParam(nh_, "local_footstep_planner/foothold_search_radius", - foothold_search_radius); - quad_utils::loadROSParam(nh_, - "/local_footstep_planner/foothold_obj_threshold", - foothold_obj_threshold); - quad_utils::loadROSParam(nh_, "/local_footstep_planner/obj_fun_layer", - obj_fun_layer); - quad_utils::loadROSParam(nh_, "/local_footstep_planner/period", period_d); - quad_utils::loadROSParam(nh_, "/local_footstep_planner/duty_cycles", - duty_cycles); - quad_utils::loadROSParam(nh_, "/local_footstep_planner/phase_offsets", - phase_offsets); - - period = period_d / dt_; - - // Confirm grf weight is valid - if (grf_weight > 1 || grf_weight < 0) { - grf_weight = std::min(std::max(grf_weight, 0.0), 1.0); - ROS_WARN("Invalid grf weight, clamping to %4.2f", grf_weight); - } + // Load parameters from server + double grf_weight, ground_clearance, hip_clearance, + standing_error_threshold, period_d, foothold_search_radius, + foothold_obj_threshold; + std::string obj_fun_layer; + int period; + std::vector duty_cycles, phase_offsets; + quad_utils::loadROSParam(nh_, "/local_footstep_planner/grf_weight", + grf_weight); + quad_utils::loadROSParam(nh_, "/local_footstep_planner/ground_clearance", + ground_clearance); + quad_utils::loadROSParam(nh_, "local_footstep_planner/hip_clearance", + hip_clearance); + quad_utils::loadROSParam(nh_, + "/local_footstep_planner/standing_error_threshold", + standing_error_threshold); + quad_utils::loadROSParam(nh_, + "local_footstep_planner/foothold_search_radius", + foothold_search_radius); + quad_utils::loadROSParam(nh_, + "/local_footstep_planner/foothold_obj_threshold", + foothold_obj_threshold); + quad_utils::loadROSParam(nh_, "/local_footstep_planner/obj_fun_layer", + obj_fun_layer); + quad_utils::loadROSParam(nh_, "/local_footstep_planner/period", period_d); + quad_utils::loadROSParam(nh_, "/local_footstep_planner/duty_cycles", + duty_cycles); + quad_utils::loadROSParam(nh_, "/local_footstep_planner/phase_offsets", + phase_offsets); + + period = period_d / dt_; + + // Confirm grf weight is valid + if (grf_weight > 1 || grf_weight < 0) { + grf_weight = std::min(std::max(grf_weight, 0.0), 1.0); + ROS_WARN("Invalid grf weight, clamping to %4.2f", grf_weight); + } - // Create footstep class, make sure we use the same dt as the local planner - local_footstep_planner_ = std::make_shared(); - local_footstep_planner_->setTemporalParams(dt_, period, N_, duty_cycles, - phase_offsets); - local_footstep_planner_->setSpatialParams( - ground_clearance, hip_clearance, standing_error_threshold, grf_weight, - quadKD_, foothold_search_radius, foothold_obj_threshold, obj_fun_layer, - toe_radius_); + // Create footstep class, make sure we use the same dt as the local planner + local_footstep_planner_ = std::make_shared(); + local_footstep_planner_->setTemporalParams(dt_, period, N_, duty_cycles, + phase_offsets); + local_footstep_planner_->setSpatialParams( + ground_clearance, hip_clearance, standing_error_threshold, grf_weight, + quadKD_, foothold_search_radius, foothold_obj_threshold, obj_fun_layer, + toe_radius_); - past_footholds_msg_.feet.resize(num_feet_); + past_footholds_msg_.feet.resize(num_feet_); } void LocalPlanner::terrainMapCallback( const grid_map_msgs::GridMap::ConstPtr &msg) { - grid_map::GridMapRosConverter::fromMessage(*msg, terrain_grid_); + grid_map::GridMapRosConverter::fromMessage(*msg, terrain_grid_); - // Convert to FastTerrainMap structure for faster querying - terrain_.loadDataFromGridMap(terrain_grid_); - local_footstep_planner_->updateMap(terrain_); - local_footstep_planner_->updateMap(terrain_grid_); + // Convert to FastTerrainMap structure for faster querying + terrain_.loadDataFromGridMap(terrain_grid_); + local_footstep_planner_->updateMap(terrain_); + local_footstep_planner_->updateMap(terrain_grid_); } void LocalPlanner::robotPlanCallback( const quad_msgs::RobotPlan::ConstPtr &msg) { - body_plan_msg_ = msg; + body_plan_msg_ = msg; } void LocalPlanner::robotStateCallback( const quad_msgs::RobotState::ConstPtr &msg) { - // Make sure the data is actually populated - if (msg->feet.feet.empty() || msg->joints.position.empty()) return; + // Make sure the data is actually populated + if (msg->feet.feet.empty() || msg->joints.position.empty()) return; - robot_state_msg_ = msg; + robot_state_msg_ = msg; } void LocalPlanner::cmdVelCallback(const geometry_msgs::Twist::ConstPtr &msg) { - // Ignore non-planar components of desired twist - cmd_vel_[0] = (1 - cmd_vel_filter_const_) * cmd_vel_[0] + - cmd_vel_filter_const_ * cmd_vel_scale_ * msg->linear.x; - cmd_vel_[1] = (1 - cmd_vel_filter_const_) * cmd_vel_[1] + - cmd_vel_filter_const_ * cmd_vel_scale_ * msg->linear.y; - cmd_vel_[2] = 0; - cmd_vel_[3] = 0; - cmd_vel_[4] = 0; - cmd_vel_[5] = (1 - cmd_vel_filter_const_) * cmd_vel_[5] + - cmd_vel_filter_const_ * cmd_vel_scale_ * msg->angular.z; - - // Record when this was last reached for safety - last_cmd_vel_msg_time_ = ros::Time::now(); + // Ignore non-planar components of desired twist + cmd_vel_[0] = (1 - cmd_vel_filter_const_) * cmd_vel_[0] + + cmd_vel_filter_const_ * cmd_vel_scale_ * msg->linear.x; + cmd_vel_[1] = (1 - cmd_vel_filter_const_) * cmd_vel_[1] + + cmd_vel_filter_const_ * cmd_vel_scale_ * msg->linear.y; + cmd_vel_[2] = 0; + cmd_vel_[3] = 0; + cmd_vel_[4] = 0; + cmd_vel_[5] = (1 - cmd_vel_filter_const_) * cmd_vel_[5] + + cmd_vel_filter_const_ * cmd_vel_scale_ * msg->angular.z; + + // Record when this was last reached for safety + last_cmd_vel_msg_time_ = ros::Time::now(); } void LocalPlanner::getReference() { - if (first_plan_) { - first_plan_ = false; - past_footholds_msg_ = robot_state_msg_->feet; - past_footholds_msg_.traj_index = current_plan_index_; - for (int i = 0; i < num_feet_; i++) { - past_footholds_msg_.feet[i].header = past_footholds_msg_.header; - past_footholds_msg_.feet[i].traj_index = past_footholds_msg_.traj_index; - } + if (first_plan_) { + first_plan_ = false; + past_footholds_msg_ = robot_state_msg_->feet; + past_footholds_msg_.traj_index = current_plan_index_; + for (int i = 0; i < num_feet_; i++) { + past_footholds_msg_.feet[i].header = past_footholds_msg_.header; + past_footholds_msg_.feet[i].traj_index = + past_footholds_msg_.traj_index; + } - // We want to start from a full period when using twist input - if (use_twist_input_) { - initial_timestamp_ = ros::Time::now() - ros::Duration(1e-6); + // We want to start from a full period when using twist input + if (use_twist_input_) { + initial_timestamp_ = ros::Time::now() - ros::Duration(1e-6); + } } - } - // Make sure we use the most recent global plan timestamp for reference - if (!use_twist_input_) { - initial_timestamp_ = body_plan_msg_->global_plan_timestamp; - } - - // Tracking trajectory so enter run mode - control_mode_ = STEP; - - // Get plan index, compare with the previous one to check if this is a - // duplicated solve - int previous_plan_index = current_plan_index_; - quad_utils::getPlanIndex(initial_timestamp_, dt_, current_plan_index_, - first_element_duration_); - plan_index_diff_ = current_plan_index_ - previous_plan_index; - - // Get the current body and foot positions into Eigen - current_state_ = quad_utils::bodyStateMsgToEigen(robot_state_msg_->body); - current_state_timestamp_ = robot_state_msg_->header.stamp; - quad_utils::multiFootStateMsgToEigen(robot_state_msg_->feet, - current_foot_positions_world_, - current_foot_velocities_world_); - local_footstep_planner_->getFootPositionsBodyFrame( - current_state_, current_foot_positions_world_, - current_foot_positions_body_); - - // Grab the appropriate states from the body plan and convert to an Eigen - // matrix - ref_body_plan_.setZero(); - ref_primitive_plan_.setZero(); - - if (use_twist_input_) { - // Use twist planner - // Check that we have recent twist data, otherwise set cmd_vel to zero - ros::Duration time_elapsed_since_msg = - ros::Time::now() - last_cmd_vel_msg_time_; - if (time_elapsed_since_msg.toSec() > last_cmd_vel_msg_time_max_) { - cmd_vel_.setZero(); - ROS_WARN_THROTTLE(1.0, "No cmd_vel data, setting twist cmd_vel to zero"); + // Make sure we use the most recent global plan timestamp for reference + if (!use_twist_input_) { + initial_timestamp_ = body_plan_msg_->global_plan_timestamp; } - // Set initial ground height - ref_ground_height_(0) = local_footstep_planner_->getTerrainHeight( - current_state_(0), current_state_(1)); + // Tracking trajectory so enter run mode + control_mode_ = STEP; + + // Get plan index, compare with the previous one to check if this is a + // duplicated solve + int previous_plan_index = current_plan_index_; + quad_utils::getPlanIndex(initial_timestamp_, dt_, current_plan_index_, + first_element_duration_); + plan_index_diff_ = current_plan_index_ - previous_plan_index; + + // Get the current body and foot positions into Eigen + current_state_ = quad_utils::bodyStateMsgToEigen(robot_state_msg_->body); + current_state_timestamp_ = robot_state_msg_->header.stamp; + quad_utils::multiFootStateMsgToEigen(robot_state_msg_->feet, + current_foot_positions_world_, + current_foot_velocities_world_); + local_footstep_planner_->getFootPositionsBodyFrame( + current_state_, current_foot_positions_world_, + current_foot_positions_body_); + + // Grab the appropriate states from the body plan and convert to an Eigen + // matrix + ref_body_plan_.setZero(); + ref_primitive_plan_.setZero(); - // If it's not initialized, set to current positions - if (stand_pose_(0) == std::numeric_limits::max() && - stand_pose_(1) == std::numeric_limits::max() && - stand_pose_(2) == std::numeric_limits::max()) { - stand_pose_ << current_state_[0], current_state_[1], current_state_[5]; - } + if (use_twist_input_) { + // Use twist planner + // Check that we have recent twist data, otherwise set cmd_vel to zero + ros::Duration time_elapsed_since_msg = + ros::Time::now() - last_cmd_vel_msg_time_; + if (time_elapsed_since_msg.toSec() > last_cmd_vel_msg_time_max_) { + cmd_vel_.setZero(); + ROS_WARN_THROTTLE(1.0, + "No cmd_vel data, setting twist cmd_vel to zero"); + } - // Set initial condition for forward integration - Eigen::Vector2d support_center; - support_center.setZero(); - for (int i = 0; i < num_feet_; i++) { - support_center.x() += - robot_state_msg_->feet.feet[i].position.x / ((double)num_feet_); - support_center.y() += - robot_state_msg_->feet.feet[i].position.y / ((double)num_feet_); - } + // Set initial ground height + ref_ground_height_(0) = local_footstep_planner_->getTerrainHeight( + current_state_(0), current_state_(1)); - // Step if velocity commanded, current velocity exceeds threshold, or too - // far from center of support - bool is_stepping = - (cmd_vel_.norm() > stand_cmd_vel_threshold_ || - current_state_.segment(6, 2).norm() > stand_vel_threshold_ || - (support_center - current_state_.segment(0, 2)).norm() > - stand_pos_error_threshold_); - - if (is_stepping) { - control_mode_ = STEP; - stand_pose_ << current_state_[0], current_state_[1], current_state_[5]; - } else { - // If it's standing, try to stablized the waggling - control_mode_ = STAND; - Eigen::Vector3d current_stand_pose; - current_stand_pose << support_center[0], support_center[1], - current_state_[5]; - stand_pose_ = stand_pose_ * (1 - 1 / update_rate_) + - current_stand_pose * 1 / update_rate_; - } + // If it's not initialized, set to current positions + if (stand_pose_(0) == std::numeric_limits::max() && + stand_pose_(1) == std::numeric_limits::max() && + stand_pose_(2) == std::numeric_limits::max()) { + stand_pose_ << current_state_[0], current_state_[1], + current_state_[5]; + } - ref_body_plan_(0, 0) = stand_pose_[0]; // support_center.x(); - ref_body_plan_(0, 1) = stand_pose_[1]; // support_center.x(); - ref_body_plan_(0, 2) = z_des_ + ref_ground_height_(0); - ref_body_plan_(0, 3) = 0; - ref_body_plan_(0, 4) = 0; - ref_body_plan_(0, 5) = stand_pose_[2]; - ref_body_plan_(0, 6) = cmd_vel_[0] * cos(current_state_[5]) - - cmd_vel_[1] * sin(current_state_[5]); - ref_body_plan_(0, 7) = cmd_vel_[0] * sin(current_state_[5]) + - cmd_vel_[1] * cos(current_state_[5]); - ref_body_plan_(0, 8) = cmd_vel_[2]; - ref_body_plan_(0, 9) = cmd_vel_[3]; - ref_body_plan_(0, 10) = cmd_vel_[4]; - ref_body_plan_(0, 11) = cmd_vel_[5]; - - // Alternatively only adaptive pitch - // ref_body_plan_(0, 4) = local_footstep_planner_->getTerrainSlope( - // current_state_(0), current_state_(1), current_state_(6), - // current_state_(7)); - - // Adaptive roll and pitch - local_footstep_planner_->getTerrainSlope( - ref_body_plan_(0, 0), ref_body_plan_(0, 1), ref_body_plan_(0, 5), - ref_body_plan_(0, 3), ref_body_plan_(0, 4)); - - // Integrate to get full body plan (Forward Euler) - for (int i = 1; i < N_; i++) { - Eigen::VectorXd current_cmd_vel = cmd_vel_; - - double yaw = ref_body_plan_(i - 1, 5); - current_cmd_vel[0] = cmd_vel_[0] * cos(yaw) - cmd_vel_[1] * sin(yaw); - current_cmd_vel[1] = cmd_vel_[0] * sin(yaw) + cmd_vel_[1] * cos(yaw); - - for (int j = 0; j < 6; j++) { - if (i == 1) { - ref_body_plan_(i, j) = ref_body_plan_(i - 1, j) + - current_cmd_vel[j] * first_element_duration_; + // Set initial condition for forward integration + Eigen::Vector2d support_center; + support_center.setZero(); + for (int i = 0; i < num_feet_; i++) { + support_center.x() += + robot_state_msg_->feet.feet[i].position.x / ((double)num_feet_); + support_center.y() += + robot_state_msg_->feet.feet[i].position.y / ((double)num_feet_); + } + + // Step if velocity commanded, current velocity exceeds threshold, or + // too far from center of support + bool is_stepping = + (cmd_vel_.norm() > stand_cmd_vel_threshold_ || + current_state_.segment(6, 2).norm() > stand_vel_threshold_ || + (support_center - current_state_.segment(0, 2)).norm() > + stand_pos_error_threshold_); + + if (is_stepping) { + control_mode_ = STEP; + stand_pose_ << current_state_[0], current_state_[1], + current_state_[5]; } else { - ref_body_plan_(i, j) = - ref_body_plan_(i - 1, j) + current_cmd_vel[j] * dt_; + // If it's standing, try to stablized the waggling + control_mode_ = STAND; + Eigen::Vector3d current_stand_pose; + current_stand_pose << support_center[0], support_center[1], + current_state_[5]; + stand_pose_ = stand_pose_ * (1 - 1 / update_rate_) + + current_stand_pose * 1 / update_rate_; } - ref_body_plan_(i, j + 6) = (current_cmd_vel[j]); - } - - ref_ground_height_(i) = local_footstep_planner_->getTerrainHeight( - ref_body_plan_(i, 0), ref_body_plan_(i, 1)); - ref_body_plan_(i, 2) = z_des_ + ref_ground_height_(i); - - // Alternatively only adaptive pitch - // ref_body_plan_(i, 4) = local_footstep_planner_->getTerrainSlope( - // ref_body_plan_(i, 0), ref_body_plan_(i, 1), ref_body_plan_(i, 6), - // ref_body_plan_(i, 7)); - - // Adaptive roll and pitch - local_footstep_planner_->getTerrainSlope( - ref_body_plan_(i, 0), ref_body_plan_(i, 1), ref_body_plan_(i, 5), - ref_body_plan_(i, 3), ref_body_plan_(i, 4)); - } - } else { - // Use global plan - for (int i = 0; i < N_; i++) { - // If the horizon extends past the reference trajectory, just hold the - // last state - if (i + current_plan_index_ > body_plan_msg_->plan_indices.back()) { - ref_body_plan_.row(i) = - quad_utils::bodyStateMsgToEigen(body_plan_msg_->states.back().body); - if (i < N_) { - ref_primitive_plan_(i) = body_plan_msg_->primitive_ids.back(); + + ref_body_plan_(0, 0) = stand_pose_[0]; // support_center.x(); + ref_body_plan_(0, 1) = stand_pose_[1]; // support_center.x(); + ref_body_plan_(0, 2) = z_des_ + ref_ground_height_(0); + ref_body_plan_(0, 3) = 0; + ref_body_plan_(0, 4) = 0; + ref_body_plan_(0, 5) = stand_pose_[2]; + ref_body_plan_(0, 6) = cmd_vel_[0] * cos(current_state_[5]) - + cmd_vel_[1] * sin(current_state_[5]); + ref_body_plan_(0, 7) = cmd_vel_[0] * sin(current_state_[5]) + + cmd_vel_[1] * cos(current_state_[5]); + ref_body_plan_(0, 8) = cmd_vel_[2]; + ref_body_plan_(0, 9) = cmd_vel_[3]; + ref_body_plan_(0, 10) = cmd_vel_[4]; + ref_body_plan_(0, 11) = cmd_vel_[5]; + + // Alternatively only adaptive pitch + // ref_body_plan_(0, 4) = local_footstep_planner_->getTerrainSlope( + // current_state_(0), current_state_(1), current_state_(6), + // current_state_(7)); + + // Adaptive roll and pitch + local_footstep_planner_->getTerrainSlope( + ref_body_plan_(0, 0), ref_body_plan_(0, 1), ref_body_plan_(0, 5), + ref_body_plan_(0, 3), ref_body_plan_(0, 4)); + + // Integrate to get full body plan (Forward Euler) + for (int i = 1; i < N_; i++) { + Eigen::VectorXd current_cmd_vel = cmd_vel_; + + double yaw = ref_body_plan_(i - 1, 5); + current_cmd_vel[0] = + cmd_vel_[0] * cos(yaw) - cmd_vel_[1] * sin(yaw); + current_cmd_vel[1] = + cmd_vel_[0] * sin(yaw) + cmd_vel_[1] * cos(yaw); + + for (int j = 0; j < 6; j++) { + if (i == 1) { + ref_body_plan_(i, j) = + ref_body_plan_(i - 1, j) + + current_cmd_vel[j] * first_element_duration_; + } else { + ref_body_plan_(i, j) = + ref_body_plan_(i - 1, j) + current_cmd_vel[j] * dt_; + } + ref_body_plan_(i, j + 6) = (current_cmd_vel[j]); + } + + ref_ground_height_(i) = local_footstep_planner_->getTerrainHeight( + ref_body_plan_(i, 0), ref_body_plan_(i, 1)); + ref_body_plan_(i, 2) = z_des_ + ref_ground_height_(i); + + // Alternatively only adaptive pitch + // ref_body_plan_(i, 4) = local_footstep_planner_->getTerrainSlope( + // ref_body_plan_(i, 0), ref_body_plan_(i, 1), ref_body_plan_(i, + // 6), ref_body_plan_(i, 7)); + + // Adaptive roll and pitch + local_footstep_planner_->getTerrainSlope( + ref_body_plan_(i, 0), ref_body_plan_(i, 1), + ref_body_plan_(i, 5), ref_body_plan_(i, 3), + ref_body_plan_(i, 4)); + } + } else { + // Use global plan + for (int i = 0; i < N_; i++) { + // If the horizon extends past the reference trajectory, just hold + // the last state + if (i + current_plan_index_ > body_plan_msg_->plan_indices.back()) { + ref_body_plan_.row(i) = quad_utils::bodyStateMsgToEigen( + body_plan_msg_->states.back().body); + if (i < N_) { + ref_primitive_plan_(i) = + body_plan_msg_->primitive_ids.back(); + } + } else { + ref_body_plan_.row(i) = quad_utils::bodyStateMsgToEigen( + body_plan_msg_->states[i + current_plan_index_].body); + if (i < N_) { + ref_primitive_plan_(i) = + body_plan_msg_->primitive_ids[i + current_plan_index_]; + } + } + ref_ground_height_(i) = local_footstep_planner_->getTerrainHeight( + ref_body_plan_(i, 0), ref_body_plan_(i, 1)); } - } else { - ref_body_plan_.row(i) = quad_utils::bodyStateMsgToEigen( - body_plan_msg_->states[i + current_plan_index_].body); - if (i < N_) { - ref_primitive_plan_(i) = - body_plan_msg_->primitive_ids[i + current_plan_index_]; + ref_ground_height_(0) = local_footstep_planner_->getTerrainHeight( + current_state_(0), current_state_(1)); + + // Stand if the plan has been tracked + if ((current_state_ - ref_body_plan_.bottomRows(1).transpose()) + .norm() <= stand_pos_error_threshold_) { + control_mode_ = STAND; } - } - ref_ground_height_(i) = local_footstep_planner_->getTerrainHeight( - ref_body_plan_(i, 0), ref_body_plan_(i, 1)); } - ref_ground_height_(0) = local_footstep_planner_->getTerrainHeight( - current_state_(0), current_state_(1)); - // Stand if the plan has been tracked - if ((current_state_ - ref_body_plan_.bottomRows(1).transpose()).norm() <= - stand_pos_error_threshold_) { - control_mode_ = STAND; + // Update the body plan to use for foot planning + int N_current_plan = body_plan_.rows(); + if (N_current_plan < N_) { + // Cold start with reference plan + body_plan_.conservativeResize(N_, 12); + + // Initialize with the current foot positions + for (int i = N_current_plan; i < N_; i++) { + body_plan_.row(i) = ref_body_plan_.row(i); + foot_positions_body_.row(i) = current_foot_positions_body_; + foot_positions_world_.row(i) = current_foot_positions_world_; + } + } else { + // Only shift the foot position if it's a solve for a new plan index + if (plan_index_diff_ > 0) { + body_plan_.topRows(N_ - 1) = body_plan_.bottomRows(N_ - 1); + grf_plan_.topRows(N_ - 2) = grf_plan_.bottomRows(N_ - 2); + + foot_positions_body_.topRows(N_ - 1) = + foot_positions_body_.bottomRows(N_ - 1); + foot_positions_world_.topRows(N_ - 1) = + foot_positions_world_.bottomRows(N_ - 1); + } } - } - // Update the body plan to use for foot planning - int N_current_plan = body_plan_.rows(); - if (N_current_plan < N_) { - // Cold start with reference plan - body_plan_.conservativeResize(N_, 12); - - // Initialize with the current foot positions - for (int i = N_current_plan; i < N_; i++) { - body_plan_.row(i) = ref_body_plan_.row(i); - foot_positions_body_.row(i) = current_foot_positions_body_; - foot_positions_world_.row(i) = current_foot_positions_world_; - } - } else { - // Only shift the foot position if it's a solve for a new plan index - if (plan_index_diff_ > 0) { - body_plan_.topRows(N_ - 1) = body_plan_.bottomRows(N_ - 1); - grf_plan_.topRows(N_ - 2) = grf_plan_.bottomRows(N_ - 2); - - foot_positions_body_.topRows(N_ - 1) = - foot_positions_body_.bottomRows(N_ - 1); - foot_positions_world_.topRows(N_ - 1) = - foot_positions_world_.bottomRows(N_ - 1); - } - } + // Unwrap yaw to avoid discontinuity at -pi to pi (requires NLP constraint + // bounds beyond [-pi, pi]) + unwrapYawReference(); - // Initialize with current foot and body positions - body_plan_.row(0) = current_state_; - foot_positions_body_.row(0) = current_foot_positions_body_; - foot_positions_world_.row(0) = current_foot_positions_world_; + // Initialize with current foot and body positions + body_plan_.row(0) = current_state_; + foot_positions_body_.row(0) = current_foot_positions_body_; + foot_positions_world_.row(0) = current_foot_positions_world_; } -bool LocalPlanner::computeLocalPlan() { - if (terrain_.isEmpty() || body_plan_msg_ == NULL && !use_twist_input_ || - robot_state_msg_ == NULL) { - ROS_WARN_STREAM( - "ComputeLocalPlan function did not recieve the expected inputs"); - return false; - } +void LocalPlanner::unwrapYawReference() { + static const int yaw_idx = 5; + auto &&yaw_ref_traj = ref_body_plan_.col(yaw_idx); - // Start the timer - quad_utils::FunctionTimer timer(__FUNCTION__); - - // Compute the contact schedule - local_footstep_planner_->computeContactSchedule( - current_plan_index_, body_plan_, ref_primitive_plan_, control_mode_, - contact_schedule_); - - // Compute the new footholds if we have a valid existing plan (i.e. if - // grf_plan is filled) - local_footstep_planner_->computeFootPlan( - current_plan_index_, contact_schedule_, body_plan_, grf_plan_, - ref_body_plan_, current_foot_positions_world_, - current_foot_velocities_world_, first_element_duration_, - past_footholds_msg_, foot_positions_world_, foot_velocities_world_, - foot_accelerations_world_); - - // Transform the new foot positions into the body frame for body planning - local_footstep_planner_->getFootPositionsBodyFrame( - body_plan_, foot_positions_world_, foot_positions_body_); - - // Compute grf position considering the toe radius - Eigen::MatrixXd grf_positions_body = foot_positions_body_; - Eigen::MatrixXd grf_positions_world = foot_positions_world_; - for (size_t i = 0; i < 4; i++) { - grf_positions_body.col(3 * i + 2) = - foot_positions_body_.col(3 * i + 2).array() - toe_radius_; - grf_positions_world.col(3 * i + 2) = - foot_positions_world_.col(3 * i + 2).array() - toe_radius_; - } + // Update first element of yaw reference to be within PI of current state + math_utils::wrapToTarget(yaw_ref_traj(0), current_state_(yaw_idx)); - Eigen::VectorXd current_full_state(36), joint_pos(12), joint_vel(12); - current_full_state.segment(0, 12) = current_state_; - quad_utils::vectorToEigen(robot_state_msg_->joints.position, joint_pos); - quad_utils::vectorToEigen(robot_state_msg_->joints.velocity, joint_vel); - current_full_state.segment(12, 12) = joint_pos; - current_full_state.segment(24, 12) = joint_vel; - - // Compute leg plan with MPC, return if solve fails - if (!local_body_planner_nonlinear_->computeLegPlan( - current_full_state, ref_body_plan_, grf_positions_body, - grf_positions_world, foot_velocities_world_, contact_schedule_, - ref_ground_height_, first_element_duration_, plan_index_diff_, - terrain_grid_, body_plan_, grf_plan_)) - return false; - - N_current_ = body_plan_.rows(); - foot_positions_world_ = grf_positions_world; - for (size_t i = 0; i < 4; i++) { - foot_positions_world_.col(3 * i + 2) = - foot_positions_world_.col(3 * i + 2).array() + toe_radius_; - } + // Unwrap remainder of yaw reference to remove discontinuities (filtering + // out differences > pi) + math_utils::unwrapVector(yaw_ref_traj); +} - // Record computation time and update exponential filter - compute_time_ = 1000.0 * timer.reportSilent(); - mean_compute_time_ = (filter_smoothing_constant_)*mean_compute_time_ + - (1 - filter_smoothing_constant_) * compute_time_; - ROS_INFO_THROTTLE(0.1, "LocalPlanner took %5.3f ms", compute_time_); +bool LocalPlanner::computeLocalPlan() { + if (terrain_.isEmpty() || body_plan_msg_ == NULL && !use_twist_input_ || + robot_state_msg_ == NULL) { + ROS_WARN_STREAM( + "ComputeLocalPlan function did not recieve the expected inputs"); + return false; + } - // Return true if made it this far - return true; -} + // Start the timer + quad_utils::FunctionTimer timer(__FUNCTION__); + + // Compute the contact schedule + local_footstep_planner_->computeContactSchedule( + current_plan_index_, body_plan_, ref_primitive_plan_, control_mode_, + contact_schedule_); + + // Compute the new footholds if we have a valid existing plan (i.e. if + // grf_plan is filled) + local_footstep_planner_->computeFootPlan( + current_plan_index_, contact_schedule_, body_plan_, grf_plan_, + ref_body_plan_, current_foot_positions_world_, + current_foot_velocities_world_, first_element_duration_, + past_footholds_msg_, foot_positions_world_, foot_velocities_world_, + foot_accelerations_world_); + + // Transform the new foot positions into the body frame for body planning + local_footstep_planner_->getFootPositionsBodyFrame( + body_plan_, foot_positions_world_, foot_positions_body_); + + // Compute grf position considering the toe radius + Eigen::MatrixXd grf_positions_body = foot_positions_body_; + Eigen::MatrixXd grf_positions_world = foot_positions_world_; + for (size_t i = 0; i < 4; i++) { + grf_positions_body.col(3 * i + 2) = + foot_positions_body_.col(3 * i + 2).array() - toe_radius_; + grf_positions_world.col(3 * i + 2) = + foot_positions_world_.col(3 * i + 2).array() - toe_radius_; + } -void LocalPlanner::publishLocalPlan() { - // Create messages to publish - quad_msgs::RobotPlan local_plan_msg; - quad_msgs::MultiFootPlanDiscrete future_footholds_msg; - quad_msgs::MultiFootPlanContinuous foot_plan_msg; - - // Update the headers of all messages - local_plan_msg.header.stamp = current_state_timestamp_; - local_plan_msg.header.frame_id = map_frame_; - local_plan_msg.global_plan_timestamp = initial_timestamp_; - local_plan_msg.compute_time = compute_time_; - future_footholds_msg.header = local_plan_msg.header; - foot_plan_msg.header = local_plan_msg.header; - - // Add NLP diagnostic information - local_body_planner_nonlinear_->getNLPDiagnostics().loadDiagnosticsMsg( - local_plan_msg.diagnostics); - - // Compute the discrete and continuous foot plan messages - local_footstep_planner_->loadFootPlanMsgs( - contact_schedule_, current_plan_index_, first_element_duration_, - foot_positions_world_, foot_velocities_world_, foot_accelerations_world_, - future_footholds_msg, foot_plan_msg); - - // Add body, foot, joint, and grf data to the local plan message - for (int i = 0; i < N_current_ - 1; i++) { - // Add the state information - quad_msgs::RobotState robot_state_msg; - robot_state_msg.body = quad_utils::eigenToBodyStateMsg(body_plan_.row(i)); - robot_state_msg.feet = foot_plan_msg.states[i]; - quad_utils::ikRobotState(*quadKD_, robot_state_msg); - - // Add the GRF information - quad_msgs::GRFArray grf_array_msg; - quad_utils::eigenToGRFArrayMsg(grf_plan_.row(i), foot_plan_msg.states[i], - grf_array_msg); - grf_array_msg.contact_states.resize(num_feet_); - for (int j = 0; j < num_feet_; j++) { - grf_array_msg.contact_states[j] = contact_schedule_[i][j]; + Eigen::VectorXd current_full_state(36), joint_pos(12), joint_vel(12); + current_full_state.segment(0, 12) = current_state_; + quad_utils::vectorToEigen(robot_state_msg_->joints.position, joint_pos); + quad_utils::vectorToEigen(robot_state_msg_->joints.velocity, joint_vel); + current_full_state.segment(12, 12) = joint_pos; + current_full_state.segment(24, 12) = joint_vel; + + // Compute leg plan with MPC, return if solve fails + if (!local_body_planner_nonlinear_->computeLegPlan( + current_full_state, ref_body_plan_, grf_positions_body, + grf_positions_world, foot_velocities_world_, contact_schedule_, + ref_ground_height_, first_element_duration_, plan_index_diff_, + terrain_grid_, body_plan_, grf_plan_)) + return false; + + N_current_ = body_plan_.rows(); + foot_positions_world_ = grf_positions_world; + for (size_t i = 0; i < 4; i++) { + foot_positions_world_.col(3 * i + 2) = + foot_positions_world_.col(3 * i + 2).array() + toe_radius_; } - // Update the headers and plan indices of the messages - ros::Time state_timestamp; + // Record computation time and update exponential filter + compute_time_ = 1000.0 * timer.reportSilent(); + mean_compute_time_ = (filter_smoothing_constant_)*mean_compute_time_ + + (1 - filter_smoothing_constant_) * compute_time_; + ROS_INFO_THROTTLE(0.1, "LocalPlanner took %5.3f ms", compute_time_); - // The first duration will vary - state_timestamp = (i == 0) ? current_state_timestamp_ - : current_state_timestamp_ + - ros::Duration(first_element_duration_) + - ros::Duration((i - 1) * dt_); + // Return true if made it this far + return true; +} - quad_utils::updateStateHeaders(robot_state_msg, state_timestamp, map_frame_, - current_plan_index_ + i); - grf_array_msg.header = robot_state_msg.header; - grf_array_msg.traj_index = robot_state_msg.traj_index; +void LocalPlanner::publishLocalPlan() { + // Create messages to publish + quad_msgs::RobotPlan local_plan_msg; + quad_msgs::MultiFootPlanDiscrete future_footholds_msg; + quad_msgs::MultiFootPlanContinuous foot_plan_msg; + + // Update the headers of all messages + local_plan_msg.header.stamp = current_state_timestamp_; + local_plan_msg.header.frame_id = map_frame_; + local_plan_msg.global_plan_timestamp = initial_timestamp_; + local_plan_msg.compute_time = compute_time_; + future_footholds_msg.header = local_plan_msg.header; + foot_plan_msg.header = local_plan_msg.header; + + // Add NLP diagnostic information + local_body_planner_nonlinear_->getNLPDiagnostics().loadDiagnosticsMsg( + local_plan_msg.diagnostics); + + // Compute the discrete and continuous foot plan messages + local_footstep_planner_->loadFootPlanMsgs( + contact_schedule_, current_plan_index_, first_element_duration_, + foot_positions_world_, foot_velocities_world_, + foot_accelerations_world_, future_footholds_msg, foot_plan_msg); + + // Add body, foot, joint, and grf data to the local plan message + for (int i = 0; i < N_current_ - 1; i++) { + // Add the state information + quad_msgs::RobotState robot_state_msg; + robot_state_msg.body = + quad_utils::eigenToBodyStateMsg(body_plan_.row(i)); + robot_state_msg.feet = foot_plan_msg.states[i]; + quad_utils::ikRobotState(*quadKD_, robot_state_msg); + + // Add the GRF information + quad_msgs::GRFArray grf_array_msg; + quad_utils::eigenToGRFArrayMsg(grf_plan_.row(i), + foot_plan_msg.states[i], grf_array_msg); + grf_array_msg.contact_states.resize(num_feet_); + for (int j = 0; j < num_feet_; j++) { + grf_array_msg.contact_states[j] = contact_schedule_[i][j]; + } - local_plan_msg.states.push_back(robot_state_msg); - local_plan_msg.grfs.push_back(grf_array_msg); - local_plan_msg.plan_indices.push_back(current_plan_index_ + i); - local_plan_msg.primitive_ids.push_back(ref_primitive_plan_(i)); - } + // Update the headers and plan indices of the messages + ros::Time state_timestamp; + + // The first duration will vary + state_timestamp = (i == 0) + ? current_state_timestamp_ + : current_state_timestamp_ + + ros::Duration(first_element_duration_) + + ros::Duration((i - 1) * dt_); + + quad_utils::updateStateHeaders(robot_state_msg, state_timestamp, + map_frame_, current_plan_index_ + i); + grf_array_msg.header = robot_state_msg.header; + grf_array_msg.traj_index = robot_state_msg.traj_index; + + local_plan_msg.states.push_back(robot_state_msg); + local_plan_msg.grfs.push_back(grf_array_msg); + local_plan_msg.plan_indices.push_back(current_plan_index_ + i); + local_plan_msg.primitive_ids.push_back(ref_primitive_plan_(i)); + } - // Update timestamps to reflect when these messages were published - local_plan_msg.state_timestamp = current_state_timestamp_; - auto t_publish = ros::Time::now(); - local_plan_msg.header.stamp = t_publish; - future_footholds_msg.header.stamp = t_publish; - foot_plan_msg.header.stamp = t_publish; - - // Publish - local_plan_pub_.publish(local_plan_msg); - foot_plan_discrete_pub_.publish(future_footholds_msg); - foot_plan_continuous_pub_.publish(foot_plan_msg); + // Update timestamps to reflect when these messages were published + local_plan_msg.state_timestamp = current_state_timestamp_; + auto t_publish = ros::Time::now(); + local_plan_msg.header.stamp = t_publish; + future_footholds_msg.header.stamp = t_publish; + foot_plan_msg.header.stamp = t_publish; + + // Publish + local_plan_pub_.publish(local_plan_msg); + foot_plan_discrete_pub_.publish(future_footholds_msg); + foot_plan_continuous_pub_.publish(foot_plan_msg); } void LocalPlanner::spin() { - ros::Rate r(update_rate_); + ros::Rate r(update_rate_); - while (ros::ok()) { - ros::spinOnce(); + while (ros::ok()) { + ros::spinOnce(); - // Wait until all required data has been received - if (terrain_.isEmpty() || (body_plan_msg_ == NULL && !use_twist_input_) || - robot_state_msg_ == NULL) - continue; + // Wait until all required data has been received + if (terrain_.isEmpty() || + (body_plan_msg_ == NULL && !use_twist_input_) || + robot_state_msg_ == NULL) + continue; - // Get the reference plan and robot state into the desired data structures - getReference(); + // Get the reference plan and robot state into the desired data + // structures + getReference(); - // Compute the local plan and publish if it solved successfully, otherwise - // just sleep - if (computeLocalPlan()) publishLocalPlan(); + // Compute the local plan and publish if it solved successfully, + // otherwise just sleep + if (computeLocalPlan()) publishLocalPlan(); - r.sleep(); - } + r.sleep(); + } } diff --git a/nmpc_controller/src/nmpc_controller.cpp b/nmpc_controller/src/nmpc_controller.cpp index 71c012199..292a725f0 100644 --- a/nmpc_controller/src/nmpc_controller.cpp +++ b/nmpc_controller/src/nmpc_controller.cpp @@ -206,7 +206,7 @@ NMPCController::NMPCController(ros::NodeHandle &nh, int robot_id) { app_->Options()->SetStringValue("print_timing_statistics", "no"); app_->Options()->SetStringValue("linear_solver", "ma27"); - app_->Options()->SetIntegerValue("print_level", 0); + app_->Options()->SetIntegerValue("print_level", 0); // default=0, verbose=5 app_->Options()->SetNumericValue("ma57_pre_alloc", 1.5); app_->Options()->SetStringValue("fixed_variable_treatment", "make_parameter_nodual"); diff --git a/nmpc_controller/src/quad_nlp.cpp b/nmpc_controller/src/quad_nlp.cpp index 491b76ea4..13811814e 100644 --- a/nmpc_controller/src/quad_nlp.cpp +++ b/nmpc_controller/src/quad_nlp.cpp @@ -215,6 +215,7 @@ bool quadNLP::get_bounds_info_single_complex_fe( g_ub.segment(g_mm_idx + n_joints_ / 2 + 3 * j, 3).fill(2e19); } } + return true; } // Returns the variable bounds diff --git a/quad_imu/CMakeLists.txt b/quad_imu/CMakeLists.txt new file mode 100644 index 000000000..cc4811962 --- /dev/null +++ b/quad_imu/CMakeLists.txt @@ -0,0 +1,75 @@ +################################################################################ +# CMake +################################################################################ +cmake_minimum_required(VERSION 2.8.3) +project(quad_imu) + +################################################################################ +# Packages +################################################################################ +find_package(catkin REQUIRED COMPONENTS + roscpp + std_msgs + sensor_msgs + tf +) + +################################################################################ +# Declare ROS messages, services and actions +################################################################################ + +################################################################################ +# Declare ROS dynamic reconfigure parameters +################################################################################ + +################################################################################ +# Catkin specific configuration +################################################################################ +catkin_package( + INCLUDE_DIRS include + LIBRARIES quad_imu + CATKIN_DEPENDS roscpp std_msgs sensor_msgs tf + DEPENDS +) + +################################################################################ +# Build +################################################################################ +include_directories( + include + ${catkin_INCLUDE_DIRS} +) + +add_executable(quad_imu src/myahrs_driver.cpp) +add_dependencies(quad_imu ${${PROJECT_NAME}_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS}) +target_link_libraries(quad_imu ${catkin_LIBRARIES}) + +# I knew the REP0003(http://www.ros.org/reps/rep-0003.html#c), +# but the code of "myAHRS SDK" must compile with C++11. +set(CMAKE_CXX_FLAGS "-std=c++11 ${CMAKE_CXX_FLAGS}") + + +################################################################################ +# Install +################################################################################ +install(TARGETS quad_imu + ARCHIVE DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} + LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} + RUNTIME DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION} +) + +install(DIRECTORY include/${PROJECT_NAME}/ + DESTINATION ${CATKIN_PACKAGE_INCLUDE_DESTINATION} +) + +install(DIRECTORY launch + DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION} +) + +install(DIRECTORY rviz_cfg + DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION} +) + +################################################################################ +# Test +############################################################################# diff --git a/quad_imu/LICENSE b/quad_imu/LICENSE new file mode 100644 index 000000000..474c02a50 --- /dev/null +++ b/quad_imu/LICENSE @@ -0,0 +1,28 @@ +Copyright (c) 2015, Yoonseok Pyo +All rights reserved. + +Redistribution and use in source and binary forms, with or without +modification, are permitted provided that the following conditions are met: + +* Redistributions of source code must retain the above copyright notice, this + list of conditions and the following disclaimer. + +* Redistributions in binary form must reproduce the above copyright notice, + this list of conditions and the following disclaimer in the documentation + and/or other materials provided with the distribution. + +* Neither the name of myahrs_driver nor the names of its + contributors may be used to endorse or promote products derived from + this software without specific prior written permission. + +THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" +AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE +IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE +DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE +FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL +DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR +SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER +CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, +OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE +OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. + diff --git a/quad_imu/README.md b/quad_imu/README.md new file mode 100644 index 000000000..ac68d7e78 --- /dev/null +++ b/quad_imu/README.md @@ -0,0 +1,97 @@ +# myahrs_driver + +## Overview + +This is a driver package for the WITHROBOT's myAHRS+ from http://www.lilliputdirect.com/odroid-myahrs|lilliputdirect and http://www.hardkernel.com/main/products/prdt_info.php?g_code=G141464363369 . The myAHRS+ is a low cost high performance AHRS(Attitude Heading Reference System) with USB/UART/I2C interface. The myAHRS+ board contains a 3-axis 16-bit gyroscope, a 3-axis 16-bit accelerometer and a 3-axis 13-bit magnetometer. The driver should also work with USB port. + +### Axes Convention + +The myAHRS+ board used NED type. The myahrs_driver contained in this package converts to the frame conventions of ROS (use the east north up (ENU) convention and right hand rule) before publishing the msgs. The driver use the coordinate frame below. Please see http://www.ros.org/reps/rep-0103.html#axis-orientation for more information. + + * x forward + * y left + * z up + + + * NED type IMU: x-north, y-east, z-down, relative to magnetic north. + * ENU type IMU: x-east, y-north, z-up, relative to magnetic north. + +### Original Source + +The original source (not support ROS) is maintained github below and tutorials are on the corresponding wiki page. A 3D visualization test like 3D-box is included in this original source. This package used the myAHRS+ SDK below. + +https://github.com/withrobot/myAHRS_plus + +## Video + +This is a visualization demonstration using RViz. + +[![test](http://img.youtube.com/vi/j5v5fKppcQo/0.jpg)](http://www.youtube.com/watch?v=j5v5fKppcQo) + +## Installation + +Install the package: + +```sh +sudo apt-get install ros-indigo-myahrs-driver +``` + +Install the package from the github: + +```sh +cd ~/catkin_ws/src +git clone https://github.com/robotpilot/myahrs_driver.git +cd ~/catkin_ws && catkin_make +``` + +# Run + +Run the driver like so: + +```sh +rosrun myahrs_driver myahrs_driver _port:=/dev/ttyACM0 +``` + +or + +```sh +roslaunch myahrs_driver myahrs_driver.launch +``` + +## Nodes + +Official ROS documentation can be found on the ROS wiki at: + +http://wiki.ros.org/myahrs_driver + + +## Communication Protocol Manual and Forum + +The myAHRS+ protocol can be found here(https://github.com/withrobot/myAHRS_plus/tree/master/tutorial). The Forum for myAHRS+ user can be found here(http://forum.odroid.com/viewforum.php?f=109). + + +## References + +### References for myAHRS+ board + +* http://www.withrobot.com/myahrs_plus_en/ +* http://www.withrobot.com/?wpdmact=process&did=MTE4LmhvdGxpbms= +* https://github.com/robotpilot/myAHRS_plus + +* http://www.hardkernel.com/main/products/prdt_info.php?g_code=G141464363369 +* http://www.lilliputdirect.com/odroid-myahrs + +### References for convention of axes and unit + +* http://www.ros.org/reps/rep-0003.html +* http://www.ros.org/reps/rep-0103.html +* https://github.com/paulbovbel/rep/blob/master/rep-0145.rst + +### References for similar IMU packages + +* http://wiki.ros.org/um6 +* http://wiki.ros.org/razor_imu_9dof +* https://github.com/KristofRobot/razor_imu_9dof + +* http://docs.ros.org/api/sensor_msgs/html/msg/Imu.html + diff --git a/quad_imu/include/myahrs_driver/myahrs_plus.hpp b/quad_imu/include/myahrs_driver/myahrs_plus.hpp new file mode 100644 index 000000000..1d4d74f29 --- /dev/null +++ b/quad_imu/include/myahrs_driver/myahrs_plus.hpp @@ -0,0 +1,3022 @@ +//***************************************************************************** +// +// Copyright (c) 2014 Withrobot, Inc. All rights reserved. +// +// Software License Agreement +// +// Withrobot, Inc.(Withrobot) is supplying this software for use solely and +// exclusively on Withrobot's products. +// +// The software is owned by Withrobot and/or its suppliers, and is protected +// under applicable copyright laws. All rights are reserved. +// Any use in violation of the foregoing restrictions may subject the user +// to criminal sanctions under applicable laws, as well as to civil liability +// for the breach of the terms and conditions of this license. +// +// THIS SOFTWARE IS PROVIDED "AS IS". NO WARRANTIES, WHETHER EXPRESS, IMPLIED +// OR STATUTORY, INCLUDING, BUT NOT LIMITED TO, IMPLIED WARRANTIES OF +// MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE APPLY TO THIS SOFTWARE. +// WITHROBOT SHALL NOT, IN ANY CIRCUMSTANCES, BE LIABLE FOR SPECIAL, INCIDENTAL, +// OR CONSEQUENTIAL DAMAGES, FOR ANY REASON WHATSOEVER. +// +//***************************************************************************** + +/* + * myahrs_plus.hpp + * - myAHRS+ SDK + * + * 2014.07.05 ('c')void + * 2014.07.15 ('c')void + * 2014.07.28 ('c')void + * 2014.08.02 ('c')void + * ver. 1.0 + * 2014.08.14 ('c')void + * version string 추가 + * 2014.08.15 ('c')void + * 통신 이외의 모든 실수형은 double + * 2014.08.27 ('c')void + * 2014.09.09 ('c')void + * + */ + +/* + * DO NOT EDIT THIS FILE + */ + +#ifndef __MYAHRS_PLUS_H_ +#define __MYAHRS_PLUS_H_ + +#include +#include +#include +#include + +#define _USE_MATH_DEFINES +#include + +#include +#include +#include +#include +#include +#include +#include +#include + +#ifdef WIN32 + #define _AFXDLL + #include + #include + #include + + #pragma warning( disable : 4996 ) // sprintf + #pragma warning( disable : 4355 ) // warning C4355: 'this' : used in base member initializer list + #pragma warning( disable : 4244 ) // warning C4244: '=' : conversion from 'double' to 'float', possible loss of data + + #define DBG_PRINTF(x, ...) {if(x) { printf(__VA_ARGS__);}} + +#else // LINUX + #include + #include + #include + #include + #include + #include + + #pragma GCC diagnostic ignored "-Wformat" + #define DBG_PRINTF(x, args...) {if(x) { printf(args);}} +#endif // WIN32 + + +/* + * Version + */ +#define WITHROBOT_MYAHRS_PLUS_SDK_VERSION "myahrs+ sdk. ver. 1.01" + +/* + * debugging sw + */ +#define DEBUG_PLATFORM false +#define DEBUG_ASCII_PROTOCOL false +#define DEBUG_BINARY_PROTOCOL false +#define DEBUG_MYAHRS_PLUS false + + +namespace WithRobot { + + class myAhrsException + { + public: + std::string err; + myAhrsException(std::string e=""): err(e){} + virtual ~myAhrsException() {} + + const char* what() const throw() { + return err.c_str(); + } + }; + +/********************************************************************************************************** + * + * Platform abstraction + * + **********************************************************************************************************/ + +#ifdef WIN32 + class Platform + { + public: + class SerialPort + { + std::string port_name; + unsigned int baudrate; + + HANDLE m_hIDComDev; + BOOL m_bOpened; + + public: + SerialPort(const char* port="COM3", unsigned int brate=115200) + : port_name(port), baudrate(brate) + , m_hIDComDev(NULL), m_bOpened(FALSE) + { + } + + ~SerialPort() { + Close(); + } + + bool Open(const char* port, int brate) { + port_name = port; + baudrate = brate; + return Open(); + } + + bool Open() { + char szPort[32]; + sprintf(szPort, "\\\\.\\%s", port_name.c_str()); + CString port_str = CString::CStringT(CA2CT(szPort)); + + DBG_PRINTF(DEBUG_PLATFORM, "portname : %s, baudrate %d\n", szPort, baudrate); + + m_hIDComDev = CreateFile( (LPCTSTR)port_str, + GENERIC_READ | GENERIC_WRITE, + 0, + NULL, + OPEN_EXISTING, + FILE_ATTRIBUTE_NORMAL, + NULL ); + + if( m_hIDComDev == NULL ) { + DBG_PRINTF(DEBUG_PLATFORM, "ERROR : m_hIDComDev == NULL\n"); + return false; + } + + DCB dcb = {0}; + + if (!GetCommState(m_hIDComDev, &dcb)) { + //error getting state + CloseHandle( m_hIDComDev ); + printf("ERROR : GetCommState()\n"); + return false; + } + + dcb.DCBlength = sizeof( DCB ); + GetCommState( m_hIDComDev, &dcb ); + dcb.BaudRate = baudrate; + dcb.ByteSize = 8; + + if(!SetCommState(m_hIDComDev, &dcb)){ + //error setting serial port state + CloseHandle( m_hIDComDev ); + DBG_PRINTF(DEBUG_PLATFORM, "ERROR : SetCommState()\n"); + return false; + } + + COMMTIMEOUTS CommTimeOuts; + + CommTimeOuts.ReadIntervalTimeout = 1; + CommTimeOuts.ReadTotalTimeoutMultiplier = 1; + CommTimeOuts.ReadTotalTimeoutConstant = 1; + CommTimeOuts.WriteTotalTimeoutMultiplier = 1; + CommTimeOuts.WriteTotalTimeoutConstant = 10; + + if(!SetCommTimeouts( m_hIDComDev, &CommTimeOuts)) { + CloseHandle( m_hIDComDev ); + DBG_PRINTF(DEBUG_PLATFORM, "ERROR : SetCommTimeouts()\n"); + return false; + } + + m_bOpened = TRUE; + + return (m_bOpened == TRUE); + } + + void Close() { + if( !m_bOpened || m_hIDComDev == NULL ) { + return; + } + + CloseHandle( m_hIDComDev ); + m_bOpened = FALSE; + m_hIDComDev = NULL; + } + + int Read(unsigned char* buf, unsigned int buf_len) { + if( !m_bOpened || m_hIDComDev == NULL ) return -1; + + BOOL bReadStatus; + DWORD dwBytesRead, dwErrorFlags; + COMSTAT ComStat; + + ClearCommError( m_hIDComDev, &dwErrorFlags, &ComStat ); + + if(ComStat.cbInQue <= 0) { + dwBytesRead = 1; + } + else { + dwBytesRead = (DWORD) ComStat.cbInQue; + } + + if( buf_len < (int) dwBytesRead ) dwBytesRead = (DWORD) buf_len; + + bReadStatus = ReadFile( m_hIDComDev, buf, dwBytesRead, &dwBytesRead, NULL ); + if( !bReadStatus ){ + //if( GetLastError() == ERROR_IO_PENDING ){ + // WaitForSingleObject( m_OverlappedRead.hEvent, 2000 ); + // return( (int) dwBytesRead ); + //} + return 0; + } + + return( (int) dwBytesRead ); + } + + int Write(unsigned char* data, unsigned int data_len) { + BOOL bWriteStat; + DWORD dwBytesWritten = 0; + + if(m_bOpened != TRUE) { + return -1; + } + + bWriteStat = WriteFile( m_hIDComDev, (LPSTR)data, data_len, &dwBytesWritten, NULL); + if(!bWriteStat) { + if (GetLastError() != ERROR_IO_PENDING) { + // WriteFile failed, but it isn't delayed. Report error and abort. + return dwBytesWritten; + } + else { + // Write is pending... 여기로 빠질리는 없지만서도... + //Sleep(10); + // retry; + return dwBytesWritten; + } + } + else { + return dwBytesWritten; + } + } + + int Flush() { + int len= 0, count = 0; + unsigned char buf[256]; + while((len = Read(buf, sizeof(buf))) > 0) { + count += len; + } + return count; + } + }; + + + class Mutex + { + CRITICAL_SECTION cs; + + public: + Mutex() { + InitializeCriticalSection(&cs); + } + + ~Mutex() { + DeleteCriticalSection(&cs); + } + + inline void lock() { + EnterCriticalSection(&cs); + } + + inline void unlock() { + LeaveCriticalSection(&cs); + } + }; + + class Event + { + HANDLE event; + + public: + Event() { + event = CreateEvent(NULL, + TRUE, // bManualReset + FALSE, // bInitialState + _T("Event")); + } + + ~Event() { + CloseHandle(event); + } + + inline bool wait(int timeout_msec=-1) { + if(timeout_msec < 0) { + timeout_msec = INFINITE; + } + DWORD res = WaitForSingleObject(event, timeout_msec); + ResetEvent(event); + return (res == WAIT_OBJECT_0); + } + + inline bool set() { + return (SetEvent(event) == TRUE); + } + }; + + + class Thread + { + DWORD thread_id; + HANDLE thread; + + public: + Thread() : thread(NULL), thread_id(0){} + + bool start(void*(*thread_proc)(void*), void* arg, size_t stack_size=16*1024) { + thread = CreateThread(NULL, (DWORD)stack_size, (LPTHREAD_START_ROUTINE)thread_proc, arg, 0, &thread_id); + return (thread != NULL); + } + + void join() { + if(thread != NULL) { + WaitForSingleObject(thread, INFINITE); + CloseHandle(thread); + } + thread = NULL; + thread_id = 0; + } + }; + + static void msleep(unsigned int msec) { + Sleep(msec); + } + }; + +#else + /* + * Unix-Like OS (Linux/OSX) + */ + class Platform + { + public: + + class SerialPort + { + std::string port_name; // ex) "/dev/tty.usbmodem14241" + int port_fd; + unsigned int baudrate; + + public: + SerialPort(const char* port = "", unsigned int brate=115200) + : port_name(port), baudrate(brate) + , port_fd(-1) + { + } + + ~SerialPort() { + Close(); + } + + bool Open(const char* port, int brate) { + port_name = port; + baudrate = brate; + return Open(); + } + + bool Open() { + int fd = 0; + struct termios options; + + if(port_fd > 0) { + return true; + } + + fd = open(port_name.c_str(), O_RDWR | O_NOCTTY | O_NDELAY); + if(fd < 0) { + return false; + } + + fcntl(fd, F_SETFL, 0); // clear all flags on descriptor, enable direct I/O + tcgetattr(fd, &options); // read serial port options + + cfsetspeed(&options, baudrate); + cfmakeraw(&options); + + options.c_cflag |= CREAD | CLOCAL; // turn on READ + options.c_cflag |= CS8; + options.c_cc[VMIN] = 0; + options.c_cc[VTIME] = 1; + + // set the new port options + tcsetattr(fd, TCSANOW, &options); + + // flush I/O buffers + tcflush(fd, TCIOFLUSH); + + port_fd = fd; + + return (fd > 0); + + } + + void Close() { + if(port_fd > 0) { + close(port_fd); + port_fd = -1; + } + } + + int Read(unsigned char* buf, unsigned int buf_len) { + if(port_fd < 0) { + return -1; + } + + int n = read(port_fd, buf, buf_len-1); + if(n > 0) { + buf[n] = 0; + } + + return n; + } + + int Write(unsigned char* data, unsigned int data_len) { + if(port_fd < 0) { + return -1; + } + + return write(port_fd, data, data_len); + } + + int Flush() { + if(port_fd < 0) { + return -1; + } + + // flush I/O buffers + return tcflush(port_fd, TCIOFLUSH); + } + }; + + + class Mutex + { + pthread_mutex_t mutex; + + public: + Mutex() { + pthread_mutex_init(&mutex, NULL); + } + + ~Mutex() { + pthread_mutex_destroy(&mutex); + } + + inline void lock() { + pthread_mutex_lock(&mutex); + } + + inline void unlock() { + pthread_mutex_unlock(&mutex); + } + }; + + class Event + { + pthread_mutex_t mutex; + pthread_cond_t cond; + + public: + Event() { + pthread_mutex_init(&mutex, NULL); + pthread_cond_init(&cond, NULL); + } + + ~Event() { + pthread_mutex_destroy(&mutex); + pthread_cond_destroy(&cond); + } + + inline bool wait(int timeout_msec=-1) { + bool res = true; + pthread_mutex_lock(&mutex); + if(timeout_msec > 0) { + struct timeval tv; + struct timespec ts; + + gettimeofday(&tv, NULL); + + ts.tv_sec = time(NULL) + timeout_msec / 1000; + ts.tv_nsec = tv.tv_usec * 1000 + 1000 * 1000 * (timeout_msec % 1000); + ts.tv_sec += ts.tv_nsec / (1000 * 1000 * 1000); + ts.tv_nsec %= (1000 * 1000 * 1000); + + int n = pthread_cond_timedwait(&cond, &mutex, &ts); + res = (n == 0); // n == 0 : signaled, n == ETIMEDOUT + } + else { + pthread_cond_wait(&cond, &mutex); + res = true; + } + pthread_mutex_unlock(&mutex); + return res; + } + + inline bool set() { + pthread_mutex_lock(&mutex); + pthread_cond_signal(&cond); + pthread_mutex_unlock(&mutex); + return true; + } + }; + + + class Thread + { + pthread_t thread; + + public: + Thread() : thread(0){ } + + bool start(void*(*thread_proc)(void*), void* arg, size_t stack_size=16*1024) { + pthread_attr_t attr; + size_t stacksize; + pthread_attr_init(&attr); + + //pthread_attr_getstacksize(&attr, &stacksize); + + pthread_attr_setstacksize(&attr, stack_size); + int res = pthread_create(&thread, &attr, thread_proc, (void*)arg); + if(res != 0) { + thread = 0; + } + + return (res == 0); + } + + void join() { + if(thread != 0) { + pthread_join(thread, NULL); + } + thread = 0; + } + }; + + static void msleep(unsigned int msec) { + usleep(msec*1000); + } + }; // Platform + +#endif // #ifdef WIN32 + + +/********************************************************************************************************** + * + * platform independent implementation + * + **********************************************************************************************************/ + + class LockGuard { + + Platform::Mutex & mutex; + + public: + LockGuard(Platform::Mutex& m) : mutex(m) { + mutex.lock(); + } + + ~LockGuard() { + mutex.unlock(); + } + }; + + + class StringUtil + { + public: + static void replace(std::string & src, std::string s1, std::string s2) { + size_t ofs = 0; + + do { + ofs = src.find(s1, ofs); + if(ofs == -1) { + return; + } + src.replace(ofs, s1.length(), s2); + ofs += s2.length(); + }while(1); + } + + // trim from start + static inline std::string <rim(std::string &s) { + s.erase(s.begin(), std::find_if(s.begin(), s.end(), std::not1(std::ptr_fun(std::isspace)))); + return s; + } + + // trim from end + static inline std::string &rtrim(std::string &s) { + s.erase(std::find_if(s.rbegin(), s.rend(), std::not1(std::ptr_fun(std::isspace))).base(), s.end()); + return s; + } + + // trim from both ends + static inline std::string &strip(std::string &s) { + return ltrim(rtrim(s)); + } + + static int split(std::vector & token_list, const char* c_str, char delimiter, int token_max=-1) { + std::stringstream is(c_str); + std::string token; + + bool check_max = (token_max > 0); + + token_list.clear(); + while(getline(is, token, delimiter)){ + token_list.push_back(token); + if(check_max) { + if(--token_max <= 0) { + break; + } + } + } + + return token_list.size(); + } + + static std::string join(std::vector& str_list, const std::string& delimiter) { + std::ostringstream s; + for(size_t i=0; i + static void to_string_list(std::vector& str_list, T* array, size_t array_num) { + for(size_t i=0; i& attribute_list, std::vector& tokens) { + std::vector attribute_pair; + + for(size_t i=0; i 0) { + parse_message(reinterpret_cast(frame_buffer.buffer)); + } + frame_buffer.reset(); + break; + + case MSG_TAIL_CR: + // nothing to do + break; + + default: + frame_buffer.push(c); + } + } + + virtual void update_attributes(std::vector& tokens) = 0; + + private: + bool parse_message(const char* ascii_frame) { + DBG_PRINTF(debug, "## ASC FRAME : [%s]\n", ascii_frame); + + std::vector tokens; + if(StringUtil::split(tokens, ascii_frame, '*') != 2) { + return false; + } + + std::string& str_message = tokens[0]; + int crc_from_sensor = std::stoul(tokens[1].c_str(), nullptr, 16); + + uint8_t crc_calc = 0; + for(size_t i=0; i*receiver)(byte); + return last_state; + } + + bool is_busy() { + return (last_state==STATE_BUSY); + } + + private: + void clear_all_states() { + state_receiving = false; + stream.reset(); + crc_calc = 0; + accumulater = 0; + } + + bool check_crc() { + uint8_t crc_rcv = accumulater & 0x00FF; + DBG_PRINTF(debug, "### CRC_R %02X, CRC_C %02X\n", crc_rcv, crc_calc); + return crc_rcv == crc_calc; + } + + void push_data(uint8_t byte) { + stream.push(byte); + + accumulater <<= 8; + accumulater |= byte; + uint8_t prv_byte = ((accumulater & 0xFF00) >> 8); + crc_calc ^= prv_byte; + } + + ReturnCode state_data(uint8_t byte) { + if(byte == DLE) { + receiver = &FilterByteStuffing::state_control; + return STATE_BUSY; + } + else { + if(state_receiving) { + push_data(byte); + return STATE_BUSY; + } + else { + return STATE_NOP; + } + } + } + + ReturnCode state_control(uint8_t byte) { + ReturnCode res; + + switch(byte) { + case STX: + clear_all_states(); + state_receiving = true; + res = STATE_BUSY; + break; + + case ETX: + state_receiving = false; + res = check_crc() ? STATE_COMPLETE : STATE_ERROR; + break; + + case DLE: + push_data(byte); + res = STATE_BUSY; + break; + + default: + clear_all_states(); + state_receiving = true; + res = STATE_ERROR; + } + + receiver = &FilterByteStuffing::state_data; + return res; + } + }; // FilterDleToPlain + + + class iNodeParser + { + public: + enum Tag { + // value type + TAG_TYPE_NONE = 0, + TAG_TYPE_INT8 = 1, + TAG_TYPE_UINT8 = 2, + TAG_TYPE_INT16 = 3, + TAG_TYPE_UINT16 = 4, + TAG_TYPE_INT32 = 5, + TAG_TYPE_UINT32 = 6, + TAG_TYPE_INT64 = 7, + TAG_TYPE_UINT64 = 8, + TAG_TYPE_FLOAT32 = 9, + TAG_TYPE_FLOAT64 = 10, + TAG_TYPE_STRING = 11, + + // node attribute + TAG_HAS_LEAF_NODES = (0x01<<7), + TAG_NEXT_NODE_EXIST = (0x01<<6), + TAG_LIST_NODE = (0x01<<5), + + TAG_TYPE_MASK = 0x0F, + }; + + protected: + struct Varient { + Varient() : type(TAG_TYPE_NONE) {} + uint8_t type; + union { + int8_t i8; + uint8_t ui8; + int16_t i16; + uint16_t ui16; + int32_t i32; + uint32_t ui32; + int64_t i64; + uint64_t ui64; + float f32; + double f64; + } value; + + template + bool set(uint8_t t, T v) { + type = t; + switch(type) { + case TAG_TYPE_INT8: value.i8 = (int8_t)v; break; + case TAG_TYPE_UINT8: value.ui8 = (uint8_t)v; break; + case TAG_TYPE_INT16: value.i16 = (int16_t)v; break; + case TAG_TYPE_UINT16: value.ui16 = (uint16_t)v; break; + case TAG_TYPE_INT32: value.i32 = (int32_t)v; break; + case TAG_TYPE_UINT32: value.ui32 = (uint32_t)v; break; + case TAG_TYPE_INT64: value.i64 = (int64_t)v; break; + case TAG_TYPE_UINT64: value.ui64 = (uint64_t)v; break; + case TAG_TYPE_FLOAT32: value.f32 = (float)v; break; + case TAG_TYPE_FLOAT64: value.f64 = (double)v; break; + default: return false; + } + return true; + } + }; + + public: + struct Node { + std::string name; + std::vector list; + }; + + private: + struct Stream { + unsigned char* buffer; + size_t pos; + size_t length; + bool debug; + + Stream(unsigned char* s, size_t l) : buffer(s), length(l), pos(0), debug(DEBUG_BINARY_PROTOCOL) { + print_buffer(); + } + + int getc() { + if(pos < length) { return buffer[pos++]; } + else { return -1; } + } + + int peek() { + if(pos < length) { return buffer[pos]; } + else { return -1; } + } + + template + void read_value(T& value) { + T* v = (T*)(&buffer[pos]); + pos += sizeof(T); + value = *v; + } + + bool read_value(uint8_t value_type, Varient& v) { + switch(value_type) { + case TAG_TYPE_INT8: { int8_t value; read_value(value); v.set(value_type, value);} break; + case TAG_TYPE_UINT8: { uint8_t value; read_value(value); v.set(value_type, value);} break; + case TAG_TYPE_INT16: { int16_t value; read_value(value); v.set(value_type, value);} break; + case TAG_TYPE_UINT16: {uint16_t value; read_value(value); v.set(value_type, value);} break; + case TAG_TYPE_INT32: { int32_t value; read_value(value); v.set(value_type, value);} break; + case TAG_TYPE_UINT32: {uint32_t value; read_value(value); v.set(value_type, value);} break; + case TAG_TYPE_INT64: { int64_t value; read_value(value); v.set(value_type, value);} break; + case TAG_TYPE_UINT64: {uint64_t value; read_value(value); v.set(value_type, value);} break; + case TAG_TYPE_FLOAT32: { float value; read_value(value); v.set(value_type, value);} break; + case TAG_TYPE_FLOAT64: { double value; read_value(value); v.set(value_type, value);} break; + default: return false; + } + return true; + } + + void read_list(uint8_t value_type, std::vector& list, size_t count) { + Varient v; + for(size_t i=0; i node_list; + + public: + iNodeParser(unsigned char* stream=0, size_t stream_len=0) : istream(stream, stream_len), debug(false){} + virtual ~iNodeParser() {} + + void parse() { + read_nodes(); + new_node(node_list); + } + + virtual void new_node(std::vector& node_list) = 0; + + private: + void read_nodes() { + if(istream.length == 0 || istream.peek() < 0) { + return; + } + + uint8_t tag = istream.getc(); + uint8_t type_of_value = (tag & TAG_TYPE_MASK); + + Node node; + node.name = istream.read_string(); + if(type_of_value) { + if(tag&TAG_LIST_NODE) { + uint16_t count = 0; + istream.read_value(count); + istream.read_list(type_of_value, node.list, count); + } + else { + Varient value; + istream.read_value(type_of_value, value); + node.list.push_back(value); + } + } + + node_list.push_back(node); + + if((tag & TAG_HAS_LEAF_NODES) || (tag & TAG_NEXT_NODE_EXIST)) { + read_nodes(); + } + } + }; + + + class iBinaryProtocol { + FilterByteStuffing filter_byte_stuffing; + FrameBuffer binary_stream; + + class BinaryNodeParser : public iNodeParser + { + iBinaryProtocol* protocol; + + public: + BinaryNodeParser(iBinaryProtocol* s, unsigned char* stream, size_t stream_len) : protocol(s), iNodeParser(stream, stream_len) {} + void new_node(std::vector& node_list) { + protocol->update_attributes(node_list); + } + }; + + public: + iBinaryProtocol() : filter_byte_stuffing(binary_stream) {} + virtual ~iBinaryProtocol() {} + + protected: + void push_byte(unsigned char c) { + if(filter_byte_stuffing(c) == FilterByteStuffing::STATE_COMPLETE) { + BinaryNodeParser parser(this, binary_stream.buffer, binary_stream.offset); + parser.parse(); + } + } + + bool is_receiving() { + return filter_byte_stuffing.is_busy(); + } + + virtual void update_attributes(std::vector& node_list) = 0; + + }; // BinaryProtocol + + + /************************************************************************** + * + * COMMUNICATION PROTOCOL + * + **************************************************************************/ + + class iProtocol : public iAsciiProtocol, public iBinaryProtocol + { + public: + iProtocol(){} + virtual ~iProtocol() {} + + bool feed(unsigned char* data, int data_len) { + if(!data || data_len <= 0) { + return false; + } + + for(int i=0; i tokens; + if(StringUtil::split(tokens, str_rpy.c_str(), delimiter) == 3) { + set(tokens); + } + else { + throw(myAhrsException("EulerAngle: Invalid String")); + } + } + + inline void set(std::vector& str_array) { + if(str_array.size() != 3) { + throw(myAhrsException("EulerAngle: size error")); + } + roll = atof(str_array[0].c_str()); + pitch = atof(str_array[1].c_str()); + yaw = atof(str_array[2].c_str()); + } + + inline std::string to_string() { + std::stringstream s; + s << roll <<", "<< pitch <<", "<< yaw; + return s.str(); + } + }; + + struct Quaternion; + struct DirectionCosineMatrix + { + double mat[9]; + + DirectionCosineMatrix() { + reset(); + } + + DirectionCosineMatrix(double dcm[9]) { + set(dcm); + } + + DirectionCosineMatrix(double& m11, double& m12, double& m13, + double& m21, double& m22, double& m23, + double& m31, double& m32, double& m33) { + set(m11, m12, m13, m21, m22, m23, m31, m32, m33); + } + + DirectionCosineMatrix(std::string str_mat, char delimiter=' ') { + set(str_mat, delimiter); + } + + inline void reset() { + memset(mat, 0, sizeof(mat)); + } + + inline void set(double dcm[9]) { + memcpy(mat, dcm, sizeof(mat)); + } + + inline void set(double& m11, double& m12, double& m13, + double& m21, double& m22, double& m23, + double& m31, double& m32, double& m33) { + mat[0] = m11, mat[1] = m12, mat[2] = m13, + mat[3] = m21, mat[4] = m22, mat[5] = m23, + mat[6] = m31, mat[7] = m32, mat[8] = m33; + } + + inline void set(std::string str_mat, char delimiter=' ') { + std::vector tokens; + if(StringUtil::split(tokens, str_mat.c_str(), delimiter) == 9) { + set(tokens); + } + else { + throw(myAhrsException("Matrix3x3: Invalid String")); + } + } + + inline void set(std::vector& str_array) { + if(str_array.size() != 9) { + throw(myAhrsException("Matrix3x3: size error")); + } + for(int i=0; i<9; i++) { + mat[i] = (double)atof(str_array[i].c_str()); + } + } + + inline std::string to_string() { + std::vector temp; + StringUtil::to_string_list(temp, mat, 9); + return StringUtil::join(temp, ", "); + } + + EulerAngle to_euler_angle() { + EulerAngle e; + double RAD2DEG = 180/M_PI; + e.roll = atan2(MAT(1, 2), MAT(2, 2))*RAD2DEG; + e.pitch = -asin(MAT(0, 2))*RAD2DEG; + e.yaw = atan2(MAT(0, 1), MAT(0, 0))*RAD2DEG; + return e; + } + +#if 0 + void set(Quaternion& q) { + // http://www.mathworks.co.kr/kr/help/aeroblks/quaternionstodirectioncosinematrix.html + double xx = q.x*q.x; + double xy = q.x*q.y; + double xz = q.x*q.z; + double xw = q.x*q.w; + double yy = q.y*q.y; + double yz = q.y*q.z; + double yw = q.y*q.w; + double zz = q.z*q.z; + double zw = q.z*q.w; + double ww = q.w*q.w; + + mat[0] = xx - yy - zz + ww; + mat[1] = 2.0*(xy + zw); + mat[2] = 2.0*(xz - yw); + + mat[3] = 2.0*(xy - zw); + mat[4] = -xx + yy - zz + ww; + mat[5] = 2.0*(yz + xw); + + mat[6] = 2.0*(xz + yw); + mat[7] = 2.0*(yz - xw); + mat[8] = -xx - yy + zz + ww; + } +#else + void set(Quaternion& q); +#endif + + private: + inline double MAT(unsigned int row, unsigned int col) { + return mat[(row)*3 + col]; + } + }; + + + struct Quaternion + { + double x, y, z, w; + + Quaternion(double _x=0, double _y=0, double _z=0, double _w=1): x(_x), y(_y), z(_z), w(_w) {} + Quaternion(std::string str_xyzw, char delimiter=' ') { + set(str_xyzw, delimiter); + } + + inline void reset() { + set(0, 0, 0, 1); + } + + inline void set(double _x, double _y, double _z, double _w) { + x=_x, y=_y, z=_z, w=_w; + } + + inline void set(std::string str_xyzw, char delimiter=' ') { + std::vector tokens; + if(StringUtil::split(tokens, str_xyzw.c_str(), delimiter) == 4) { + set(tokens); + } + else { + throw(myAhrsException("Quaternion: Invalid String")); + } + } + + inline void set(std::vector& str_array) { + if(str_array.size() != 4) { + throw(myAhrsException("Quaternion: size error")); + } + x = atof(str_array[0].c_str()); + y = atof(str_array[1].c_str()); + z = atof(str_array[2].c_str()); + w = atof(str_array[3].c_str()); + } + + inline std::string to_string() { + std::stringstream s; + s << x <<", "<< y <<", "<< z <<", "<< w; + return s.str(); + } + + void normalize() { + double norm = sqrt(x*x + y*y + z*z + w*w); + x = x/norm; + y = y/norm; + z = z/norm; + w = w/norm; + } + + Quaternion conj() { + Quaternion q; + q.x = -x; + q.y = -y; + q.z = -z; + q.w = w; + return q; + } + + // http://www.mathworks.co.kr/kr/help/aerotbx/ug/quatmultiply.html + // qxr = q*r + static Quaternion product(Quaternion& q, Quaternion& r) { + Quaternion qxr; + + qxr.w = r.w*q.w - r.x*q.x - r.y*q.y - r.z*q.z; + qxr.x = r.w*q.x + r.x*q.w - r.y*q.z + r.z*q.y; + qxr.y = r.w*q.y + r.x*q.z + r.y*q.w - r.z*q.x; + qxr.z = r.w*q.z - r.x*q.y + r.y*q.x + r.z*q.w; + + return qxr; + } + + EulerAngle to_euler_angle() { + double xx = x*x; + double xy = x*y; + double xz = x*z; + double xw = x*w; + double yy = y*y; + double yz = y*z; + double yw = y*w; + double zz = z*z; + double zw = z*w; + double ww = w*w; + + double RAD2DEG = 180/M_PI; + EulerAngle e; + e.roll = atan2(2.0*(yz + xw), -xx - yy + zz + ww)*RAD2DEG; + e.pitch = -asin(2.0*(xz - yw))*RAD2DEG; + e.yaw = atan2(2.0*(xy + zw), xx - yy - zz + ww)*RAD2DEG; + return e; + } + + DirectionCosineMatrix to_dcm() { + // http://www.mathworks.co.kr/kr/help/aeroblks/quaternionstodirectioncosinematrix.html + double xx = x*x; + double xy = x*y; + double xz = x*z; + double xw = x*w; + double yy = y*y; + double yz = y*z; + double yw = y*w; + double zz = z*z; + double zw = z*w; + double ww = w*w; + + DirectionCosineMatrix dcm; + + dcm.mat[0] = xx - yy - zz + ww; + dcm.mat[1] = 2.0*(xy + zw); + dcm.mat[2] = 2.0*(xz - yw); + + dcm.mat[3] = 2.0*(xy - zw); + dcm.mat[4] = -xx + yy - zz + ww; + dcm.mat[5] = 2.0*(yz + xw); + + dcm.mat[6] = 2.0*(xz + yw); + dcm.mat[7] = 2.0*(yz - xw); + dcm.mat[8] = -xx - yy + zz + ww; + + return dcm; + } + }; + + void DirectionCosineMatrix::set(Quaternion& q) { + *this = q.to_dcm(); + } + + + /************************************************************************** + * + * IMU + * + **************************************************************************/ + template + struct ImuData + { + Type ax, ay, az, gx, gy, gz, mx, my, mz, temperature; + + ImuData() { + ax = ay = az = gx = gy = gz = mx = my = mz = temperature = 0; + } + + ImuData(Type data[10]) { + set(data); + } + + inline void reset() { + Type d[10]; + memset(d, 0, sizeof(d)); + set(d); + } + + inline void set(Type data[10]) { + int i=0; + ax = data[i++]; + ay = data[i++]; + az = data[i++]; + + gx = data[i++]; + gy = data[i++]; + gz = data[i++]; + + mx = data[i++]; + my = data[i++]; + mz = data[i++]; + + temperature = data[i++]; + } + + inline void set(std::string str_mat, char delimiter=' ') { + std::vector tokens; + if(StringUtil::split(tokens, str_mat.c_str(), delimiter) == 10) { + set(tokens); + } + else { + throw(myAhrsException("imu: Invalid String")); + } + } + + inline void set(std::vector& str_array) { + if(str_array.size() != 10) { + throw(myAhrsException("imu: size error")); + } + Type data[10]; + for(int i=0; i<10; i++) { + data[i] = (Type)atof(str_array[i].c_str()); + } + set(data); + } + + inline std::string to_string() { + std::stringstream s; + s << ax <<", "< imu_rawdata; + ImuData imu; + + SensorData() { + reset(); + } + + void reset() { + sequence_number = -1; + euler_angle.reset(); + quaternion.reset(); + imu_rawdata.reset(); + imu.reset(); + attitude_type = NOT_DEF_ATTITUDE; + imu_type = NOT_DEF_IMU; + } + + void update_attitude(EulerAngle& e) { + euler_angle = e; + attitude_type = EULER_ANGLE; + } + + void update_attitude(Quaternion& q) { + quaternion = q; + attitude_type = QUATERNION; + } + + void update_imu(ImuData& i) { + imu_rawdata = i; + imu_type = RAW; + } + + void update_imu(ImuData& i) { + imu = i; + imu_type = COMPENSATED; + } + + std::string to_string() { + std::vector str_array; + char temp[32]; + sprintf(temp, "%d", sequence_number); + str_array.push_back("sequence = " + std::string(temp)); + + switch(attitude_type) { + case EULER_ANGLE: + str_array.push_back("euler_angle = " + euler_angle.to_string()); + break; + case QUATERNION: + str_array.push_back("quaternion = " + quaternion.to_string()); + break; + default: + str_array.push_back("no attitude "); + break; + } + + switch(imu_type) { + case RAW: + str_array.push_back("imu_raw = " + imu_rawdata.to_string()); + break; + case COMPENSATED: + str_array.push_back("imu_comp = " + imu.to_string()); + break; + default: + str_array.push_back("no imu "); + break; + } + return StringUtil::join(str_array, "\n"); + } + }; + + + class iMyAhrsPlus + { + bool debug; + + Platform::SerialPort serial; + + Platform::Mutex mutex_attribute; + Platform::Mutex mutex_communication; + + bool thread_receiver_ready; + Platform::Thread thread_receiver; + + bool activate_user_event; + Platform::Thread thread_event; + + SensorData sensor_data; + + int sensor_id; + std::map attribute_map; + + typedef bool (iMyAhrsPlus::*AscHandler)(std::vector& tokens); + std::map ascii_handler_data_map; + + typedef bool (iMyAhrsPlus::*AscRspHandler)(std::map& attributes); + std::map ascii_handler_rsp_map; + + typedef bool (iMyAhrsPlus::*BinHandler)(iNodeParser::Node& node); + std::map binary_handler_data_map; + + static const int ACCEL_RANGE = 16; + static const int GYRO_RANGE = 2000; + static const int MAGNET_RANGE = 300; + static const int TEMP_RANGE = 200; + static const int EULER_RANGE = 180; + + class Protocol : public iProtocol + { + iMyAhrsPlus* ahrs; + + public: + Protocol(iMyAhrsPlus* s) : ahrs(s) {} + + void update_attributes(std::vector& tokens) { + if(tokens.size() >= 2) { + ahrs->ascii_parse_response(tokens); + } + } + + void update_attributes(std::vector& node_list) { + if(node_list.size() > 0) { + ahrs->binary_parse_response(node_list); + } + } + } protocol; + + class ResponsQueue { + + Platform::Mutex lock; + Platform::Event event; + std::deque< std::vector > queue; + + public: + ResponsQueue() {} + ~ResponsQueue() {} + + void clear() { + LockGuard _l(lock); + queue.clear(); + } + + bool wait(int timeout_msec) { + if(size() <= 0) { + return event.wait(timeout_msec); + } + else { + return true; + } + } + + void push_back(std::vector& list) { + LockGuard _l(lock); + queue.push_back(list); + event.set(); + } + + size_t size() { + LockGuard _l(lock); + return queue.size(); + } + + bool pop(std::vector& out) { + LockGuard _l(lock); + if(queue.size() > 0) { + out = queue.front(); + queue.pop_front(); + return true; + } + else { + return false; + } + } + } response_message_queue; + + class EventItem + { + public: + enum EventId { + NONE = 0, + EXIT, + ATTRIBUTE, + DATA, + }; + + EventId event_id; + + EventItem(EventId id=NONE) : event_id(id) {} + virtual ~EventItem() {} + + virtual SensorData* get_sensor_data() { return 0; } + virtual std::map* get_attribute() { return 0; } + }; + + class EventQueue { + private: + class EventItemExit : public EventItem + { + public: + EventItemExit() : EventItem(EXIT){} + }; + + class EventItemData : public EventItem + { + SensorData data; + + public: + EventItemData(SensorData& d) : EventItem(DATA), data(d) {} + ~EventItemData() {} + SensorData* get_sensor_data() { return &data; } + }; + + class EventItemAttribute : public EventItem + { + std::map attribute; + + public: + EventItemAttribute(std::map& a) : EventItem(ATTRIBUTE), attribute(a) {} + ~EventItemAttribute() {} + std::map* get_attribute() { return &attribute; } + }; + + std::deque deque; + Platform::Mutex lock; + Platform::Event event; + + static const size_t EVENT_MAX_NUM = 5; + + public: + EventQueue() {} + ~EventQueue() { + int num = deque.size(); + for(int i=0; i attribute; + attribute[attr_name] = attr_value; + deque.push_back(new EventItemAttribute(attribute)); + event.set(); + } + + void push_event_data(SensorData& data) { + LockGuard _l(lock); + if(deque.size() < EVENT_MAX_NUM) { + deque.push_back(new EventItemData(data)); + } + event.set(); + } + + EventItem* pop_event() { + LockGuard _l(lock); + EventItem* i = 0; + if(deque.size() > 0) { + i = deque.front(); + deque.pop_front(); + } + return i; + } + } event_queue; + + // prevent copy + iMyAhrsPlus(iMyAhrsPlus& rhs):protocol(this) {} + + public: + iMyAhrsPlus(std::string port_name="", unsigned int baudrate=115200) + : serial(port_name.c_str(), baudrate) + , debug(DEBUG_MYAHRS_PLUS) + , protocol(this) + , sensor_id(-1) + , activate_user_event(false) + , thread_receiver_ready(false) + { + // response message parser (ascii) + ascii_handler_rsp_map[std::string("~trig")] = &iMyAhrsPlus::ascii_rsp_trigger; + ascii_handler_rsp_map[std::string("~ping")] = &iMyAhrsPlus::ascii_rsp_ping; + ascii_handler_rsp_map[std::string("~divider")] = &iMyAhrsPlus::ascii_rsp_divider; + ascii_handler_rsp_map[std::string("~mode")] = &iMyAhrsPlus::ascii_rsp_mode; + ascii_handler_rsp_map[std::string("~asc_out")] = &iMyAhrsPlus::ascii_rsp_asc_out; + ascii_handler_rsp_map[std::string("~bin_out")] = &iMyAhrsPlus::ascii_rsp_bin_out; + ascii_handler_rsp_map[std::string("~set_offset")] = &iMyAhrsPlus::ascii_rsp_user_orientation; + ascii_handler_rsp_map[std::string("~clr_offset")] = &iMyAhrsPlus::ascii_rsp_user_orientation; + ascii_handler_rsp_map[std::string("~calib")] = &iMyAhrsPlus::ascii_rsp_calib; + ascii_handler_rsp_map[std::string("~factory")] = &iMyAhrsPlus::ascii_rsp_factory; + ascii_handler_rsp_map[std::string("~stat")] = &iMyAhrsPlus::ascii_rsp_stat; + ascii_handler_rsp_map[std::string("~version")] = &iMyAhrsPlus::ascii_rsp_version; + ascii_handler_rsp_map[std::string("~id")] = &iMyAhrsPlus::ascii_rsp_id; + ascii_handler_rsp_map[std::string("~sn")] = &iMyAhrsPlus::ascii_rsp_serial_number; + ascii_handler_rsp_map[std::string("~sensitivity")] = &iMyAhrsPlus::ascii_rsp_sensitivity; + ascii_handler_rsp_map[std::string("~baudrate")] = &iMyAhrsPlus::ascii_rsp_baudrate; + ascii_handler_rsp_map[std::string("~save")] = &iMyAhrsPlus::ascii_rsp_save; + + // data message (ascii) + ascii_handler_data_map[std::string("$RPY")] = &iMyAhrsPlus::ascii_update_euler; + ascii_handler_data_map[std::string("$QUAT")] = &iMyAhrsPlus::ascii_update_quaternion; + ascii_handler_data_map[std::string("$RPYIMU")] = &iMyAhrsPlus::ascii_update_rpyimu; + ascii_handler_data_map[std::string("$QUATIMU")] = &iMyAhrsPlus::ascii_update_quatimu; + ascii_handler_data_map[std::string("$RIIMU")] = &iMyAhrsPlus::ascii_update_riimu; + ascii_handler_data_map[std::string("$IMU")] = &iMyAhrsPlus::ascii_update_imu; + + // data node (binary) + static const char* NAME_DATA_ROOT = "d"; + static const char* NAME_RIIMU = "r"; + static const char* NAME_IMU = "i"; + static const char* NAME_EULER = "e"; + static const char* NAME_QUATERNION = "q"; + static const char* NAME_DCM = "c"; + static const char* NAME_SEQUANCE = "s"; + + binary_handler_data_map[std::string(NAME_SEQUANCE)] = &iMyAhrsPlus::binary_update_sequence; + binary_handler_data_map[std::string(NAME_EULER)] = &iMyAhrsPlus::binary_update_euler; + binary_handler_data_map[std::string(NAME_QUATERNION)] = &iMyAhrsPlus::binary_update_quaternion; + binary_handler_data_map[std::string(NAME_IMU)] = &iMyAhrsPlus::binary_update_imu; + binary_handler_data_map[std::string(NAME_RIIMU)] = &iMyAhrsPlus::binary_update_riimu; + + thread_event.start(thread_proc_callback, (void*)this); + } + + virtual ~iMyAhrsPlus() { + event_queue.push_event_exit(); + stop(); + thread_event.join(); + } + + const char* sdk_version() { + return WITHROBOT_MYAHRS_PLUS_SDK_VERSION; + } + + bool start(std::string port_name="", int baudrate=-1) { + { + LockGuard _l(mutex_communication); + if(port_name == "" || baudrate < 0) { + if(serial.Open() == false) { + return false; + } + } + else { + if(serial.Open(port_name.c_str(), baudrate) == false) { + return false; + } + } + + serial.Flush(); + + thread_receiver_ready = false; + if(thread_receiver.start(thread_proc_receiver, (void*)this) == false) { + return false; + } + + while(thread_receiver_ready == false) { + Platform::msleep(10); + } + } + + for(int i=0; i<3; i++) { + cmd_ping(1000); + } + + activate_user_event = resync(); + return activate_user_event; + } + + void stop() { + LockGuard _l(mutex_communication); + clear_all_attribute(); + activate_user_event = false; + serial.Close(); + thread_receiver.join(); + } + + inline int get_sensor_id() { + LockGuard _l(mutex_attribute); + return sensor_id; + } + + inline SensorData get_data() { + LockGuard _l(mutex_attribute); + return sensor_data; + } + + inline void get_data(SensorData& data) { + LockGuard _l(mutex_attribute); + data = sensor_data; + } + + /* + * access attributes + */ + bool get_attribute(const char* attrib_name, std::string& attrib_value) { + LockGuard _l(mutex_attribute); + + std::string attribute_name(attrib_name); + std::map::iterator it = attribute_map.find(attribute_name); + if(it != attribute_map.end()) { + attrib_value = it->second; + return true; + } + else { + return false; + } + } + + private: + bool is_exist(const std::string& attribute_name) { + std::map::iterator it = attribute_map.find(attribute_name); + if(it != attribute_map.end()) { + return true; + } + else { + return false; + } + } + + void set_attribute(const std::string& attribute_name, const std::string& value) { + attribute_map[attribute_name] = value; + + if(activate_user_event) { + std::string attr_name = attribute_name; + std::string attr_value = value; + event_queue.push_event_attribute_change(attr_name, attr_value); + } + } + + void clear_all_attribute() { + LockGuard _l(mutex_attribute); + attribute_map.clear(); + sensor_id = -1; + } + + public: + // ######################################################################################################################## + // # accel_bias : -1.334012e+01, 2.941270e+01, 1.579460e+02 + // # accel_max : 16 + // # accel_sensitivity : 4.882813e-04 + // # accel_sensitivity_matrix : 4.888283e-04, -1.201399e-06, -4.818384e-06, 0.000000e+00, 4.901073e-04, -1.257056e-06, 0.000000e+00, 0.000000e+00, 4.853439e-04 + // # ascii_format : QUATIMU + // # baudrate : 115200 + // # binary_format : QUATERNION, IMU + // # build_date : Jul 12 2014 18:55:53 + // # coordinate_transformation_global_to_user : 0.000000e+00, 0.000000e+00, 0.000000e+00, 1.000000e+00 + // # coordinate_transformation_sensor_to_vehicle : 0.000000e+00, 0.000000e+00, 0.000000e+00, 1.000000e+00 + // # divider : 1 + // # firmware_version : 1.5 + // # gyro_bias : -1.633780e+01, -7.488200e+00, -1.367940e+01 + // # gyro_max : 2000 + // # gyro_sensitivity : 6.097561e-02 + // # gyro_sensitivity_matrix : 6.128651e-02, -2.204396e-04, -9.754782e-04, 1.441085e-05, 6.082033e-02, 5.099393e-04, -1.803549e-04, 7.779496e-04, 6.148182e-02 + // # magnet_bias : 9.936985e+01, 1.039952e+01, -9.331067e+01 + // # magnet_sensitivity_matrix : 6.128308e-01, 1.720049e-03, 9.577197e-03, 1.720049e-03, 5.859380e-01, 1.020694e-03, 9.577197e-03, 1.020694e-03, 6.279327e-01 + // # max_rate : 100 + // # mode : BT + // # platform : myAhrsRevC + // # product_name : myAHRS+ + // # sensor_id : 0 + // # sensor_serial_number : 464432970808430886 + // # yaw_offset : OFF + // ######################################################################################################################## + + std::vector get_attribute_list() { + LockGuard _l(mutex_attribute); + std::vector attribute_list; + for(std::map::iterator it=attribute_map.begin(); it!=attribute_map.end(); it++) { + attribute_list.push_back(it->first); + } + return attribute_list; + } + + /* + * re-sync all attributes + */ + bool resync() { + bool ok = false; + std::string mode; + + do { + if(cmd_mode() == false) { + DBG_PRINTF(true, "cmd_mode() returns false\n"); + break; + } + if(get_attribute("mode", mode) == false) { + DBG_PRINTF(true, "get_attribute('mode') returns false\n"); + break; + } + if(cmd_mode("T") == false) { + DBG_PRINTF(true, "cmd_mode(T) returns false\n"); + break; + } + if(cmd_divider() == false) { + DBG_PRINTF(true, "cmd_divider() returns false\n"); + break; + } + if(cmd_ascii_data_format() == false) { + DBG_PRINTF(true, "cmd_ascii_data_format() returns false\n"); + break; + } + if(cmd_binary_data_format() == false) { + DBG_PRINTF(true, "cmd_binary_data_format() returns false\n"); + break; + } + if(cmd_set_user_orientation_offset() == false) { + DBG_PRINTF(true, "cmd_set_user_orientation_offset() returns false\n"); + break; + } + if(cmd_calibration_parameter('A') == false) { + DBG_PRINTF(true, "cmd_calibration_parameter(A) returns false\n"); + break; + } + if(cmd_calibration_parameter('G') == false) { + DBG_PRINTF(true, "cmd_calibration_parameter(G) returns false\n"); + break; + } + if(cmd_calibration_parameter('M') == false) { + DBG_PRINTF(true, "cmd_calibration_parameter(M) returns false\n"); + break; + } + if(cmd_version() == false) { + DBG_PRINTF(true, "cmd_version() returns false\n"); + break; + } + if(cmd_id() == false) { + DBG_PRINTF(true, "cmd_id() returns false\n"); + break; + } + if(cmd_sensitivity() == false) { + DBG_PRINTF(true, "cmd_sensitivity() returns false\n"); + break; + } + if(cmd_baudrate() == false) { + DBG_PRINTF(true, "cmd_baudrate() returns false\n"); + break; + } + if(cmd_mode(mode.c_str()) == false) { + DBG_PRINTF(true, "cmd_mode(restore) returns false\n"); + break; + } + + ok = true; + }while(0); + + if(ok == true && debug) { + DBG_PRINTF(debug, "\n\n### ATTRIBUTES #####\n"); + for (std::map::iterator it=attribute_map.begin(); it!=attribute_map.end(); ++it) { + DBG_PRINTF(debug, "\t- attribute(%s) = \"%s\"\n", it->first.c_str(), it->second.c_str()); + } + } + + if(debug && ok == false) { + Platform::msleep(100); + } + + return ok; + } + + /* + * myahrs_plus commands + */ + void cmd_trigger() { + // no response, no wait + send_command("@trig", -1); + } + + bool cmd_ping(int timeout_msec=500) { + return send_command("@ping", timeout_msec); + } + + bool cmd_divider(int timeout_msec=500) { + return send_command("@divider", timeout_msec); + } + + bool cmd_divider(const char* divider, int timeout_msec=500) { + if(strlen(divider) > 100) { + return false; + } + char buf[128]; + sprintf(buf, "@divider,%s", divider); + return send_command(buf, timeout_msec); + } + + bool cmd_mode(const char* mode_string=0, int timeout_msec=500) { + if(mode_string == 0) { + return send_command("@mode", timeout_msec); + } + else { + std::string command = std::string("@mode,") + std::string(mode_string); + return send_command(command.c_str(), timeout_msec); + } + } + + bool cmd_ascii_data_format(const char* asc_output=0, int timeout_msec=500) { + if(asc_output == 0) { + return send_command("@asc_out", timeout_msec); + } + else { + std::string command = std::string("@asc_out,") + std::string(asc_output); + return send_command(command.c_str(), timeout_msec); + } + } + + bool cmd_binary_data_format(const char* bin_output=0, int timeout_msec=500) { + if(bin_output == 0) { + return send_command("@bin_out", timeout_msec); + } + else { + std::string command = std::string("@bin_out,") + std::string(bin_output); + return send_command(command.c_str(), timeout_msec); + } + } + + bool cmd_set_user_orientation_offset(int timeout_msec=500) { + return send_command("@set_offset", timeout_msec); + } + + bool cmd_set_user_orientation_offset(const char* enable_yaw_offset, int timeout_msec=500) { + std::string command = std::string("@set_offset,") + std::string(enable_yaw_offset); + return send_command(command.c_str(), timeout_msec); + } + + bool cmd_clear_user_orientation_offset(int timeout_msec=500) { + return send_command("@clr_offset", timeout_msec); + } + + bool cmd_calibration_parameter(char sensor_type, const char* calibration_parameters=0, int timeout_msec=500) { + char buf[512]; + if(calibration_parameters == 0) { + sprintf(buf, "@calib,%c", sensor_type); + return send_command(buf, timeout_msec); + } + else { + if(strlen(calibration_parameters) > sizeof(buf)-20) { + return false; + } + sprintf(buf, "@calib,%c,%s", sensor_type, calibration_parameters); + return send_command(buf, timeout_msec); + } + } + + bool cmd_restore_all_default(int timeout_msec=500) { + return send_command("@factory", timeout_msec); + } + + bool cmd_version(int timeout_msec=500) { + return send_command("@version", timeout_msec); + } + + bool cmd_id(int timeout_msec=500) { + return send_command("@id", timeout_msec); + } + + bool cmd_id(const char* str_sensor_id, int timeout_msec=500) { + if(strlen(str_sensor_id) > 100) { + return false; + } + char buf[128]; + sprintf(buf, "@id,%s", str_sensor_id); + return send_command(buf, timeout_msec); + } + + bool cmd_serial_number(int timeout_msec=500) { + return send_command("@sn", timeout_msec); + } + + bool cmd_sensitivity(int timeout_msec=500) { + return send_command("@sensitivity", timeout_msec); + } + + bool cmd_baudrate(int timeout_msec=500) { + return send_command("@baudrate", timeout_msec); + } + + bool cmd_baudrate(const char* baudrate, int timeout_msec=500) { + if(strlen(baudrate) > 100) { + return false; + } + char buf[128]; + sprintf(buf, "@baudrate,%s", baudrate); + return send_command(buf, timeout_msec); + } + + bool cmd_save(int timeout_msec=500) { + return send_command("@save", timeout_msec); + } + + protected: + /* + * Event handler interface + */ + virtual void OnSensorData(int sensor_id, SensorData sensor_data) {} + virtual void OnAttributeChange(int sensor_id, std::string attribute_name, std::string value) {} + + private: + bool send_command(std::string command_string, int timeout_msec) { + LockGuard _l(mutex_communication); + + DBG_PRINTF(debug, "### SEND : %s\n", command_string.c_str()); + + uint8_t crc = 0; + char crc_string[16]; + for(size_t i=0; i tokens; + std::vector cmd_tokens; + + do { + /* + * wait for response + */ + DBG_PRINTF(debug, "### WAITING RESPONSE\n"); + if(response_message_queue.wait(timeout_msec) == false) { + DBG_PRINTF(debug, "TIMEOUT!!(%d)\n", timeout_msec); + return false; + } + + #ifdef WIN32 + /* + * stupid windows... + */ + while(response_message_queue.size() == 0); + #endif // WIN32 + + /* + * check response + */ + tokens.clear(); + if(response_message_queue.pop(tokens) == false) { + if(debug) { + Platform::msleep(10); + DBG_PRINTF(debug, "ERROR??? : response_queue.pop() returns false (response_queue.size() %lu, cmd=%s)\n", response_message_queue.size(), command_string.c_str()); + } + return false; + } + + cmd_tokens.clear(); + StringUtil::split(cmd_tokens, command_string.c_str(), ','); + StringUtil::replace(cmd_tokens[0], "@", "~"); + if(tokens[0] == cmd_tokens[0]) { + DBG_PRINTF(debug, "### RCV OK(%s)\n", cmd_tokens[0].c_str()); + break; + } + else { + DBG_PRINTF(debug, "ERROR: invalid response. command %s, response %s)\n", cmd_tokens[0].c_str(), tokens[0].c_str()); + continue; + } + }while(1); + + + /* + * handle error response + */ + if(tokens[1] != std::string("OK")) { + // print error message + DBG_PRINTF(debug, "ERROR: status is not OK. command %s)\n", cmd_tokens[0].c_str()); + return false; + } + + /* + * run response handler + */ + std::map::iterator it = ascii_handler_rsp_map.find(tokens[0]); + if(it != ascii_handler_rsp_map.end()) { + std::map attributes; + bool res = true; + if(StringUtil::extract_attributes(attributes, tokens) > 0) { + LockGuard _l(mutex_attribute); + res = (this->*(it->second))(attributes); + if(res == false) { + DBG_PRINTF(debug, "ERROR: message hander returns false. command %s)\n", cmd_tokens[0].c_str()); + } + } + + if(res) { + DBG_PRINTF(debug, "### OK : message hander returns true. command %s, rcvq_sz %d)\n", cmd_tokens[0].c_str(), response_message_queue.size()); + } + + return res; + } + else { + return false; + } + } + + /* + * threads + */ + void proc_receiver() { + int len; + unsigned char buffer[1024]; + + thread_receiver_ready = true; + + while(true) { + memset(buffer, 0, sizeof(buffer)); + len = serial.Read(buffer, sizeof(buffer)-1); + if(len == 0) { + Platform::msleep(1); + continue; + } + else if(len < 0) { + break; // stop thread + } + else if(len > 0) { + DBG_PRINTF(debug, "### SZ(%d) [%s]\n", len, buffer); + protocol.feed(buffer, len); + } + } + + DBG_PRINTF(debug, "### %s() exit\n", __FUNCTION__); + } + + void proc_callback() { + EventItem* event; + bool exit_thread = false; + + while(exit_thread == false) { + event = event_queue.pop_event(); + if(event == 0) { + event_queue.wait(); + continue; + } + + try { + switch(event->event_id) { + case EventItem::EXIT: + DBG_PRINTF(debug, "receive EventItem::EXIT\n"); + exit_thread = true; + break; + case EventItem::ATTRIBUTE: { + std::map* attribute_pair = event->get_attribute(); + for(std::map::iterator it=attribute_pair->begin(); it!=attribute_pair->end(); it++) { + OnAttributeChange(sensor_id, it->first, it->second); + } + } + break; + case EventItem::DATA: { + SensorData* sensor_data = event->get_sensor_data(); + OnSensorData(sensor_id, *sensor_data); + } + break; + default: + break; + } + } + catch(...) {} + + delete event; + } // while(exit_thread == false) + + DBG_PRINTF(debug, "### %s() exit\n", __FUNCTION__); + } + + static void* thread_proc_receiver(void* arg) { + ((iMyAhrsPlus*)arg)->proc_receiver(); + return 0; + } + + static void* thread_proc_callback(void* arg) { + ((iMyAhrsPlus*)arg)->proc_callback(); + return 0; + } + + + /* + * ascii response handlers + */ + bool ascii_parse_response(std::vector& tokens) { + if(tokens[0][0] == iAsciiProtocol::MSG_HDR_RESPONSE) { + // message parsing will be delegated to send_command() + response_message_queue.push_back(tokens); + return true; + } + else { + LockGuard _l(mutex_attribute); + + sensor_data.reset(); + bool res = false; + + std::map::iterator it = ascii_handler_data_map.find(tokens[0]); + if(it != ascii_handler_data_map.end()) { + res = (this->*(it->second))(tokens); + } + + if(res && activate_user_event) { + event_queue.push_event_data(sensor_data); + } + + return res; + } + } + + void dbg_show_all_attributes(std::map& attributes) { + for (std::map::iterator it=attributes.begin(); it!=attributes.end(); ++it) { + DBG_PRINTF(debug, " --- %s : %s\n", (it->first).c_str(), (it->second).c_str()); + } + } + + bool ascii_rsp_trigger(std::map& attributes) { + return true; + } + + bool ascii_rsp_ping(std::map& attributes) { + return true; + } + + bool ascii_rsp_divider(std::map& attributes) { + dbg_show_all_attributes(attributes); + + set_attribute("divider", attributes["divider"]); + set_attribute("max_rate", attributes["max_rate"]); + + return true; + } + + bool ascii_rsp_mode(std::map& attributes) { + dbg_show_all_attributes(attributes); + + set_attribute("mode", attributes["mode"]); + + return true; + } + + bool ascii_rsp_asc_out(std::map& attributes) { + dbg_show_all_attributes(attributes); + + set_attribute("ascii_format", attributes["fmt"]); + + return true; + } + + bool ascii_rsp_bin_out(std::map& attributes) { + dbg_show_all_attributes(attributes); + + std::vector prop_bin_format; + StringUtil::split(prop_bin_format, attributes["fmt"].c_str(), ' '); + set_attribute("binary_format", StringUtil::join(prop_bin_format, ", ")); + + return true; + } + + bool ascii_rsp_user_orientation(std::map& attributes) { + dbg_show_all_attributes(attributes); + + // ~set_offset,OK,yaw_offset=OFF,q_s2v=0.000000e+00 0.000000e+00 0.000000e+00 1.000000e+00,q_g2u=0.000000e+00 0.000000e+00 0.000000e+00 1.000000e+00*12 + + set_attribute("yaw_offset", attributes["yaw_offset"]); + + std::vector parameters; + + if(StringUtil::split(parameters, attributes["q_s2v"].c_str(), ' ') != 4) { + return false; + } + set_attribute("coordinate_transformation_sensor_to_vehicle", StringUtil::join(parameters, ", ")); + + if(StringUtil::split(parameters, attributes["q_g2u"].c_str(), ' ') != 4) { + return false; + } + set_attribute("coordinate_transformation_global_to_user", StringUtil::join(parameters, ", ")); + + return true; + } + + bool ascii_rsp_calib(std::map& attributes) { + dbg_show_all_attributes(attributes); + + // ~calib,OK,sensor=A,param=4.882813e-04 0.000000e+00 0.000000e+00 0.000000e+00 4.882813e-04 0.000000e+00 0.000000e+00 0.000000e+00 4.882813e-04 0.000000e+00 0.000000e+00 0.000000e+00*0E + + std::string sensor = attributes["sensor"]; + + switch((char)sensor[0]) { + case 'A': + sensor = "accel"; + break; + case 'G': + sensor = "gyro"; + break; + case 'M': + sensor = "magnet"; + break; + default: + return false; + } + + std::vector parameters; + if(StringUtil::split(parameters, attributes["param"].c_str(), ' ') != 12) { + return false; + } + + std::vector transform(9, ""); + std::vector bias(3, ""); + + std::copy_n(parameters.begin(), 9, transform.begin()); + std::copy_n(parameters.begin()+9, 3, bias.begin()); + + set_attribute(sensor + "_calibration_matrix", StringUtil::join(transform, ", ")); + set_attribute(sensor + "_bias", StringUtil::join(bias, ", ")); + + return true; + } + + bool ascii_rsp_factory(std::map& attributes) { + dbg_show_all_attributes(attributes); + + return true; + } + + bool ascii_rsp_stat(std::map& attributes) { + dbg_show_all_attributes(attributes); + + return true; + } + + bool ascii_rsp_version(std::map& attributes) { + dbg_show_all_attributes(attributes); + + set_attribute("build_date", attributes["build"]); + set_attribute("platform", attributes["platform"]); + set_attribute("product_name", attributes["product"]); + set_attribute("sensor_serial_number", attributes["sn"]); + set_attribute("firmware_version", attributes["ver"]); + + return true; + } + + bool ascii_rsp_id(std::map& attributes) { + dbg_show_all_attributes(attributes); + + std::string str_sensor_id = attributes["id"]; + set_attribute("sensor_id", str_sensor_id); + sensor_id = atoi(str_sensor_id.c_str()); + + return true; + } + + bool ascii_rsp_serial_number(std::map& attributes) { + dbg_show_all_attributes(attributes); + + set_attribute("sensor_serial_number", attributes["sn"]); + + return true; + } + + bool ascii_rsp_sensitivity(std::map& attributes) { + dbg_show_all_attributes(attributes); + + set_attribute("accel_max", attributes["acc_range"]); + set_attribute("gyro_max", attributes["gyro_range"]); + set_attribute("accel_sensitivity", attributes["acc_sensitivity"]); + set_attribute("gyro_sensitivity", attributes["gyro_sensitivity"]); + + return true; + } + + bool ascii_rsp_baudrate(std::map& attributes) { + dbg_show_all_attributes(attributes); + + set_attribute("baudrate", attributes["baudrate"]); + + return true; + } + + bool ascii_rsp_save(std::map& attributes) { + dbg_show_all_attributes(attributes); + + return true; + } + + + /* + * ascii data message handlers + */ + + // $RPY,04,-1.55,-1.25,96.94*50 + bool ascii_update_euler(std::vector& tokens) { + static const int DATA_NUM = 3; + if(tokens.size() != DATA_NUM + 2) { + return false; + } + + sensor_data.sequence_number = atoi(tokens[1].c_str()); + + std::vector str_euler(DATA_NUM, ""); + std::copy_n(tokens.begin() + 2, DATA_NUM, str_euler.begin()); + + EulerAngle e; + e.set(str_euler); + sensor_data.update_attitude(e); + + DBG_PRINTF(debug, "### %s(%s)\n", __FUNCTION__, StringUtil::join(str_euler, ", ").c_str()); + + return true; + } + + // $QUAT,68,0.0006,0.0174,-0.7489,-0.6625*16 + bool ascii_update_quaternion(std::vector& tokens) { + static const int DATA_NUM = 4; + if(tokens.size() != DATA_NUM + 2) { + return false; + } + + sensor_data.sequence_number = atoi(tokens[1].c_str()); + + std::vector str_quat(DATA_NUM, ""); + std::copy_n(tokens.begin() + 2, DATA_NUM, str_quat.begin()); + + Quaternion q; + q.set(str_quat); + sensor_data.update_attitude(q); + + DBG_PRINTF(debug, "### %s(%s)\n", __FUNCTION__, StringUtil::join(str_quat, ", ").c_str()); + + return true; + } + + // $RPYIMU,15,-1.55,-1.25,97.31,-0.0142,-0.0010,-0.9224,-0.9756,-0.3659,-0.8537,-8.4000,-46.8000,5.4000,38.3*36 + bool ascii_update_rpyimu(std::vector& tokens) { + static const int DATA_NUM_EULER = 3; + static const int DATA_NUM_IMU = 10; + if(tokens.size() != DATA_NUM_EULER + DATA_NUM_IMU + 2) { + return false; + } + + sensor_data.sequence_number = atoi(tokens[1].c_str()); + + std::vector str_euler(DATA_NUM_EULER, ""); + std::copy_n(tokens.begin() + 2, DATA_NUM_EULER, str_euler.begin()); + + EulerAngle e; + e.set(str_euler); + sensor_data.update_attitude(e); + + std::vector str_imu(DATA_NUM_IMU, ""); + std::copy_n(tokens.begin() + 2 + DATA_NUM_EULER, DATA_NUM_IMU, str_imu.begin()); + + ImuData imu; + imu.set(str_imu); + sensor_data.update_imu(imu); + + DBG_PRINTF(debug, "### %s(euler=(%s), imu=(%s))\n", __FUNCTION__, StringUtil::join(str_euler, ", ").c_str(), StringUtil::join(str_imu, ", ").c_str()); + + return true; + } + + // $QUATIMU,53,0.0424,-0.1791,0.2366,0.9540,-0.3636,0.0027,-0.9260,0.0156,0.1537,0.2896,212.2648,-72.7573,168.2144,36.8*7F + bool ascii_update_quatimu(std::vector& tokens) { + static const int DATA_NUM_QUAT = 4; + static const int DATA_NUM_IMU = 10; + if(tokens.size() != DATA_NUM_QUAT + DATA_NUM_IMU + 2) { + return false; + } + + sensor_data.sequence_number = atoi(tokens[1].c_str()); + + std::vector str_quat(DATA_NUM_QUAT, ""); + std::copy_n(tokens.begin() + 2, DATA_NUM_QUAT, str_quat.begin()); + + Quaternion q; + q.set(str_quat); + sensor_data.update_attitude(q); + + std::vector str_imu(DATA_NUM_IMU, ""); + std::copy_n(tokens.begin() + 2 + DATA_NUM_QUAT, DATA_NUM_IMU, str_imu.begin()); + + ImuData imu; + imu.set(str_imu); + sensor_data.update_imu(imu); + + DBG_PRINTF(debug, "### %s(quaternion=(%s), imu=(%s))\n", __FUNCTION__, StringUtil::join(str_quat, ", ").c_str(), StringUtil::join(str_imu, ", ").c_str()); + + + return true; + } + + // $RIIMU,59,-16,-8,-1897,-14,-7,-12,-26,-156,18,1101*79 + bool ascii_update_riimu(std::vector& tokens) { + static const int DATA_NUM = 10; + if(tokens.size() != DATA_NUM + 2) { + return false; + } + + sensor_data.sequence_number = atoi(tokens[1].c_str()); + + std::vector str_imu(DATA_NUM, ""); + std::copy_n(tokens.begin() + 2, DATA_NUM, str_imu.begin()); + + ImuData imu_rawdata; + imu_rawdata.set(str_imu); + sensor_data.update_imu(imu_rawdata); + + DBG_PRINTF(debug, "### %s(%s)\n", __FUNCTION__, StringUtil::join(str_imu, ", ").c_str()); + + return true; + } + + // $IMU,74,-0.0054,-0.0015,-0.9204,-0.7317,-0.4878,-0.7317,-7.2000,-45.6000,6.6000,38.2*68 + bool ascii_update_imu(std::vector& tokens) { + static const int DATA_NUM = 10; + if(tokens.size() != DATA_NUM + 2) { + return false; + } + + sensor_data.sequence_number = atoi(tokens[1].c_str()); + + std::vector str_imu(DATA_NUM, ""); + std::copy_n(tokens.begin() + 2, DATA_NUM, str_imu.begin()); + + ImuData imu; + imu.set(str_imu); + sensor_data.update_imu(imu); + + DBG_PRINTF(debug, "### %s(%s)\n", __FUNCTION__, StringUtil::join(str_imu, ", ").c_str()); + + return true; + } + + + /* + * binary data message handlers + */ + bool binary_parse_response(std::vector& node_list) { + LockGuard _l(mutex_attribute); + + sensor_data.reset(); + + if(debug) { + for(size_t i=0; iname[%02X] : %s\n", i, node_list.size(), node_list[i].name[0], node_list[i].name.c_str()); + } + } + + for(size_t i=0; i::iterator it = binary_handler_data_map.find(node.name); + if(it != binary_handler_data_map.end()) { + if((this->*(it->second))(node) == false) { + return false; + } + } + } + + if(activate_user_event) { + event_queue.push_event_data(sensor_data); + } + + return true; + } + + float int16tofloat(int16_t value, float value_max) { + static const int SCALE_FACTOR = 0x7FFF; + double ratio = ((double)value)/SCALE_FACTOR; + return (float)(ratio*value_max); + } + + bool binary_update_sequence(iNodeParser::Node& node) { + sensor_data.sequence_number = node.list[0].value.ui8; + + DBG_PRINTF(debug, "### binary_update_sequence(%d)\n", node.list[0].value.ui8); + + return true; + } + + bool binary_update_euler(iNodeParser::Node& node) { + static const int DATA_NUM = 3; + if(node.list.size() != DATA_NUM) { + return false; + } + + float roll = int16tofloat(node.list[0].value.i16, (float)EULER_RANGE); + float pitch = int16tofloat(node.list[1].value.i16, (float)EULER_RANGE); + float yaw = int16tofloat(node.list[2].value.i16, (float)EULER_RANGE); + + EulerAngle e; + e.set(roll, pitch, yaw); + sensor_data.update_attitude(e); + + DBG_PRINTF(debug, "### %s(%f, %f, %f)\n", __FUNCTION__, roll, pitch, yaw); + + return true; + } + + bool binary_update_quaternion(iNodeParser::Node& node) { + static const int DATA_NUM = 4; + if(node.list.size() != DATA_NUM) { + return false; + } + + float x = int16tofloat(node.list[0].value.i16, 1); + float y = int16tofloat(node.list[1].value.i16, 1); + float z = int16tofloat(node.list[2].value.i16, 1); + float w = int16tofloat(node.list[3].value.i16, 1); + + Quaternion q; + q.set(x, y, z, w); + sensor_data.update_attitude(q); + + DBG_PRINTF(debug, "### %s(%f, %f, %f, %f)\n", __FUNCTION__, x,y,z,w); + + return true; + } + + bool binary_update_imu(iNodeParser::Node& node) { + static const int DATA_NUM = 10; + if(node.list.size() != DATA_NUM) { + return false; + } + + float m[DATA_NUM]; + + m[0] = int16tofloat(node.list[0].value.i16, (float)ACCEL_RANGE); + m[1] = int16tofloat(node.list[1].value.i16, (float)ACCEL_RANGE); + m[2] = int16tofloat(node.list[2].value.i16, (float)ACCEL_RANGE); + + m[3] = int16tofloat(node.list[3].value.i16, (float)GYRO_RANGE); + m[4] = int16tofloat(node.list[4].value.i16, (float)GYRO_RANGE); + m[5] = int16tofloat(node.list[5].value.i16, (float)GYRO_RANGE); + + m[6] = int16tofloat(node.list[6].value.i16, (float)MAGNET_RANGE); + m[7] = int16tofloat(node.list[7].value.i16, (float)MAGNET_RANGE); + m[8] = int16tofloat(node.list[8].value.i16, (float)MAGNET_RANGE); + + m[9] = int16tofloat(node.list[9].value.i16, (float)TEMP_RANGE); + + ImuData imu; + imu.set(m); + sensor_data.update_imu(imu); + + if(debug) { + std::vector str_list; + StringUtil::to_string_list(str_list, m, DATA_NUM); + DBG_PRINTF(debug, "### %s(%s)\n", __FUNCTION__, StringUtil::join(str_list, ", ").c_str()); + } + + return true; + } + + bool binary_update_riimu(iNodeParser::Node& node) { + static const int DATA_NUM = 10; + if(node.list.size() != DATA_NUM) { + return false; + } + + int m[DATA_NUM]; + for(int i=0; i imu_rawdata; + imu_rawdata.set(m); + sensor_data.update_imu(imu_rawdata); + + if(debug) { + std::vector str_list; + StringUtil::to_string_list(str_list, m, DATA_NUM); + DBG_PRINTF(debug, "### %s(%s)\n", __FUNCTION__, StringUtil::join(str_list, ", ").c_str()); + } + + return true; + } + }; // iMyAhrsPlus + + + class MyAhrsPlus : public iMyAhrsPlus + { + Platform::Event event; + Platform::Mutex lock; + + void(*attribute_callback)(void* context, int sensor_id, const char* attribute_name, const char* value); + void* attribute_callback_context; + + void(*data_callback)(void* context, int sensor_id, SensorData* sensor_data); + void* data_callback_context; + + uint32_t sample_count; + + public: + MyAhrsPlus(std::string port="", unsigned int baudrate=115200) + : iMyAhrsPlus(port, baudrate), sample_count(0) + , attribute_callback(0), attribute_callback_context(0) + , data_callback(0), data_callback_context(0) + {} + + virtual ~MyAhrsPlus() { + attribute_callback = 0; + attribute_callback_context = 0; + data_callback = 0; + data_callback_context = 0; + } + + bool wait_data(int timeout_msec=500) { + return event.wait(timeout_msec); + } + + void register_attribute_callback(void(*callback)(void* context, int sensor_id, const char* attribute_name, const char* value), void* callback_context) { + LockGuard _l(lock); + attribute_callback_context = callback_context; + attribute_callback = callback; + } + + void register_data_callback(void(*callback)(void* context, int sensor_id, SensorData* sensor_data), void* callback_context) { + LockGuard _l(lock); + data_callback_context = callback_context; + data_callback = callback; + } + + inline uint32_t get_sample_count() { + LockGuard _l(lock); + return sample_count; + } + + protected: + void OnAttributeChange(int sensor_id, std::string attribute_name, std::string value) { + LockGuard _l(lock); + if(attribute_callback) { + try { + attribute_callback(attribute_callback_context, sensor_id, attribute_name.c_str(), value.c_str()); + } catch(...) {} + } + } + + void OnSensorData(int sensor_id, SensorData data) { + LockGuard _l(lock); + sample_count++; + event.set(); + if(data_callback) { + try { + data_callback(data_callback_context, sensor_id, &data); + } catch(...) {} + } + } + }; +}; // WithRobot + +#endif // __MYAHRS_PLUS_H_ diff --git a/quad_imu/launch/quad_imu.launch b/quad_imu/launch/quad_imu.launch new file mode 100644 index 000000000..9db7dbd35 --- /dev/null +++ b/quad_imu/launch/quad_imu.launch @@ -0,0 +1,8 @@ + + + + + + + + diff --git a/quad_imu/package.xml b/quad_imu/package.xml new file mode 100644 index 000000000..f8f943af8 --- /dev/null +++ b/quad_imu/package.xml @@ -0,0 +1,23 @@ + + + quad_imu + 0.1.2 + + myahrs_driver is a driver package for the WITHROBOT's myAHRS+. The myAHRS+ is a low cost high performance AHRS(Attitude Heading Reference System) with USB/UART/I2C interface. The myAHRS+ board contains a 3-axis 16-bit gyroscope, a 3-axis 16-bit accelerometer and a 3-axis 13-bit magnetometer. The driver should also work with USB port. + + BSD + Yoonseok Pyo + Yoonseok Pyo + https://github.com/robotpilot/myahrs_driver/issues + https://github.com/robotpilot/myahrs_driver + http://wiki.ros.org/myahrs_driver + catkin + roscpp + std_msgs + sensor_msgs + tf + roscpp + std_msgs + sensor_msgs + tf + diff --git a/quad_imu/rviz_cfg/imu_test.rviz b/quad_imu/rviz_cfg/imu_test.rviz new file mode 100644 index 000000000..90a06b0b3 --- /dev/null +++ b/quad_imu/rviz_cfg/imu_test.rviz @@ -0,0 +1,149 @@ +Panels: + - Class: rviz/Displays + Help Height: 78 + Name: Displays + Property Tree Widget: + Expanded: + - /Global Options1 + - /Status1 + - /Axes1 + - /TF1 + - /TF1/Status1 + - /TF1/Frames1 + - /TF1/Tree1 + Splitter Ratio: 0.5 + Tree Height: 776 + - Class: rviz/Selection + Name: Selection + - Class: rviz/Tool Properties + Expanded: + - /2D Pose Estimate1 + - /2D Nav Goal1 + - /Publish Point1 + Name: Tool Properties + Splitter Ratio: 0.588679 + - Class: rviz/Views + Expanded: + - /Current View1 + Name: Views + Splitter Ratio: 0.5 + - Class: rviz/Time + Experimental: false + Name: Time + SyncMode: 0 + SyncSource: "" +Visualization Manager: + Class: "" + Displays: + - Alpha: 0.5 + Cell Size: 1 + Class: rviz/Grid + Color: 160; 160; 164 + Enabled: true + Line Style: + Line Width: 0.03 + Value: Lines + Name: Grid + Normal Cell Count: 0 + Offset: + X: 0 + Y: 0 + Z: 0 + Plane: XY + Plane Cell Count: 10 + Reference Frame: + Value: true + - Class: rviz/Axes + Enabled: false + Length: 0.5 + Name: Axes + Radius: 0.03 + Reference Frame: imu_link + Value: false + - Class: rviz/TF + Enabled: true + Frame Timeout: 15 + Frames: + All Enabled: true + base_link: + Value: true + imu_link: + Value: true + Marker Scale: 4 + Name: TF + Show Arrows: true + Show Axes: true + Show Names: false + Tree: + imu_link: + base_link: + {} + Update Interval: 0 + Value: true + - Alpha: 1 + Class: rviz_plugin_tutorials/Imu + Color: 204; 51; 204 + Enabled: false + History Length: 1 + Name: Imu + Topic: /imu/data + Value: false + Enabled: true + Global Options: + Background Color: 48; 48; 48 + Fixed Frame: imu_link + Frame Rate: 30 + Name: root + Tools: + - Class: rviz/Interact + Hide Inactive Objects: true + - Class: rviz/MoveCamera + - Class: rviz/Select + - Class: rviz/FocusCamera + - Class: rviz/Measure + - Class: rviz/SetInitialPose + Topic: /initialpose + - Class: rviz/SetGoal + Topic: /move_base_simple/goal + - Class: rviz/PublishPoint + Single click: true + Topic: /clicked_point + Value: true + Views: + Current: + Class: rviz/Orbit + Distance: 2.05211 + Enable Stereo Rendering: + Stereo Eye Separation: 0.06 + Stereo Focal Distance: 1 + Swap Stereo Eyes: false + Value: false + Focal Point: + X: 0.121842 + Y: 0.228229 + Z: 0.06683 + Name: Current View + Near Clip Distance: 0.01 + Pitch: 0.675398 + Target Frame: + Value: Orbit (rviz) + Yaw: 4.89859 + Saved: ~ +Window Geometry: + Displays: + collapsed: false + Height: 1056 + Hide Left Dock: false + Hide Right Dock: false + QMainWindow State: 000000ff00000000fd00000004000000000000013c00000396fc0200000008fb0000001200530065006c0065006300740069006f006e00000001e10000009b0000005e00fffffffb0000001e0054006f006f006c002000500072006f007000650072007400690065007302000001ed000001df00000185000000a3fb000000120056006900650077007300200054006f006f02000001df000002110000018500000122fb000000200054006f006f006c002000500072006f0070006500720074006900650073003203000002880000011d000002210000017afb000000100044006900730070006c006100790073010000002800000396000000d000fffffffb0000002000730065006c0065006300740069006f006e00200062007500660066006500720200000138000000aa0000023a00000294fb00000014005700690064006500530074006500720065006f02000000e6000000d2000003ee0000030bfb0000000c004b0069006e0065006300740200000186000001060000030c00000261000000010000010f00000396fc0200000003fb0000001e0054006f006f006c002000500072006f00700065007200740069006500730100000041000000780000000000000000fb0000000a00560069006500770073010000002800000396000000a900fffffffb0000001200530065006c0065006300740069006f006e010000025a000000b200000000000000000000000200000490000000a9fc0100000001fb0000000a00560069006500770073030000004e00000080000002e100000197000000030000074f0000003efc0100000002fb0000000800540069006d006501000000000000074f000002f600fffffffb0000000800540069006d00650100000000000004500000000000000000000004f80000039600000004000000040000000800000008fc0000000100000002000000010000000a0054006f006f006c00730100000000ffffffff0000000000000000 + Selection: + collapsed: false + Time: + collapsed: false + Tool Properties: + collapsed: false + Views: + collapsed: false + Width: 1871 + X: 1969 + Y: 24 diff --git a/quad_imu/src/myahrs_driver.cpp b/quad_imu/src/myahrs_driver.cpp new file mode 100644 index 000000000..fc06ed0cc --- /dev/null +++ b/quad_imu/src/myahrs_driver.cpp @@ -0,0 +1,280 @@ +//------------------------------------------------------------------------------ +// Copyright (c) 2015, Yoonseok Pyo +// All rights reserved. + +// License: BSD + +// Redistribution and use in source and binary forms, with or without +// modification, are permitted provided that the following conditions are met: + +// * Redistributions of source code must retain the above copyright notice, this +// list of conditions and the following disclaimer. + +// * Redistributions in binary form must reproduce the above copyright notice, +// this list of conditions and the following disclaimer in the documentation +// and/or other materials provided with the distribution. + +// * Neither the name of myahrs_driver nor the names of its +// contributors may be used to endorse or promote products derived from +// this software without specific prior written permission. + +// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" +// AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE +// IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE +// DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE +// FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL +// DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR +// SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER +// CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, +// OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE +// OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. +//------------------------------------------------------------------------------ +#include + +#include +#include +#include +#include +#include +#include + +//------------------------------------------------------------------------------ +using namespace WithRobot; + +//------------------------------------------------------------------------------ +class MyAhrsDriverForROS : public iMyAhrsPlus +{ +private: + ros::NodeHandle nh_; + ros::NodeHandle nh_priv_; + + ros::Publisher imu_data_raw_pub_; + ros::Publisher imu_data_pub_; + ros::Publisher imu_mag_pub_; + ros::Publisher imu_temperature_pub_; + + tf::TransformBroadcaster broadcaster_; + + Platform::Mutex lock_; + SensorData sensor_data_; + bool publish_tf; + std::string parent_frame_id_; + std::string frame_id_; + double linear_acceleration_stddev_; + double angular_velocity_stddev_; + double magnetic_field_stddev_; + double orientation_stddev_; + + void OnSensorData(int sensor_id, SensorData data) + { + LockGuard _l(lock_); + sensor_data_ = data; + publish_topic(sensor_id); + } + + void OnAttributeChange(int sensor_id, std::string attribute_name, std::string value) + { + printf("OnAttributeChange(id %d, %s, %s)\n", sensor_id, attribute_name.c_str(), value.c_str()); + } + +public: + MyAhrsDriverForROS(std::string port="", int baud_rate=115200) + : iMyAhrsPlus(port, baud_rate), + nh_priv_("~") + { + // dependent on user device + nh_priv_.setParam("port", port); + nh_priv_.setParam("baud_rate", baud_rate); + nh_priv_.param("publish_tf", publish_tf, false); + // default frame id + nh_priv_.param("frame_id", frame_id_, std::string("imu_link")); + // for testing the tf + nh_priv_.param("parent_frame_id", parent_frame_id_, std::string("base_link")); + // defaults obtained experimentally from device + nh_priv_.param("linear_acceleration_stddev", linear_acceleration_stddev_, 0.026831); + nh_priv_.param("angular_velocity_stddev", angular_velocity_stddev_, 0.002428); + nh_priv_.param("magnetic_field_stddev", magnetic_field_stddev_, 0.00000327486); + nh_priv_.param("orientation_stddev", orientation_stddev_, 0.002143); + // publisher for streaming + imu_data_raw_pub_ = nh_.advertise("imu/data_raw", 1); + imu_data_pub_ = nh_.advertise("imu/data", 1); + imu_mag_pub_ = nh_.advertise("imu/mag", 1); + imu_temperature_pub_= nh_.advertise("imu/temperature", 1); + } + + ~MyAhrsDriverForROS() + {} + + bool initialize() + { + bool ok = false; + + do + { + if(start() == false) break; + //Euler angle(x, y, z axis) + //IMU(linear_acceleration, angular_velocity, magnetic_field) + if(cmd_binary_data_format("EULER, IMU") == false) break; + // 100Hz + if(cmd_divider("1") == false) break; + // Binary and Continue mode + if(cmd_mode("BC") == false) break; + ok = true; + } while(0); + + return ok; + } + + inline void get_data(SensorData& data) + { + LockGuard _l(lock_); + data = sensor_data_; + } + + inline SensorData get_data() + { + LockGuard _l(lock_); + return sensor_data_; + } + + void publish_topic(int sensor_id) + { + sensor_msgs::Imu imu_data_raw_msg; + sensor_msgs::Imu imu_data_msg; + sensor_msgs::MagneticField imu_magnetic_msg; + std_msgs::Float64 imu_temperature_msg; + + double linear_acceleration_cov = linear_acceleration_stddev_ * linear_acceleration_stddev_; + double angular_velocity_cov = angular_velocity_stddev_ * angular_velocity_stddev_; + double magnetic_field_cov = magnetic_field_stddev_ * magnetic_field_stddev_; + double orientation_cov = orientation_stddev_ * orientation_stddev_; + + imu_data_raw_msg.linear_acceleration_covariance[0] = + imu_data_raw_msg.linear_acceleration_covariance[4] = + imu_data_raw_msg.linear_acceleration_covariance[8] = + imu_data_msg.linear_acceleration_covariance[0] = + imu_data_msg.linear_acceleration_covariance[4] = + imu_data_msg.linear_acceleration_covariance[8] = linear_acceleration_cov; + + imu_data_raw_msg.angular_velocity_covariance[0] = + imu_data_raw_msg.angular_velocity_covariance[4] = + imu_data_raw_msg.angular_velocity_covariance[8] = + imu_data_msg.angular_velocity_covariance[0] = + imu_data_msg.angular_velocity_covariance[4] = + imu_data_msg.angular_velocity_covariance[8] = angular_velocity_cov; + + imu_data_msg.orientation_covariance[0] = + imu_data_msg.orientation_covariance[4] = + imu_data_msg.orientation_covariance[8] = orientation_cov; + + imu_magnetic_msg.magnetic_field_covariance[0] = + imu_magnetic_msg.magnetic_field_covariance[4] = + imu_magnetic_msg.magnetic_field_covariance[8] = magnetic_field_cov; + + static double convertor_g2a = 9.80665; // for linear_acceleration (g to m/s^2) + static double convertor_d2r = M_PI/180.0; // for angular_velocity (degree to radian) + static double convertor_r2d = 180.0/M_PI; // for easy understanding (radian to degree) + static double convertor_ut2t = 1000000; // for magnetic_field (uT to Tesla) + static double convertor_c = 1.0; // for temperature (celsius) + + double roll, pitch, yaw; + + // original sensor data used the degree unit, convert to radian (see ROS REP103) + // we used the ROS's axes orientation like x forward, y left and z up + // so changed the y and z aixs of myAHRS+ board + roll = sensor_data_.euler_angle.roll*convertor_d2r; + pitch = -sensor_data_.euler_angle.pitch*convertor_d2r; + yaw = -sensor_data_.euler_angle.yaw*convertor_d2r; + + ImuData& imu = sensor_data_.imu; + + tf::Quaternion orientation = tf::createQuaternionFromRPY(roll, pitch, yaw); + + ros::Time now = ros::Time::now(); + + imu_data_raw_msg.header.stamp = + imu_data_msg.header.stamp = + imu_magnetic_msg.header.stamp = now; + + imu_data_raw_msg.header.frame_id = + imu_data_msg.header.frame_id = + imu_magnetic_msg.header.frame_id = frame_id_; + + // orientation + imu_data_msg.orientation.x = orientation[0]; + imu_data_msg.orientation.y = orientation[1]; + imu_data_msg.orientation.z = orientation[2]; + imu_data_msg.orientation.w = orientation[3]; + + // original data used the g unit, convert to m/s^2 + imu_data_raw_msg.linear_acceleration.x = + imu_data_msg.linear_acceleration.x = imu.ax * convertor_g2a; + imu_data_raw_msg.linear_acceleration.y = + imu_data_msg.linear_acceleration.y = -imu.ay * convertor_g2a; + imu_data_raw_msg.linear_acceleration.z = + imu_data_msg.linear_acceleration.z = -imu.az * convertor_g2a; + + // original data used the degree/s unit, convert to radian/s + imu_data_raw_msg.angular_velocity.x = + imu_data_msg.angular_velocity.x = imu.gx * convertor_d2r; + imu_data_raw_msg.angular_velocity.y = + imu_data_msg.angular_velocity.y = -imu.gy * convertor_d2r; + imu_data_raw_msg.angular_velocity.z = + imu_data_msg.angular_velocity.z = -imu.gz * convertor_d2r; + + // original data used the uTesla unit, convert to Tesla + imu_magnetic_msg.magnetic_field.x = imu.mx / convertor_ut2t; + imu_magnetic_msg.magnetic_field.y = -imu.my / convertor_ut2t; + imu_magnetic_msg.magnetic_field.z = -imu.mz / convertor_ut2t; + + // original data used the celsius unit + imu_temperature_msg.data = imu.temperature; + + // publish the IMU data + imu_data_raw_pub_.publish(imu_data_raw_msg); + imu_data_pub_.publish(imu_data_msg); + imu_mag_pub_.publish(imu_magnetic_msg); + imu_temperature_pub_.publish(imu_temperature_msg); + + // publish tf + if(publish_tf) + { + broadcaster_.sendTransform(tf::StampedTransform(tf::Transform(tf::createQuaternionFromRPY(roll, pitch, yaw), + tf::Vector3(0.0, 0.0, 0.0)), + ros::Time::now(), + frame_id_, + parent_frame_id_)); + } + } +}; + + +//------------------------------------------------------------------------------ +int main(int argc, char* argv[]) +{ + ros::init(argc, argv, "myahrs_driver"); + + std::string port = std::string("/dev/ttyACM0"); + int baud_rate = 115200; + + ros::param::get("~port", port); + ros::param::get("~baud_rate", baud_rate); + + MyAhrsDriverForROS sensor(port, baud_rate); + + if(sensor.initialize() == false) + { + ROS_ERROR("%s\n", "Initialize() returns false, please check your devices."); + return 0; + } + else + { + ROS_INFO("Initialization OK!\n"); + } + + ros::spin(); + + return 0; +} + +//------------------------------------------------------------------------------ diff --git a/quad_logger/bags/frames.gv b/quad_logger/bags/frames.gv new file mode 100644 index 000000000..a67eb23c4 --- /dev/null +++ b/quad_logger/bags/frames.gv @@ -0,0 +1,54 @@ +digraph G { +"robot_1_trajectory/body" -> "robot_1_trajectory/text_left"[label="Broadcaster: /robot_1/trajectory/robot_state_publisher\nAverage rate: 10000.000 Hz\nMost recent transform: 0.000 ( 1669416651.141 sec old)\nBuffer length: 0.000 sec\n"]; +"robot_1_trajectory/body" -> "robot_1_trajectory/text_right"[label="Broadcaster: /robot_1/trajectory/robot_state_publisher\nAverage rate: 10000.000 Hz\nMost recent transform: 0.000 ( 1669416651.141 sec old)\nBuffer length: 0.000 sec\n"]; +"robot_1_trajectory/lower0" -> "robot_1_trajectory/toe0"[label="Broadcaster: /robot_1/trajectory/robot_state_publisher\nAverage rate: 10000.000 Hz\nMost recent transform: 0.000 ( 1669416651.141 sec old)\nBuffer length: 0.000 sec\n"]; +"robot_1_trajectory/lower1" -> "robot_1_trajectory/toe1"[label="Broadcaster: /robot_1/trajectory/robot_state_publisher\nAverage rate: 10000.000 Hz\nMost recent transform: 0.000 ( 1669416651.141 sec old)\nBuffer length: 0.000 sec\n"]; +"robot_1_trajectory/lower2" -> "robot_1_trajectory/toe2"[label="Broadcaster: /robot_1/trajectory/robot_state_publisher\nAverage rate: 10000.000 Hz\nMost recent transform: 0.000 ( 1669416651.141 sec old)\nBuffer length: 0.000 sec\n"]; +"robot_1_trajectory/lower3" -> "robot_1_trajectory/toe3"[label="Broadcaster: /robot_1/trajectory/robot_state_publisher\nAverage rate: 10000.000 Hz\nMost recent transform: 0.000 ( 1669416651.141 sec old)\nBuffer length: 0.000 sec\n"]; +"robot_1_ground_truth/body" -> "robot_1_ground_truth/text_left"[label="Broadcaster: /robot_1/ground_truth/robot_state_publisher\nAverage rate: 10000.000 Hz\nMost recent transform: 0.000 ( 1669416651.141 sec old)\nBuffer length: 0.000 sec\n"]; +"robot_1_ground_truth/body" -> "robot_1_ground_truth/text_right"[label="Broadcaster: /robot_1/ground_truth/robot_state_publisher\nAverage rate: 10000.000 Hz\nMost recent transform: 0.000 ( 1669416651.141 sec old)\nBuffer length: 0.000 sec\n"]; +"robot_1_ground_truth/lower0" -> "robot_1_ground_truth/toe0"[label="Broadcaster: /robot_1/ground_truth/robot_state_publisher\nAverage rate: 10000.000 Hz\nMost recent transform: 0.000 ( 1669416651.141 sec old)\nBuffer length: 0.000 sec\n"]; +"robot_1_ground_truth/lower1" -> "robot_1_ground_truth/toe1"[label="Broadcaster: /robot_1/ground_truth/robot_state_publisher\nAverage rate: 10000.000 Hz\nMost recent transform: 0.000 ( 1669416651.141 sec old)\nBuffer length: 0.000 sec\n"]; +"robot_1_ground_truth/lower2" -> "robot_1_ground_truth/toe2"[label="Broadcaster: /robot_1/ground_truth/robot_state_publisher\nAverage rate: 10000.000 Hz\nMost recent transform: 0.000 ( 1669416651.141 sec old)\nBuffer length: 0.000 sec\n"]; +"robot_1_ground_truth/lower3" -> "robot_1_ground_truth/toe3"[label="Broadcaster: /robot_1/ground_truth/robot_state_publisher\nAverage rate: 10000.000 Hz\nMost recent transform: 0.000 ( 1669416651.141 sec old)\nBuffer length: 0.000 sec\n"]; +edge [style=invis]; + subgraph cluster_legend { style=bold; color=black; label ="view_frames Result"; +"Recorded at time: 1669416651.141"[ shape=plaintext ] ; + }->"robot_1_trajectory/body"; +edge [style=invis]; + subgraph cluster_legend { style=bold; color=black; label ="view_frames Result"; +"Recorded at time: 1669416651.141"[ shape=plaintext ] ; + }->"robot_1_trajectory/lower0"; +edge [style=invis]; + subgraph cluster_legend { style=bold; color=black; label ="view_frames Result"; +"Recorded at time: 1669416651.141"[ shape=plaintext ] ; + }->"robot_1_trajectory/lower1"; +edge [style=invis]; + subgraph cluster_legend { style=bold; color=black; label ="view_frames Result"; +"Recorded at time: 1669416651.141"[ shape=plaintext ] ; + }->"robot_1_trajectory/lower2"; +edge [style=invis]; + subgraph cluster_legend { style=bold; color=black; label ="view_frames Result"; +"Recorded at time: 1669416651.141"[ shape=plaintext ] ; + }->"robot_1_trajectory/lower3"; +edge [style=invis]; + subgraph cluster_legend { style=bold; color=black; label ="view_frames Result"; +"Recorded at time: 1669416651.141"[ shape=plaintext ] ; + }->"robot_1_ground_truth/body"; +edge [style=invis]; + subgraph cluster_legend { style=bold; color=black; label ="view_frames Result"; +"Recorded at time: 1669416651.141"[ shape=plaintext ] ; + }->"robot_1_ground_truth/lower0"; +edge [style=invis]; + subgraph cluster_legend { style=bold; color=black; label ="view_frames Result"; +"Recorded at time: 1669416651.141"[ shape=plaintext ] ; + }->"robot_1_ground_truth/lower1"; +edge [style=invis]; + subgraph cluster_legend { style=bold; color=black; label ="view_frames Result"; +"Recorded at time: 1669416651.141"[ shape=plaintext ] ; + }->"robot_1_ground_truth/lower2"; +edge [style=invis]; + subgraph cluster_legend { style=bold; color=black; label ="view_frames Result"; +"Recorded at time: 1669416651.141"[ shape=plaintext ] ; + }->"robot_1_ground_truth/lower3"; +} \ No newline at end of file diff --git a/quad_logger/scripts/processLog.m b/quad_logger/scripts/processLog.m index f7b7ff561..b8262734e 100644 --- a/quad_logger/scripts/processLog.m +++ b/quad_logger/scripts/processLog.m @@ -47,6 +47,8 @@ function processLog(varargin) controlGRFs = data.controlGRFs; localPlan = data.localPlan; +% disp(stateGroundTruth); +% disp(stateEstimate); %% Plot the data % Plot the state @@ -54,42 +56,42 @@ function processLog(varargin) if ~isempty(stateGroundTruth) [stateFigs] = plotState(stateGroundTruth,tWindowStates,'-', bTitles, stateFigs); end -if ~isempty(stateTrajectory) - [stateFigs] = plotState(stateTrajectory,tWindowStates, ':', bTitles, stateFigs); -end +% if ~isempty(stateTrajectory) +% [stateFigs] = plotState(stateTrajectory,tWindowStates, ':', bTitles, stateFigs); +% end if ~isempty(stateEstimate) [stateFigs] = plotState(stateEstimate,tWindowStates, '--', bTitles, stateFigs); end - -% Plot the control -controlFigs = []; -if ~isempty(stateGRFs) - controlFigs = plotControl(stateGRFs,tWindowControl,'-', bTitles, controlFigs); -end -if ~isempty(controlGRFs) - controlFigs = plotControl(controlGRFs,tWindowControl,':', bTitles,controlFigs); -end - -% Plot local plan information if desired -localPlanFigs = []; -if bPlotLocalPlanInfo && ~isempty(localPlan) - localPlanFigs = plotLocalPlan(localPlan,tWindowLocalPlan,'-', bTitles,localPlanFigs); -end - -% Add figures to array -figArray = [stateFigs, controlFigs, localPlanFigs]; - -%% Save the logs and figures in one directory -logDir = []; -if bSave - logDir = saveLog(trialName, figArray); -end - -%% Animate and save - -if bAnimate - robot_path = '../../quad_simulator/spirit_description/urdf/spirit.urdf'; - robot = importrobot(robot_path); - videosDir = fullfile(logDir,'videos/'); - animateData(robot,stateGroundTruth, fullfile(videosDir, trialName), bSave); -end +% +% % Plot the control +% controlFigs = []; +% if ~isempty(stateGRFs) +% controlFigs = plotControl(stateGRFs,tWindowControl,'-', bTitles, controlFigs); +% end +% if ~isempty(controlGRFs) +% controlFigs = plotControl(controlGRFs,tWindowControl,':', bTitles,controlFigs); +% end +% +% % Plot local plan information if desired +% localPlanFigs = []; +% if bPlotLocalPlanInfo && ~isempty(localPlan) +% localPlanFigs = plotLocalPlan(localPlan,tWindowLocalPlan,'-', bTitles,localPlanFigs); +% end +% +% % Add figures to array +%figArray = [stateFigs, controlFigs, localPlanFigs]; +% +% %% Save the logs and figures in one directory +% logDir = []; +% if bSave +% logDir = saveLog(trialName, figArray); +% end +% +% %% Animate and save +% +% if bAnimate +% robot_path = '../../quad_simulator/spirit_description/urdf/spirit.urdf'; +% robot = importrobot(robot_path); +% videosDir = fullfile(logDir,'videos/'); +% animateData(robot,stateGroundTruth, fullfile(videosDir, trialName), bSave); +% end diff --git a/quad_logger/scripts/processLogEKF.m b/quad_logger/scripts/processLogEKF.m new file mode 100644 index 000000000..62164e2ee --- /dev/null +++ b/quad_logger/scripts/processLogEKF.m @@ -0,0 +1,148 @@ +function processLog(varargin) +% processLog Process a quad data log file to generate figures +% processLog uses the default 'quad_log_current' file name to +% yield a data structure containing select topic data. If this bag does +% not exist, the user can select the bag via a UI. +% +% processLog(FILENAME) uses the data in a bag with the specified +% file name, looking in '../bags/'. + +%% Prepare the environment +close all;clc; + +% Check that this is the right current directory otherwise paths won't work +% if ~endsWith(pwd, '/quad_logger/scripts') +% error('This script must be run from quad_logger/scripts/'); +% end + +%% Select rosbag to parse + +% If a trial name is provided, use that to save everything +if nargin>0 + trialName = varargin{1}; + namespace = varargin{2}; +else + trialName = ''; % Set to '' to load via GUI + namespace = 'robot_1'; % Namespace of the robot bag, set to '' if none +end + +%% Set parameters + +bSave = false; % Save the figures/videos +bAnimate = false; % Animate the trajectory (no translation) +bTitles = true; % Turn on figure titles +bPlotLocalPlanInfo = true; % Turn on to plot local plan information +tWindowStates = []; % Specify time window for state (use [] for no clipping) +tWindowControl = []; % Specify time window for control (use [] for no clipping) +tWindowLocalPlan = []; % Specify time window for local plan (use [] for no clipping) + +%% Load the data + +% Load the data +[data, trialName] = parseQuadBag(trialName, namespace); +stateEstimate = data.stateEstimate; +stateGroundTruth = data.stateGroundTruth; +% stateTrajectory = data.stateTrajectory; +% stateGRFs = data.stateGRFs; +% controlGRFs = data.controlGRFs; +% localPlan = data.localPlan; +% ind = find(stateGroundTruth.time == stateEstimate.time(1), 1) +% t_ind_se = find(stateEstimate.time > 0, 1); +% t_ind_gt = length(stateGroundTruth.time) - (length(stateEstimate.time)) + (t_ind_se); +% tstart_gt = stateGroundTruth.time(t_ind_gt); +% tstart_se = stateEstimate.time(t_ind_se); +% +% t_state = stateGroundTruth.time(t_ind_gt: end); +% state_estimate_pos_x = stateEstimate.position(:,1); +% state_estimate_vel_x = stateEstimate.velocity(:,1); +% ground_truth_pos_x = stateGroundTruth.position(:,1); +% ground_truth_vel_x = stateGroundTruth.velocity(:,1); +% +% state_estimate_pos_x = state_estimate_pos_x(t_ind_se: end); +% state_estimate_vel_x = state_estimate_vel_x(t_ind_se: end); +% ground_truth_pos_x = ground_truth_pos_x(t_ind_gt: end); +% ground_truth_vel_x = ground_truth_vel_x(t_ind_gt: end); + + +figure(1) +t = tiledlayout(3,1); +nexttile +% subplot(3,1,1); +hold on +plot(stateGroundTruth.time, stateGroundTruth.velocity(:,1), 'b-','LineWidth',2) +plot(stateEstimate.time, stateEstimate.velocity(:,1), 'r-', 'LineWidth',2) +xlim([2 7]) +ylim([-3 3]) +title("X Velocity") +legend("Ground Truth", "State Estimate") +grid on +nexttile +% subplot(3,1,2); +hold on +plot(stateGroundTruth.time, stateGroundTruth.velocity(:,2), 'b', 'LineWidth',2 ) +plot(stateEstimate.time, stateEstimate.velocity(:,2), 'r-' , 'LineWidth',2) +xlim([2 7]) +ylim([-1.5 1.5]) +title("Y Velocity") +legend("Ground Truth", "State Estimate") +grid on +nexttile +% subplot(3,1,3); +hold on +plot(stateGroundTruth.time, stateGroundTruth.velocity(:,3), 'b-','LineWidth',2) +plot(stateEstimate.time, stateEstimate.velocity(:,3), 'r-','LineWidth',2) +xlim([2 7]) +ylim([-1.5 1.5]) +title("Z Velocity") +legend("Ground Truth", "State Estimate") +grid on + +xlabel(t, "Time (s)") +ylabel(t, 'Body Velocity (m/s)') + +%% Plot the data + +% Plot the state +% stateFigs = []; +% if ~isempty(stateGroundTruth) +% [stateFigs] = plotState(stateGroundTruth,tWindowStates,'-', bTitles, stateFigs); +% end +% if ~isempty(stateTrajectory) +% [stateFigs] = plotState(stateTrajectory,tWindowStates, ':', bTitles, stateFigs); +% end +% if ~isempty(stateEstimate) +% [stateFigs] = plotState(stateEstimate,tWindowStates, '--', bTitles, stateFigs); +% end + +% Plot the control +% controlFigs = []; +% if ~isempty(stateGRFs) +% controlFigs = plotControl(stateGRFs,tWindowControl,'-', bTitles, controlFigs); +% end +% if ~isempty(controlGRFs) +% controlFigs = plotControl(controlGRFs,tWindowControl,':', bTitles,controlFigs); +% end + +% Plot local plan information if desired +% localPlanFigs = []; +% if bPlotLocalPlanInfo && ~isempty(localPlan) +% localPlanFigs = plotLocalPlan(localPlan,tWindowLocalPlan,'-', bTitles,localPlanFigs); +% end + +% Add figures to array +% figArray = [stateFigs, controlFigs, localPlanFigs]; + +%% Save the logs and figures in one directory +% logDir = []; +% if bSave +% logDir = saveLog(trialName, figArray); +% end + +%% Animate and save + +% if bAnimate +% robot_path = '../../quad_simulator/spirit_description/urdf/spirit.urdf'; +% robot = importrobot(robot_path); +% videosDir = fullfile(logDir,'videos/'); +% animateData(robot,stateGroundTruth, fullfile(videosDir, trialName), bSave); +% end diff --git a/quad_msgs/CMakeLists.txt b/quad_msgs/CMakeLists.txt index e34e06cc8..f27b41974 100644 --- a/quad_msgs/CMakeLists.txt +++ b/quad_msgs/CMakeLists.txt @@ -18,6 +18,7 @@ find_package(catkin REQUIRED COMPONENTS ## Generate messages in the 'msg' folder add_message_files( FILES + BodyForceEstimate.msg RobotState.msg BodyState.msg GRFArray.msg diff --git a/quad_msgs/msg/BodyForceEstimate.msg b/quad_msgs/msg/BodyForceEstimate.msg new file mode 100644 index 000000000..7a5919e3e --- /dev/null +++ b/quad_msgs/msg/BodyForceEstimate.msg @@ -0,0 +1,8 @@ +# This is a message to hold a body contact force estimate +# +# The body contact force vector holds the estimated external torques acting on each of the 12 joints. +# Accurate timing information is stored in the header + +Header header + +float64[] joint_torques \ No newline at end of file diff --git a/quad_simulator/a1_description/package.xml b/quad_simulator/a1_description/package.xml index c6a38a8f5..debb63860 100644 --- a/quad_simulator/a1_description/package.xml +++ b/quad_simulator/a1_description/package.xml @@ -10,5 +10,14 @@ catkin roscpp std_msgs + controller_manager + joint_state_controller + gazebo_ros_pkgs + ros_control + gazebo_ros_control + effort_controllers + robot_state_publisher + imu_tools + message_to_tf diff --git a/quad_simulator/a1_description/sdf_mesh/a1.sdf b/quad_simulator/a1_description/sdf_mesh/a1.sdf index de06e4c1b..26a2391b4 100644 --- a/quad_simulator/a1_description/sdf_mesh/a1.sdf +++ b/quad_simulator/a1_description/sdf_mesh/a1.sdf @@ -68,7 +68,7 @@ 1 1 1 - model://a1_description/meshes/trunk.dae + model:///meshes/trunk.dae @@ -154,7 +154,7 @@ 1 1 1 - model://a1_description/meshes/hip.dae + model:///meshes/hip.dae @@ -170,6 +170,9 @@ hip2 body + + true + 1 0 0 @@ -228,7 +231,7 @@ 1 1 1 - model://a1_description/meshes/thigh_mirror.dae + model:///meshes/thigh_mirror.dae @@ -245,6 +248,9 @@ upper2 hip2 + + true + 0 -1 0 @@ -300,7 +306,7 @@ 1 1 1 - model://a1_description/meshes/calf.dae + model:///meshes/calf.dae @@ -317,6 +323,9 @@ lower2 upper2 + + true + 0 1 0 @@ -465,7 +474,7 @@ 1 1 1 - model://a1_description/meshes/hip.dae + model:///meshes/hip.dae @@ -481,6 +490,9 @@ hip3 body + + true + 1 0 0 @@ -539,7 +551,7 @@ 1 1 1 - model://a1_description/meshes/thigh_mirror.dae + model:///meshes/thigh_mirror.dae @@ -556,6 +568,9 @@ upper3 hip3 + + true + 0 -1 0 @@ -611,7 +626,7 @@ 1 1 1 - model://a1_description/meshes/calf.dae + model:///meshes/calf.dae @@ -628,6 +643,9 @@ lower3 upper3 + + true + 0 1 0 @@ -776,7 +794,7 @@ 1 1 1 - model://a1_description/meshes/hip.dae + model:///meshes/hip.dae @@ -792,6 +810,9 @@ hip0 body + + true + 1 0 0 @@ -850,7 +871,7 @@ 1 1 1 - model://a1_description/meshes/thigh.dae + model:///meshes/thigh.dae @@ -867,6 +888,9 @@ upper0 hip0 + + true + 0 -1 0 @@ -922,7 +946,7 @@ 1 1 1 - model://a1_description/meshes/calf.dae + model:///meshes/calf.dae @@ -939,6 +963,9 @@ lower0 upper0 + + true + 0 1 0 @@ -1087,7 +1114,7 @@ 1 1 1 - model://a1_description/meshes/hip.dae + model:///meshes/hip.dae @@ -1103,6 +1130,9 @@ hip1 body + + true + 1 0 0 @@ -1161,7 +1191,7 @@ 1 1 1 - model://a1_description/meshes/thigh.dae + model:///meshes/thigh.dae @@ -1178,6 +1208,9 @@ upper1 hip1 + + true + 0 -1 0 @@ -1233,7 +1266,7 @@ 1 1 1 - model://a1_description/meshes/calf.dae + model:///meshes/calf.dae @@ -1250,6 +1283,9 @@ lower1 upper1 + + true + 0 1 0 diff --git a/quad_simulator/a1_description/sdf_mesh/model.config b/quad_simulator/a1_description/sdf_mesh/model.config new file mode 100644 index 000000000..65e247ce7 --- /dev/null +++ b/quad_simulator/a1_description/sdf_mesh/model.config @@ -0,0 +1,6 @@ + + + + a1 + a1.sdf + \ No newline at end of file diff --git a/quad_simulator/gazebo_scripts/CMakeLists.txt b/quad_simulator/gazebo_scripts/CMakeLists.txt index be706bf67..0b6eb85da 100644 --- a/quad_simulator/gazebo_scripts/CMakeLists.txt +++ b/quad_simulator/gazebo_scripts/CMakeLists.txt @@ -105,6 +105,9 @@ add_dependencies(ground_truth_estimator ${catkin_EXPORTED_TARGETS}) add_library(contact_state_publisher SHARED src/contact_state_publisher.cpp) add_dependencies(contact_state_publisher ${catkin_EXPORTED_TARGETS}) +# add_library(imu_plugin SHARED src/imu_plugin.cpp) +# add_dependencies(imu_plugin ${catkin_EXPORTED_TARGETS}) + ## Declare a C++ executable ## With catkin_make all packages are built within a single CMake context ## The recommended prefix ensures that target names across packages don't collide @@ -115,3 +118,4 @@ target_link_libraries(contact ${GAZEBO_LIBRARIES}) target_link_libraries(controller ${GAZEBO_LIBRARIES}) target_link_libraries(ground_truth_estimator ${catkin_LIBRARIES} ${GAZEBO_LIBRARIES}) target_link_libraries(contact_state_publisher_node contact_state_publisher ${catkin_LIBRARIES} ${GAZEBO_LIBRARIES}) +# target_link_libraries(imu_plugin ${catkin_LIBRARIES} ${GAZEBO_LIBRARIES}) \ No newline at end of file diff --git a/quad_simulator/gazebo_scripts/package.xml b/quad_simulator/gazebo_scripts/package.xml index 31f573988..08d61458d 100644 --- a/quad_simulator/gazebo_scripts/package.xml +++ b/quad_simulator/gazebo_scripts/package.xml @@ -71,6 +71,13 @@ controller_manager joint_state_controller robot_state_publisher + + gazebo_ros_pkgs + ros_control + gazebo_ros_control + effort_controllers + imu_tools + message_to_tf diff --git a/quad_simulator/gazebo_scripts/src/controller_plugin.cpp b/quad_simulator/gazebo_scripts/src/controller_plugin.cpp index 7eb5c392e..df422f841 100644 --- a/quad_simulator/gazebo_scripts/src/controller_plugin.cpp +++ b/quad_simulator/gazebo_scripts/src/controller_plugin.cpp @@ -141,7 +141,7 @@ void QuadController::update(const ros::Time& time, double motor_model_ub = torque_lims_[ind.second] * (1.0 - current_vel / speed_lims_[ind.second]); double motor_model_lb = -torque_lims_[ind.second] * - (1.0 - current_vel / speed_lims_[ind.second]); + (1.0 + current_vel / speed_lims_[ind.second]); double torque_command = std::min( std::max(torque_feedback + torque_ff, -torque_lim), torque_lim); bool apply_motor_model = false; diff --git a/quad_simulator/gazebo_scripts/src/estimator_plugin.cpp b/quad_simulator/gazebo_scripts/src/estimator_plugin.cpp index 85f4b60d6..8292e1888 100644 --- a/quad_simulator/gazebo_scripts/src/estimator_plugin.cpp +++ b/quad_simulator/gazebo_scripts/src/estimator_plugin.cpp @@ -128,11 +128,23 @@ void QuadEstimatorGroundTruth::OnUpdate() { // std::cout << joint->Position() << std::endl; // std::cout << joint->GetVelocity(0) << std::endl; - physics::JointPtr joint = model_-> GetJoint(state.joints.name[i]); - // physics::JointWrench wrench = joint->GetForceTorque(0); - double torque = 0; // wrench.body1Torque.Z(); - // Note that this doesn't - // seem to work but at least will populate with zeros + physics::JointPtr joint = model_->GetJoint(state.joints.name[i]); + physics::JointWrench wrench = joint->GetForceTorque(0); + double torque; + switch (i%3) { + case 0: + // abad motors + torque = wrench.body1Torque.X(); + break; + case 1: + // hip motors + torque = -wrench.body1Torque.Y(); + break; + case 2: + // knee motors + torque = wrench.body1Torque.Y(); + break; + } state.joints.position.push_back(joint->Position()); state.joints.velocity.push_back(joint->GetVelocity(0)); diff --git a/quad_simulator/other/sensor_description/imu/model.sdf b/quad_simulator/other/sensor_description/imu/model.sdf index a9bfdcd69..d90adcc7a 100644 --- a/quad_simulator/other/sensor_description/imu/model.sdf +++ b/quad_simulator/other/sensor_description/imu/model.sdf @@ -22,7 +22,7 @@ true - 100 + 1000 true __default_topic__ diff --git a/quad_simulator/setup_deps.sh b/quad_simulator/setup_deps.sh index a725b05ba..2b4b8be72 100755 --- a/quad_simulator/setup_deps.sh +++ b/quad_simulator/setup_deps.sh @@ -1,4 +1,4 @@ -GAZEBO_MODEL_PATH_UPDATE="export GAZEBO_MODEL_PATH=${GAZEBO_MODEL_PATH}:$(pwd)/spirit_description:$(pwd)/other/sensor_description:$(pwd)/other/objects_description:$(pwd)/gazebo_scripts/worlds" +GAZEBO_MODEL_PATH_UPDATE="export GAZEBO_MODEL_PATH=${GAZEBO_MODEL_PATH}:$(pwd)/spirit_description:$(pwd)/a1_description:$(pwd)/other/sensor_description:$(pwd)/other/objects_description:$(pwd)/gazebo_scripts/worlds" if grep "$GAZEBO_MODEL_PATH_UPDATE" ~/.bashrc > /dev/null then @@ -12,16 +12,16 @@ sudo sh -c 'echo "deb http://packages.osrfoundation.org/gazebo/ubuntu-stable `ls cat /etc/apt/sources.list.d/gazebo-stable.list wget https://packages.osrfoundation.org/gazebo.key -O - | sudo apt-key add - sudo apt update -sudo apt install -y gazebo9 -y +sudo apt install -y gazebo11 -y -sudo apt install -y ros-melodic-controller-manager -y -sudo apt install -y ros-melodic-joint-state-controller -y -sudo apt install -y ros-melodic-gazebo-ros-pkgs -y -sudo apt install -y ros-melodic-ros-control -y -sudo apt install -y ros-melodic-gazebo-ros-control -y -sudo apt install -y ros-melodic-effort-controllers -y -sudo apt install -y ros-melodic-robot-state-publisher -y -sudo apt install -y ros-melodic-imu-tools -y -sudo apt install -y ros-melodic-message-to-tf -y \ No newline at end of file +sudo apt install -y ros-noetic-controller-manager -y +sudo apt install -y ros-noetic-joint-state-controller -y +sudo apt install -y ros-noetic-gazebo-ros-pkgs -y +sudo apt install -y ros-noetic-ros-control -y +sudo apt install -y ros-noetic-gazebo-ros-control -y +sudo apt install -y ros-noetic-effort-controllers -y +sudo apt install -y ros-noetic-robot-state-publisher -y +sudo apt install -y ros-noetic-imu-tools -y +sudo apt install -y ros-noetic-message-to-tf -y diff --git a/quad_simulator/spirit_description/materials/model.config b/quad_simulator/spirit_description/materials/model.config new file mode 100644 index 000000000..a63312b4d --- /dev/null +++ b/quad_simulator/spirit_description/materials/model.config @@ -0,0 +1,17 @@ + + + + materials + 1.0 + + + Cole Biesemeyer + robomechanacis@andrew.cmu.edu + + + + + Spirit Material + + + \ No newline at end of file diff --git a/quad_simulator/spirit_description/package.xml b/quad_simulator/spirit_description/package.xml index 2b26720f7..8639d3d02 100644 --- a/quad_simulator/spirit_description/package.xml +++ b/quad_simulator/spirit_description/package.xml @@ -50,6 +50,16 @@ catkin + controller_manager + joint_state_controller + gazebo_ros_pkgs + ros_control + gazebo_ros_control + effort_controllers + robot_state_publisher + imu_tools + message_to_tf + diff --git a/quad_simulator/spirit_description/sdf_mesh/spirit.sdf b/quad_simulator/spirit_description/sdf_mesh/spirit.sdf index 6f8fc0dcb..4e93ba9fc 100644 --- a/quad_simulator/spirit_description/sdf_mesh/spirit.sdf +++ b/quad_simulator/spirit_description/sdf_mesh/spirit.sdf @@ -72,6 +72,22 @@ SOFTWARE. + + 1 + 1000 + 1 + robot_1_state_imu + + /robot_1/state/imu + imu_link + 100.0 + 0.02 + 0 0 0 + 0 0 0 + imu_link + + 0 0 0 0 -0 0 + 1 @@ -79,7 +95,7 @@ SOFTWARE. 0.2263 0.07 0 0 -0 0 - 0 0 0 0 -0 0 + 0 0.03 0 0 -0 0 0.575 0.000669635 @@ -128,6 +144,9 @@ SOFTWARE. hip0 body + + true + 1 0 0 @@ -149,15 +168,15 @@ SOFTWARE. 0.2263 0.17098 0 0 -0 0 - -0.040 -0.041 0 0 -0 0 + -0.049 -0.039 0.006 0 -0 0 0.792 - 5e-03 - 0 + 4e-04 + -0.0012 0 - 0.0070 + 0.0066 0 - 0.0070 + 0.0066 @@ -197,6 +216,9 @@ SOFTWARE. upper0 hip0 + + true + 0 -1 0 @@ -268,6 +290,9 @@ SOFTWARE. lower0 upper0 + + true + 0 1 0 @@ -289,7 +314,7 @@ SOFTWARE. -0.2263 0.07 0 0 -0 0 - 0 0 0 0 -0 0 + 0 0.03 0 0 -0 0 0.575 0.000669635 @@ -338,6 +363,9 @@ SOFTWARE. hip1 body + + true + 1 0 0 @@ -359,15 +387,15 @@ SOFTWARE. -0.2263 0.17098 0 0 -0 0 - -0.040 -0.041 0 0 -0 0 + -0.049 -0.039 0.006 0 -0 0 0.792 - 5e-03 - 0 + 4e-04 + -0.0012 0 - 0.0070 + 0.0066 0 - 0.0070 + 0.0066 @@ -407,6 +435,9 @@ SOFTWARE. upper1 hip1 + + true + 0 -1 0 @@ -478,6 +509,9 @@ SOFTWARE. lower1 upper1 + + true + 0 1 0 @@ -500,7 +534,7 @@ SOFTWARE. 0.2263 -0.07 0 0 -0 0 - 0 0 0 0 -0 0 + 0 -0.03 0 0 -0 0 0.575 0.000669635 @@ -549,6 +583,9 @@ SOFTWARE. hip2 body + + true + 1 0 0 @@ -570,15 +607,15 @@ SOFTWARE. 0.2263 -0.17098 0 0 -0 0 - -0.040 0.041 0 0 -0 0 + -0.049 0.039 0.006 0 -0 0 0.792 - 5e-03 - 0 + 4e-04 + -0.0012 0 - 0.0070 + 0.0066 0 - 0.0070 + 0.0066 @@ -618,6 +655,9 @@ SOFTWARE. upper2 hip2 + + true + 0 -1 0 @@ -689,6 +729,9 @@ SOFTWARE. lower2 upper2 + + true + 0 1 0 @@ -710,7 +753,7 @@ SOFTWARE. -0.2263 -0.07 0 0 -0 0 - 0 0 0 0 -0 0 + 0 -0.03 0 0 -0 0 0.575 0.000669635 @@ -759,6 +802,9 @@ SOFTWARE. hip3 body + + true + 1 0 0 @@ -780,15 +826,15 @@ SOFTWARE. -0.2263 -0.17098 0 0 -0 0 - -0.040 0.041 0 0 -0 0 + -0.049 0.039 0.006 0 -0 0 0.792 - 5e-03 - 0 + 4e-04 + -0.0012 0 - 0.0070 + 0.0066 0 - 0.0070 + 0.0066 @@ -828,6 +874,9 @@ SOFTWARE. upper3 hip3 + + true + 0 -1 0 @@ -899,6 +948,9 @@ SOFTWARE. lower3 upper3 + + true + 0 1 0 @@ -1265,6 +1317,7 @@ SOFTWARE. + model://imu 0 0 0 0 0 0 diff --git a/quad_simulator/spirit_description/sdf_mesh/spirit_rotors.sdf b/quad_simulator/spirit_description/sdf_mesh/spirit_rotors.sdf index b8902a900..a8a8ebfc8 100644 --- a/quad_simulator/spirit_description/sdf_mesh/spirit_rotors.sdf +++ b/quad_simulator/spirit_description/sdf_mesh/spirit_rotors.sdf @@ -75,12 +75,18 @@ SOFTWARE. 1 + 0.2263 0.07 0 0 -0 0 0 0.03 0 0 -0 0 - 0.575 + 0.555 0.000669635 0 @@ -128,6 +134,9 @@ SOFTWARE. hip0 body + + true + 1 0 0 @@ -149,15 +158,15 @@ SOFTWARE. 0.2263 0.17098 0 0 -0 0 - -0.0676 -0.041 0 0 -0 0 + -0.049 -0.039 0.006 0 -0 0 0.792 - 5e-03 - 0.0010 + 4e-04 + -0.0012 0 - 0.0049 + 0.0066 0 - 0.0049 + 0.0066 @@ -197,6 +206,9 @@ SOFTWARE. upper0 hip0 + + true + 0 -1 0 @@ -268,6 +280,9 @@ SOFTWARE. lower0 upper0 + + true + 0 1 0 @@ -290,7 +305,7 @@ SOFTWARE. -0.2263 0.07 0 0 -0 0 0 0.03 0 0 -0 0 - 0.575 + 0.555 0.000669635 0 @@ -338,6 +353,9 @@ SOFTWARE. hip1 body + + true + 1 0 0 @@ -359,15 +377,15 @@ SOFTWARE. -0.2263 0.17098 0 0 -0 0 - -0.0676 -0.041 0 0 -0 0 + -0.049 -0.039 0.006 0 -0 0 0.792 - 5e-03 - 0.0010 + 4e-04 + -0.0012 0 - 0.0049 + 0.0066 0 - 0.0049 + 0.0066 @@ -407,6 +425,9 @@ SOFTWARE. upper1 hip1 + + true + 0 -1 0 @@ -478,6 +499,9 @@ SOFTWARE. lower1 upper1 + + true + 0 1 0 @@ -501,7 +525,7 @@ SOFTWARE. 0.2263 -0.07 0 0 -0 0 0 -0.03 0 0 -0 0 - 0.575 + 0.555 0.000669635 0 @@ -549,6 +573,9 @@ SOFTWARE. hip2 body + + true + 1 0 0 @@ -570,15 +597,15 @@ SOFTWARE. 0.2263 -0.17098 0 0 -0 0 - -0.0676 0.041 0 0 -0 0 + -0.049 0.039 0.006 0 -0 0 0.792 - 5e-03 - 0.0010 + 4e-04 + -0.0012 0 - 0.0049 + 0.0066 0 - 0.0049 + 0.0066 @@ -618,6 +645,9 @@ SOFTWARE. upper2 hip2 + + true + 0 -1 0 @@ -689,6 +719,9 @@ SOFTWARE. lower2 upper2 + + true + 0 1 0 @@ -711,7 +744,7 @@ SOFTWARE. -0.2263 -0.07 0 0 -0 0 0 -0.03 0 0 -0 0 - 0.575 + 0.555 0.000669635 0 @@ -759,6 +792,9 @@ SOFTWARE. hip3 body + + true + 1 0 0 @@ -780,15 +816,15 @@ SOFTWARE. -0.2263 -0.17098 0 0 -0 0 - -0.0676 0.041 0 0 -0 0 + -0.049 0.039 0.006 0 -0 0 0.792 - 5e-03 - 0.0010 + 4e-04 + -0.0012 0 - 0.0049 + 0.0066 0 - 0.0049 + 0.0066 @@ -828,6 +864,9 @@ SOFTWARE. upper3 hip3 + + true + 0 -1 0 @@ -899,6 +938,9 @@ SOFTWARE. lower3 upper3 + + true + 0 1 0 @@ -1256,14 +1298,14 @@ SOFTWARE. 0.2263 0.07 0 0 0 0 - 0.001 + 0.002 - 1.231E-4 + 1.0E-4 0 0 - 1.231E-4 + 1.0E-4 0 - 1.231E-4 + 1.0E-4 + + + Justin Yim + + + + + + TODO + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + catkin + + + + + + diff --git a/quad_simulator/underbrush_description/sdf/compliant_beam_horizontal.sdf b/quad_simulator/underbrush_description/sdf/compliant_beam_horizontal.sdf new file mode 100644 index 000000000..eeae8a756 --- /dev/null +++ b/quad_simulator/underbrush_description/sdf/compliant_beam_horizontal.sdf @@ -0,0 +1,64 @@ + + + + + + + 0 0 0.01 -1.570796 0 0 + + 0 0 0.2 0 0 0 + + + 0.4 + 0.01 + + + + + + 0 0 0.2 0 0 0 + + + 0.4 + 0.01 + + + + + + 0 0 0.2 0 0 0 + 0.1 + + 4e-3 + 0 + 0 + 4e-3 + 0 + 2e-5 + + + + + + world + branch + 0 0 0 0 0 0 + + 0 1 0 + + 1.0 + 0 + 10.0 + + + + 1 0 0 + + 1.0 + 0 + 10.0 + + + + + \ No newline at end of file diff --git a/quad_simulator/underbrush_description/sdf/compliant_cord.sdf b/quad_simulator/underbrush_description/sdf/compliant_cord.sdf new file mode 100644 index 000000000..69b3fb78e --- /dev/null +++ b/quad_simulator/underbrush_description/sdf/compliant_cord.sdf @@ -0,0 +1,500 @@ + + + + + + 0 0 0.01 -1.570796 0 0 + + 0 0 0.1 0 0 0 + + + 0.22 + 0.005 + + + + + + 0.5 + 0.5 + + + + + + + 0 0 0.1 0 0 0 + + + 0.22 + 0.005 + + + + 0.05 0.3 0.01 + + + + 0 0 0 0 0 0 + + + 0.03 + 0.015 + + + + 0.05 0.3 0.01 + + + + + 0 0 0.1 0 0 0 + 0.01 + + 4e-4 + 0 + 0 + 4e-4 + 0 + 1e-6 + + + + + + world + l1 + 0 0 0 0 0 0 + + 0 1 0 + + 0.005 + 0 + 0.01 + + + + 1 0 0 + + 0.005 + 0 + 0.01 + + + + + + 0 0.2 0.01 -1.570796 0 0 + + 0 0 0.1 0 0 0 + + + 0.22 + 0.005 + + + + + + 0.5 + 0.5 + + + + + + + 0 0 0.1 0 0 0 + + + 0.22 + 0.005 + + + + 0.05 0.3 0.01 + + + + + 0 0 0.1 0 0 0 + 0.01 + + 4e-4 + 0 + 0 + 4e-4 + 0 + 1e-6 + + + + + + l1 + l2 + 0 0 0 0 0 0 + + 0 1 0 + + 0.005 + 0 + 0.01 + + + + 1 0 0 + + 0.005 + 0 + 0.01 + + + + + + 0 0.4 0.01 -1.570796 0 0 + + 0 0 0.1 0 0 0 + + + 0.22 + 0.005 + + + + + + 0.5 + 0.5 + + + + + + + 0 0 0.1 0 0 0 + + + 0.22 + 0.005 + + + + 0.05 0.3 0.01 + + + + + 0 0 0.1 0 0 0 + 0.01 + + 4e-4 + 0 + 0 + 4e-4 + 0 + 1e-6 + + + + + + l2 + l3 + 0 0 0 0 0 0 + + 0 1 0 + + 0.005 + 0 + 0.01 + + + + 1 0 0 + + 0.005 + 0 + 0.01 + + + + + + 0 0.6 0.01 -1.570796 0 0 + + 0 0 0.1 0 0 0 + + + 0.22 + 0.005 + + + + + + 0.5 + 0.5 + + + + + + + 0 0 0.1 0 0 0 + + + 0.22 + 0.005 + + + + 0.05 0.3 0.01 + + + + + 0 0 0.1 0 0 0 + 0.01 + + 4e-4 + 0 + 0 + 4e-4 + 0 + 1e-6 + + + + + + l3 + l4 + 0 0 0 0 0 0 + + 0 1 0 + + 0.005 + 0 + 0.01 + + + + 1 0 0 + + 0.005 + 0 + 0.01 + + + + + + 0 0.8 0.01 -1.570796 0 0 + + 0 0 0.1 0 0 0 + + + 0.22 + 0.005 + + + + + + 0.5 + 0.5 + + + + + + + 0 0 0.1 0 0 0 + + + 0.22 + 0.005 + + + + 0.05 0.3 0.01 + + + + + 0 0 0.1 0 0 0 + 0.01 + + 4e-4 + 0 + 0 + 4e-4 + 0 + 1e-6 + + + + + + l4 + l5 + 0 0 0 0 0 0 + + 0 1 0 + + 0.005 + 0 + 0.01 + + + + 1 0 0 + + 0.005 + 0 + 0.01 + + + + + + 0 1.0 0.01 -1.570796 0 0 + + 0 0 0.1 0 0 0 + + + 0.22 + 0.005 + + + + + + 0.5 + 0.5 + + + + + + + 0 0 0.1 0 0 0 + + + 0.22 + 0.005 + + + + 0.05 0.3 0.01 + + + + + 0 0 0.1 0 0 0 + 0.01 + + 4e-4 + 0 + 0 + 4e-4 + 0 + 1e-6 + + + + + + l5 + l6 + 0 0 0 0 0 0 + + 0 1 0 + + 0.005 + 0 + 0.01 + + + + 1 0 0 + + 0.005 + 0 + 0.01 + + + + + + 0 1.0 0.01 -1.570796 0 0 + + 0 0 0 0 0 0 + + + 0.03 + 0.015 + + + + 0.05 0.3 0.01 + + + + + 0 0 0.1 0 0 0 + 0.01 + + 1e-5 + 0 + 0 + 1e-5 + 0 + 1e-5 + + + + + + world + l_end + 0 0 0 0 0 0 + + 0 1 0 + + 0.005 + 0 + 0.01 + + + + 1 0 0 + + 0.005 + 0 + 0.01 + + + + + + l_end + l6 + 0 0 0 0 0 0 + + 0 0 1 + + 5.0 + 0 + 200.0 + + + -0.1 + 0 + + + + + \ No newline at end of file diff --git a/quad_simulator/underbrush_description/sdf/model.config b/quad_simulator/underbrush_description/sdf/model.config new file mode 100644 index 000000000..2d7daf9f6 --- /dev/null +++ b/quad_simulator/underbrush_description/sdf/model.config @@ -0,0 +1,15 @@ + + + underbrush + 1.0 + compliant_beam_horizontal.sdf + + + Justin Yim + jkyim@andrew.cmu.edu + + + + Compliant beam to simulate underbrush + + \ No newline at end of file diff --git a/quad_utils/config/a1.yaml b/quad_utils/config/a1.yaml index bf907457c..22f6e6fc0 100644 --- a/quad_utils/config/a1.yaml +++ b/quad_utils/config/a1.yaml @@ -22,10 +22,10 @@ nmpc_controller: g_dim: 28 x_weights: [5.0, 5.0, 5.0, 0.5, 0.5, 0.5, 0.1, 0.1, 0.2, 0.05, 0.05, 0.01] u_weights: [!!float 5e-5, !!float 5e-5, !!float 5e-5, !!float 5e-5, !!float 5e-5, !!float 5e-5, !!float 5e-5, !!float 5e-5, !!float 5e-5, !!float 5e-5, !!float 5e-5, !!float 5e-5] - x_lb: [!!float -2e19, !!float -2e19, 0, -3.14159, -3.14159, -3.14159, !!float -2e19, !!float -2e19, !!float -2e19, !!float -2e19, !!float -2e19, !!float -2e19] - x_ub: [!!float 2e19, !!float 2e19, !!float 2e19, 3.14159, 3.14159, 3.14159, !!float 2e19, !!float 2e19, !!float 2e19, !!float 2e19, !!float 2e19, !!float 2e19] - x_lb_soft: [!!float -2e19, !!float -2e19, 0, -3.14159, -3.14159, -3.14159, !!float -2e19, !!float -2e19, !!float -2e19, !!float -2e19, !!float -2e19, !!float -2e19] - x_ub_soft: [!!float 2e19, !!float 2e19, !!float 2e19, 3.14159, 3.14159, 3.14159, !!float 2e19, !!float 2e19, !!float 2e19, !!float 2e19, !!float 2e19, !!float 2e19] + x_lb: [!!float -2e19, !!float -2e19, 0, -3.14159, -3.14159, -10.0, !!float -2e19, !!float -2e19, !!float -2e19, !!float -2e19, !!float -2e19, !!float -2e19] + x_ub: [!!float 2e19, !!float 2e19, !!float 2e19, 3.14159, 3.14159, 10.0, !!float 2e19, !!float 2e19, !!float 2e19, !!float 2e19, !!float 2e19, !!float 2e19] + x_lb_soft: [!!float -2e19, !!float -2e19, 0, -3.14159, -3.14159, -10.0, !!float -2e19, !!float -2e19, !!float -2e19, !!float -2e19, !!float -2e19, !!float -2e19] + x_ub_soft: [!!float 2e19, !!float 2e19, !!float 2e19, 3.14159, 3.14159, 10.0, !!float 2e19, !!float 2e19, !!float 2e19, !!float 2e19, !!float 2e19, !!float 2e19] u_lb: [!!float -2e19, !!float -2e19, 10, !!float -2e19, !!float -2e19, 10, !!float -2e19, !!float -2e19, 10, !!float -2e19, !!float -2e19, 10] u_ub: [!!float 2e19, !!float 2e19, 150, !!float 2e19, !!float 2e19, 150, !!float 2e19, !!float 2e19, 150, !!float 2e19, !!float 2e19, 150] g_lb: [0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, !!float -2e19, !!float -2e19, !!float -2e19, !!float -2e19, !!float -2e19, !!float -2e19, !!float -2e19, !!float -2e19, !!float -2e19, !!float -2e19, !!float -2e19, !!float -2e19, !!float -2e19, !!float -2e19, !!float -2e19, !!float -2e19] diff --git a/quad_utils/config/spirit.yaml b/quad_utils/config/spirit.yaml index 57237cb77..92a870249 100644 --- a/quad_utils/config/spirit.yaml +++ b/quad_utils/config/spirit.yaml @@ -22,10 +22,10 @@ nmpc_controller: g_dim: 28 x_weights: [5.0, 5.0, 5.0, 0.5, 0.5, 0.5, 0.1, 0.1, 0.2, 0.05, 0.05, 0.01] u_weights: [!!float 5e-5, !!float 5e-5, !!float 5e-5, !!float 5e-5, !!float 5e-5, !!float 5e-5, !!float 5e-5, !!float 5e-5, !!float 5e-5, !!float 5e-5, !!float 5e-5, !!float 5e-5] - x_lb: [!!float -2e19, !!float -2e19, 0, -3.14159, -3.14159, -3.14159, !!float -2e19, !!float -2e19, !!float -2e19, !!float -2e19, !!float -2e19, !!float -2e19] - x_ub: [!!float 2e19, !!float 2e19, !!float 2e19, 3.14159, 3.14159, 3.14159, !!float 2e19, !!float 2e19, !!float 2e19, !!float 2e19, !!float 2e19, !!float 2e19] - x_lb_soft: [!!float -2e19, !!float -2e19, 0, -3.14159, -3.14159, -3.14159, !!float -2e19, !!float -2e19, !!float -2e19, !!float -2e19, !!float -2e19, !!float -2e19] - x_ub_soft: [!!float 2e19, !!float 2e19, !!float 2e19, 3.14159, 3.14159, 3.14159, !!float 2e19, !!float 2e19, !!float 2e19, !!float 2e19, !!float 2e19, !!float 2e19] + x_lb: [!!float -2e19, !!float -2e19, 0, -3.14159, -3.14159, -10.0, !!float -2e19, !!float -2e19, !!float -2e19, !!float -2e19, !!float -2e19, !!float -2e19] + x_ub: [!!float 2e19, !!float 2e19, !!float 2e19, 3.14159, 3.14159, 10.0, !!float 2e19, !!float 2e19, !!float 2e19, !!float 2e19, !!float 2e19, !!float 2e19] + x_lb_soft: [!!float -2e19, !!float -2e19, 0, -3.14159, -3.14159, -10.0, !!float -2e19, !!float -2e19, !!float -2e19, !!float -2e19, !!float -2e19, !!float -2e19] + x_ub_soft: [!!float 2e19, !!float 2e19, !!float 2e19, 3.14159, 3.14159, 10.0, !!float 2e19, !!float 2e19, !!float 2e19, !!float 2e19, !!float 2e19, !!float 2e19] u_lb: [!!float -2e19, !!float -2e19, 10, !!float -2e19, !!float -2e19, 10, !!float -2e19, !!float -2e19, 10, !!float -2e19, !!float -2e19, 10] u_ub: [!!float 2e19, !!float 2e19, 150, !!float 2e19, !!float 2e19, 150, !!float 2e19, !!float 2e19, 150, !!float 2e19, !!float 2e19, 150] g_lb: [0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, !!float -2e19, !!float -2e19, !!float -2e19, !!float -2e19, !!float -2e19, !!float -2e19, !!float -2e19, !!float -2e19, !!float -2e19, !!float -2e19, !!float -2e19, !!float -2e19, !!float -2e19, !!float -2e19, !!float -2e19, !!float -2e19] diff --git a/quad_utils/config/topics_global.yaml b/quad_utils/config/topics_global.yaml index 82bc809e5..8e24c84ae 100644 --- a/quad_utils/config/topics_global.yaml +++ b/quad_utils/config/topics_global.yaml @@ -5,3 +5,4 @@ topics: remote: heartbeat/remote control: joint_command: control/joint_command + mocap: /mocap_node/quad/pose diff --git a/quad_utils/config/topics_robot.yaml b/quad_utils/config/topics_robot.yaml index 8a53b1230..bac96ab4a 100644 --- a/quad_utils/config/topics_robot.yaml +++ b/quad_utils/config/topics_robot.yaml @@ -56,4 +56,3 @@ topics: toe1_contact_state: gazebo/toe1_contact_state toe2_contact_state: gazebo/toe2_contact_state toe3_contact_state: gazebo/toe3_contact_state - mocap: mocap_node/quad/pose diff --git a/quad_utils/include/quad_utils/math_utils.h b/quad_utils/include/quad_utils/math_utils.h index 09aa9e538..f0b191862 100644 --- a/quad_utils/include/quad_utils/math_utils.h +++ b/quad_utils/include/quad_utils/math_utils.h @@ -58,7 +58,7 @@ inline double wrapToPi(double val) { * @param[in] data data to wrap * @return Wrapped data */ -inline std::vector wrapToPi(std::vector data) { +inline std::vector wrapToPi(const std::vector &data) { std::vector data_wrapped = data; for (int i = 0; i < data.size(); i++) { data_wrapped[i] = wrapToPi(data[i]); @@ -75,9 +75,10 @@ inline std::vector wrapToPi(std::vector data) { * @param[in] input_val Query point * @return Vector of interpolated values */ -std::vector interpMat(const std::vector input_vec, - const std::vector> output_mat, - const double query_point); +std::vector interpMat( + const std::vector &input_vec, + const std::vector> &output_mat, + const double query_point); /** * @brief Interpolate data from column vectors contained in a matrix (vector of @@ -88,8 +89,8 @@ std::vector interpMat(const std::vector input_vec, * @param[in] input_val Query point * @return Vector of interpolated values */ -Eigen::Vector3d interpVector3d(const std::vector input_vec, - const std::vector output_mat, +Eigen::Vector3d interpVector3d(const std::vector &input_vec, + const std::vector &output_mat, const double query_point); /** @@ -102,8 +103,8 @@ Eigen::Vector3d interpVector3d(const std::vector input_vec, * @return Vector of interpolated values */ std::vector interpMatVector3d( - const std::vector input_vec, - const std::vector> output_mat, + const std::vector &input_vec, + const std::vector> &output_mat, const double query_point); /** @@ -113,8 +114,8 @@ std::vector interpMatVector3d( * @param[in] input_val Query point * @return Correct output int corresponsing to the query point */ -int interpInt(const std::vector input_vec, std::vector output_vec, - const double query_point); +int interpInt(const std::vector &input_vec, + std::vector &output_vec, const double query_point); /** * @brief Filter a stl vector with a moving average window. @@ -123,7 +124,7 @@ int interpInt(const std::vector input_vec, std::vector output_vec, * add one to maintain symmetry * @return Vector of filtered values */ -std::vector movingAverageFilter(std::vector data, +std::vector movingAverageFilter(const std::vector &data, int window_size); /** @@ -132,14 +133,62 @@ std::vector movingAverageFilter(std::vector data, * @param[in] dt The (constant) timestep between values in data. * @return Vector of differentiated signal */ -std::vector centralDiff(std::vector data, double dt); +std::vector centralDiff(const std::vector &data, double dt); + +/** + * @brief Wrap a given scalar to within PI of a given target by adding or + * subtracting 2*PI + * + * @tparam ScalarType + * @param[in] val Value to be wrapped to val_target + * @param[in] val_target Target to wrap towards + */ +template +void wrapToTarget(ScalarType &val, const ScalarType &val_target = 0.0) { + while (val_target - val > M_PI) { + val += 2.0 * M_PI; + } + while (val_target - val < -M_PI) { + val -= 2.0 * M_PI; + } +} + +/** + * @brief Unwrap a phase variable by filtering out differences > pi + * @param[in] vec Input vector containing a wrapped signal + * @return Flag for if the vector was modified by unwrapping + */ +template +bool unwrapVector(VecType &vec) { + bool modified = false; + for (int i = 1; i < vec.size(); i++) { + double diff = vec[i] - vec[i - 1]; + if (diff > M_PI) { + modified = true; + for (int j = i; j < vec.size(); j++) { + vec[j] = vec[j] - 2 * M_PI; + } + } else if (diff < -M_PI) { + modified = true; + for (int j = i; j < vec.size(); j++) { + vec[j] = vec[j] + 2 * M_PI; + } + } + } + return modified; +} /** * @brief Unwrap a phase variable by filtering out differences > pi - * @param[in] data Input vector containing a wrapped signal + * @param[in] vec Input vector containing a wrapped signal * @return Vector of unwrapped signal */ -std::vector unwrap(std::vector data); +template +VecType getUnwrappedVector(const VecType &vec) { + VecType vec_unwrapped = vec; + unwrapVector(vec_unwrapped); + return vec_unwrapped; +} /** * @brief Selective damping least square matrix inverse diff --git a/quad_utils/include/quad_utils/quad_kd.h b/quad_utils/include/quad_utils/quad_kd.h index 51f914e96..7b82bdad9 100644 --- a/quad_utils/include/quad_utils/quad_kd.h +++ b/quad_utils/include/quad_utils/quad_kd.h @@ -27,430 +27,447 @@ namespace quad_utils { */ class QuadKD { public: - /** - * @brief Constructor for QuadKD Class - * @return Constructed object of type QuadKD - */ - QuadKD(); - - /** - * @brief Constructor for QuadKD Class - * @param[in] ns Namespace - * @return Constructed object of type QuadKD - */ - QuadKD(std::string ns); - - /** - * @brief Initialize model for the class - * @param[in] ns Namespace - */ - void initModel(std::string ns); - - /** - * @brief Create an Eigen Eigen::Matrix4d containing a homogeneous transform - * from a specified translation and a roll, pitch, and yaw vector - * @param[in] trans Translation from input frame to output frame - * @param[in] rpy Rotation from input frame to output frame as roll, pitch, - * yaw - * @return Homogenous transformation matrix - */ - Eigen::Matrix4d createAffineMatrix(Eigen::Vector3d trans, - Eigen::Vector3d rpy) const; - - /** - * @brief Create an Eigen Eigen::Matrix4d containing a homogeneous transform - * from a specified translation and an AngleAxis object - * @param[in] trans Translation from input frame to output frame - * @param[in] rot Rotation from input frame to output frame as AngleAxis - * @return Homogenous transformation matrix - */ - Eigen::Matrix4d createAffineMatrix(Eigen::Vector3d trans, - Eigen::AngleAxisd rot) const; - - /** - * @brief Transform a transformation matrix from the body frame to the world - * frame - * @param[in] body_pos Position of center of body frame - * @param[in] body_rpy Orientation of body frame in roll, pitch, yaw - * @param[in] transform_body Specified transform in the body frame - * @param[out] transform_world Specified transform in the world frame - */ - void transformBodyToWorld(Eigen::Vector3d body_pos, Eigen::Vector3d body_rpy, - Eigen::Matrix4d transform_body, - Eigen::Matrix4d &transform_world) const; - - /** - * @brief Transform a transformation matrix from the world frame to the body - * frame - * @param[in] body_pos Position of center of body frame - * @param[in] body_rpy Orientation of body frame in roll, pitch, yaw - * @param[in] transform_world Specified transform in the world frame - * @param[out] transform_body Specified transform in the body frame - */ - void transformWorldToBody(Eigen::Vector3d body_pos, Eigen::Vector3d body_rpy, - Eigen::Matrix4d transform_world, - Eigen::Matrix4d &transform_body) const; - - /** - * @brief Compute forward kinematics for a specified leg from the body COM - * @param[in] leg_index Quad leg (0 = FL, 1 = BL, 2 = FR, 3 = BR) - * @param[in] joint_state Joint states for the specified leg (abad, hip, knee) - * @param[out] g_body_foot Transform of the specified foot in world frame - */ - void bodyToFootFKBodyFrame(int leg_index, Eigen::Vector3d joint_state, - Eigen::Matrix4d &g_body_foot) const; - - /** - * @brief Compute forward kinematics for a specified leg from the body COM - * @param[in] leg_index Quad leg (0 = FL, 1 = BL, 2 = FR, 3 = BR) - * @param[in] joint_state Joint states for the specified leg (abad, hip, knee) - * @param[out] foot_pos_world Position of the specified foot in world frame - */ - void bodyToFootFKBodyFrame(int leg_index, Eigen::Vector3d joint_state, - Eigen::Vector3d &foot_pos_body) const; - - /** - * @brief Compute forward kinematics for a specified leg - * @param[in] leg_index Quad leg (0 = FL, 1 = BL, 2 = FR, 3 = BR) - * @param[in] body_pos Position of center of body frame - * @param[in] body_rpy Orientation of body frame in roll, pitch, yaw - * @param[in] joint_state Joint states for the specified leg (abad, hip, knee) - * @param[out] g_world_foot Transform of the specified foot in world frame - */ - void worldToFootFKWorldFrame(int leg_index, Eigen::Vector3d body_pos, - Eigen::Vector3d body_rpy, - Eigen::Vector3d joint_state, - Eigen::Matrix4d &g_world_foot) const; - - /** - * @brief Compute forward kinematics for a specified leg - * @param[in] leg_index Quad leg (0 = FL, 1 = BL, 2 = FR, 3 = BR) - * @param[in] body_pos Position of center of body frame - * @param[in] body_rpy Orientation of body frame in roll, pitch, yaw - * @param[in] joint_state Joint states for the specified leg (abad, hip, knee) - * @param[out] foot_pos_world Position of the specified foot in world frame - */ - void worldToFootFKWorldFrame(int leg_index, Eigen::Vector3d body_pos, - Eigen::Vector3d body_rpy, - Eigen::Vector3d joint_state, - Eigen::Vector3d &foot_pos_world) const; - - /** - * @brief Compute forward kinematics for a specified leg - * @param[in] leg_index Spirit leg (0 = FL, 1 = BL, 2 = FR, 3 = BR) - * @param[in] body_pos Position of center of body frame - * @param[in] body_rpy Orientation of body frame in roll, pitch, yaw - * @param[in] joint_state Joint states for the specified leg (abad, hip, knee) - * @param[out] g_world_knee Transform of the specified knee in world frame - */ - void worldToKneeFKWorldFrame(int leg_index, Eigen::Vector3d body_pos, - Eigen::Vector3d body_rpy, - Eigen::Vector3d joint_state, - Eigen::Matrix4d &g_world_knee) const; - - /** - * @brief Compute forward kinematics for a specified leg - * @param[in] leg_index Spirit leg (0 = FL, 1 = BL, 2 = FR, 3 = BR) - * @param[in] body_pos Position of center of body frame - * @param[in] body_rpy Orientation of body frame in roll, pitch, yaw - * @param[in] joint_state Joint states for the specified leg (abad, hip, knee) - * @param[out] knee_pos_world Position of the specified knee in world frame - */ - void worldToKneeFKWorldFrame(int leg_index, Eigen::Vector3d body_pos, - Eigen::Vector3d body_rpy, - Eigen::Vector3d joint_state, - Eigen::Vector3d &knee_pos_world) const; - - /** - * @brief Compute inverse kinematics for a specified leg - * @param[in] leg_index Quad leg (0 = FL, 1 = BL, 2 = FR, 3 = BR) - * @param[in] body_pos Position of center of body frame - * @param[in] body_rpy Orientation of body frame in roll, pitch, yaw - * @param[in] foot_pos_world Position of the specified foot in world frame - * @param[out] joint_state Joint states for the specified leg (abad, hip, - * knee) - */ - bool worldToFootIKWorldFrame(int leg_index, Eigen::Vector3d body_pos, - Eigen::Vector3d body_rpy, - Eigen::Vector3d foot_pos_world, - Eigen::Vector3d &joint_state) const; - - /** - * @brief Compute inverse kinematics for a specified leg in the leg base frame - * @param[in] leg_index Quad leg (0 = FL, 1 = BL, 2 = FR, 3 = BR) - * @param[in] foot_pos_legbase Position of the specified foot in leg base - * frame - * @param[out] joint_state Joint states for the specified leg (abad, hip, - * knee) - */ - bool legbaseToFootIKLegbaseFrame(int leg_index, - Eigen::Vector3d foot_pos_legbase, - Eigen::Vector3d &joint_state) const; - - /** - * @brief Get the lower joint limit of a particular joint - * @param[in] leg_index Quad leg (0 = FL, 1 = BL, 2 = FR, 3 = BR) - * @param[in] joint_index Index for joint (0 = abad, 1 = hip, 2 = knee) - * @return Requested joint limit - */ - double getJointLowerLimit(int leg_index, int joint_index) const; - - /** - * @brief Get the upper joint limit of a particular joint - * @param[in] leg_index Quad leg (0 = FL, 1 = BL, 2 = FR, 3 = BR) - * @param[in] joint_index Index for joint (0 = abad, 1 = hip, 2 = knee) - * @return Requested joint limit - */ - double getJointUpperLimit(int leg_index, int joint_index) const; - - /** - * @brief Get the upper joint limit of a particular joint - * @param[in] leg_index Quad leg (0 = FL, 1 = BL, 2 = FR, 3 = BR) - * @param[in] link_index Index for link (0 = abad, 1 = upper, 2 = lower) - * @return Requested link length - */ - double getLinkLength(int leg_index, int link_index) const; - - /** - * @brief Get the transform from the world frame to the leg base - * @param[in] leg_index Quad leg (0 = FL, 1 = BL, 2 = FR, 3 = BR) - * @param[in] body_pos Position of center of body frame - * @param[in] body_rpy Orientation of body frame in roll, pitch, yaw - * @param[out] g_world_legbase Transformation matrix of world to leg base - */ - void worldToLegbaseFKWorldFrame(int leg_index, Eigen::Vector3d body_pos, - Eigen::Vector3d body_rpy, - Eigen::Matrix4d &g_world_legbase) const; - - /** - * @brief Get the position of the leg base frame origin in the world frame - * @param[in] leg_index Quad leg (0 = FL, 1 = BL, 2 = FR, 3 = BR) - * @param[in] body_pos Position of center of body frame - * @param[in] body_rpy Orientation of body frame in roll, pitch, yaw - * @param[out] leg_base_pos_world Origin of leg base frame in world frame - */ - void worldToLegbaseFKWorldFrame(int leg_index, Eigen::Vector3d body_pos, - Eigen::Vector3d body_rpy, - Eigen::Vector3d &leg_base_pos_world) const; - - /** - * @brief Get the position of the nominal hip location in the world frame - * @param[in] leg_index Quad leg (0 = FL, 1 = BL, 2 = FR, 3 = BR) - * @param[in] body_pos Position of center of body frame - * @param[in] body_rpy Orientation of body frame in roll, pitch, yaw - * @param[out] nominal_hip_pos_world Location of nominal hip in world frame - */ - void worldToNominalHipFKWorldFrame( - int leg_index, Eigen::Vector3d body_pos, Eigen::Vector3d body_rpy, - Eigen::Vector3d &nominal_hip_pos_world) const; - - /** - * @brief Compute Jacobian for generalized coordinates - * @param[in] state Joint and body states - * @param[out] jacobian Jacobian for generalized coordinates - */ - void getJacobianGenCoord(const Eigen::VectorXd &state, - Eigen::MatrixXd &jacobian) const; - - /** - * @brief Compute Jacobian for angular velocity in body frame - * @param[in] state Joint and body states - * @param[out] jacobian Jacobian for angular velocity in body frame - */ - void getJacobianBodyAngVel(const Eigen::VectorXd &state, + /** + * @brief Constructor for QuadKD Class + * @return Constructed object of type QuadKD + */ + QuadKD(); + + /** + * @brief Constructor for QuadKD Class + * @param[in] ns Namespace + * @return Constructed object of type QuadKD + */ + QuadKD(const std::string &ns); + + /** + * @brief Initialize model for the class + * @param[in] ns Namespace + */ + void initModel(const std::string &ns); + + /** + * @brief Create an Eigen Eigen::Matrix4d containing a homogeneous transform + * from a specified translation and a roll, pitch, and yaw vector + * @param[in] trans Translation from input frame to output frame + * @param[in] rpy Rotation from input frame to output frame as roll, pitch, + * yaw + * @return Homogenous transformation matrix + */ + Eigen::Matrix4d createAffineMatrix(const Eigen::Vector3d &trans, + const Eigen::Vector3d &rpy) const; + + /** + * @brief Create an Eigen Eigen::Matrix4d containing a homogeneous transform + * from a specified translation and an AngleAxis object + * @param[in] trans Translation from input frame to output frame + * @param[in] rot Rotation from input frame to output frame as AngleAxis + * @return Homogenous transformation matrix + */ + Eigen::Matrix4d createAffineMatrix(const Eigen::Vector3d &trans, + const Eigen::AngleAxisd &rot) const; + + /** + * @brief Transform a transformation matrix from the body frame to the world + * frame + * @param[in] body_pos Position of center of body frame + * @param[in] body_rpy Orientation of body frame in roll, pitch, yaw + * @param[in] transform_body Specified transform in the body frame + * @param[out] transform_world Specified transform in the world frame + */ + void transformBodyToWorld(const Eigen::Vector3d &body_pos, + const Eigen::Vector3d &body_rpy, + const Eigen::Matrix4d &transform_body, + Eigen::Matrix4d &transform_world) const; + + /** + * @brief Transform a transformation matrix from the world frame to the body + * frame + * @param[in] body_pos Position of center of body frame + * @param[in] body_rpy Orientation of body frame in roll, pitch, yaw + * @param[in] transform_world Specified transform in the world frame + * @param[out] transform_body Specified transform in the body frame + */ + void transformWorldToBody(const Eigen::Vector3d &body_pos, + const Eigen::Vector3d &body_rpy, + const Eigen::Matrix4d &transform_world, + Eigen::Matrix4d &transform_body) const; + + /** + * @brief Compute forward kinematics for a specified leg from the body COM + * @param[in] leg_index Quad leg (0 = FL, 1 = BL, 2 = FR, 3 = BR) + * @param[in] joint_state Joint states for the specified leg (abad, hip, + * knee) + * @param[out] g_body_foot Transform of the specified foot in world frame + */ + void bodyToFootFKBodyFrame(int leg_index, + const Eigen::Vector3d &joint_state, + Eigen::Matrix4d &g_body_foot) const; + + /** + * @brief Compute forward kinematics for a specified leg from the body COM + * @param[in] leg_index Quad leg (0 = FL, 1 = BL, 2 = FR, 3 = BR) + * @param[in] joint_state Joint states for the specified leg (abad, hip, + * knee) + * @param[out] foot_pos_world Position of the specified foot in world frame + */ + void bodyToFootFKBodyFrame(int leg_index, + const Eigen::Vector3d &joint_state, + Eigen::Vector3d &foot_pos_body) const; + + /** + * @brief Compute forward kinematics for a specified leg + * @param[in] leg_index Quad leg (0 = FL, 1 = BL, 2 = FR, 3 = BR) + * @param[in] body_pos Position of center of body frame + * @param[in] body_rpy Orientation of body frame in roll, pitch, yaw + * @param[in] joint_state Joint states for the specified leg (abad, hip, + * knee) + * @param[out] g_world_foot Transform of the specified foot in world frame + */ + void worldToFootFKWorldFrame(int leg_index, const Eigen::Vector3d &body_pos, + const Eigen::Vector3d &body_rpy, + const Eigen::Vector3d &joint_state, + Eigen::Matrix4d &g_world_foot) const; + + /** + * @brief Compute forward kinematics for a specified leg + * @param[in] leg_index Quad leg (0 = FL, 1 = BL, 2 = FR, 3 = BR) + * @param[in] body_pos Position of center of body frame + * @param[in] body_rpy Orientation of body frame in roll, pitch, yaw + * @param[in] joint_state Joint states for the specified leg (abad, hip, + * knee) + * @param[out] foot_pos_world Position of the specified foot in world frame + */ + void worldToFootFKWorldFrame(int leg_index, const Eigen::Vector3d &body_pos, + const Eigen::Vector3d &body_rpy, + const Eigen::Vector3d &joint_state, + Eigen::Vector3d &foot_pos_world) const; + + /** + * @brief Compute forward kinematics for a specified leg + * @param[in] leg_index Spirit leg (0 = FL, 1 = BL, 2 = FR, 3 = BR) + * @param[in] body_pos Position of center of body frame + * @param[in] body_rpy Orientation of body frame in roll, pitch, yaw + * @param[in] joint_state Joint states for the specified leg (abad, hip, + * knee) + * @param[out] g_world_knee Transform of the specified knee in world frame + */ + void worldToKneeFKWorldFrame(int leg_index, const Eigen::Vector3d &body_pos, + const Eigen::Vector3d &body_rpy, + const Eigen::Vector3d &joint_state, + Eigen::Matrix4d &g_world_knee) const; + + /** + * @brief Compute forward kinematics for a specified leg + * @param[in] leg_index Spirit leg (0 = FL, 1 = BL, 2 = FR, 3 = BR) + * @param[in] body_pos Position of center of body frame + * @param[in] body_rpy Orientation of body frame in roll, pitch, yaw + * @param[in] joint_state Joint states for the specified leg (abad, hip, + * knee) + * @param[out] knee_pos_world Position of the specified knee in world frame + */ + void worldToKneeFKWorldFrame(int leg_index, const Eigen::Vector3d &body_pos, + const Eigen::Vector3d &body_rpy, + const Eigen::Vector3d &joint_state, + Eigen::Vector3d &knee_pos_world) const; + + /** + * @brief Compute inverse kinematics for a specified leg + * @param[in] leg_index Quad leg (0 = FL, 1 = BL, 2 = FR, 3 = BR) + * @param[in] body_pos Position of center of body frame + * @param[in] body_rpy Orientation of body frame in roll, pitch, yaw + * @param[in] foot_pos_world Position of the specified foot in world frame + * @param[out] joint_state Joint states for the specified leg (abad, hip, + * knee) + */ + bool worldToFootIKWorldFrame(int leg_index, const Eigen::Vector3d &body_pos, + const Eigen::Vector3d &body_rpy, + const Eigen::Vector3d &foot_pos_world, + Eigen::Vector3d &joint_state) const; + + /** + * @brief Compute inverse kinematics for a specified leg in the leg base + * frame + * @param[in] leg_index Quad leg (0 = FL, 1 = BL, 2 = FR, 3 = BR) + * @param[in] foot_pos_legbase Position of the specified foot in leg base + * frame + * @param[out] joint_state Joint states for the specified leg (abad, hip, + * knee) + */ + bool legbaseToFootIKLegbaseFrame(int leg_index, + const Eigen::Vector3d &foot_pos_legbase, + Eigen::Vector3d &joint_state) const; + + /** + * @brief Get the lower joint limit of a particular joint + * @param[in] leg_index Quad leg (0 = FL, 1 = BL, 2 = FR, 3 = BR) + * @param[in] joint_index Index for joint (0 = abad, 1 = hip, 2 = knee) + * @return Requested joint limit + */ + double getJointLowerLimit(int leg_index, int joint_index) const; + + /** + * @brief Get the upper joint limit of a particular joint + * @param[in] leg_index Quad leg (0 = FL, 1 = BL, 2 = FR, 3 = BR) + * @param[in] joint_index Index for joint (0 = abad, 1 = hip, 2 = knee) + * @return Requested joint limit + */ + double getJointUpperLimit(int leg_index, int joint_index) const; + + /** + * @brief Get the upper joint limit of a particular joint + * @param[in] leg_index Quad leg (0 = FL, 1 = BL, 2 = FR, 3 = BR) + * @param[in] link_index Index for link (0 = abad, 1 = upper, 2 = lower) + * @return Requested link length + */ + double getLinkLength(int leg_index, int link_index) const; + + /** + * @brief Get the transform from the world frame to the leg base + * @param[in] leg_index Quad leg (0 = FL, 1 = BL, 2 = FR, 3 = BR) + * @param[in] body_pos Position of center of body frame + * @param[in] body_rpy Orientation of body frame in roll, pitch, yaw + * @param[out] g_world_legbase Transformation matrix of world to leg base + */ + void worldToLegbaseFKWorldFrame(int leg_index, + const Eigen::Vector3d &body_pos, + const Eigen::Vector3d &body_rpy, + Eigen::Matrix4d &g_world_legbase) const; + + /** + * @brief Get the position of the leg base frame origin in the world frame + * @param[in] leg_index Quad leg (0 = FL, 1 = BL, 2 = FR, 3 = BR) + * @param[in] body_pos Position of center of body frame + * @param[in] body_rpy Orientation of body frame in roll, pitch, yaw + * @param[out] leg_base_pos_world Origin of leg base frame in world frame + */ + void worldToLegbaseFKWorldFrame(int leg_index, + const Eigen::Vector3d &body_pos, + const Eigen::Vector3d &body_rpy, + Eigen::Vector3d &leg_base_pos_world) const; + + /** + * @brief Get the position of the nominal hip location in the world frame + * @param[in] leg_index Quad leg (0 = FL, 1 = BL, 2 = FR, 3 = BR) + * @param[in] body_pos Position of center of body frame + * @param[in] body_rpy Orientation of body frame in roll, pitch, yaw + * @param[out] nominal_hip_pos_world Location of nominal hip in world frame + */ + void worldToNominalHipFKWorldFrame( + int leg_index, const Eigen::Vector3d &body_pos, + const Eigen::Vector3d &body_rpy, + Eigen::Vector3d &nominal_hip_pos_world) const; + + /** + * @brief Compute Jacobian for generalized coordinates + * @param[in] state Joint and body states + * @param[out] jacobian Jacobian for generalized coordinates + */ + void getJacobianGenCoord(const Eigen::VectorXd &state, Eigen::MatrixXd &jacobian) const; - /** - * @brief Compute Jacobian for angular velocity in world frame - * @param[in] state Joint and body states - * @param[out] jacobian Jacobian for angular velocity in world frame - */ - void getJacobianWorldAngVel(const Eigen::VectorXd &state, - Eigen::MatrixXd &jacobian) const; - - /** - * @brief Compute rotation matrix given roll pitch and yaw - * @param[in] rpy Roll pitch and yaw - * @param[out] rot Rotation matrix - */ - void getRotationMatrix(const Eigen::VectorXd &rpy, - Eigen::Matrix3d &rot) const; - - /** - * @brief Compute inverse dynamics for swing leg - * @param[in] state_pos Position states - * @param[in] state_vel Velocity states - * @param[in] foot_acc Foot absolute acceleration in world frame - * @param[in] grf Ground reaction force - * @param[in] contact_mode Contact mode of the legs - * @param[out] tau Joint torques - */ - void computeInverseDynamics(const Eigen::VectorXd &state_pos, - const Eigen::VectorXd &state_vel, - const Eigen::VectorXd &foot_acc, - const Eigen::VectorXd &grf, - const std::vector &contact_mode, - Eigen::VectorXd &tau) const; - - /** - * @brief Convert centroidal model states (foot coordinates and grfs) to full - * body (joints and torques) - * @param[in] body_state Position states - * @param[in] foot_positions Foot positions in the world frame - * @param[in] foot_velocities Foot velocities in the world frame - * @param[in] grfs Ground reaction forces - * @param[out] joint_positions Joint positions - * @param[out] joint_velocities Joint velocities - * @param[out] tau Joint torques - * @return boolean for exactness of kinematics - */ - bool convertCentroidalToFullBody(const Eigen::VectorXd &body_state, - const Eigen::VectorXd &foot_positions, - const Eigen::VectorXd &foot_velocities, - const Eigen::VectorXd &grfs, - Eigen::VectorXd &joint_positions, - Eigen::VectorXd &joint_velocities, - Eigen::VectorXd &torques); - - /** - * @brief Apply a uniform maximum torque to a given set of joint torques - * @param[in] torques Joint torques. in Nm - * @param[in] constrained_torques Joint torques after applying max, in Nm - * @return Boolean to indicate if initial torques is feasible (checks if - * torques == constrained_torques) - */ - bool applyMotorModel(const Eigen::VectorXd &torques, - Eigen::VectorXd &constrained_torques); - - /** - * @brief Apply a linear motor model to a given set of joint torques and - * velocities - * @param[in] torques Joint torques. in Nm - * @param[in] joint_velocities Velocities of each joint. in rad/s - * @param[in] constrained_torques Joint torques after applying motor model, in - * Nm - * @return Boolean to indicate if initial torques is feasible (checks if - * torques == constrained_torques) - */ - bool applyMotorModel(const Eigen::VectorXd &torques, - const Eigen::VectorXd &joint_velocities, - Eigen::VectorXd &constrained_torques); - - /** - * @brief Check if state is valid - * @param[in] body_state Robot body positions and velocities - * @param[in] joint_state Joint positions and velocities - * @param[in] torques Joint torques - * @param[in] terrain Map of the terrain for collision checking - * @return Boolean for state validity - */ - bool isValidFullState(const Eigen::VectorXd &body_state, - const Eigen::VectorXd &joint_state, - const Eigen::VectorXd &torques, - const grid_map::GridMap &terrain, - Eigen::VectorXd &state_violation, - Eigen::VectorXd &control_violation); - - /** - * @brief Check if state is valid - * @param[in] body_state Robot body positions and velocities - * @param[in] foot_positions Foot positions - * @param[in] foot_velocities Foot velocities - * @param[in] grfs Ground reaction forces in the world frame - * @param[in] terrain Map of the terrain for collision checking - * @return Boolean for state validity - */ - bool isValidCentroidalState( - const Eigen::VectorXd &body_state, const Eigen::VectorXd &foot_positions, - const Eigen::VectorXd &foot_velocities, const Eigen::VectorXd &grfs, - const grid_map::GridMap &terrain, Eigen::VectorXd &joint_positions, - Eigen::VectorXd &joint_velocities, Eigen::VectorXd &torques, - Eigen::VectorXd &state_violation, Eigen::VectorXd &control_violation); - - inline double getGroundClearance(const Eigen::Vector3d &point, - const grid_map::GridMap &terrain) { - grid_map::Position pos = {point.x(), point.y()}; - return (point.z() - terrain.atPosition("z", pos)); - } + /** + * @brief Compute Jacobian for angular velocity in body frame + * @param[in] state Joint and body states + * @param[out] jacobian Jacobian for angular velocity in body frame + */ + void getJacobianBodyAngVel(const Eigen::VectorXd &state, + Eigen::MatrixXd &jacobian) const; + + /** + * @brief Compute Jacobian for angular velocity in world frame + * @param[in] state Joint and body states + * @param[out] jacobian Jacobian for angular velocity in world frame + */ + void getJacobianWorldAngVel(const Eigen::VectorXd &state, + Eigen::MatrixXd &jacobian) const; + + /** + * @brief Compute rotation matrix given roll pitch and yaw + * @param[in] rpy Roll pitch and yaw + * @param[out] rot Rotation matrix + */ + void getRotationMatrix(const Eigen::VectorXd &rpy, + Eigen::Matrix3d &rot) const; + + /** + * @brief Compute inverse dynamics for swing leg + * @param[in] state_pos Position states + * @param[in] state_vel Velocity states + * @param[in] foot_acc Foot absolute acceleration in world frame + * @param[in] grf Ground reaction force + * @param[in] contact_mode Contact mode of the legs + * @param[out] tau Joint torques + */ + void computeInverseDynamics(const Eigen::VectorXd &state_pos, + const Eigen::VectorXd &state_vel, + const Eigen::VectorXd &foot_acc, + const Eigen::VectorXd &grf, + const std::vector &contact_mode, + Eigen::VectorXd &tau) const; + + /** + * @brief Convert centroidal model states (foot coordinates and grfs) to + * full body (joints and torques) + * @param[in] body_state Position states + * @param[in] foot_positions Foot positions in the world frame + * @param[in] foot_velocities Foot velocities in the world frame + * @param[in] grfs Ground reaction forces + * @param[out] joint_positions Joint positions + * @param[out] joint_velocities Joint velocities + * @param[out] tau Joint torques + * @return boolean for exactness of kinematics + */ + bool convertCentroidalToFullBody(const Eigen::VectorXd &body_state, + const Eigen::VectorXd &foot_positions, + const Eigen::VectorXd &foot_velocities, + const Eigen::VectorXd &grfs, + Eigen::VectorXd &joint_positions, + Eigen::VectorXd &joint_velocities, + Eigen::VectorXd &torques); + + /** + * @brief Apply a uniform maximum torque to a given set of joint torques + * @param[in] torques Joint torques. in Nm + * @param[in] constrained_torques Joint torques after applying max, in Nm + * @return Boolean to indicate if initial torques is feasible (checks if + * torques == constrained_torques) + */ + bool applyMotorModel(const Eigen::VectorXd &torques, + Eigen::VectorXd &constrained_torques); + + /** + * @brief Apply a linear motor model to a given set of joint torques and + * velocities + * @param[in] torques Joint torques. in Nm + * @param[in] joint_velocities Velocities of each joint. in rad/s + * @param[in] constrained_torques Joint torques after applying motor model, + * in Nm + * @return Boolean to indicate if initial torques is feasible (checks if + * torques == constrained_torques) + */ + bool applyMotorModel(const Eigen::VectorXd &torques, + const Eigen::VectorXd &joint_velocities, + Eigen::VectorXd &constrained_torques); + + /** + * @brief Check if state is valid + * @param[in] body_state Robot body positions and velocities + * @param[in] joint_state Joint positions and velocities + * @param[in] torques Joint torques + * @param[in] terrain Map of the terrain for collision checking + * @return Boolean for state validity + */ + bool isValidFullState(const Eigen::VectorXd &body_state, + const Eigen::VectorXd &joint_state, + const Eigen::VectorXd &torques, + const grid_map::GridMap &terrain, + Eigen::VectorXd &state_violation, + Eigen::VectorXd &control_violation); + + /** + * @brief Check if state is valid + * @param[in] body_state Robot body positions and velocities + * @param[in] foot_positions Foot positions + * @param[in] foot_velocities Foot velocities + * @param[in] grfs Ground reaction forces in the world frame + * @param[in] terrain Map of the terrain for collision checking + * @return Boolean for state validity + */ + bool isValidCentroidalState( + const Eigen::VectorXd &body_state, + const Eigen::VectorXd &foot_positions, + const Eigen::VectorXd &foot_velocities, const Eigen::VectorXd &grfs, + const grid_map::GridMap &terrain, Eigen::VectorXd &joint_positions, + Eigen::VectorXd &joint_velocities, Eigen::VectorXd &torques, + Eigen::VectorXd &state_violation, Eigen::VectorXd &control_violation); + + inline double getGroundClearance(const Eigen::Vector3d &point, + const grid_map::GridMap &terrain) { + grid_map::Position pos = {point.x(), point.y()}; + return (point.z() - terrain.atPosition("z", pos)); + } private: - /// Number of feet - const int num_feet_ = 4; + /// Number of feet + const int num_feet_ = 4; - /// Vector of the abad link lengths - std::vector l0_vec_; + /// Vector of the abad link lengths + std::vector l0_vec_; - /// Upper link length - double l1_; + /// Upper link length + double l1_; - /// Lower link length - double l2_; + /// Lower link length + double l2_; - /// Abad offset from legbase - Eigen::Vector3d abad_offset_; + /// Abad offset from legbase + Eigen::Vector3d abad_offset_; - /// Knee offset from hip - Eigen::Vector3d knee_offset_; + /// Knee offset from hip + Eigen::Vector3d knee_offset_; - /// Foot offset from knee - Eigen::Vector3d foot_offset_; + /// Foot offset from knee + Eigen::Vector3d foot_offset_; - /// Vector of legbase offsets - std::vector legbase_offsets_; + /// Vector of legbase offsets + std::vector legbase_offsets_; - /// Vector of legbase offsets - std::vector g_body_legbases_; + /// Vector of legbase offsets + std::vector g_body_legbases_; - /// Epsilon offset for joint bounds - const double joint_eps = 0.1; + /// Epsilon offset for joint bounds + const double joint_eps = 0.1; - /// Vector of the joint lower limits - std::vector> joint_min_; + /// Vector of the joint lower limits + std::vector> joint_min_; - /// Vector of the joint upper limits - std::vector> joint_max_; + /// Vector of the joint upper limits + std::vector> joint_max_; - RigidBodyDynamics::Model *model_; + RigidBodyDynamics::Model *model_; - std::vector body_name_list_; + std::vector body_name_list_; - std::vector body_id_list_; + std::vector body_id_list_; - std::vector leg_idx_list_; + std::vector leg_idx_list_; - /// Abad max joint torque - const double abad_tau_max_ = 21; + /// Abad max joint torque + const double abad_tau_max_ = 21; - /// Hip max joint torque - const double hip_tau_max_ = 21; + /// Hip max joint torque + const double hip_tau_max_ = 21; - /// Knee max joint torque - const double knee_tau_max_ = 32; + /// Knee max joint torque + const double knee_tau_max_ = 32; - /// Vector of max torques - const Eigen::VectorXd tau_max_ = - (Eigen::VectorXd(12) << abad_tau_max_, hip_tau_max_, knee_tau_max_, - abad_tau_max_, hip_tau_max_, knee_tau_max_, abad_tau_max_, hip_tau_max_, - knee_tau_max_, abad_tau_max_, hip_tau_max_, knee_tau_max_) - .finished(); + /// Vector of max torques + const Eigen::VectorXd tau_max_ = + (Eigen::VectorXd(12) << abad_tau_max_, hip_tau_max_, knee_tau_max_, + abad_tau_max_, hip_tau_max_, knee_tau_max_, abad_tau_max_, + hip_tau_max_, knee_tau_max_, abad_tau_max_, hip_tau_max_, + knee_tau_max_) + .finished(); - /// Abad max joint velocity - const double abad_vel_max_ = 37.7; + /// Abad max joint velocity + const double abad_vel_max_ = 37.7; - /// Hip max joint velocity - const double hip_vel_max_ = 37.7; + /// Hip max joint velocity + const double hip_vel_max_ = 37.7; - /// Knee max joint velocity - const double knee_vel_max_ = 25.1; + /// Knee max joint velocity + const double knee_vel_max_ = 25.1; - /// Vector of max velocities - const Eigen::VectorXd vel_max_ = - (Eigen::VectorXd(12) << abad_vel_max_, hip_vel_max_, knee_vel_max_, - abad_vel_max_, hip_vel_max_, knee_vel_max_, abad_vel_max_, hip_vel_max_, - knee_vel_max_, abad_vel_max_, hip_vel_max_, knee_vel_max_) - .finished(); + /// Vector of max velocities + const Eigen::VectorXd vel_max_ = + (Eigen::VectorXd(12) << abad_vel_max_, hip_vel_max_, knee_vel_max_, + abad_vel_max_, hip_vel_max_, knee_vel_max_, abad_vel_max_, + hip_vel_max_, knee_vel_max_, abad_vel_max_, hip_vel_max_, + knee_vel_max_) + .finished(); - const Eigen::VectorXd mm_slope_ = tau_max_.cwiseQuotient(vel_max_); + const Eigen::VectorXd mm_slope_ = tau_max_.cwiseQuotient(vel_max_); }; } // namespace quad_utils diff --git a/quad_utils/include/quad_utils/ros_utils.h b/quad_utils/include/quad_utils/ros_utils.h index b8272565b..43a3e6c2b 100644 --- a/quad_utils/include/quad_utils/ros_utils.h +++ b/quad_utils/include/quad_utils/ros_utils.h @@ -14,9 +14,9 @@ namespace quad_utils { * @param[in] t_compare ROS time we wish to compare to * @return Age in ms (compared to t_compare) */ -inline double getROSMessageAgeInMs(std_msgs::Header header, - ros::Time t_compare) { - return (t_compare - header.stamp).toSec() * 1000.0; +inline double getROSMessageAgeInMs(const std_msgs::Header &header, + const ros::Time &t_compare) { + return (t_compare - header.stamp).toSec() * 1000.0; } /** @@ -24,9 +24,9 @@ inline double getROSMessageAgeInMs(std_msgs::Header header, * @param[in] header ROS Header that we wish to compute the age of * @return Age in ms (compared to ros::Time::now()) */ -inline double getROSMessageAgeInMs(std_msgs::Header header) { - ros::Time t_compare = ros::Time::now(); - return quad_utils::getROSMessageAgeInMs(header, t_compare); +inline double getROSMessageAgeInMs(const std_msgs::Header &header) { + ros::Time t_compare = ros::Time::now(); + return quad_utils::getROSMessageAgeInMs(header, t_compare); } /** @@ -34,8 +34,8 @@ inline double getROSMessageAgeInMs(std_msgs::Header header) { * @param[in] plan_start ROS Time to to compare to * @return Time in plan (compared to ros::Time::now()) */ -inline double getDurationSinceTime(ros::Time plan_start) { - return (ros::Time::now() - plan_start).toSec(); +inline double getDurationSinceTime(const ros::Time &plan_start) { + return (ros::Time::now() - plan_start).toSec(); } /** @@ -46,11 +46,11 @@ inline double getDurationSinceTime(ros::Time plan_start) { * @param[in] plan_start ROS Time to to compare to * @param[in] dt Timestep used to discretize the plan */ -inline void getPlanIndex(ros::Time plan_start, double dt, int &index, +inline void getPlanIndex(const ros::Time &plan_start, double dt, int &index, double &first_element_duration) { - double duration = getDurationSinceTime(plan_start); - index = std::floor(duration / dt); - first_element_duration = (index + 1) * dt - duration; + double duration = getDurationSinceTime(plan_start); + index = std::floor(duration / dt); + first_element_duration = (index + 1) * dt - duration; } /** @@ -61,13 +61,14 @@ inline void getPlanIndex(ros::Time plan_start, double dt, int &index, * @return boolean success */ template -inline bool loadROSParam(ros::NodeHandle nh, std::string paramName, +inline bool loadROSParam(ros::NodeHandle nh, const std::string ¶mName, ParamType &varName) { - if (!nh.getParam(paramName, varName)) { - ROS_ERROR("Can't find param %s from parameter server", paramName.c_str()); - return false; - } - return true; + if (!nh.getParam(paramName, varName)) { + ROS_ERROR("Can't find param %s from parameter server", + paramName.c_str()); + return false; + } + return true; } /** @@ -80,15 +81,18 @@ inline bool loadROSParam(ros::NodeHandle nh, std::string paramName, * @return boolean (true if found rosparam, false if loaded default) */ template -inline bool loadROSParamDefault(ros::NodeHandle nh, std::string paramName, - ParamType &varName, ParamType defaultVal) { - if (!nh.getParam(paramName, varName)) { - varName = defaultVal; - ROS_INFO("Can't find param %s on rosparam server, loading default value.", - paramName.c_str()); - return false; - } - return true; +inline bool loadROSParamDefault(ros::NodeHandle nh, + const std::string ¶mName, + ParamType &varName, + const ParamType &defaultVal) { + if (!nh.getParam(paramName, varName)) { + varName = defaultVal; + ROS_INFO( + "Can't find param %s on rosparam server, loading default value.", + paramName.c_str()); + return false; + } + return true; } // /** @@ -107,8 +111,8 @@ inline bool loadROSParamDefault(ros::NodeHandle nh, std::string paramName, * @param[in] frame Frame_id for the state message * @param[in] traj_index Trajectory index of this state message */ -void updateStateHeaders(quad_msgs::RobotState &msg, ros::Time stamp, - std::string frame, int traj_index); +void updateStateHeaders(quad_msgs::RobotState &msg, const ros::Time &stamp, + const std::string &frame, int traj_index); /** * @brief Interpolate two headers @@ -117,8 +121,9 @@ void updateStateHeaders(quad_msgs::RobotState &msg, ros::Time stamp, * @param[in] t_interp Fraction of time between the messages [0,1] * @param[out] interp_state Interpolated header */ -void interpHeader(std_msgs::Header header_1, std_msgs::Header header_2, - double t_interp, std_msgs::Header &interp_header); +void interpHeader(const std_msgs::Header &header_1, + const std_msgs::Header &header_2, double t_interp, + std_msgs::Header &interp_header); /** * @brief Interpolate data between two Odometry messages. @@ -127,8 +132,9 @@ void interpHeader(std_msgs::Header header_1, std_msgs::Header header_2, * @param[in] t_interp Fraction of time between the messages [0,1] * @param[out] interp_state Interpolated Odometry message */ -void interpOdometry(quad_msgs::BodyState state_1, quad_msgs::BodyState state_2, - double t_interp, quad_msgs::BodyState &interp_state); +void interpOdometry(const quad_msgs::BodyState &state_1, + const quad_msgs::BodyState &state_2, double t_interp, + quad_msgs::BodyState &interp_state); /** * @brief Interpolate data between two JointState messages. @@ -137,8 +143,8 @@ void interpOdometry(quad_msgs::BodyState state_1, quad_msgs::BodyState state_2, * @param[in] t_interp Fraction of time between the messages [0,1] * @param[out] interp_state Interpolated JointState message */ -void interpJointState(sensor_msgs::JointState state_1, - sensor_msgs::JointState state_2, double t_interp, +void interpJointState(const sensor_msgs::JointState &state_1, + const sensor_msgs::JointState &state_2, double t_interp, sensor_msgs::JointState &interp_state); /** @@ -148,8 +154,9 @@ void interpJointState(sensor_msgs::JointState state_1, * @param[in] t_interp Fraction of time between the messages [0,1] * @param[out] interp_state Interpolated FootState message */ -void interpMultiFootState(quad_msgs::MultiFootState state_1, - quad_msgs::MultiFootState state_2, double t_interp, +void interpMultiFootState(const quad_msgs::MultiFootState &state_1, + const quad_msgs::MultiFootState &state_2, + double t_interp, quad_msgs::MultiFootState &interp_state); /** @@ -159,8 +166,9 @@ void interpMultiFootState(quad_msgs::MultiFootState state_1, * @param[in] t_interp Fraction of time between the messages [0,1] * @param[out] interp_state Interpolated GRFArray message */ -void interpGRFArray(quad_msgs::GRFArray state_1, quad_msgs::GRFArray state_2, - double t_interp, quad_msgs::GRFArray &interp_state); +void interpGRFArray(const quad_msgs::GRFArray &state_1, + const quad_msgs::GRFArray &state_2, double t_interp, + quad_msgs::GRFArray &interp_state); /** * @brief Interpolate data between two RobotState messages. @@ -169,8 +177,8 @@ void interpGRFArray(quad_msgs::GRFArray state_1, quad_msgs::GRFArray state_2, * @param[in] t_interp Fraction of time between the messages [0,1] * @param[out] interp_state Interpolated RobotState message */ -void interpRobotState(quad_msgs::RobotState state_1, - quad_msgs::RobotState state_2, double t_interp, +void interpRobotState(const quad_msgs::RobotState &state_1, + const quad_msgs::RobotState &state_2, double t_interp, quad_msgs::RobotState &interp_state); /** @@ -182,7 +190,7 @@ void interpRobotState(quad_msgs::RobotState state_1, * @param[out] interp_primitive_id Interpolated primitive id * @param[out] interp_grf Interpolated GRF array */ -void interpRobotPlan(quad_msgs::RobotPlan msg, double t, +void interpRobotPlan(const quad_msgs::RobotPlan &msg, double t, quad_msgs::RobotState &interp_state, int &interp_primitive_id, quad_msgs::GRFArray &interp_grf); @@ -194,7 +202,7 @@ void interpRobotPlan(quad_msgs::RobotPlan msg, double t, * @return MultiFootState message */ quad_msgs::MultiFootState interpMultiFootPlanContinuous( - quad_msgs::MultiFootPlanContinuous msg, double t); + const quad_msgs::MultiFootPlanContinuous &msg, double t); // /** // * @brief Interpolate data from a robot state trajectory message. @@ -216,8 +224,8 @@ quad_msgs::MultiFootState interpMultiFootPlanContinuous( * @param[out] joint_state message of the corresponding joint state */ void ikRobotState(const quad_utils::QuadKD &kinematics, - quad_msgs::BodyState body_state, - quad_msgs::MultiFootState multi_foot_state, + const quad_msgs::BodyState &body_state, + const quad_msgs::MultiFootState &multi_foot_state, sensor_msgs::JointState &joint_state); /** @@ -237,8 +245,8 @@ void ikRobotState(const quad_utils::QuadKD &kinematics, * @param[out] multi_foot_state message of state of each foot */ void fkRobotState(const quad_utils::QuadKD &kinematics, - quad_msgs::BodyState body_state, - sensor_msgs::JointState joint_state, + const quad_msgs::BodyState &body_state, + const sensor_msgs::JointState &joint_state, quad_msgs::MultiFootState &multi_foot_state); /** @@ -270,8 +278,8 @@ Eigen::VectorXd bodyStateMsgToEigen(const quad_msgs::BodyState &body); * information * @param[out] grf_msg GRFArray msg containing GRF data */ -void eigenToGRFArrayMsg(Eigen::VectorXd grf_array, - quad_msgs::MultiFootState multi_foot_state_msg, +void eigenToGRFArrayMsg(const Eigen::VectorXd &grf_array, + const quad_msgs::MultiFootState &multi_foot_state_msg, quad_msgs::GRFArray &grf_msg); /** @@ -331,8 +339,8 @@ void multiFootStateMsgToEigen( * @param[out] foot_state_msg FootState msg containing foot position and * velocity data */ -void eigenToFootStateMsg(Eigen::VectorXd foot_position, - Eigen::VectorXd foot_velocity, +void eigenToFootStateMsg(const Eigen::VectorXd &foot_position, + const Eigen::VectorXd &foot_velocity, quad_msgs::FootState &foot_state_msg); /** @@ -343,9 +351,9 @@ void eigenToFootStateMsg(Eigen::VectorXd foot_position, * @param[out] foot_state_msg FootState msg containing foot position and * velocity data */ -void eigenToFootStateMsg(Eigen::VectorXd foot_position, - Eigen::VectorXd foot_velocity, - Eigen::VectorXd foot_acceleration, +void eigenToFootStateMsg(const Eigen::VectorXd &foot_position, + const Eigen::VectorXd &foot_velocity, + const Eigen::VectorXd &foot_acceleration, quad_msgs::FootState &foot_state_msg); /** diff --git a/quad_utils/launch/estimation.launch b/quad_utils/launch/estimation.launch new file mode 100644 index 000000000..91172c1a2 --- /dev/null +++ b/quad_utils/launch/estimation.launch @@ -0,0 +1,11 @@ + + + + + + + + + + + diff --git a/quad_utils/launch/load_global_params.launch b/quad_utils/launch/load_global_params.launch index 166d53af4..92f155090 100644 --- a/quad_utils/launch/load_global_params.launch +++ b/quad_utils/launch/load_global_params.launch @@ -6,6 +6,7 @@ + @@ -19,6 +20,11 @@ + + + + + diff --git a/quad_utils/launch/load_robot_params.launch b/quad_utils/launch/load_robot_params.launch index 3fddfe526..645b4afd3 100644 --- a/quad_utils/launch/load_robot_params.launch +++ b/quad_utils/launch/load_robot_params.launch @@ -10,6 +10,12 @@ + + + + + + diff --git a/quad_utils/launch/logging.launch b/quad_utils/launch/logging.launch index 99d3948ea..ef67d3b41 100644 --- a/quad_utils/launch/logging.launch +++ b/quad_utils/launch/logging.launch @@ -22,6 +22,8 @@ /$(arg namespace)/control/mode /$(arg namespace)/foot_plan_continuous /$(arg namespace)/foot_plan_discrete + /$(arg namespace)/body_force/joint_torques + /$(arg namespace)/body_force/toe_forces /terrain_map" /> @@ -40,6 +42,8 @@ /$(arg namespace)/control/mode /$(arg namespace)/foot_plan_continuous /$(arg namespace)/foot_plan_discrete + /$(arg namespace)/body_force/joint_torques + /$(arg namespace)/body_force/toe_forces /terrain_map" /> diff --git a/quad_utils/launch/planning.launch b/quad_utils/launch/planning.launch index 3dc2d0968..47c318633 100644 --- a/quad_utils/launch/planning.launch +++ b/quad_utils/launch/planning.launch @@ -39,6 +39,9 @@ + + + diff --git a/quad_utils/launch/quad_gazebo.launch b/quad_utils/launch/quad_gazebo.launch index 42fc5ebd5..0179f9a9a 100644 --- a/quad_utils/launch/quad_gazebo.launch +++ b/quad_utils/launch/quad_gazebo.launch @@ -34,7 +34,7 @@ - + diff --git a/quad_utils/launch/quad_plan.launch b/quad_utils/launch/quad_plan.launch index a2897ad38..d8ae5b6c4 100644 --- a/quad_utils/launch/quad_plan.launch +++ b/quad_utils/launch/quad_plan.launch @@ -20,6 +20,7 @@ + diff --git a/quad_utils/launch/quad_spawn.launch b/quad_utils/launch/quad_spawn.launch index 2748cf2df..39b7f3bf4 100644 --- a/quad_utils/launch/quad_spawn.launch +++ b/quad_utils/launch/quad_spawn.launch @@ -1,6 +1,6 @@ - + @@ -16,6 +16,9 @@ + + + @@ -28,5 +31,8 @@ - + + + diff --git a/quad_utils/launch/remote_driver.launch b/quad_utils/launch/remote_driver.launch index 64d7d79da..b8dfb8c69 100644 --- a/quad_utils/launch/remote_driver.launch +++ b/quad_utils/launch/remote_driver.launch @@ -1,6 +1,9 @@ + - + + + diff --git a/quad_utils/launch/robot_driver.launch b/quad_utils/launch/robot_driver.launch index bda0c0dfc..b0244a6b2 100644 --- a/quad_utils/launch/robot_driver.launch +++ b/quad_utils/launch/robot_driver.launch @@ -1,21 +1,50 @@ - + + - - + + - + + + - - - - + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + diff --git a/quad_utils/launch/underbrush_gazebo.launch b/quad_utils/launch/underbrush_gazebo.launch new file mode 100644 index 000000000..7462241e6 --- /dev/null +++ b/quad_utils/launch/underbrush_gazebo.launch @@ -0,0 +1,44 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + \ No newline at end of file diff --git a/quad_utils/launch/visualization_plugins.launch b/quad_utils/launch/visualization_plugins.launch index d813435b8..4016648c6 100644 --- a/quad_utils/launch/visualization_plugins.launch +++ b/quad_utils/launch/visualization_plugins.launch @@ -22,6 +22,14 @@ + + + + + + + + diff --git a/quad_utils/package.xml b/quad_utils/package.xml index 045c5022a..aaa2af618 100644 --- a/quad_utils/package.xml +++ b/quad_utils/package.xml @@ -77,6 +77,8 @@ grid_map_ros grid_map_pcl + plotjuggler_ros + diff --git a/quad_utils/rviz/quad_viewer.rviz b/quad_utils/rviz/quad_viewer.rviz index 73a4e8128..73136cc83 100644 --- a/quad_utils/rviz/quad_viewer.rviz +++ b/quad_utils/rviz/quad_viewer.rviz @@ -5,7 +5,7 @@ Panels: Property Tree Widget: Expanded: ~ Splitter Ratio: 0.5008665323257446 - Tree Height: 799 + Tree Height: 441 - Class: rviz/Selection Name: Selection - Class: rviz/Tool Properties @@ -21,7 +21,6 @@ Panels: Name: Views Splitter Ratio: 0.5 - Class: rviz/Time - Experimental: false Name: Time SyncMode: 0 SyncSource: "" @@ -32,12 +31,14 @@ Toolbars: Visualization Manager: Class: "" Displays: - - Class: rviz/Axes + - Alpha: 1 + Class: rviz/Axes Enabled: false Length: 0.20000000298023224 Name: Axes Radius: 0.009999999776482582 Reference Frame: + Show Trail: false Value: false - Alpha: 1 Autocompute Intensity Bounds: true @@ -240,7 +241,7 @@ Visualization Manager: Marker Topic: /robot_1/visualization/current_grf Name: Robot 1 Desired GRFs Namespaces: - "": true + {} Queue Size: 100 Value: true - Class: rviz/MarkerArray @@ -248,7 +249,7 @@ Visualization Manager: Marker Topic: /robot_2/visualization/current_grf Name: Robot 2 Desired GRFs Namespaces: - "": true + {} Queue Size: 100 Value: true - Class: rviz/Marker @@ -324,6 +325,7 @@ Visualization Manager: Z: 0 Pose Color: 255; 85; 255 Pose Style: None + Queue Size: 10 Radius: 0.029999999329447746 Shaft Diameter: 0.10000000149011612 Shaft Length: 0.10000000149011612 @@ -347,6 +349,7 @@ Visualization Manager: Z: 0 Pose Color: 255; 85; 255 Pose Style: None + Queue Size: 10 Radius: 0.029999999329447746 Shaft Diameter: 0.10000000149011612 Shaft Length: 0.10000000149011612 @@ -370,6 +373,7 @@ Visualization Manager: Z: 0 Pose Color: 255; 85; 255 Pose Style: None + Queue Size: 10 Radius: 0.029999999329447746 Shaft Diameter: 0.10000000149011612 Shaft Length: 0.10000000149011612 @@ -393,6 +397,7 @@ Visualization Manager: Z: 0 Pose Color: 255; 85; 255 Pose Style: None + Queue Size: 10 Radius: 0.029999999329447746 Shaft Diameter: 0.10000000149011612 Shaft Length: 0.10000000149011612 @@ -424,6 +429,7 @@ Visualization Manager: Z: 0 Pose Color: 255; 85; 255 Pose Style: None + Queue Size: 10 Radius: 0.029999999329447746 Shaft Diameter: 0.10000000149011612 Shaft Length: 0.10000000149011612 @@ -447,6 +453,7 @@ Visualization Manager: Z: 0 Pose Color: 255; 85; 255 Pose Style: None + Queue Size: 10 Radius: 0.029999999329447746 Shaft Diameter: 0.10000000149011612 Shaft Length: 0.10000000149011612 @@ -470,6 +477,7 @@ Visualization Manager: Z: 0 Pose Color: 255; 85; 255 Pose Style: None + Queue Size: 10 Radius: 0.029999999329447746 Shaft Diameter: 0.10000000149011612 Shaft Length: 0.10000000149011612 @@ -493,6 +501,7 @@ Visualization Manager: Z: 0 Pose Color: 255; 85; 255 Pose Style: None + Queue Size: 10 Radius: 0.029999999329447746 Shaft Diameter: 0.10000000149011612 Shaft Length: 0.10000000149011612 @@ -547,6 +556,7 @@ Visualization Manager: Stereo Focal Distance: 1 Swap Stereo Eyes: false Value: false + Field of View: 0.7853981852531433 Focal Point: X: 1.643458604812622 Y: 0.952698826789856 @@ -558,16 +568,15 @@ Visualization Manager: Near Clip Distance: 0.009999999776482582 Pitch: 0.5647967457771301 Target Frame: - Value: Orbit (rviz) Yaw: 4.203597068786621 Saved: ~ Window Geometry: Displays: collapsed: true - Height: 1052 + Height: 434 Hide Left Dock: true Hide Right Dock: true - QMainWindow State: 000000ff00000000fd00000004000000000000025a0000035cfc0200000008fb0000001200530065006c0065006300740069006f006e00000001e10000009b0000005c00fffffffb0000001e0054006f006f006c002000500072006f007000650072007400690065007302000001ed000001df00000185000000b0fb000000120056006900650077007300200054006f006f02000001df000002110000018500000122fb000000200054006f006f006c002000500072006f0070006500720074006900650073003203000002880000011d000002210000017afb000000100044006900730070006c006100790073000000003d0000035c000000c900fffffffb0000002000730065006c0065006300740069006f006e00200062007500660066006500720200000138000000aa0000023a00000294fb00000014005700690064006500530074006500720065006f02000000e6000000d2000003ee0000030bfb0000000c004b0069006e0065006300740200000186000001060000030c00000261000000010000010f0000040efc0200000003fb0000001e0054006f006f006c002000500072006f00700065007200740069006500730100000041000000780000000000000000fb0000000a0056006900650077007300000000280000040e000000a400fffffffb0000001200530065006c0065006300740069006f006e010000025a000000b200000000000000000000000200000490000000a9fc0100000001fb0000000a00560069006500770073030000004e00000080000002e100000197000000030000078000000060fc0100000002fb0000000800540069006d0065010000000000000780000002eb00fffffffb0000000800540069006d00650100000000000004500000000000000000000007800000035c00000004000000040000000800000008fc0000000100000002000000010000000a0054006f006f006c00730100000000ffffffff0000000000000000 + QMainWindow State: 000000ff00000000fd00000004000000000000025a0000035cfc0200000008fb0000001200530065006c0065006300740069006f006e00000001e10000009b0000005c00fffffffb0000001e0054006f006f006c002000500072006f007000650072007400690065007302000001ed000001df00000185000000b0fb000000120056006900650077007300200054006f006f02000001df000002110000018500000122fb000000200054006f006f006c002000500072006f0070006500720074006900650073003203000002880000011d000002210000017afb000000100044006900730070006c006100790073000000003d0000035c000000c900fffffffb0000002000730065006c0065006300740069006f006e00200062007500660066006500720200000138000000aa0000023a00000294fb00000014005700690064006500530074006500720065006f02000000e6000000d2000003ee0000030bfb0000000c004b0069006e0065006300740200000186000001060000030c00000261000000010000010f0000040efc0200000003fb0000001e0054006f006f006c002000500072006f00700065007200740069006500730100000041000000780000000000000000fb0000000a0056006900650077007300000000280000040e000000a400fffffffb0000001200530065006c0065006300740069006f006e010000025a000000b200000000000000000000000200000490000000a9fc0100000001fb0000000a00560069006500770073030000004e00000080000002e100000197000000030000034e00000060fc0100000002fb0000000800540069006d006501000000000000034e0000033700fffffffb0000000800540069006d006501000000000000045000000000000000000000034e000000f200000004000000040000000800000008fc0000000100000002000000010000000a0054006f006f006c00730100000000ffffffff0000000000000000 Selection: collapsed: false Time: @@ -576,6 +585,6 @@ Window Geometry: collapsed: false Views: collapsed: true - Width: 1920 - X: 1920 - Y: 0 + Width: 846 + X: 856 + Y: 495 diff --git a/quad_utils/setup_deps.sh b/quad_utils/setup_deps.sh index dcbfa8f87..f0b08ff7e 100755 --- a/quad_utils/setup_deps.sh +++ b/quad_utils/setup_deps.sh @@ -1,4 +1,4 @@ sudo apt install -y libeigen3-dev sudo apt install -y python-pygame -sudo apt install -y ros-melodic-plotjuggler-ros -sudo apt install -y tmux \ No newline at end of file +sudo apt install -y ros-noetic-plotjuggler-ros +sudo apt install -y tmux diff --git a/quad_utils/src/math_utils.cpp b/quad_utils/src/math_utils.cpp index e92d5e842..e24337336 100644 --- a/quad_utils/src/math_utils.cpp +++ b/quad_utils/src/math_utils.cpp @@ -2,15 +2,16 @@ namespace math_utils { -std::vector interpMat(const std::vector input_vec, - const std::vector> input_mat, +std::vector interpMat(const std::vector &input_vec, + const std::vector> &input_mat, const double query_point) { // Check bounds, throw an error if invalid since this shouldn't ever happen if ((query_point < input_vec.front()) || (query_point > input_vec.back())) { throw std::runtime_error("Tried to interp out of bounds"); } - // Declare variables for interpolating between, both for input and output data + // Declare variables for interpolating between, both for input and output + // data double t1, t2; std::vector y1, y2, interp_data; @@ -34,15 +35,16 @@ std::vector interpMat(const std::vector input_vec, return interp_data; } -Eigen::Vector3d interpVector3d(const std::vector input_vec, - const std::vector input_mat, +Eigen::Vector3d interpVector3d(const std::vector &input_vec, + const std::vector &input_mat, const double query_point) { // Check bounds, throw an error if invalid since this shouldn't ever happen if ((query_point < input_vec.front()) || (query_point > input_vec.back())) { throw std::runtime_error("Tried to interp out of bounds"); } - // Declare variables for interpolating between, both for input and output data + // Declare variables for interpolating between, both for input and output + // data double t1, t2; Eigen::Vector3d y1, y2, interp_data; @@ -65,15 +67,16 @@ Eigen::Vector3d interpVector3d(const std::vector input_vec, } std::vector interpMatVector3d( - const std::vector input_vec, - const std::vector> output_mat, + const std::vector &input_vec, + const std::vector> &output_mat, const double query_point) { // Check bounds, throw an error if invalid since this shouldn't ever happen if ((query_point < input_vec.front()) || (query_point > input_vec.back())) { throw std::runtime_error("Tried to interp out of bounds"); } - // Declare variables for interpolating between, both for input and output data + // Declare variables for interpolating between, both for input and output + // data double t1, t2; std::vector y1, y2, interp_data; @@ -99,14 +102,15 @@ std::vector interpMatVector3d( return interp_data; } -int interpInt(const std::vector input_vec, std::vector output_vec, - const double query_point) { +int interpInt(const std::vector &input_vec, + std::vector &output_vec, const double query_point) { // Check bounds, throw an error if invalid since this shouldn't ever happen if ((query_point < input_vec.front()) || (query_point > input_vec.back())) { throw std::runtime_error("Tried to interp out of bounds"); } - // Declare variables for interpolating between, both for input and output data + // Declare variables for interpolating between, both for input and output + // data double t1, t2; Eigen::Vector3d y1, y2, interp_data; @@ -121,12 +125,13 @@ int interpInt(const std::vector input_vec, std::vector output_vec, throw std::runtime_error("Didn't find the query point, something happened"); } -std::vector movingAverageFilter(std::vector data, +std::vector movingAverageFilter(const std::vector &data, int window_size) { std::vector filtered_data; int N = data.size(); - // Check to ensure window size is an odd integer, if not add one to make it so + // Check to ensure window size is an odd integer, if not add one to make it + // so if ((window_size % 2) == 0) { window_size += 1; ROS_WARN_THROTTLE( @@ -165,12 +170,12 @@ std::vector movingAverageFilter(std::vector data, return filtered_data; } -std::vector centralDiff(std::vector data, double dt) { +std::vector centralDiff(const std::vector &data, double dt) { std::vector data_diff; for (int i = 0; i < data.size(); i++) { - // Compute lower and upper indices, with forward/backward difference at the - // ends + // Compute lower and upper indices, with forward/backward difference at + // the ends int lower_index = std::max(i - 1, 0); int upper_index = std::min(i + 1, (int)data.size() - 1); diff --git a/quad_utils/src/quad_kd.cpp b/quad_utils/src/quad_kd.cpp index afb314515..f3b9f15b4 100644 --- a/quad_utils/src/quad_kd.cpp +++ b/quad_utils/src/quad_kd.cpp @@ -6,561 +6,581 @@ Eigen::IOFormat CleanFmt(4, 0, ", ", "\n", "[", "]"); QuadKD::QuadKD() { initModel(""); } -QuadKD::QuadKD(std::string ns) { initModel("/" + ns + "/"); } - -void QuadKD::initModel(std::string ns) { - std::string robot_description_string; - - if (!ros::param::get("robot_description", robot_description_string)) { - std::cerr << "Error loading robot_description " << std::endl; - abort(); - } - - model_ = new RigidBodyDynamics::Model(); - if (!RigidBodyDynamics::Addons::URDFReadFromString( - robot_description_string.c_str(), model_, true)) { - std::cerr << "Error loading model " << std::endl; - abort(); - } - - body_name_list_ = {"toe0", "toe1", "toe2", "toe3"}; - - body_id_list_.resize(4); - for (size_t i = 0; i < body_name_list_.size(); i++) { - body_id_list_.at(i) = model_->GetBodyId(body_name_list_.at(i).c_str()); - } - - leg_idx_list_.resize(4); - std::iota(leg_idx_list_.begin(), leg_idx_list_.end(), 0); - std::sort(leg_idx_list_.begin(), leg_idx_list_.end(), [&](int i, int j) { - return body_id_list_.at(i) < body_id_list_.at(j); - }); - - // Read leg geometry from URDF - legbase_offsets_.resize(4); - l0_vec_.resize(4); - std::vector hip_name_list = {"hip0", "hip1", "hip2", "hip3"}; - std::vector upper_name_list = {"upper0", "upper1", "upper2", - "upper3"}; - std::vector lower_name_list = {"lower0", "lower1", "lower2", - "lower3"}; - std::vector toe_name_list = {"toe0", "toe1", "toe2", "toe3"}; - RigidBodyDynamics::Math::SpatialTransform tform; - for (size_t i = 0; i < 4; i++) { - // From body COM to abad - tform = - model_->GetJointFrame(model_->GetBodyId(hip_name_list.at(i).c_str())); - legbase_offsets_[i] = tform.r; - - // From abad to hip - tform = - model_->GetJointFrame(model_->GetBodyId(upper_name_list.at(i).c_str())); - l0_vec_[i] = tform.r(1); - - // From hip to knee (we know they should be the same and the equation in IK - // uses the magnitute of it) - tform = - model_->GetJointFrame(model_->GetBodyId(lower_name_list.at(i).c_str())); - l1_ = tform.r.cwiseAbs().maxCoeff(); - knee_offset_ = tform.r; - - // From knee to toe (we know they should be the same and the equation in IK - // uses the magnitute of it) - tform = - model_->GetJointFrame(model_->GetBodyId(toe_name_list.at(i).c_str())); - l2_ = tform.r.cwiseAbs().maxCoeff(); - foot_offset_ = tform.r; - } - - // Abad offset from legbase - abad_offset_ = {0, 0, 0}; - - g_body_legbases_.resize(4); - for (int leg_index = 0; leg_index < 4; leg_index++) { - // Compute transforms - g_body_legbases_[leg_index] = - createAffineMatrix(legbase_offsets_[leg_index], - Eigen::AngleAxisd(0, Eigen::Vector3d::UnitZ())); - } - - joint_min_.resize(num_feet_); - joint_max_.resize(num_feet_); - - std::vector joint_min_front = {-0.707, -M_PI * 0.5, 0}; - std::vector joint_min_back = {-0.707, -M_PI, 0}; - std::vector joint_max_front = {0.707, M_PI, M_PI}; - std::vector joint_max_back = {0.707, M_PI * 0.5, M_PI}; - - joint_min_ = {joint_min_front, joint_min_back, joint_min_front, - joint_min_back}; - joint_max_ = {joint_max_front, joint_max_back, joint_max_front, - joint_max_back}; +QuadKD::QuadKD(const std::string &ns) { initModel("/" + ns + "/"); } + +void QuadKD::initModel(const std::string &ns) { + std::string robot_description_string; + + if (!ros::param::get("robot_description", robot_description_string)) { + std::cerr << "Error loading robot_description " << std::endl; + abort(); + } + + model_ = new RigidBodyDynamics::Model(); + if (!RigidBodyDynamics::Addons::URDFReadFromString( + robot_description_string.c_str(), model_, true)) { + std::cerr << "Error loading model " << std::endl; + abort(); + } + + body_name_list_ = {"toe0", "toe1", "toe2", "toe3"}; + + body_id_list_.resize(4); + for (size_t i = 0; i < body_name_list_.size(); i++) { + body_id_list_.at(i) = model_->GetBodyId(body_name_list_.at(i).c_str()); + } + + leg_idx_list_.resize(4); + std::iota(leg_idx_list_.begin(), leg_idx_list_.end(), 0); + std::sort(leg_idx_list_.begin(), leg_idx_list_.end(), [&](int i, int j) { + return body_id_list_.at(i) < body_id_list_.at(j); + }); + + // Read leg geometry from URDF + legbase_offsets_.resize(4); + l0_vec_.resize(4); + std::vector hip_name_list = {"hip0", "hip1", "hip2", "hip3"}; + std::vector upper_name_list = {"upper0", "upper1", "upper2", + "upper3"}; + std::vector lower_name_list = {"lower0", "lower1", "lower2", + "lower3"}; + std::vector toe_name_list = {"toe0", "toe1", "toe2", "toe3"}; + RigidBodyDynamics::Math::SpatialTransform tform; + for (size_t i = 0; i < 4; i++) { + // From body COM to abad + tform = model_->GetJointFrame( + model_->GetBodyId(hip_name_list.at(i).c_str())); + legbase_offsets_[i] = tform.r; + + // From abad to hip + tform = model_->GetJointFrame( + model_->GetBodyId(upper_name_list.at(i).c_str())); + l0_vec_[i] = tform.r(1); + + // From hip to knee (we know they should be the same and the equation in + // IK uses the magnitute of it) + tform = model_->GetJointFrame( + model_->GetBodyId(lower_name_list.at(i).c_str())); + l1_ = tform.r.cwiseAbs().maxCoeff(); + knee_offset_ = tform.r; + + // From knee to toe (we know they should be the same and the equation in + // IK uses the magnitute of it) + tform = model_->GetJointFrame( + model_->GetBodyId(toe_name_list.at(i).c_str())); + l2_ = tform.r.cwiseAbs().maxCoeff(); + foot_offset_ = tform.r; + } + + // Abad offset from legbase + abad_offset_ = {0, 0, 0}; + + g_body_legbases_.resize(4); + for (int leg_index = 0; leg_index < 4; leg_index++) { + // Compute transforms + g_body_legbases_[leg_index] = + createAffineMatrix(legbase_offsets_[leg_index], + Eigen::AngleAxisd(0, Eigen::Vector3d::UnitZ())); + } + + joint_min_.resize(num_feet_); + joint_max_.resize(num_feet_); + + std::vector joint_min_front = {-0.707, -M_PI * 0.5, 0}; + std::vector joint_min_back = {-0.707, -M_PI, 0}; + std::vector joint_max_front = {0.707, M_PI, M_PI}; + std::vector joint_max_back = {0.707, M_PI * 0.5, M_PI}; + + joint_min_ = {joint_min_front, joint_min_back, joint_min_front, + joint_min_back}; + joint_max_ = {joint_max_front, joint_max_back, joint_max_front, + joint_max_back}; } -Eigen::Matrix4d QuadKD::createAffineMatrix(Eigen::Vector3d trans, - Eigen::Vector3d rpy) const { - Eigen::Transform t; - t = Eigen::Translation(trans); - t.rotate(Eigen::AngleAxisd(rpy[2], Eigen::Vector3d::UnitZ())); - t.rotate(Eigen::AngleAxisd(rpy[1], Eigen::Vector3d::UnitY())); - t.rotate(Eigen::AngleAxisd(rpy[0], Eigen::Vector3d::UnitX())); +Eigen::Matrix4d QuadKD::createAffineMatrix(const Eigen::Vector3d &trans, + const Eigen::Vector3d &rpy) const { + Eigen::Transform t; + t = Eigen::Translation(trans); + t.rotate(Eigen::AngleAxisd(rpy[2], Eigen::Vector3d::UnitZ())); + t.rotate(Eigen::AngleAxisd(rpy[1], Eigen::Vector3d::UnitY())); + t.rotate(Eigen::AngleAxisd(rpy[0], Eigen::Vector3d::UnitX())); - return t.matrix(); + return t.matrix(); } -Eigen::Matrix4d QuadKD::createAffineMatrix(Eigen::Vector3d trans, - Eigen::AngleAxisd rot) const { - Eigen::Transform t; - t = Eigen::Translation(trans); - t.rotate(rot); +Eigen::Matrix4d QuadKD::createAffineMatrix(const Eigen::Vector3d &trans, + const Eigen::AngleAxisd &rot) const { + Eigen::Transform t; + t = Eigen::Translation(trans); + t.rotate(rot); - return t.matrix(); + return t.matrix(); } double QuadKD::getJointLowerLimit(int leg_index, int joint_index) const { - return joint_min_[leg_index][joint_index]; + return joint_min_[leg_index][joint_index]; } double QuadKD::getJointUpperLimit(int leg_index, int joint_index) const { - return joint_max_[leg_index][joint_index]; + return joint_max_[leg_index][joint_index]; } double QuadKD::getLinkLength(int leg_index, int link_index) const { - switch (link_index) { - case 0: - return l0_vec_[leg_index]; - case 1: - return l1_; - case 2: - return l2_; - default: - throw std::runtime_error("Invalid link index"); - } + switch (link_index) { + case 0: + return l0_vec_[leg_index]; + case 1: + return l1_; + case 2: + return l2_; + default: + throw std::runtime_error("Invalid link index"); + } } -void QuadKD::transformBodyToWorld(Eigen::Vector3d body_pos, - Eigen::Vector3d body_rpy, - Eigen::Matrix4d transform_body, +void QuadKD::transformBodyToWorld(const Eigen::Vector3d &body_pos, + const Eigen::Vector3d &body_rpy, + const Eigen::Matrix4d &transform_body, Eigen::Matrix4d &transform_world) const { - // Compute transform from world to body frame - Eigen::Matrix4d g_world_body = createAffineMatrix(body_pos, body_rpy); + // Compute transform from world to body frame + Eigen::Matrix4d g_world_body = createAffineMatrix(body_pos, body_rpy); - // Get the desired transform in the world frame - transform_world = g_world_body * transform_body; + // Get the desired transform in the world frame + transform_world = g_world_body * transform_body; } -void QuadKD::transformWorldToBody(Eigen::Vector3d body_pos, - Eigen::Vector3d body_rpy, - Eigen::Matrix4d transform_world, +void QuadKD::transformWorldToBody(const Eigen::Vector3d &body_pos, + const Eigen::Vector3d &body_rpy, + const Eigen::Matrix4d &transform_world, Eigen::Matrix4d &transform_body) const { - // Compute transform from world to body frame - Eigen::Matrix4d g_world_body = createAffineMatrix(body_pos, body_rpy); + // Compute transform from world to body frame + Eigen::Matrix4d g_world_body = createAffineMatrix(body_pos, body_rpy); - // Compute the desired transform in the body frame - transform_body = g_world_body.inverse() * transform_world; + // Compute the desired transform in the body frame + transform_body = g_world_body.inverse() * transform_world; } void QuadKD::worldToLegbaseFKWorldFrame( - int leg_index, Eigen::Vector3d body_pos, Eigen::Vector3d body_rpy, - Eigen::Matrix4d &g_world_legbase) const { - // Compute transforms - Eigen::Matrix4d g_world_body = createAffineMatrix(body_pos, body_rpy); + int leg_index, const Eigen::Vector3d &body_pos, + const Eigen::Vector3d &body_rpy, Eigen::Matrix4d &g_world_legbase) const { + // Compute transforms + Eigen::Matrix4d g_world_body = createAffineMatrix(body_pos, body_rpy); - // Compute transform for leg base relative to the world frame - g_world_legbase = g_world_body * g_body_legbases_[leg_index]; + // Compute transform for leg base relative to the world frame + g_world_legbase = g_world_body * g_body_legbases_[leg_index]; } void QuadKD::worldToLegbaseFKWorldFrame( - int leg_index, Eigen::Vector3d body_pos, Eigen::Vector3d body_rpy, + int leg_index, const Eigen::Vector3d &body_pos, + const Eigen::Vector3d &body_rpy, Eigen::Vector3d &leg_base_pos_world) const { - Eigen::Matrix4d g_world_legbase; - worldToLegbaseFKWorldFrame(leg_index, body_pos, body_rpy, g_world_legbase); + Eigen::Matrix4d g_world_legbase; + worldToLegbaseFKWorldFrame(leg_index, body_pos, body_rpy, g_world_legbase); - leg_base_pos_world = g_world_legbase.block<3, 1>(0, 3); + leg_base_pos_world = g_world_legbase.block<3, 1>(0, 3); } void QuadKD::worldToNominalHipFKWorldFrame( - int leg_index, Eigen::Vector3d body_pos, Eigen::Vector3d body_rpy, + int leg_index, const Eigen::Vector3d &body_pos, + const Eigen::Vector3d &body_rpy, Eigen::Vector3d &nominal_hip_pos_world) const { - // Compute transforms - Eigen::Matrix4d g_world_body = createAffineMatrix(body_pos, body_rpy); - // Compute transform from body to legbase but offset by l0 - Eigen::Matrix4d g_body_nominal_hip = g_body_legbases_[leg_index]; - g_body_nominal_hip(1, 3) += 1.0 * l0_vec_[leg_index]; + // Compute transforms + Eigen::Matrix4d g_world_body = createAffineMatrix(body_pos, body_rpy); + // Compute transform from body to legbase but offset by l0 + Eigen::Matrix4d g_body_nominal_hip = g_body_legbases_[leg_index]; + g_body_nominal_hip(1, 3) += 1.0 * l0_vec_[leg_index]; - // Compute transform for offset leg base relative to the world frame - Eigen::Matrix4d g_world_nominal_hip = g_world_body * g_body_nominal_hip; + // Compute transform for offset leg base relative to the world frame + Eigen::Matrix4d g_world_nominal_hip = g_world_body * g_body_nominal_hip; - nominal_hip_pos_world = g_world_nominal_hip.block<3, 1>(0, 3); + nominal_hip_pos_world = g_world_nominal_hip.block<3, 1>(0, 3); } -void QuadKD::bodyToFootFKBodyFrame(int leg_index, Eigen::Vector3d joint_state, +void QuadKD::bodyToFootFKBodyFrame(int leg_index, + const Eigen::Vector3d &joint_state, Eigen::Matrix4d &g_body_foot) const { - if (leg_index > (legbase_offsets_.size() - 1) || leg_index < 0) { - throw std::runtime_error("Leg index is outside valid range"); - } + if (leg_index > (legbase_offsets_.size() - 1) || leg_index < 0) { + throw std::runtime_error("Leg index is outside valid range"); + } - // Define hip offset - Eigen::Vector3d hip_offset = {0, l0_vec_[leg_index], 0}; + // Define hip offset + Eigen::Vector3d hip_offset = {0, l0_vec_[leg_index], 0}; - // Initialize transforms - Eigen::Matrix4d g_legbase_abad; - Eigen::Matrix4d g_abad_hip; - Eigen::Matrix4d g_hip_knee; - Eigen::Matrix4d g_knee_foot; + // Initialize transforms + Eigen::Matrix4d g_legbase_abad; + Eigen::Matrix4d g_abad_hip; + Eigen::Matrix4d g_hip_knee; + Eigen::Matrix4d g_knee_foot; - g_legbase_abad = createAffineMatrix( - abad_offset_, - Eigen::AngleAxisd(joint_state[0], Eigen::Vector3d::UnitX())); + g_legbase_abad = createAffineMatrix( + abad_offset_, + Eigen::AngleAxisd(joint_state[0], Eigen::Vector3d::UnitX())); - g_abad_hip = createAffineMatrix( - hip_offset, Eigen::AngleAxisd(joint_state[1], -Eigen::Vector3d::UnitY())); + g_abad_hip = createAffineMatrix( + hip_offset, + Eigen::AngleAxisd(joint_state[1], -Eigen::Vector3d::UnitY())); - g_hip_knee = createAffineMatrix( - knee_offset_, - Eigen::AngleAxisd(joint_state[2], Eigen::Vector3d::UnitY())); + g_hip_knee = createAffineMatrix( + knee_offset_, + Eigen::AngleAxisd(joint_state[2], Eigen::Vector3d::UnitY())); - g_knee_foot = createAffineMatrix( - foot_offset_, Eigen::AngleAxisd(0, Eigen::Vector3d::UnitY())); + g_knee_foot = createAffineMatrix( + foot_offset_, Eigen::AngleAxisd(0, Eigen::Vector3d::UnitY())); - // Get foot transform in world frame - g_body_foot = g_body_legbases_[leg_index] * g_legbase_abad * g_abad_hip * - g_hip_knee * g_knee_foot; + // Get foot transform in world frame + g_body_foot = g_body_legbases_[leg_index] * g_legbase_abad * g_abad_hip * + g_hip_knee * g_knee_foot; } -void QuadKD::bodyToFootFKBodyFrame(int leg_index, Eigen::Vector3d joint_state, +void QuadKD::bodyToFootFKBodyFrame(int leg_index, + const Eigen::Vector3d &joint_state, Eigen::Vector3d &foot_pos_body) const { - Eigen::Matrix4d g_body_foot; - QuadKD::bodyToFootFKBodyFrame(leg_index, joint_state, g_body_foot); + Eigen::Matrix4d g_body_foot; + QuadKD::bodyToFootFKBodyFrame(leg_index, joint_state, g_body_foot); - // Extract cartesian position of foot - foot_pos_body = g_body_foot.block<3, 1>(0, 3); + // Extract cartesian position of foot + foot_pos_body = g_body_foot.block<3, 1>(0, 3); } -void QuadKD::worldToFootFKWorldFrame(int leg_index, Eigen::Vector3d body_pos, - Eigen::Vector3d body_rpy, - Eigen::Vector3d joint_state, +void QuadKD::worldToFootFKWorldFrame(int leg_index, + const Eigen::Vector3d &body_pos, + const Eigen::Vector3d &body_rpy, + const Eigen::Vector3d &joint_state, Eigen::Matrix4d &g_world_foot) const { - if (leg_index > (legbase_offsets_.size() - 1) || leg_index < 0) { - throw std::runtime_error("Leg index is outside valid range"); - } + if (leg_index > (legbase_offsets_.size() - 1) || leg_index < 0) { + throw std::runtime_error("Leg index is outside valid range"); + } - // Define hip offset - Eigen::Vector3d hip_offset = {0, l0_vec_[leg_index], 0}; + // Define hip offset + Eigen::Vector3d hip_offset = {0, l0_vec_[leg_index], 0}; - // Initialize transforms - Eigen::Matrix4d g_body_legbase; - Eigen::Matrix4d g_legbase_abad; - Eigen::Matrix4d g_abad_hip; - Eigen::Matrix4d g_hip_knee; - Eigen::Matrix4d g_knee_foot; + // Initialize transforms + Eigen::Matrix4d g_body_legbase; + Eigen::Matrix4d g_legbase_abad; + Eigen::Matrix4d g_abad_hip; + Eigen::Matrix4d g_hip_knee; + Eigen::Matrix4d g_knee_foot; - // Compute transforms - Eigen::Matrix4d g_world_legbase; - worldToLegbaseFKWorldFrame(leg_index, body_pos, body_rpy, g_world_legbase); + // Compute transforms + Eigen::Matrix4d g_world_legbase; + worldToLegbaseFKWorldFrame(leg_index, body_pos, body_rpy, g_world_legbase); - g_legbase_abad = createAffineMatrix( - abad_offset_, - Eigen::AngleAxisd(joint_state[0], Eigen::Vector3d::UnitX())); + g_legbase_abad = createAffineMatrix( + abad_offset_, + Eigen::AngleAxisd(joint_state[0], Eigen::Vector3d::UnitX())); - g_abad_hip = createAffineMatrix( - hip_offset, Eigen::AngleAxisd(joint_state[1], -Eigen::Vector3d::UnitY())); + g_abad_hip = createAffineMatrix( + hip_offset, + Eigen::AngleAxisd(joint_state[1], -Eigen::Vector3d::UnitY())); - g_hip_knee = createAffineMatrix( - knee_offset_, - Eigen::AngleAxisd(joint_state[2], Eigen::Vector3d::UnitY())); + g_hip_knee = createAffineMatrix( + knee_offset_, + Eigen::AngleAxisd(joint_state[2], Eigen::Vector3d::UnitY())); - g_knee_foot = createAffineMatrix( - foot_offset_, Eigen::AngleAxisd(0, Eigen::Vector3d::UnitY())); + g_knee_foot = createAffineMatrix( + foot_offset_, Eigen::AngleAxisd(0, Eigen::Vector3d::UnitY())); - // Get foot transform in world frame - g_world_foot = - g_world_legbase * g_legbase_abad * g_abad_hip * g_hip_knee * g_knee_foot; + // Get foot transform in world frame + g_world_foot = g_world_legbase * g_legbase_abad * g_abad_hip * g_hip_knee * + g_knee_foot; } -void QuadKD::worldToFootFKWorldFrame(int leg_index, Eigen::Vector3d body_pos, - Eigen::Vector3d body_rpy, - Eigen::Vector3d joint_state, +void QuadKD::worldToFootFKWorldFrame(int leg_index, + const Eigen::Vector3d &body_pos, + const Eigen::Vector3d &body_rpy, + const Eigen::Vector3d &joint_state, Eigen::Vector3d &foot_pos_world) const { - Eigen::Matrix4d g_world_foot; - worldToFootFKWorldFrame(leg_index, body_pos, body_rpy, joint_state, - g_world_foot); + Eigen::Matrix4d g_world_foot; + worldToFootFKWorldFrame(leg_index, body_pos, body_rpy, joint_state, + g_world_foot); - // Extract cartesian position of foot - foot_pos_world = g_world_foot.block<3, 1>(0, 3); + // Extract cartesian position of foot + foot_pos_world = g_world_foot.block<3, 1>(0, 3); } -void QuadKD::worldToKneeFKWorldFrame(int leg_index, Eigen::Vector3d body_pos, - Eigen::Vector3d body_rpy, - Eigen::Vector3d joint_state, +void QuadKD::worldToKneeFKWorldFrame(int leg_index, + const Eigen::Vector3d &body_pos, + const Eigen::Vector3d &body_rpy, + const Eigen::Vector3d &joint_state, Eigen::Matrix4d &g_world_knee) const { - if (leg_index > (legbase_offsets_.size() - 1) || leg_index < 0) { - throw std::runtime_error("Leg index is outside valid range"); - } + if (leg_index > (legbase_offsets_.size() - 1) || leg_index < 0) { + throw std::runtime_error("Leg index is outside valid range"); + } - // Define hip offset - Eigen::Vector3d hip_offset = {0, l0_vec_[leg_index], 0}; + // Define hip offset + Eigen::Vector3d hip_offset = {0, l0_vec_[leg_index], 0}; - // Initialize transforms - Eigen::Matrix4d g_body_legbase; - Eigen::Matrix4d g_legbase_abad; - Eigen::Matrix4d g_abad_hip; - Eigen::Matrix4d g_hip_knee; + // Initialize transforms + Eigen::Matrix4d g_body_legbase; + Eigen::Matrix4d g_legbase_abad; + Eigen::Matrix4d g_abad_hip; + Eigen::Matrix4d g_hip_knee; - // Compute transforms - Eigen::Matrix4d g_world_legbase; - worldToLegbaseFKWorldFrame(leg_index, body_pos, body_rpy, g_world_legbase); + // Compute transforms + Eigen::Matrix4d g_world_legbase; + worldToLegbaseFKWorldFrame(leg_index, body_pos, body_rpy, g_world_legbase); - g_legbase_abad = createAffineMatrix( - abad_offset_, - Eigen::AngleAxisd(joint_state[0], Eigen::Vector3d::UnitX())); + g_legbase_abad = createAffineMatrix( + abad_offset_, + Eigen::AngleAxisd(joint_state[0], Eigen::Vector3d::UnitX())); - g_abad_hip = createAffineMatrix( - hip_offset, Eigen::AngleAxisd(joint_state[1], -Eigen::Vector3d::UnitY())); + g_abad_hip = createAffineMatrix( + hip_offset, + Eigen::AngleAxisd(joint_state[1], -Eigen::Vector3d::UnitY())); - g_hip_knee = createAffineMatrix( - knee_offset_, - Eigen::AngleAxisd(joint_state[2], Eigen::Vector3d::UnitY())); + g_hip_knee = createAffineMatrix( + knee_offset_, + Eigen::AngleAxisd(joint_state[2], Eigen::Vector3d::UnitY())); - // Get foot transform in world frame - g_world_knee = g_world_legbase * g_legbase_abad * g_abad_hip * g_hip_knee; + // Get foot transform in world frame + g_world_knee = g_world_legbase * g_legbase_abad * g_abad_hip * g_hip_knee; } -void QuadKD::worldToKneeFKWorldFrame(int leg_index, Eigen::Vector3d body_pos, - Eigen::Vector3d body_rpy, - Eigen::Vector3d joint_state, +void QuadKD::worldToKneeFKWorldFrame(int leg_index, + const Eigen::Vector3d &body_pos, + const Eigen::Vector3d &body_rpy, + const Eigen::Vector3d &joint_state, Eigen::Vector3d &knee_pos_world) const { - Eigen::Matrix4d g_world_knee; - worldToKneeFKWorldFrame(leg_index, body_pos, body_rpy, joint_state, - g_world_knee); + Eigen::Matrix4d g_world_knee; + worldToKneeFKWorldFrame(leg_index, body_pos, body_rpy, joint_state, + g_world_knee); - // Extract cartesian position of foot - knee_pos_world = g_world_knee.block<3, 1>(0, 3); + // Extract cartesian position of foot + knee_pos_world = g_world_knee.block<3, 1>(0, 3); } -bool QuadKD::worldToFootIKWorldFrame(int leg_index, Eigen::Vector3d body_pos, - Eigen::Vector3d body_rpy, - Eigen::Vector3d foot_pos_world, +bool QuadKD::worldToFootIKWorldFrame(int leg_index, + const Eigen::Vector3d &body_pos, + const Eigen::Vector3d &body_rpy, + const Eigen::Vector3d &foot_pos_world, Eigen::Vector3d &joint_state) const { - if (leg_index > (legbase_offsets_.size() - 1) || leg_index < 0) { - throw std::runtime_error("Leg index is outside valid range"); - } + if (leg_index > (legbase_offsets_.size() - 1) || leg_index < 0) { + throw std::runtime_error("Leg index is outside valid range"); + } - // Calculate offsets - Eigen::Vector3d legbase_offset = legbase_offsets_[leg_index]; - double l0 = l0_vec_[leg_index]; + // Calculate offsets + Eigen::Vector3d legbase_offset = legbase_offsets_[leg_index]; + double l0 = l0_vec_[leg_index]; - // Initialize transforms - Eigen::Matrix4d g_world_legbase; - Eigen::Matrix4d g_world_foot; - Eigen::Matrix4d g_legbase_foot; - Eigen::Vector3d foot_pos_legbase; + // Initialize transforms + Eigen::Matrix4d g_world_legbase; + Eigen::Matrix4d g_world_foot; + Eigen::Matrix4d g_legbase_foot; + Eigen::Vector3d foot_pos_legbase; - // Compute transforms - worldToLegbaseFKWorldFrame(leg_index, body_pos, body_rpy, g_world_legbase); + // Compute transforms + worldToLegbaseFKWorldFrame(leg_index, body_pos, body_rpy, g_world_legbase); - g_world_foot = createAffineMatrix( - foot_pos_world, Eigen::AngleAxisd(0, Eigen::Vector3d::UnitY())); + g_world_foot = createAffineMatrix( + foot_pos_world, Eigen::AngleAxisd(0, Eigen::Vector3d::UnitY())); - // Compute foot position relative to the leg base in cartesian coordinates - g_legbase_foot = g_world_legbase.inverse() * g_world_foot; - foot_pos_legbase = g_legbase_foot.block<3, 1>(0, 3); + // Compute foot position relative to the leg base in cartesian coordinates + g_legbase_foot = g_world_legbase.inverse() * g_world_foot; + foot_pos_legbase = g_legbase_foot.block<3, 1>(0, 3); - return legbaseToFootIKLegbaseFrame(leg_index, foot_pos_legbase, joint_state); + return legbaseToFootIKLegbaseFrame(leg_index, foot_pos_legbase, + joint_state); } -bool QuadKD::legbaseToFootIKLegbaseFrame(int leg_index, - Eigen::Vector3d foot_pos_legbase, - Eigen::Vector3d &joint_state) const { - // Initialize exact bool - bool is_exact = true; - - // Calculate offsets - Eigen::Vector3d legbase_offset = legbase_offsets_[leg_index]; - double l0 = l0_vec_[leg_index]; - - // Extract coordinates and declare joint variables - double x = foot_pos_legbase[0]; - double y = foot_pos_legbase[1]; - double z = foot_pos_legbase[2]; - double q0; - double q1; - double q2; - - // Start IK, check foot pos is at least l0 away from leg base, clamp otherwise - double temp = l0 / sqrt(z * z + y * y); - if (abs(temp) > 1) { - ROS_DEBUG_THROTTLE(0.5, "Foot too close, choosing closest alternative\n"); - is_exact = false; - temp = std::max(std::min(temp, 1.0), -1.0); - } - - // Compute both solutions of q0, use hip-above-knee if z<0 (preferred) - // Store the inverted solution in case hip limits are exceeded - if (z > 0) { - q0 = -acos(temp) + atan2(z, y); - } else { - q0 = acos(temp) + atan2(z, y); - } - - // Make sure abad is within joint limits, clamp otherwise - if (q0 > joint_max_[leg_index][0] || q0 < joint_min_[leg_index][0]) { - q0 = std::max(std::min(q0, joint_max_[leg_index][0]), - joint_min_[leg_index][0]); - is_exact = false; - ROS_DEBUG_THROTTLE(0.5, "Abad limits exceeded, clamping to %5.3f \n", q0); - } - - // Rotate to ab-ad fixed frame - double z_body_frame = z; - z = -sin(q0) * y + cos(q0) * z_body_frame; - - // Check reachibility for hip - double acos_eps = 1.0; - double temp2 = - (l1_ * l1_ + x * x + z * z - l2_ * l2_) / (2 * l1_ * sqrt(x * x + z * z)); - if (abs(temp2) > acos_eps) { - ROS_DEBUG_THROTTLE(0.5, - "Foot location too far for hip, choosing closest" - " alternative \n"); - is_exact = false; - temp2 = std::max(std::min(temp2, acos_eps), -acos_eps); - } - - // Check reachibility for knee - double temp3 = (l1_ * l1_ + l2_ * l2_ - x * x - z * z) / (2 * l1_ * l2_); - - if (temp3 > acos_eps || temp3 < -acos_eps) { - ROS_DEBUG_THROTTLE(0.5, - "Foot location too far for knee, choosing closest" - " alternative \n"); - is_exact = false; - - temp3 = std::max(std::min(temp3, acos_eps), -acos_eps); - } - - // Compute joint angles - q1 = 0.5 * M_PI + atan2(x, -z) - acos(temp2); - - // Make sure hip is within joint limits - if (q1 > joint_max_[leg_index][1] || q1 < joint_min_[leg_index][1]) { - q1 = std::max(std::min(q1, joint_max_[leg_index][1]), - joint_min_[leg_index][1]); - is_exact = false; - ROS_DEBUG_THROTTLE(0.5, "Hip limits exceeded, clamping to %5.3f \n", q1); - } - - // Compute knee val to get closest toe position in the plane - Eigen::Vector2d knee_pos, toe_pos, toe_offset; - knee_pos << -l1_ * cos(q1), -l1_ * sin(q1); - toe_pos << x, z; - toe_offset = toe_pos - knee_pos; - q2 = atan2(-toe_offset(1), toe_offset(0)) + q1; - - // Make sure knee is within joint limits - if (q2 > joint_max_[leg_index][2] || q2 < joint_min_[leg_index][2]) { - q2 = std::max(std::min(q2, joint_max_[leg_index][2]), - joint_min_[leg_index][2]); - is_exact = false; - ROS_DEBUG_THROTTLE(0.5, "Knee limit exceeded, clamping to %5.3f \n", q2); - } - - // q1 is undefined if q2=0, resolve this - if (q2 == 0) { - q1 = 0; - ROS_DEBUG_THROTTLE(0.5, - "Hip value undefined (in singularity), setting to" - " %5.3f \n", - q1); - is_exact = false; - } - - if (z_body_frame - l0 * sin(q0) > 0) { - ROS_DEBUG_THROTTLE(0.5, "IK solution is in hip-inverted region! Beware!\n"); - is_exact = false; - } - - joint_state = {q0, q1, q2}; - return is_exact; +bool QuadKD::legbaseToFootIKLegbaseFrame( + int leg_index, const Eigen::Vector3d &foot_pos_legbase, + Eigen::Vector3d &joint_state) const { + // Initialize exact bool + bool is_exact = true; + + // Calculate offsets + Eigen::Vector3d legbase_offset = legbase_offsets_[leg_index]; + double l0 = l0_vec_[leg_index]; + + // Extract coordinates and declare joint variables + double x = foot_pos_legbase[0]; + double y = foot_pos_legbase[1]; + double z = foot_pos_legbase[2]; + double q0; + double q1; + double q2; + + // Start IK, check foot pos is at least l0 away from leg base, clamp + // otherwise + double temp = l0 / sqrt(z * z + y * y); + if (abs(temp) > 1) { + ROS_DEBUG_THROTTLE(0.5, + "Foot too close, choosing closest alternative\n"); + is_exact = false; + temp = std::max(std::min(temp, 1.0), -1.0); + } + + // Compute both solutions of q0, use hip-above-knee if z<0 (preferred) + // Store the inverted solution in case hip limits are exceeded + if (z > 0) { + q0 = -acos(temp) + atan2(z, y); + } else { + q0 = acos(temp) + atan2(z, y); + } + + // Make sure abad is within joint limits, clamp otherwise + if (q0 > joint_max_[leg_index][0] || q0 < joint_min_[leg_index][0]) { + q0 = std::max(std::min(q0, joint_max_[leg_index][0]), + joint_min_[leg_index][0]); + is_exact = false; + ROS_DEBUG_THROTTLE(0.5, "Abad limits exceeded, clamping to %5.3f \n", + q0); + } + + // Rotate to ab-ad fixed frame + double z_body_frame = z; + z = -sin(q0) * y + cos(q0) * z_body_frame; + + // Check reachibility for hip + double acos_eps = 1.0; + double temp2 = (l1_ * l1_ + x * x + z * z - l2_ * l2_) / + (2 * l1_ * sqrt(x * x + z * z)); + if (abs(temp2) > acos_eps) { + ROS_DEBUG_THROTTLE(0.5, + "Foot location too far for hip, choosing closest" + " alternative \n"); + is_exact = false; + temp2 = std::max(std::min(temp2, acos_eps), -acos_eps); + } + + // Check reachibility for knee + double temp3 = (l1_ * l1_ + l2_ * l2_ - x * x - z * z) / (2 * l1_ * l2_); + + if (temp3 > acos_eps || temp3 < -acos_eps) { + ROS_DEBUG_THROTTLE(0.5, + "Foot location too far for knee, choosing closest" + " alternative \n"); + is_exact = false; + + temp3 = std::max(std::min(temp3, acos_eps), -acos_eps); + } + + // Compute joint angles + q1 = 0.5 * M_PI + atan2(x, -z) - acos(temp2); + + // Make sure hip is within joint limits + if (q1 > joint_max_[leg_index][1] || q1 < joint_min_[leg_index][1]) { + q1 = std::max(std::min(q1, joint_max_[leg_index][1]), + joint_min_[leg_index][1]); + is_exact = false; + ROS_DEBUG_THROTTLE(0.5, "Hip limits exceeded, clamping to %5.3f \n", + q1); + } + + // Compute knee val to get closest toe position in the plane + Eigen::Vector2d knee_pos, toe_pos, toe_offset; + knee_pos << -l1_ * cos(q1), -l1_ * sin(q1); + toe_pos << x, z; + toe_offset = toe_pos - knee_pos; + q2 = atan2(-toe_offset(1), toe_offset(0)) + q1; + + // Make sure knee is within joint limits + if (q2 > joint_max_[leg_index][2] || q2 < joint_min_[leg_index][2]) { + q2 = std::max(std::min(q2, joint_max_[leg_index][2]), + joint_min_[leg_index][2]); + is_exact = false; + ROS_DEBUG_THROTTLE(0.5, "Knee limit exceeded, clamping to %5.3f \n", + q2); + } + + // q1 is undefined if q2=0, resolve this + if (q2 == 0) { + q1 = 0; + ROS_DEBUG_THROTTLE(0.5, + "Hip value undefined (in singularity), setting to" + " %5.3f \n", + q1); + is_exact = false; + } + + if (z_body_frame - l0 * sin(q0) > 0) { + ROS_DEBUG_THROTTLE(0.5, + "IK solution is in hip-inverted region! Beware!\n"); + is_exact = false; + } + + joint_state = {q0, q1, q2}; + return is_exact; } void QuadKD::getJacobianGenCoord(const Eigen::VectorXd &state, Eigen::MatrixXd &jacobian) const { - this->getJacobianBodyAngVel(state, jacobian); - - // RBDL uses Jacobian w.r.t. floating base angular velocity in body frame, - // which is multiplied by Jacobian to map it to Euler angle change rate here - for (size_t i = 0; i < 4; i++) { - Eigen::MatrixXd transform_jac(3, 3); - transform_jac << 1, 0, -sin(state(16)), 0, cos(state(15)), - cos(state(16)) * sin(state(15)), 0, -sin(state(15)), - cos(state(15)) * cos(state(16)); - jacobian.block(3 * i, 15, 3, 3) = - jacobian.block(3 * i, 15, 3, 3) * transform_jac; - } + this->getJacobianBodyAngVel(state, jacobian); + + // RBDL uses Jacobian w.r.t. floating base angular velocity in body frame, + // which is multiplied by Jacobian to map it to Euler angle change rate here + for (size_t i = 0; i < 4; i++) { + Eigen::MatrixXd transform_jac(3, 3); + transform_jac << 1, 0, -sin(state(16)), 0, cos(state(15)), + cos(state(16)) * sin(state(15)), 0, -sin(state(15)), + cos(state(15)) * cos(state(16)); + jacobian.block(3 * i, 15, 3, 3) = + jacobian.block(3 * i, 15, 3, 3) * transform_jac; + } } void QuadKD::getJacobianBodyAngVel(const Eigen::VectorXd &state, Eigen::MatrixXd &jacobian) const { - assert(state.size() == 18); + assert(state.size() == 18); - // RBDL state vector has the floating base state in the front and the joint - // state in the back When reading from URDF, the order of the legs is 2301, - // which should be corrected by sorting the bodyID - Eigen::VectorXd q(19); - q.setZero(); + // RBDL state vector has the floating base state in the front and the joint + // state in the back When reading from URDF, the order of the legs is 2301, + // which should be corrected by sorting the bodyID + Eigen::VectorXd q(19); + q.setZero(); - q.head(3) = state.segment(12, 3); + q.head(3) = state.segment(12, 3); - tf2::Quaternion quat_tf; - quat_tf.setRPY(state(15), state(16), state(17)); - q(3) = quat_tf.getX(); - q(4) = quat_tf.getY(); - q(5) = quat_tf.getZ(); + tf2::Quaternion quat_tf; + quat_tf.setRPY(state(15), state(16), state(17)); + q(3) = quat_tf.getX(); + q(4) = quat_tf.getY(); + q(5) = quat_tf.getZ(); - // RBDL uses quaternion for floating base direction, but w is placed at the - // end of the state vector - q(18) = quat_tf.getW(); + // RBDL uses quaternion for floating base direction, but w is placed at the + // end of the state vector + q(18) = quat_tf.getW(); - for (size_t i = 0; i < leg_idx_list_.size(); i++) { - q.segment(6 + 3 * i, 3) = state.segment(3 * leg_idx_list_.at(i), 3); - } + for (size_t i = 0; i < leg_idx_list_.size(); i++) { + q.segment(6 + 3 * i, 3) = state.segment(3 * leg_idx_list_.at(i), 3); + } - jacobian.setZero(); + jacobian.setZero(); - for (size_t i = 0; i < body_id_list_.size(); i++) { - Eigen::MatrixXd jac_block(3, 18); - jac_block.setZero(); - RigidBodyDynamics::CalcPointJacobian(*model_, q, body_id_list_.at(i), - Eigen::Vector3d::Zero(), jac_block); + for (size_t i = 0; i < body_id_list_.size(); i++) { + Eigen::MatrixXd jac_block(3, 18); + jac_block.setZero(); + RigidBodyDynamics::CalcPointJacobian(*model_, q, body_id_list_.at(i), + Eigen::Vector3d::Zero(), + jac_block); - for (size_t j = 0; j < 4; j++) { - jacobian.block(3 * i, 3 * leg_idx_list_.at(j), 3, 3) = - jac_block.block(0, 6 + 3 * j, 3, 3); + for (size_t j = 0; j < 4; j++) { + jacobian.block(3 * i, 3 * leg_idx_list_.at(j), 3, 3) = + jac_block.block(0, 6 + 3 * j, 3, 3); + } + jacobian.block(3 * i, 12, 3, 6) = jac_block.block(0, 0, 3, 6); } - jacobian.block(3 * i, 12, 3, 6) = jac_block.block(0, 0, 3, 6); - } } void QuadKD::getJacobianWorldAngVel(const Eigen::VectorXd &state, Eigen::MatrixXd &jacobian) const { - this->getJacobianBodyAngVel(state, jacobian); - - // RBDL uses Jacobian w.r.t. floating base angular velocity in body frame, - // which is multiplied by rotation matrix to map it to angular velocity in - // world frame here - for (size_t i = 0; i < 4; i++) { - Eigen::Matrix3d rot; - this->getRotationMatrix(state.segment(15, 3), rot); - jacobian.block(3 * i, 15, 3, 3) = jacobian.block(3 * i, 15, 3, 3) * rot; - } + this->getJacobianBodyAngVel(state, jacobian); + + // RBDL uses Jacobian w.r.t. floating base angular velocity in body frame, + // which is multiplied by rotation matrix to map it to angular velocity in + // world frame here + for (size_t i = 0; i < 4; i++) { + Eigen::Matrix3d rot; + this->getRotationMatrix(state.segment(15, 3), rot); + jacobian.block(3 * i, 15, 3, 3) = jacobian.block(3 * i, 15, 3, 3) * rot; + } } void QuadKD::getRotationMatrix(const Eigen::VectorXd &rpy, Eigen::Matrix3d &rot) const { - rot = Eigen::AngleAxisd(rpy(2), Eigen::Vector3d::UnitZ()) * - Eigen::AngleAxisd(rpy(1), Eigen::Vector3d::UnitY()) * - Eigen::AngleAxisd(rpy(0), Eigen::Vector3d::UnitX()); + rot = Eigen::AngleAxisd(rpy(2), Eigen::Vector3d::UnitZ()) * + Eigen::AngleAxisd(rpy(1), Eigen::Vector3d::UnitY()) * + Eigen::AngleAxisd(rpy(0), Eigen::Vector3d::UnitX()); } void QuadKD::computeInverseDynamics(const Eigen::VectorXd &state_pos, @@ -569,134 +589,140 @@ void QuadKD::computeInverseDynamics(const Eigen::VectorXd &state_pos, const Eigen::VectorXd &grf, const std::vector &contact_mode, Eigen::VectorXd &tau) const { - // Convert q, q_dot into RBDL order - Eigen::VectorXd q(19), q_dot(18); - q.setZero(); - q_dot.setZero(); - - q.head(3) = state_pos.segment(12, 3); - q_dot.head(3) = state_vel.segment(12, 3); - - tf2::Quaternion quat_tf; - quat_tf.setRPY(state_pos(15), state_pos(16), state_pos(17)); - q(3) = quat_tf.getX(); - q(4) = quat_tf.getY(); - q(5) = quat_tf.getZ(); - - // RBDL uses quaternion for floating base direction, but w is placed at the - // end of the state vector - q(18) = quat_tf.getW(); - - q_dot.segment(3, 3) = state_vel.segment(15, 3); - - for (size_t i = 0; i < leg_idx_list_.size(); i++) { - q.segment(6 + 3 * i, 3) = state_pos.segment(3 * leg_idx_list_.at(i), 3); - q_dot.segment(6 + 3 * i, 3) = state_vel.segment(3 * leg_idx_list_.at(i), 3); - } - - // Compute jacobians - Eigen::MatrixXd jacobian = Eigen::MatrixXd::Zero(12, 18); - jacobian.setZero(); - for (size_t i = 0; i < body_id_list_.size(); i++) { - Eigen::MatrixXd jac_block(3, 18); - jac_block.setZero(); - RigidBodyDynamics::CalcPointJacobian(*model_, q, body_id_list_.at(i), - Eigen::Vector3d::Zero(), jac_block); - jacobian.block(3 * i, 0, 3, 18) = jac_block; - } - - // Compute the equivalent force in generalized coordinates - Eigen::VectorXd tau_stance = -jacobian.transpose() * grf; - - // Compute EOM - Eigen::MatrixXd M(18, 18); - M.setZero(); - Eigen::VectorXd N(18); - RigidBodyDynamics::CompositeRigidBodyAlgorithm(*model_, q, M); - RigidBodyDynamics::NonlinearEffects(*model_, q, q_dot, N); - - // Compute J_dot*q_dot - Eigen::VectorXd foot_acc_J_dot(12); - for (size_t i = 0; i < 4; i++) { - foot_acc_J_dot.segment(3 * i, 3) = RigidBodyDynamics::CalcPointAcceleration( - *model_, q, q_dot, Eigen::VectorXd::Zero(18), body_id_list_.at(i), - Eigen::Vector3d::Zero()); - } - - // Compute constraint Jacobian A and A_dot*q_dot - int constraints_num = - 3 * std::count(contact_mode.begin(), contact_mode.end(), true); - Eigen::MatrixXd A(constraints_num, 18); - Eigen::VectorXd A_dotq_dot(constraints_num); - int constraints_count = 0; - for (size_t i = 0; i < 4; i++) { - if (contact_mode.at(i)) { - A.block(3 * constraints_count, 0, 3, 18) = - jacobian.block(3 * i, 0, 3, 18); - A_dotq_dot.segment(3 * constraints_count, 3) = - foot_acc_J_dot.segment(3 * i, 3); - constraints_count++; - } - } - - // Compute acceleration from J*q_ddot - Eigen::VectorXd foot_acc_q_ddot = foot_acc - foot_acc_J_dot; - - // Compuate damped jacobian inverser - Eigen::MatrixXd jacobian_inv = - math_utils::sdlsInv(jacobian.block(0, 6, 12, 12)); - - // In the EOM, we know M, N, tau_grf, and a = J_b*q_ddot_b + J_l*q_ddot_l, we - // need to solve q_ddot_b and tau_swing - Eigen::MatrixXd blk_mat = - Eigen::MatrixXd::Zero(18 + constraints_num, 18 + constraints_num); - blk_mat.block(0, 0, 6, 6) = - -M.block(0, 0, 6, 6) + - M.block(0, 6, 6, 12) * jacobian_inv * jacobian.block(0, 0, 12, 6); - blk_mat.block(6, 0, 12, 6) = - -M.block(6, 0, 12, 6) + - M.block(6, 6, 12, 12) * jacobian_inv * jacobian.block(0, 0, 12, 6); - for (size_t i = 0; i < 4; i++) { - if (!contact_mode.at(leg_idx_list_.at(i))) { - blk_mat.block(3 * i + 6, 3 * i + 6, 3, 3).diagonal().fill(1); - } - } - blk_mat.block(0, 18, 18, constraints_num) = -A.transpose(); - blk_mat.block(18, 0, constraints_num, 6) = - -A.leftCols(6) + - A.rightCols(12) * jacobian_inv * jacobian.block(0, 0, 12, 6); - - // Perform inverse dynamics - Eigen::VectorXd tau_swing(12), blk_sol(18 + constraints_num), - blk_vec(18 + constraints_num); - blk_vec.segment(0, 6) << N.segment(0, 6) + M.block(0, 6, 6, 12) * - jacobian_inv * foot_acc_q_ddot; - blk_vec.segment(6, 12) << N.segment(6, 12) + - M.block(6, 6, 12, 12) * jacobian_inv * - foot_acc_q_ddot - - tau_stance.segment(6, 12); - blk_vec.segment(18, constraints_num) - << A_dotq_dot + A.leftCols(12) * jacobian_inv * foot_acc_q_ddot; - blk_sol = - blk_mat.bdcSvd(Eigen::ComputeThinU | Eigen::ComputeThinV).solve(blk_vec); - tau_swing = blk_sol.segment(6, 12); - - // Convert the order back - for (size_t i = 0; i < 4; i++) { - if (contact_mode.at(leg_idx_list_.at(i))) { - tau.segment(3 * leg_idx_list_.at(i), 3) = - tau_stance.segment(6 + 3 * i, 3); - } else { - tau.segment(3 * leg_idx_list_.at(i), 3) = tau_swing.segment(3 * i, 3); - // tau.segment(3 * leg_idx_list_.at(i), 3) = Eigen::VectorXd::Zero(3); + // Convert q, q_dot into RBDL order + Eigen::VectorXd q(19), q_dot(18); + q.setZero(); + q_dot.setZero(); + + q.head(3) = state_pos.segment(12, 3); + q_dot.head(3) = state_vel.segment(12, 3); + + tf2::Quaternion quat_tf; + quat_tf.setRPY(state_pos(15), state_pos(16), state_pos(17)); + q(3) = quat_tf.getX(); + q(4) = quat_tf.getY(); + q(5) = quat_tf.getZ(); + + // RBDL uses quaternion for floating base direction, but w is placed at the + // end of the state vector + q(18) = quat_tf.getW(); + + q_dot.segment(3, 3) = state_vel.segment(15, 3); + + for (size_t i = 0; i < leg_idx_list_.size(); i++) { + q.segment(6 + 3 * i, 3) = state_pos.segment(3 * leg_idx_list_.at(i), 3); + q_dot.segment(6 + 3 * i, 3) = + state_vel.segment(3 * leg_idx_list_.at(i), 3); + } + + // Compute jacobians + Eigen::MatrixXd jacobian = Eigen::MatrixXd::Zero(12, 18); + jacobian.setZero(); + for (size_t i = 0; i < body_id_list_.size(); i++) { + Eigen::MatrixXd jac_block(3, 18); + jac_block.setZero(); + RigidBodyDynamics::CalcPointJacobian(*model_, q, body_id_list_.at(i), + Eigen::Vector3d::Zero(), + jac_block); + jacobian.block(3 * i, 0, 3, 18) = jac_block; } - } - // Check inf or nan - if (!(tau.array() == tau.array()).all() || - !((tau - tau).array() == (tau - tau).array()).all()) { - tau.setZero(); - } + // Compute the equivalent force in generalized coordinates + Eigen::VectorXd tau_stance = -jacobian.transpose() * grf; + + // Compute EOM + Eigen::MatrixXd M(18, 18); + M.setZero(); + Eigen::VectorXd N(18); + RigidBodyDynamics::CompositeRigidBodyAlgorithm(*model_, q, M); + RigidBodyDynamics::NonlinearEffects(*model_, q, q_dot, N); + + // Compute J_dot*q_dot + Eigen::VectorXd foot_acc_J_dot(12); + for (size_t i = 0; i < 4; i++) { + foot_acc_J_dot.segment(3 * i, 3) = + RigidBodyDynamics::CalcPointAcceleration( + *model_, q, q_dot, Eigen::VectorXd::Zero(18), + body_id_list_.at(i), Eigen::Vector3d::Zero()); + } + + // Compute constraint Jacobian A and A_dot*q_dot + int constraints_num = + 3 * std::count(contact_mode.begin(), contact_mode.end(), true); + Eigen::MatrixXd A(constraints_num, 18); + Eigen::VectorXd A_dotq_dot(constraints_num); + int constraints_count = 0; + for (size_t i = 0; i < 4; i++) { + if (contact_mode.at(i)) { + A.block(3 * constraints_count, 0, 3, 18) = + jacobian.block(3 * i, 0, 3, 18); + A_dotq_dot.segment(3 * constraints_count, 3) = + foot_acc_J_dot.segment(3 * i, 3); + constraints_count++; + } + } + + // Compute acceleration from J*q_ddot + Eigen::VectorXd foot_acc_q_ddot = foot_acc - foot_acc_J_dot; + + // Compuate damped jacobian inverser + Eigen::MatrixXd jacobian_inv = + math_utils::sdlsInv(jacobian.block(0, 6, 12, 12)); + + // In the EOM, we know M, N, tau_grf, and a = J_b*q_ddot_b + J_l*q_ddot_l, + // we need to solve q_ddot_b and tau_swing + Eigen::MatrixXd blk_mat = + Eigen::MatrixXd::Zero(18 + constraints_num, 18 + constraints_num); + blk_mat.block(0, 0, 6, 6) = + -M.block(0, 0, 6, 6) + + M.block(0, 6, 6, 12) * jacobian_inv * jacobian.block(0, 0, 12, 6); + blk_mat.block(6, 0, 12, 6) = + -M.block(6, 0, 12, 6) + + M.block(6, 6, 12, 12) * jacobian_inv * jacobian.block(0, 0, 12, 6); + for (size_t i = 0; i < 4; i++) { + if (!contact_mode.at(leg_idx_list_.at(i))) { + blk_mat.block(3 * i + 6, 3 * i + 6, 3, 3).diagonal().fill(1); + } + } + blk_mat.block(0, 18, 18, constraints_num) = -A.transpose(); + blk_mat.block(18, 0, constraints_num, 6) = + -A.leftCols(6) + + A.rightCols(12) * jacobian_inv * jacobian.block(0, 0, 12, 6); + + // Perform inverse dynamics + Eigen::VectorXd tau_swing(12), blk_sol(18 + constraints_num), + blk_vec(18 + constraints_num); + blk_vec.segment(0, 6) << N.segment(0, 6) + M.block(0, 6, 6, 12) * + jacobian_inv * + foot_acc_q_ddot; + blk_vec.segment(6, 12) << N.segment(6, 12) + + M.block(6, 6, 12, 12) * jacobian_inv * + foot_acc_q_ddot - + tau_stance.segment(6, 12); + blk_vec.segment(18, constraints_num) + << A_dotq_dot + A.leftCols(12) * jacobian_inv * foot_acc_q_ddot; + blk_sol = blk_mat.bdcSvd(Eigen::ComputeThinU | Eigen::ComputeThinV) + .solve(blk_vec); + tau_swing = blk_sol.segment(6, 12); + + // Convert the order back + for (size_t i = 0; i < 4; i++) { + if (contact_mode.at(leg_idx_list_.at(i))) { + tau.segment(3 * leg_idx_list_.at(i), 3) = + tau_stance.segment(6 + 3 * i, 3); + } else { + tau.segment(3 * leg_idx_list_.at(i), 3) = + tau_swing.segment(3 * i, 3); + // tau.segment(3 * leg_idx_list_.at(i), 3) = + // Eigen::VectorXd::Zero(3); + } + } + + // Check inf or nan + if (!(tau.array() == tau.array()).all() || + !((tau - tau).array() == (tau - tau).array()).all()) { + tau.setZero(); + } } bool QuadKD::convertCentroidalToFullBody(const Eigen::VectorXd &body_state, @@ -706,95 +732,98 @@ bool QuadKD::convertCentroidalToFullBody(const Eigen::VectorXd &body_state, Eigen::VectorXd &joint_positions, Eigen::VectorXd &joint_velocities, Eigen::VectorXd &torques) { - // Assume the conversion is exact unless a check below fails - bool is_exact = true; - - // Extract kinematic quantities - Eigen::Vector3d body_pos = body_state.segment<3>(0); - Eigen::Vector3d body_rpy = body_state.segment<3>(3); - - auto t_start = std::chrono::steady_clock::now(); - // Perform IK for each leg - for (int i = 0; i < num_feet_; i++) { - Eigen::Vector3d leg_joint_state; - Eigen::Vector3d foot_pos = foot_positions.segment<3>(3 * i); - is_exact = is_exact && worldToFootIKWorldFrame(i, body_pos, body_rpy, - foot_pos, leg_joint_state); - joint_positions.segment<3>(3 * i) = leg_joint_state; - } - - auto t_ik = std::chrono::steady_clock::now(); - - // Load state positions - Eigen::VectorXd state_positions(18), state_velocities(18); - state_positions << joint_positions, body_pos, body_rpy; - - // Compute jacobian - Eigen::MatrixXd jacobian = Eigen::MatrixXd::Zero(12, 18); - getJacobianBodyAngVel(state_positions, jacobian); - - auto t_jacob = std::chrono::steady_clock::now(); - - // Compute joint velocities - joint_velocities = jacobian.leftCols(12).colPivHouseholderQr().solve( - foot_velocities - jacobian.rightCols(6) * body_state.tail(6)); - state_velocities << joint_velocities, body_state.tail(6); - - auto t_ik_vel = std::chrono::steady_clock::now(); - - torques = -jacobian.leftCols(12).transpose() * grfs; - - // computeInverseDynamics(state_positions, state_velocities, foot_acc, grfs, - // contact_mode, torques); - - auto t_id = std::chrono::steady_clock::now(); - - std::chrono::duration t_diff_ik = - std::chrono::duration_cast>(t_ik - t_start); - std::chrono::duration t_diff_jacob = - std::chrono::duration_cast>(t_jacob - t_ik); - std::chrono::duration t_diff_ik_vel = - std::chrono::duration_cast>(t_ik_vel - - t_jacob); - std::chrono::duration t_diff_id = - std::chrono::duration_cast>(t_id - - t_ik_vel); - - // std::cout << "t_diff_ik = " << t_diff_ik.count() << std::endl; - // std::cout << "t_diff_jacob = " << t_diff_jacob.count() << std::endl; - // std::cout << "t_diff_ik_vel = " << t_diff_ik_vel.count() << std::endl; - // std::cout << "t_diff_id = " << t_diff_id.count() << std::endl; - - return is_exact; + // Assume the conversion is exact unless a check below fails + bool is_exact = true; + + // Extract kinematic quantities + Eigen::Vector3d body_pos = body_state.segment<3>(0); + Eigen::Vector3d body_rpy = body_state.segment<3>(3); + + auto t_start = std::chrono::steady_clock::now(); + // Perform IK for each leg + for (int i = 0; i < num_feet_; i++) { + Eigen::Vector3d leg_joint_state; + Eigen::Vector3d foot_pos = foot_positions.segment<3>(3 * i); + is_exact = + is_exact && worldToFootIKWorldFrame(i, body_pos, body_rpy, foot_pos, + leg_joint_state); + joint_positions.segment<3>(3 * i) = leg_joint_state; + } + + auto t_ik = std::chrono::steady_clock::now(); + + // Load state positions + Eigen::VectorXd state_positions(18), state_velocities(18); + state_positions << joint_positions, body_pos, body_rpy; + + // Compute jacobian + Eigen::MatrixXd jacobian = Eigen::MatrixXd::Zero(12, 18); + getJacobianBodyAngVel(state_positions, jacobian); + + auto t_jacob = std::chrono::steady_clock::now(); + + // Compute joint velocities + joint_velocities = jacobian.leftCols(12).colPivHouseholderQr().solve( + foot_velocities - jacobian.rightCols(6) * body_state.tail(6)); + state_velocities << joint_velocities, body_state.tail(6); + + auto t_ik_vel = std::chrono::steady_clock::now(); + + torques = -jacobian.leftCols(12).transpose() * grfs; + + // computeInverseDynamics(state_positions, state_velocities, foot_acc, grfs, + // contact_mode, torques); + + auto t_id = std::chrono::steady_clock::now(); + + std::chrono::duration t_diff_ik = + std::chrono::duration_cast>(t_ik - + t_start); + std::chrono::duration t_diff_jacob = + std::chrono::duration_cast>(t_jacob - + t_ik); + std::chrono::duration t_diff_ik_vel = + std::chrono::duration_cast>(t_ik_vel - + t_jacob); + std::chrono::duration t_diff_id = + std::chrono::duration_cast>(t_id - + t_ik_vel); + + // std::cout << "t_diff_ik = " << t_diff_ik.count() << std::endl; + // std::cout << "t_diff_jacob = " << t_diff_jacob.count() << std::endl; + // std::cout << "t_diff_ik_vel = " << t_diff_ik_vel.count() << std::endl; + // std::cout << "t_diff_id = " << t_diff_id.count() << std::endl; + + return is_exact; } bool QuadKD::applyMotorModel(const Eigen::VectorXd &torques, Eigen::VectorXd &constrained_torques) { - // Constrain torques to max values - constrained_torques.resize(torques.size()); - constrained_torques = torques.cwiseMax(-tau_max_).cwiseMin(tau_max_); + // Constrain torques to max values + constrained_torques.resize(torques.size()); + constrained_torques = torques.cwiseMax(-tau_max_).cwiseMin(tau_max_); - // Check if torques was modified - return constrained_torques.isApprox(torques); + // Check if torques was modified + return constrained_torques.isApprox(torques); } bool QuadKD::applyMotorModel(const Eigen::VectorXd &joint_torques, const Eigen::VectorXd &joint_velocities, Eigen::VectorXd &constrained_joint_torques) { - // Constrain torques to max values - Eigen::VectorXd constraint_violation(joint_torques.size()); - constrained_joint_torques.resize(joint_torques.size()); - constrained_joint_torques = - joint_torques.cwiseMax(-tau_max_).cwiseMin(tau_max_); - - // Apply linear motor model - Eigen::VectorXd emf = joint_velocities.cwiseProduct(mm_slope_); - constrained_joint_torques = - constrained_joint_torques.cwiseMax(-tau_max_ - emf) - .cwiseMin(tau_max_ - emf); - - // Check if torques were modified - return constrained_joint_torques.isApprox(joint_torques); + // Constrain torques to max values + Eigen::VectorXd constraint_violation(joint_torques.size()); + constrained_joint_torques.resize(joint_torques.size()); + constrained_joint_torques = + joint_torques.cwiseMax(-tau_max_).cwiseMin(tau_max_); + + // Apply linear motor model + Eigen::VectorXd emf = joint_velocities.cwiseProduct(mm_slope_); + constrained_joint_torques = + constrained_joint_torques.cwiseMax(-tau_max_ - emf) + .cwiseMin(tau_max_ - emf); + + // Check if torques were modified + return constrained_joint_torques.isApprox(joint_torques); } bool QuadKD::isValidFullState(const Eigen::VectorXd &body_state, @@ -803,28 +832,28 @@ bool QuadKD::isValidFullState(const Eigen::VectorXd &body_state, const grid_map::GridMap &terrain, Eigen::VectorXd &state_violation, Eigen::VectorXd &control_violation) { - // Check state constraints - // Kinematics - state_violation.setZero(num_feet_); - for (int i = 0; i < num_feet_; i++) { - Eigen::Vector3d knee_pos_world; - worldToKneeFKWorldFrame(i, body_state.segment<3>(0), - body_state.segment<3>(3), - joint_state.segment<3>(3 * i), knee_pos_world); - state_violation[i] = getGroundClearance(knee_pos_world, terrain); - } - bool state_valid = (state_violation.array() >= 0).all(); - - // Check control constraints - // Motor model - Eigen::VectorXd constrained_joint_torques(12); - bool control_valid = applyMotorModel(joint_torques, joint_state.tail(12), - constrained_joint_torques); - control_violation.setZero(joint_torques.size()); - control_violation = -(constrained_joint_torques - joint_torques).cwiseAbs(); - - // Only valid if each subcheck is valid - return (state_valid && control_valid); + // Check state constraints + // Kinematics + state_violation.setZero(num_feet_); + for (int i = 0; i < num_feet_; i++) { + Eigen::Vector3d knee_pos_world; + worldToKneeFKWorldFrame(i, body_state.segment<3>(0), + body_state.segment<3>(3), + joint_state.segment<3>(3 * i), knee_pos_world); + state_violation[i] = getGroundClearance(knee_pos_world, terrain); + } + bool state_valid = (state_violation.array() >= 0).all(); + + // Check control constraints + // Motor model + Eigen::VectorXd constrained_joint_torques(12); + bool control_valid = applyMotorModel(joint_torques, joint_state.tail(12), + constrained_joint_torques); + control_violation.setZero(joint_torques.size()); + control_violation = -(constrained_joint_torques - joint_torques).cwiseAbs(); + + // Only valid if each subcheck is valid + return (state_valid && control_valid); } bool QuadKD::isValidCentroidalState( @@ -833,15 +862,16 @@ bool QuadKD::isValidCentroidalState( const grid_map::GridMap &terrain, Eigen::VectorXd &joint_positions, Eigen::VectorXd &joint_velocities, Eigen::VectorXd &joint_torques, Eigen::VectorXd &state_violation, Eigen::VectorXd &control_violation) { - // Convert to full - bool is_exact = convertCentroidalToFullBody( - body_state, foot_positions, foot_velocities, grfs, joint_positions, - joint_velocities, joint_torques); - - Eigen::VectorXd joint_state(24); - joint_state << joint_positions, joint_velocities; - bool is_valid = isValidFullState(body_state, joint_state, joint_torques, - terrain, state_violation, control_violation); - - return (is_exact && is_valid); + // Convert to full + bool is_exact = convertCentroidalToFullBody( + body_state, foot_positions, foot_velocities, grfs, joint_positions, + joint_velocities, joint_torques); + + Eigen::VectorXd joint_state(24); + joint_state << joint_positions, joint_velocities; + bool is_valid = + isValidFullState(body_state, joint_state, joint_torques, terrain, + state_violation, control_violation); + + return (is_exact && is_valid); } diff --git a/quad_utils/src/ros_utils.cpp b/quad_utils/src/ros_utils.cpp index 125a4c305..11aab00c2 100644 --- a/quad_utils/src/ros_utils.cpp +++ b/quad_utils/src/ros_utils.cpp @@ -2,8 +2,8 @@ namespace quad_utils { -void updateStateHeaders(quad_msgs::RobotState &msg, ros::Time stamp, - std::string frame, int traj_index) { +void updateStateHeaders(quad_msgs::RobotState &msg, const ros::Time &stamp, + const std::string &frame, int traj_index) { // Fill in the data across the messages msg.header.stamp = stamp; msg.header.frame_id = frame; @@ -22,7 +22,8 @@ void updateStateHeaders(quad_msgs::RobotState &msg, ros::Time stamp, } } -void interpHeader(std_msgs::Header header_1, std_msgs::Header header_2, +void interpHeader(const std_msgs::Header &header_1, + const std_msgs::Header &header_2, double t_interp, std_msgs::Header &interp_header) { // Copy everything from the first header interp_header.frame_id = header_1.frame_id; @@ -36,7 +37,8 @@ void interpHeader(std_msgs::Header header_1, std_msgs::Header header_2, interp_header.stamp = header_1.stamp + ros::Duration(interp_duration); } -void interpOdometry(quad_msgs::BodyState state_1, quad_msgs::BodyState state_2, +void interpOdometry(const quad_msgs::BodyState &state_1, + const quad_msgs::BodyState &state_2, double t_interp, quad_msgs::BodyState &interp_state) { interpHeader(state_1.header, state_2.header, t_interp, interp_state.header); @@ -71,8 +73,8 @@ void interpOdometry(quad_msgs::BodyState state_1, quad_msgs::BodyState state_2, state_1.twist.angular.z, state_2.twist.angular.z, t_interp); } -void interpJointState(sensor_msgs::JointState state_1, - sensor_msgs::JointState state_2, double t_interp, +void interpJointState(const sensor_msgs::JointState &state_1, + const sensor_msgs::JointState &state_2, double t_interp, sensor_msgs::JointState &interp_state) { interpHeader(state_1.header, state_2.header, t_interp, interp_state.header); @@ -92,8 +94,9 @@ void interpJointState(sensor_msgs::JointState state_1, } } -void interpMultiFootState(quad_msgs::MultiFootState state_1, - quad_msgs::MultiFootState state_2, double t_interp, +void interpMultiFootState(const quad_msgs::MultiFootState &state_1, + const quad_msgs::MultiFootState &state_2, + double t_interp, quad_msgs::MultiFootState &interp_state) { interpHeader(state_1.header, state_2.header, t_interp, interp_state.header); @@ -133,7 +136,8 @@ void interpMultiFootState(quad_msgs::MultiFootState state_1, } } -void interpGRFArray(quad_msgs::GRFArray state_1, quad_msgs::GRFArray state_2, +void interpGRFArray(const quad_msgs::GRFArray &state_1, + const quad_msgs::GRFArray &state_2, double t_interp, quad_msgs::GRFArray &interp_state) { interpHeader(state_1.header, state_2.header, t_interp, interp_state.header); @@ -161,8 +165,8 @@ void interpGRFArray(quad_msgs::GRFArray state_1, quad_msgs::GRFArray state_2, } } -void interpRobotState(quad_msgs::RobotState state_1, - quad_msgs::RobotState state_2, double t_interp, +void interpRobotState(const quad_msgs::RobotState &state_1, + const quad_msgs::RobotState &state_2, double t_interp, quad_msgs::RobotState &interp_state) { // Interp individual elements interpHeader(state_1.header, state_2.header, t_interp, interp_state.header); @@ -172,7 +176,7 @@ void interpRobotState(quad_msgs::RobotState state_1, interpMultiFootState(state_1.feet, state_2.feet, t_interp, interp_state.feet); } -void interpRobotPlan(quad_msgs::RobotPlan msg, double t, +void interpRobotPlan(const quad_msgs::RobotPlan &msg, double t, quad_msgs::RobotState &interp_state, int &interp_primitive_id, quad_msgs::GRFArray &interp_grf) { @@ -219,7 +223,7 @@ void interpRobotPlan(quad_msgs::RobotPlan msg, double t, } quad_msgs::MultiFootState interpMultiFootPlanContinuous( - quad_msgs::MultiFootPlanContinuous msg, double t) { + const quad_msgs::MultiFootPlanContinuous &msg, double t) { // Define some useful timing parameters ros::Time t0_ros = msg.states.front().header.stamp; ros::Time t_ros = t0_ros + ros::Duration(t); @@ -257,53 +261,9 @@ quad_msgs::MultiFootState interpMultiFootPlanContinuous( return interp_state; } -// quad_msgs::RobotState interpRobotStateTraj(quad_msgs::RobotStateTrajectory -// msg, -// double t) { -// // Define some useful timing parameters -// ros::Time t0_ros = msg.states.front().header.stamp; -// ros::Time tf_ros = msg.states.back().header.stamp; -// ros::Duration traj_duration = tf_ros - t0_ros; - -// t = std::max(std::min(t, traj_duration.toSec()), 0.0); -// ros::Time t_ros = t0_ros + ros::Duration(t); - -// // Declare variables for interpolating between, both for input and output -// data quad_msgs::RobotState state_1, state_2, interp_state; - -// // Find the correct index for interp (return the first index if t < 0) -// int index = 0; -// if (t >= 0) { -// for (int i = 0; i < msg.states.size() - 1; i++) { -// index = i; -// if (msg.states[i].header.stamp <= t_ros && -// t_ros < msg.states[i + 1].header.stamp) { -// break; -// } -// } -// } - -// // Extract correct states -// state_1 = msg.states[index]; -// state_2 = msg.states[index + 1]; - -// // Compute t_interp = [0,1] -// double t1, t2; -// ros::Duration t1_ros = state_1.header.stamp - t0_ros; -// t1 = t1_ros.toSec(); -// ros::Duration t2_ros = state_2.header.stamp - t0_ros; -// t2 = t2_ros.toSec(); -// double t_interp = (t - t1) / (t2 - t1); - -// // Compute interpolation -// interpRobotState(state_1, state_2, t_interp, interp_state); - -// return interp_state; -// } - void ikRobotState(const quad_utils::QuadKD &kinematics, - quad_msgs::BodyState body_state, - quad_msgs::MultiFootState multi_foot_state, + const quad_msgs::BodyState &body_state, + const quad_msgs::MultiFootState &multi_foot_state, sensor_msgs::JointState &joint_state) { joint_state.header = multi_foot_state.header; // If this message is empty set the joint names @@ -391,8 +351,8 @@ void ikRobotState(const quad_utils::QuadKD &kinematics, } void fkRobotState(const quad_utils::QuadKD &kinematics, - quad_msgs::BodyState body_state, - sensor_msgs::JointState joint_state, + const quad_msgs::BodyState &body_state, + const sensor_msgs::JointState &joint_state, quad_msgs::MultiFootState &multi_foot_state) { multi_foot_state.header = joint_state.header; // If this message is empty set the joint names @@ -535,8 +495,8 @@ Eigen::VectorXd bodyStateMsgToEigen(const quad_msgs::BodyState &body) { return state; } -void eigenToGRFArrayMsg(Eigen::VectorXd grf_array, - quad_msgs::MultiFootState multi_foot_state_msg, +void eigenToGRFArrayMsg(const Eigen::VectorXd &grf_array, + const quad_msgs::MultiFootState &multi_foot_state_msg, quad_msgs::GRFArray &grf_msg) { grf_msg.vectors.clear(); grf_msg.points.clear(); @@ -619,9 +579,9 @@ void multiFootStateMsgToEigen( } } -void eigenToFootStateMsg(Eigen::VectorXd foot_positions, - Eigen::VectorXd foot_velocities, - Eigen::VectorXd foot_acceleration, +void eigenToFootStateMsg(const Eigen::VectorXd &foot_positions, + const Eigen::VectorXd &foot_velocities, + const Eigen::VectorXd &foot_acceleration, quad_msgs::FootState &foot_state_msg) { eigenToFootStateMsg(foot_positions, foot_velocities, foot_state_msg); @@ -630,8 +590,8 @@ void eigenToFootStateMsg(Eigen::VectorXd foot_positions, foot_state_msg.acceleration.z = foot_acceleration[2]; } -void eigenToFootStateMsg(Eigen::VectorXd foot_positions, - Eigen::VectorXd foot_velocities, +void eigenToFootStateMsg(const Eigen::VectorXd &foot_positions, + const Eigen::VectorXd &foot_velocities, quad_msgs::FootState &foot_state_msg) { foot_state_msg.position.x = foot_positions[0]; foot_state_msg.position.y = foot_positions[1]; diff --git a/quad_utils/test/test_math_utils.cpp b/quad_utils/test/test_math_utils.cpp index 791f0d62a..3a9fa9f99 100644 --- a/quad_utils/test/test_math_utils.cpp +++ b/quad_utils/test/test_math_utils.cpp @@ -5,23 +5,24 @@ #include "quad_utils/ros_utils.h" TEST(MathTest, testWrap) { - int N = 201; - double amplitude = 10; - double period = 4 * M_PI; - std::vector data(N), t(N); - for (int i = 0; i < data.size(); i++) { - t[i] = i * period / N; - data[i] = amplitude * sin(t[i]); - } + int N = 201; + double amplitude = 10; + double period = 4 * M_PI; + std::vector data(N), t(N); + for (int i = 0; i < data.size(); i++) { + t[i] = i * period / N; + data[i] = amplitude * sin(t[i]); + } - std::vector data_wrapped = math_utils::wrapToPi(data); - std::vector data_unwrapped = math_utils::unwrap(data_wrapped); + std::vector data_wrapped = math_utils::wrapToPi(data); + std::vector data_unwrapped = + math_utils::getUnwrappedVector(data_wrapped); - double error = 0; - for (int i = 0; i < data.size(); i++) { - error += abs(data[i] - data_unwrapped[i]); - } + double error = 0; + for (int i = 0; i < data.size(); i++) { + error += abs(data[i] - data_unwrapped[i]); + } - double tolerance = 1e-4; - EXPECT_TRUE(error <= tolerance); + double tolerance = 1e-4; + EXPECT_TRUE(error <= tolerance); } diff --git a/robot_driver/CMakeLists.txt b/robot_driver/CMakeLists.txt index 533813c16..3e7428555 100644 --- a/robot_driver/CMakeLists.txt +++ b/robot_driver/CMakeLists.txt @@ -2,28 +2,13 @@ cmake_minimum_required(VERSION 3.0.2) project(robot_driver) ## Compile as C++14, supported in ROS Melodic and newer -add_compile_options(-std=c++14) +add_compile_options(-std=c++17) ## Set default cmake build type to release if(NOT CMAKE_CONFIGURATION_TYPES AND NOT CMAKE_BUILD_TYPE) set(CMAKE_BUILD_TYPE Release) endif() -## Setup MBLINK and MAVLINK -set(CMAKE_CXX_STANDARD 17) -set(MBLINK_DIR ${CMAKE_CURRENT_SOURCE_DIR}/../external/mblink/) - -message(STATUS "Using architecture ${CMAKE_SYSTEM_PROCESSOR}") - -if (CMAKE_SYSTEM_PROCESSOR STREQUAL "arm" OR CMAKE_SYSTEM_PROCESSOR STREQUAL "aarch64") - set(GRMBLINK libgrmblink_tx2.a) -else() - set(GRMBLINK libgrmblink_linux.a) -endif() - -include_directories(${MBLINK_DIR}/include) -link_directories(${MBLINK_DIR}/lib) - find_package(Eigen3 REQUIRED) ## Find catkin macros and libraries @@ -53,9 +38,12 @@ catkin_package( ## Specify additional locations of header files ## Your package locations should be listed before other locations include_directories( + moteus_driver/include include ${catkin_INCLUDE_DIRS} ) +# Motors driver +add_subdirectory(moteus_driver) ## Declare a C++ library for control add_library(robot_driver @@ -63,6 +51,8 @@ add_library(robot_driver src/robot_driver_utils.cpp src/controllers/leg_controller.cpp src/controllers/inverse_dynamics_controller.cpp + src/controllers/underbrush_inverse_dynamics.cpp + src/controllers/inertia_estimation_controller.cpp src/controllers/grf_pid_controller.cpp src/controllers/joint_controller.cpp src/estimators/state_estimator.cpp @@ -73,8 +63,7 @@ add_library(robot_driver ) add_dependencies(robot_driver ${robot_driver_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS}) -target_link_libraries(robot_driver - ${GRMBLINK} +target_link_libraries(robot_driver moteus_driver ${catkin_LIBRARIES} ) @@ -85,7 +74,7 @@ add_executable(robot_driver_node src/robot_driver_node.cpp) ## Specify libraries to link a library or executable target against target_link_libraries(robot_driver_node - robot_driver + robot_driver moteus_driver ${catkin_LIBRARIES} ) diff --git a/robot_driver/include/robot_driver/controllers/inertia_estimation_controller.h b/robot_driver/include/robot_driver/controllers/inertia_estimation_controller.h new file mode 100644 index 000000000..8e85f6f72 --- /dev/null +++ b/robot_driver/include/robot_driver/controllers/inertia_estimation_controller.h @@ -0,0 +1,44 @@ +#ifndef INERTIA_ESTIMATION_CONTROLLER_H +#define INERTIA_ESTIMATION_CONTROLLER_H + +#include + +//! Special-purpose inertia parameter estimation leg controller. +class InertiaEstimationController : public LegController { + public: + /** + * @brief Constructor for InertiaEstimationController + * @return Constructed object of type InertiaEstimationController + */ + InertiaEstimationController(); + + /** + * @brief Compute the leg command array message for a given current state and + * reference plan + * @param[in] robot_state_msg Message of the current robot state + * @param[out] leg_command_array_msg Command message after solving inverse + * dynamics and including reference setpoints for each joint + * @param[out] grf_array_msg GRF command message + */ + bool computeLegCommandArray(const quad_msgs::RobotState &robot_state_msg, + quad_msgs::LegCommandArray &leg_command_array_msg, + quad_msgs::GRFArray &grf_array_msg); + + /** + * @brief Return the reference state used for current tracking + * @return Reference state + */ + inline quad_msgs::RobotState getReferenceState() { return ref_state_msg_; } + + private: + /// Prior grf_array + Eigen::VectorXd last_grf_array_; + + /// Reference state for tracking + quad_msgs::RobotState ref_state_msg_; + + /// GRF exponential filter constant + const double grf_exp_filter_const_ = 1.0; // 1.0 = no filtering +}; + +#endif // INERTIA_ESTIMATION_CONTROLLER diff --git a/robot_driver/include/robot_driver/controllers/underbrush_inverse_dynamics.h b/robot_driver/include/robot_driver/controllers/underbrush_inverse_dynamics.h new file mode 100644 index 000000000..0a4a7260a --- /dev/null +++ b/robot_driver/include/robot_driver/controllers/underbrush_inverse_dynamics.h @@ -0,0 +1,89 @@ +#ifndef UNDERBRUSH_INVERSE_DYNAMICS_H +#define UNDERBRUSH_INVERSE_DYNAMICS_H + +#include +#include + +//! Implements inverse dynamics as a controller within the ROS framework. +/*! + InverseDynamicsController implements inverse dynamics logic. It should expose + a constructor that does any initialization required and an update method + called at some frequency. +*/ +class UnderbrushInverseDynamicsController : public LegController { + public: + /** + * @brief Constructor for InverseDynamicsController + * @return Constructed object of type InverseDynamicsController + */ + UnderbrushInverseDynamicsController(); + + /** + * @brief Update body force estimate + * @param[in] msg current force estimates + */ + void updateBodyForceEstimate( + const quad_msgs::BodyForceEstimate::ConstPtr &msg); + + /** + * @brief Set underbrush-specific gains and parameters + * @param[in] retract_vel Retraction speed in rad/s when in contact + * @param[in] tau_push Contact orwards push torque for distal link in N m + * @param[in] tau_contact_start Threshold for contact initiation in N m + * @param[in] tau_contact_end Threshold for contact ending in N m + * @param[in] min_switch Minimum time between transitions in s + * @param[in] t_down Time for foot to come down in s + */ + void setUnderbrushParams(double retract_vel, double tau_push, + double tau_contact_start, double tau_contact_end, + double min_switch, double t_down, double t_up); + + /** + * @brief Compute the leg command array message for a given current state and + * reference plan + * @param[in] robot_state_msg Message of the current robot state + * @param[out] leg_command_array_msg Command message after solving inverse + * dynamics and including reference setpoints for each joint + * @param[out] grf_array_msg GRF command message + */ + bool computeLegCommandArray(const quad_msgs::RobotState &robot_state_msg, + quad_msgs::LegCommandArray &leg_command_array_msg, + quad_msgs::GRFArray &grf_array_msg); + + /** + * @brief Return the reference state used for current tracking + * @return Reference state + */ + inline quad_msgs::RobotState getReferenceState() { return ref_state_msg_; } + + private: + /// Prior grf_array + Eigen::VectorXd last_grf_array_; + + /// Reference state for tracking + quad_msgs::RobotState ref_state_msg_; + + /// GRF exponential filter constant + const double grf_exp_filter_const_ = 1.0; // 1.0 = no filtering + + /// Most recent body force estimate + quad_msgs::BodyForceEstimate::ConstPtr last_body_force_estimate_msg_; + + /// Leg swing mode logic + std::vector force_mode_; + std::vector last_mode_; + std::vector t_switch_; + std::vector t_LO_; + std::vector t_TD_; + + /// Underbrush swing parameters + double retract_vel_; + double tau_push_; + double tau_contact_start_; + double tau_contact_end_; + double min_switch_; + double t_down_; + double t_up_; +}; + +#endif // UNDERBRUSH_INVERSE_DYNAMICS_H diff --git a/robot_driver/include/robot_driver/estimators/ekf_estimator.h b/robot_driver/include/robot_driver/estimators/ekf_estimator.h index 5815fec1b..9d651bb21 100644 --- a/robot_driver/include/robot_driver/estimators/ekf_estimator.h +++ b/robot_driver/include/robot_driver/estimators/ekf_estimator.h @@ -1,32 +1,364 @@ -#ifndef EKF_H -#define EKF_H +#ifndef EKF_ESTIMATOR_H +#define EKF_ESTIMATOR_H +#include +#include +#include +#include +#include +#include +#include #include +#include +#include +#include +#include +#include -//! Implements Extended Kalman Filter as an estimator within the ROS framework. +#include +#include + +//! Implements online EKF based state estimation +/*! + EKFEstimator implements all estimator logic. It should expose a constructor + that does any initialization required and an update method called at some + frequency. +*/ class EKFEstimator : public StateEstimator { public: /** * @brief Constructor for EKFEstimator + * @param[in] nh ROS NodeHandle to publish and subscribe from * @return Constructed object of type EKFEstimator */ EKFEstimator(); + void init(ros::NodeHandle& nh); + bool updateOnce(quad_msgs::RobotState& last_robot_state_msg_); + /** - * @brief Initialize EKF - * @param[in] nh Node Handler to load parameters from yaml file + * @brief Calls ros spinOnce and pubs data at set frequency */ - void init(ros::NodeHandle& nh); + // void spin(); Commented Out, Doesn't Exist, Spins with Robot Driver /** - * @brief Perform EKF update once - * @param[out] last_robot_state_msg_ + * @brief set X as Xin */ - bool updateOnce(quad_msgs::RobotState& last_robot_state_msg_); + void setX(Eigen::VectorXd Xin); + + /** + * @brief set P as Pin + */ + void setP(Eigen::MatrixXd Pin); + /** + * @brief set lastX as Xin + */ + void setlast_X(Eigen::VectorXd Xin); + + /** + * @brief return the value of X + * @return return X + */ + Eigen::VectorXd getX(); + + /** + * @brief return the value of last_X + * @return return last_X + */ + Eigen::VectorXd getlastX(); + + /** + * @brief return the value of X_pre + * @return return X_pre + */ + Eigen::VectorXd getX_pre(); + + /** + * @brief return the value of X + * @return return X + */ + Eigen::VectorXd getStates(); + + // private: + /** + * @brief Callback function to handle new ground truth data + * @param[in] msg ground_truth_msg quad_msgs + */ + void groundtruthCallback(const quad_msgs::RobotState::ConstPtr& msg); + + /** + * @brief Callback function to handle new joint encoder data + * @param[in] joint_encoder_msg sensor_msgs containing joint + * pos,vel,current + */ + void jointEncoderCallback(const sensor_msgs::JointState::ConstPtr& msg); + + /** + * @brief Callback function to handle new imu data + * @param[in] imu_msg sensors_msgs containing new imu data + */ + void imuCallback(const sensor_msgs::Imu::ConstPtr& msg); + + /** + * @brief Callback function to handle new contact estimates + * @param[in] msg quad_msgs::ContactMode containing new contact data + */ + void contactCallback(const quad_msgs::ContactMode::ConstPtr& msg); + + /** + * @brief Callback function to handle new grf controls + * @param[in] msg quad_msgs::GRFArray containing new grf control data + */ + void grfCallback(const quad_msgs::GRFArray::ConstPtr& msg); + + /** + * @brief execute EKF Update step, return state estimate + * @return state estimate of custom type RobotState + */ + + void localPlanCallback(const quad_msgs::RobotPlan::ConstPtr& msg); + quad_msgs::RobotState StepOnce(); + + /** + * @brief EKF prediction step + * @param[in] dt double time interval + * @param[in] fk Eigen::VectorXd linear acceleration data (3 * 1) + * @param[in] wk Eigen::VectorXd angular acceleration data (3 * 1) + * @param[in] qk Eigen::Quaterniond orientation in quaternion (4 * 1) + */ + void predict(const double& dt, const Eigen::VectorXd& fk, + const Eigen::VectorXd& wk, const Eigen::Quaterniond& qk); + + /** + * @brief EKF update step + * @param[in] jk Eigen::VectorXd joint encoder data (12 * 1) + * @param[in] vk Eigen::VectorXd joint encoder velcoity data (12 * 1) + * @param[in] wk Eigen::VectorXd imu angular acceleration data (3 * 1) + */ + void update(const Eigen::VectorXd& jk, const Eigen::VectorXd& fk, + const Eigen::VectorXd& vk, const Eigen::VectorXd& wk); + + /** + * @brief Function to set initial robot state for ekf state estimator + */ + void setInitialState(quad_msgs::RobotState& last_robot_state_msg_); + + Eigen::VectorXd quaternionDynamics(const Eigen::VectorXd& w, + const Eigen::VectorXd& q); + + /** + * @brief calculate skew-symmetric matrix from a vector + * @param[in] w Eigen::VectorXd angular velocity vector (3 * 1) + * @return Eigen::MatrixXd skew- symmetric matrix (3 * 3) + */ + Eigen::MatrixXd calcSkewsym(const Eigen::VectorXd& w); + + /** + * @brief calculate rodrigues incremental rotation matrix from a vector + * @param[in] dt time in second + * @param[in] w Eigen::VectorXd angular velocity vector (3 * 1) + * @param[in] sub subscript + * @return Eigen::MatrixXd rodrigues incremental rotation matrix matrix (3 * + * 3) + */ + Eigen::MatrixXd calcRodrigues(const double& dt, const Eigen::VectorXd& w, + const int& sub); + + /** + * @brief set sensor noise + */ + void setNoise(); + + // number of states position (3 * 1) + velocity (3 * 1) + quaternion (4 * 1) + + // feet position (12 * 1) + bias_acc (3 * 1) + bias_gyro (3 * 1) + static const int num_state = 18; + + // number of covariances equals number of states + static const int num_cov = 18; + + // measurement number equals feet positions (12) + static const int num_measure = 28; + + const int num_feet = 4; + + /// Boolean for whether robot layer is hardware (else sim) + bool is_hardware_; + + /// Subscriber for ground_truth RobotState messages + ros::Subscriber state_ground_truth_sub_; + + /// Subscriber for joint encoder messages + ros::Subscriber joint_encoder_sub_; - private: - /// Nodehandle to get param + /// Subscriber for imu messages + ros::Subscriber imu_sub_; + + /// Subscriber for grf messages + ros::Subscriber grf_sub_; + + /// Subscriber for contact detection messages + ros::Subscriber contact_sub_; + + /// ROS subscriber for local plan + ros::Subscriber local_plan_sub_; + + /// Nodehandle to pub to and sub from ros::NodeHandle nh_; -}; -#endif // EKF_H + /// Update rate for sending and receiving data; + double update_rate_; + + /// Last state estimate + quad_msgs::RobotState last_state_est_; + + /// Most recent local plan + quad_msgs::RobotPlan::ConstPtr last_local_plan_msg_; + + /// Last grf control message + quad_msgs::GRFArray::ConstPtr last_grf_msg_; + + /// Last contact detection message (should be timestamped!) + quad_msgs::ContactMode::ConstPtr last_contact_msg_; + + /// Most recent IMU callback (should be timestamped!) + // sensor_msgs::Imu::ConstPtr last_imu_msg_; + + /// Most recent encoder callback (should be timestamped!) + // sensor_msgs::JointState::ConstPtr last_joint_state_msg_; + + /// Maximum amount of time to still use joint state message in EKF data + double joint_state_msg_time_diff_max_; + + /// QuadKD class + std::shared_ptr quadKD_; + + // gravity vector (3 * 1) + Eigen::VectorXd g; + + // initial state vector (28 * 1) + Eigen::VectorXd X0; + + // state vector (28 * 1) + Eigen::VectorXd X; + + // last state vector (28 * 1) + Eigen::VectorXd last_X; + + // prediction state vector (28 * 1) + Eigen::VectorXd X_pre; + + // prediction state vector (28 * 1) + Eigen::Matrix SC; + + Eigen::Matrix S; + + // rotation matrix generated from imu quaternion (3 * 3) + Eigen::Matrix3d C1; + + // measurement matrix + Eigen::MatrixXd C; + + // initial covariance matrix (27 * 27) + Eigen::Matrix Serror_y; + + // state covariance matrix (27 * 27) + Eigen::MatrixXd P; + + // prediction state covariance matrix (27 * 27) + Eigen::MatrixXd P_pre; + + // state transition matrix (27 * 27) + Eigen::MatrixXd F; + + // process covariance matrix (27 * 27) + Eigen::MatrixXd Q; + + // "control" compensated body acceletation vector (3 * 1) + Eigen::Vector3d u; + + // measurement matrix (12 * 27) + Eigen::MatrixXd H; + + // measurement covariance matrix (12 * 12) + Eigen::MatrixXd R; + + // foot forward kinematics vector (24*1) + Eigen::VectorXd foot_state; + + // error measurement displacement vector (18 * 1) + Eigen::Matrix error_y; + + // measurement Generated from Leg Kinematics (18 * 1) + Eigen::VectorXd y; + + Eigen::VectorXd q; + + // previous time variable + ros::Time last_time; + + // IMU linear acceleration bias (3*3) + Eigen::MatrixXd bias_acc; + + // IMU linear acceleration noise (3*3) + Eigen::MatrixXd noise_acc; + + // IMU augular acceleration bias (3*3) + Eigen::MatrixXd bias_gyro; + + // IMU augular acceleration noise (3*3) + Eigen::MatrixXd noise_gyro; + + // individual noise at feet (3*3) + Eigen::MatrixXd noise_feet; + + // noise at feet (3*3) + Eigen::MatrixXd noise_fk; + + // noise at encoder + double noise_encoder; + + // imu bias + double bias_x_; + + double bias_y_; + + double bias_z_; + + double bias_r_; + + double bias_p_; + + double bias_w_; + + int counter = 0; + + // noise term + // noise in accelerometer + double na_; + // noise in gyro + double ng_; + // bias in accelerometer + double ba_; + // bias in gyro + double bg_; + // noise in feet + double nf_; + // noise in forward kinematics + double nfk_; + // noise in encoder + double ne_; + // initial covariance value + double P0_; + // weight on foot contact value + double contact_w_; + // Innovation Norm Threshold, Reject Measurement Update if too large + double thresh_out; + // initialized the estimator + bool initialized = true; + + // Record whether we have good imu and joint state data + bool good_imu = false; + bool good_joint_state = false; + bool good_ground_truth_state = false; +}; +#endif // EKF_ESTIMATOR_H diff --git a/robot_driver/include/robot_driver/estimators/state_estimator.h b/robot_driver/include/robot_driver/estimators/state_estimator.h index c9a67cf3e..d6d48a3a0 100644 --- a/robot_driver/include/robot_driver/estimators/state_estimator.h +++ b/robot_driver/include/robot_driver/estimators/state_estimator.h @@ -44,18 +44,19 @@ class StateEstimator { * @param[out] wk Angular acceleration * @param[out] qk Orientation in quaternion */ - void readIMU(const sensor_msgs::Imu::ConstPtr& last_imu_msg, - Eigen::Vector3d& fk, Eigen::Vector3d& wk, + void readIMU(const sensor_msgs::Imu& last_imu_msg, + Eigen::VectorXd& fk, Eigen::VectorXd& wk, Eigen::Quaterniond& qk); /** * @brief Read joint encoder data * @param[in] last_joint_state_msg Joint state sensor message - * @param[out] jk Jointstate in vector (12 * 1) + * @param[out] jk Joint state in vector (12 * 1) + * @param[out] vk Joint velocity in vector (12 * 1) */ void readJointEncoder( - const sensor_msgs::JointState::ConstPtr& last_joint_state_msg, - Eigen::VectorXd& jk); + const sensor_msgs::JointState& last_joint_state_msg, + Eigen::VectorXd& jk, Eigen::VectorXd& vk); /** * @brief Load Mocap data to protected variable @@ -75,6 +76,9 @@ class StateEstimator { /// Last state estimate quad_msgs::RobotState state_est_; + /// Most recent state estimate (added 02/05) + quad_msgs::RobotState last_robot_state_msg_; + /// QuadKD class std::shared_ptr quadKD_; diff --git a/robot_driver/include/robot_driver/hardware_interfaces/spirit_interface.h b/robot_driver/include/robot_driver/hardware_interfaces/spirit_interface.h index d1f526cb5..8806da548 100644 --- a/robot_driver/include/robot_driver/hardware_interfaces/spirit_interface.h +++ b/robot_driver/include/robot_driver/hardware_interfaces/spirit_interface.h @@ -1,87 +1,76 @@ #ifndef SPIRIT_INTERFACE_H #define SPIRIT_INTERFACE_H +#pragma once #include #include +#include "moteus_driver/YloTwoPcanToMoteus.hpp" // ylo2 library #include #include #include #include - #include -#include +#include // to call imu subscriber loop under a thread + +#define MATH_PI 3.141592 -using gr::MBLink; +class SpiritInterface : public HardwareInterface { -struct LimbCmd_t { - Eigen::Vector3f pos, vel, tau; - short kp[3]; - short kd[3]; - bool restart_flag; -}; + private: + + /* send a canFD MOVE frame command to send power to motors at very low fftorque, + this avoids shocks at moves start, and query informations about moteus controller*/ + bool booting_motors(); -typedef std::unordered_map MBData_t; + /** @brief Sends a zero command to the robot */ + bool send_zero_command(); -//! Hardware interface for the Spirit40 quadruped from Ghost Robotics. -/*! - SpiritInterface listens for joint control messages and outputs low level - commands to the mainboard over mblink. -*/ -class SpiritInterface : public HardwareInterface { - public: - /** - * @brief Constructor for SpiritInterface - * @return Constructed object of type SpiritInterface - */ - SpiritInterface(); - - /** - * @brief Load the hardware interface - * @param[in] argc Argument count - * @param[in] argv Argument vector - */ - virtual void loadInterface(int argc, char** argv); - - /** - * @brief Unload the hardware interface - */ - virtual void unloadInterface(); - - /** - * @brief Send commands to the robot via the mblink protocol - * @param[in] leg_command_array_msg Message containing leg commands - * @param[in] user_data Vector containing user data - * @return boolean indicating success of transmission - */ - virtual bool send(const quad_msgs::LegCommandArray& leg_command_array_msg, - const Eigen::VectorXd& user_tx_data); - - /** - * @brief Recieve data from the robot via the mblink protocol - * @param[out] joint_state_msg Message containing joint state information - * @param[out] imu_msg Message containing imu information - * @param[out] user_data Vector containing user data - * @return Boolean for whether data was successfully received - */ - virtual bool recv(sensor_msgs::JointState& joint_state_msg, - sensor_msgs::Imu& imu_msg, Eigen::VectorXd& user_rx_data); - - /// Pointer to MBLink object - MBLink mblink_; - - /// Mainboard data - MBData_t mbdata_; - - /// Vector of joint names - std::vector joint_names_ = {"8", "0", "1", "9", "2", "3", + /** @brief Executes the robot's startup routine */ + bool startup_routine(); + + // 2*PI + const float TWO_M_PI = 6.28318531; + + // flag to authorize walk + bool is_up_flag = false; + + // robot position is sitted + std::vectorsit_down_joints_pose = { -0.0461273193359375, -0.1825408935546875, 0.371002197265625, // 3, 1, 2 + 0.04315185546875, 0.18280029296875, -0.373260498046875, // 6, 4, 5 + 0.0385894775390625, -0.1865081787109375, 0.371063232421875, // 9, 7, 8 + -0.04425048828125, 0.188568115234375, -0.372222900390625}; // 12, 10, 11 + + public: + + SpiritInterface(); + + virtual void loadInterface(int argc, char** argv) override; + + virtual void unloadInterface() override; + + virtual bool send(const quad_msgs::LegCommandArray& leg_command_array_msg, + const Eigen::VectorXd& user_tx_data) override; + + virtual bool recv(sensor_msgs::JointState& joint_state_msg, + sensor_msgs::Imu& imu_msg, Eigen::VectorXd& user_rx_data) override; + + /// Vector of joint names + std::vector joint_names_ = {"8", "0", "1", "9", "2", "3", "10", "4", "5", "11", "6", "7"}; - /// Vector denoting joint indices - std::vector joint_indices_ = {8, 0, 1, 9, 2, 3, 10, 4, 5, 11, 6, 7}; + /// Vector denoting joint indices + std::vector joint_indices_ = {8, 0, 1, 9, 2, 3, 10, 4, 5, 11, 6, 7}; - /// Vector of kt values for each joint - std::vector kt_vec_ = {0.546, 0.546, 1.092, 0.546, 0.546, 1.092, + /// Vector of kt values for each joint + std::vector kt_vec_ = {0.546, 0.546, 1.092, 0.546, 0.546, 1.092, 0.546, 0.546, 1.092, 0.546, 0.546, 1.092}; -}; -#endif // SPIRIT_INTERFACE_H + float joint_position = 0.0; + float joint_velocity = 0.0; + float joint_fftorque = 0.0; + float joint_kp = 0.0; + float joint_kd = 0.0; + +}; // end class + +#endif // SPIRIT_INTERFACE_H \ No newline at end of file diff --git a/robot_driver/include/robot_driver/robot_driver.h b/robot_driver/include/robot_driver/robot_driver.h index 0e82f2f23..15b2cc8f1 100644 --- a/robot_driver/include/robot_driver/robot_driver.h +++ b/robot_driver/include/robot_driver/robot_driver.h @@ -20,9 +20,11 @@ #include #include "robot_driver/controllers/grf_pid_controller.h" +#include "robot_driver/controllers/inertia_estimation_controller.h" #include "robot_driver/controllers/inverse_dynamics_controller.h" #include "robot_driver/controllers/joint_controller.h" #include "robot_driver/controllers/leg_controller.h" +#include "robot_driver/controllers/underbrush_inverse_dynamics.h" #include "robot_driver/estimators/comp_filter_estimator.h" #include "robot_driver/estimators/ekf_estimator.h" #include "robot_driver/estimators/state_estimator.h" @@ -74,6 +76,12 @@ class RobotDriver { * @brief Verifies and updates new control mode * @param[in] msg New control mode */ + void grfCallback(const quad_msgs::GRFArray::ConstPtr& msg); + + /** + * @brief execute EKF Update step, return state estimate + * @return state estimate of custom type RobotState + */ void controlModeCallback(const std_msgs::UInt8::ConstPtr& msg); /** @@ -106,6 +114,13 @@ class RobotDriver { */ void singleJointCommandCallback(const geometry_msgs::Vector3::ConstPtr& msg); + /** + * @brief Callback to handle new body force estimates + * @param[in] msg body force estimates + */ + void bodyForceEstimateCallback( + const quad_msgs::BodyForceEstimate::ConstPtr& msg); + /** * @brief Callback to handle control restart flag messages */ @@ -164,6 +179,9 @@ class RobotDriver { /// ROS subscriber for state estimate ros::Subscriber robot_state_sub_; + /// ROS subscriber for body force estimates + ros::Subscriber body_force_estimate_sub_; + /// ROS subscriber for control restart flag ros::Subscriber control_restart_flag_sub_; @@ -179,6 +197,9 @@ class RobotDriver { /// ROS subscriber for single joint command ros::Subscriber single_joint_cmd_sub_; + // /// ROS subscriber for desired GRF + // ros::Subscriber grf_sub_; + /// ROS publisher for robot heartbeat ros::Publisher robot_heartbeat_pub_; @@ -194,6 +215,9 @@ class RobotDriver { /// ROS publisher for joint data ros::Publisher joint_state_pub_; + /// Publisher for state estimate messages + ros::Publisher state_estimate_pub_; + /// Nodehandle to pub to and sub from ros::NodeHandle nh_; @@ -206,6 +230,9 @@ class RobotDriver { /// Estimator type std::string estimator_id_; + /// Ground Truth Source + std::string ground_truth_; + /// Update rate for computing new controls; double update_rate_; @@ -218,6 +245,9 @@ class RobotDriver { /// Robot mode int control_mode_; + /// Robot State Intialization + int initialized_; + /// Torque limits std::vector torque_limits_; @@ -236,6 +266,9 @@ class RobotDriver { /// Define ids for control modes: Safety const int SAFETY = 4; + /// Define ids for for state initialization + const int REST = 5; + /// Define ids for input types: none const int NONE = 0; @@ -248,9 +281,15 @@ class RobotDriver { /// Most recent local plan quad_msgs::RobotPlan::ConstPtr last_local_plan_msg_; - /// Most recent state estimate + /// Ground Truth Robot State from Simulation quad_msgs::RobotState last_robot_state_msg_; + /// Robot State Estimate Used in Control + quad_msgs::RobotState state_estimate_; + + /// EKF State Estimate Output + quad_msgs::RobotState estimated_state_; + /// Most recent local plan quad_msgs::GRFArray::ConstPtr last_grf_array_msg_; @@ -338,6 +377,9 @@ class RobotDriver { /// State Estimator template class std::shared_ptr state_estimator_; + /// State Estimator template class + std::shared_ptr ekf_estimator_; + /// Mblink converter object std::shared_ptr hardware_interface_; diff --git a/robot_driver/moteus_driver/CMakeLists.txt b/robot_driver/moteus_driver/CMakeLists.txt new file mode 100755 index 000000000..22de31706 --- /dev/null +++ b/robot_driver/moteus_driver/CMakeLists.txt @@ -0,0 +1,35 @@ +cmake_minimum_required(VERSION 3.10) +project(moteus_driver) + +add_compile_options(-lmraa) + +if(NOT CMAKE_CXX_STANDARD) + set(CMAKE_CXX_STANDARD 17) +endif() + +find_package(PkgConfig REQUIRED) +pkg_check_modules(mraa QUIET mraa) + +if(NOT "${mraa_FOUND}") + string(ASCII 27 Esc) + message(WARNING "${Esc}[1;33m MRAA LIB NOT FOUND !!!${Esc}[m") +endif() + +set(CAN_LIBS pthread rt pcanbasic) + +include_directories( + include + (${PROJECT_SOURCE_DIR}/mraa_api) + (${PROJECT_SOURCE_DIR}/mraa_api/mraa) + ${catkin_INCLUDE_DIRS} + ${mraa_INCLUDE_DIRS} +) + +if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang") + add_compile_options(-Wall -Wextra -Wpedantic) +endif() + +file(GLOB MOTEUS_SRC src/*.cpp) + +add_library(moteus_driver ${MOTEUS_SRC}) +target_link_libraries(moteus_driver ${CAN_LIBS} ${catkin_LIBRARIES} mraa stdc++) diff --git a/robot_driver/moteus_driver/include/moteus_driver/YloTwoPcanToMoteus.hpp b/robot_driver/moteus_driver/include/moteus_driver/YloTwoPcanToMoteus.hpp new file mode 100755 index 000000000..a269b0049 --- /dev/null +++ b/robot_driver/moteus_driver/include/moteus_driver/YloTwoPcanToMoteus.hpp @@ -0,0 +1,271 @@ +#ifndef PCANTOMOTEUS_HPP +#define PCANTOMOTEUS_HPP + +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include + +#include // Peak m2canFd board lib + +#include "mraa/common.hpp" // for GPIO security switch +#include "mraa/gpio.hpp" + +#include // pour la fonction champ_standup + +// define GPIO switch port +#define BTN_PIN 29 + +//define pcan 4 ports to their respective physical adress +#define PCAN_DEV1 PCAN_PCIBUS1 +#define PCAN_DEV2 PCAN_PCIBUS2 +#define PCAN_DEV3 PCAN_PCIBUS3 +#define PCAN_DEV4 PCAN_PCIBUS4 + +/* moteus controllers TX bytes adress */ + +#define MSGTX_ADDR_POSITION 0x06 +#define MSGTX_ADDR_VELOCITY 0x0A +#define MSGTX_ADDR_FFTORQUE 0x0E +#define MSGTX_ADDR_KP 0x12 +#define MSGTX_ADDR_KD 0x16 +#define MSGTX_ADDR_MAXTORQUE 0x1A +#define MSGTX_ADDR_STOP_MODE 0x1E +#define MSGTX_ADDR_WATCHDOG_ 0x22 +#define MSGTX_ADDR_TARG_VEL 0x26 +#define MSGTX_ADDR_TARG_ACC 0x2A + +/* moteus controllers RX bytes adress */ +#define MSGRX_ADDR_POSITION 0x02 +#define MSGRX_ADDR_VELOCITY 0x06 +#define MSGRX_ADDR_TORQUE 0x0A +#define MSGRX_ADDR_VOLTAGE 0x0E +#define MSGRX_ADDR_TEMPERATURE 0x12 +#define MSGRX_ADDR_FAULT 0x16 + +// about moteus controllers faults errors: +/* 32 - calibration fault - the encoder was not able to sense a magnet during calibration + 33 - motor driver fault - the most common reason for this is undervoltage, moteus attempted to draw more current than the supply could provide. Other electrical faults may also report this error, the drv8323 diagnostic tree has more information. + 34 - over voltage - the bus voltage exceeded servo.max_voltage. This can happen due to misconfiguration, or if the controller regenerated power with a supply that cannot sink power and no flux braking was configured. + 35 - encoder fault - the encoder readings are not consistent with a magnet being present. + 36 - motor not configured - the moteus_tool --calibrate procedure has not been run on this motor. + 37 - pwm cycle overrun - an internal firmware error + 38 - over temperature - the maximum configured temperature has been exceeded + 39 - outside limit - an attempt was made to start position control while outside the bounds configured by servopos.position_min and servopos.position_max.*/ + +/* power board RX bytes adress */ // TODO check adresses +#define MSGPBRX_ADDR_STATE 0x02 // 2 bytes per value (int16) +#define MSGPBRX_ADDR_FAULT_CODE 0x04 +#define MSGPBRX_ADDR_SWITCH_STATUS 0x06 +#define MSGPBRX_ADDR_OUT_VOLTAGE 0x08 +#define MSGPBRX_ADDR_OUT_CURRENT 0x0A +#define MSGPBRX_ADDR_TEMPERATURE 0x0C +#define MSGPBRX_ADDR_ENERGY 0x0E + +// about moteus power board faults errors: +/* TODO */ + +// a structure for ylo2 controllers setup +struct MotorAdapter{ + public: + MotorAdapter(){ + idx_ = -1; + sign_ = 1; + port_ = -1; + leg_index_ = 0; + joint_index_ = 0; + } + + MotorAdapter(int idx, int sign, int port, int leg_index, int joint_index){ + idx_ = idx; + sign_ = sign; + port_ = port; + leg_index_ = leg_index; + joint_index_ = joint_index; + } + + const int& getIdx() {return idx_;} + const int& getSign() {return sign_;} + const int& getPort() {return port_;} + const int& getLeg_index() {return leg_index_;} + const int& getJoint_index() {return joint_index_;} + + void setIdx(int idx) {idx_ = idx;} + void setSign(int sign) {sign_ = sign;} + void setPort(int port) {port_ = port;} + void setLeg_index(int leg_index) {leg_index_ = leg_index;} + void setJoint_index(int joint_index) {joint_index_ = joint_index;} + + private: + int idx_; + int sign_; + int port_; + int leg_index_; + int joint_index_; +}; + +// the YloTwoPcanToMoteus class +class YloTwoPcanToMoteus{ + + public: + + /* SECURITY RED SWITCH + wires diagram : + - black is ground (pin 1); + - red is +3.3vcc with 10k resistor (pin 6); + - white is gpio read (pin 29)*/ + bool security_switch(); + + /* PEAK BOARD M2 4 CANFD PORTS + initialize all 4 ports*/ + bool Can_init(); + + /* reset all 4 ports*/ + bool Can_reset(); + + /* Imu functions */ + static sensor_msgs::Imu ylo3_imu; // the imu variable + void subscribeToImuData(); + void imuCallback(const sensor_msgs::Imu::ConstPtr& imu_msg); + sensor_msgs::Imu getYlo3Imu(); + + /* zero a single moteus controller*/ + bool send_moteus_zero_order(int id, int port, float position); + + /* send a single canFD STOP frame command to clear any faults, + and query informations about moteus controller*/ + bool send_moteus_stop_order(int id, int port); + + /* sending a velocity command */ + bool send_to_moteus(int id, int port, float pos, float vel, float fftorque, float kp, float kd, float target_vel, float target_accel); + bool send_to_moteus(int id, int port, float pos, float vel, float fftorque, float kp, float kd); + + /* query a single canFD RX Queue, + and read ID params*/ + bool read_moteus_RX_queue(int id, int port, float& position, float& velocity, float& torque, float& voltage, float& temperature, float& fault); + bool read_moteus_RX_queue(int id, int port, float& voltage, float& temperature, float& fault); + + /* send a single canFD command frame + ask informations about moteus power board*/ + bool send_power_board_order(); + + /* query power board to read values*/ + bool read_power_board_RX_queue(float& state, float& fault_code, float& switch_status, float& out_volt, float& out_curr, float& board_temp); + + /* initialize Peak canFD board, + reset the 4 ports.*/ + bool peak_fdcan_board_initialization(); + + /* send a stop order to all motors */ + bool stop_motors(); + + /*1/ zero joints with initial_ground_joints_pose vector + 2/ check angle error between asked rezero, and read position + 3/ loop until success */ + bool check_initial_ground_pose(); + + uint32_t _id; // ID of a moteus controller + YloTwoPcanToMoteus(); + virtual ~YloTwoPcanToMoteus(); + + std::vector motor_adapters_; + + bool all_moteus_controllers_ok = true; + bool can_error = false; // error in Peak FDCAN send order or query + bool is_calibrated = true; // is the robot in right startup pose + + //for moteus RX + float RX_mode = 0; + float RX_pos = 0.0; + float RX_vel = 0.0; + float RX_tor = 0.0; + float RX_volt = 0.0; + float RX_temp = 0.0; + float RX_fault = 0; + + //for moteus TX + float _comm_position = 0.0; // NAN for torque mode + float _comm_velocity = 0.0; + float _comm_fftorque = 1.0; // variable Tau + float _comm_kp = 10.0; + float _comm_kd = 1.0; + float _comm_maxtorque = 0.10; // Max possible torque is NAN value + float _comm_stop_mode = NAN; + float _comm_watchdog_timeout = 10; + float _comm_target_vel = 0.6; + float _comm_target_acc = 0.3; + + + // for mraa library GPIO (security switch) + mraa_gpio_context btnPin; // Will be used to represnt the button pin + + private: + + // for pcanbasic library + TPCANStatus Status; // the return of a command, to check success + + // Define the compatible Moteus FD Bitrate string + TPCANBitrateFD BitrateFD = (char*) "f_clock_mhz = 80, nom_brp = 1, nom_tseg1 = 50, nom_tseg2 = 29, nom_sjw = 10, data_brp = 1, data_tseg1 = 8, data_tseg2 = 7, data_sjw = 12"; + TPCANTimestampFD timestamp; + + TPCANMsgFD _stop; // the stop canFD message + TPCANMsgFD _zero; // the stop canFD message + TPCANMsgFD moteus_tx_msg; // the canFD message to send order to a moteus controller + TPCANMsgFD moteus_rx_msg; // the canFD message to read moteus controller + TPCANMsgFD _power_board_tx_msg; // the canFD message to ask moteus power board + TPCANMsgFD _power_board_rx_msg; // the canFD message to read moteus power board + + char strMsg[256]; + std::vector pcanPorts_ = {PCAN_DEV1, PCAN_DEV2, PCAN_DEV3, PCAN_DEV4}; + + int idx_; + int sign_; + int port_; + int stop_pos_low_; + int stop_pos_high_; + + + + /* query variables for moteus controllers */ + float _position = 0.0; + float _velocity = 0.0; + float _torque = 0.0; + float _voltage = 0.0; + float _temperature = 0.0; + float _fault = 0.0; + + /* query variables for moteus power board */ + float _state = 0.0; + float _fault_code = 0.0; + float _switch_status = 0.0; + float _lock_time = 0.0; + float _boot_time = 0.0; + float _out_volt = 0.0; + float _out_curr = 0.0; + float _board_temp = 0.0; + float _energy = 0.0; + + + // robot position is sitted + std::vectorsit_down_joints_pose = { -0.0461273193359375, -0.1825408935546875, 0.371002197265625, // 3, 1, 2 + 0.0385894775390625, -0.1865081787109375, 0.371063232421875, // 9, 7, 8 + 0.04315185546875, 0.18280029296875, -0.373260498046875, // 6, 4, 5 + -0.04425048828125, 0.188568115234375, -0.372222900390625}; // 12, 10, 11 + + + // stand up target pose (en tours/s) - pattes un peu trop écartées, dues a leur position au sol ! + std::vectorstandup_joints_pose = { 0.000582622, 0.1629, -0.28942, + 0.000582622, -0.1629, 0.28942, + -0.000582622, 0.1629, -0.28942, + -0.000582622, -0.1629, 0.28942}; + + float calibration_error = 0.03; // 10.8 degrees ca passe aussi avec 0.01 (3.6 degrees) +}; + +#endif // PCANTOMOTEUS_HPP \ No newline at end of file diff --git a/robot_driver/moteus_driver/src/YloTwoPcanToMoteus.cpp b/robot_driver/moteus_driver/src/YloTwoPcanToMoteus.cpp new file mode 100755 index 000000000..8c0ee17fb --- /dev/null +++ b/robot_driver/moteus_driver/src/YloTwoPcanToMoteus.cpp @@ -0,0 +1,511 @@ +/* +Copyright (c) 08/2022, Vincent FOUCAULT +All rights reserved. + +Redistribution and use in source and binary forms, with or without +modification, are permitted provided that the following conditions are met: + * Redistributions of source code must retain the above copyright + notice, this list of conditions and the following disclaimer. + * Redistributions in binary form must reproduce the above copyright + notice, this list of conditions and the following disclaimer in the + documentation and/or other materials provided with the distribution. + * Neither the name of the copyright holder nor the names of its + contributors may be used to endorse or promote products derived + from this software without specific prior written permission. + +THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND +ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED +WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE +DISCLAIMED. IN NO EVENT SHALL COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY +DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES +(INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; +LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND +ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT +(INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS +SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. +*/ + +#include "moteus_driver/YloTwoPcanToMoteus.hpp" + +YloTwoPcanToMoteus::YloTwoPcanToMoteus() +{ + + pcanPorts_.resize(4); // resize the pcanports_ vector to the number or real ports. + + // NOTE: we should load that from file + motor_adapters_.resize(12); // exact motors order, on Ylo2 + + // IDX SIGN PCAN BOARD PORTS + + /*HAA*/ motor_adapters_[0].setIdx(3); motor_adapters_[0].setSign(-1); motor_adapters_[0].setLeg_index(0); + motor_adapters_[0].setJoint_index(0); motor_adapters_[0].setPort(PCAN_DEV1); + + /*HFE*/ motor_adapters_[1].setIdx(1); motor_adapters_[1].setSign(1); motor_adapters_[1].setLeg_index(0); + motor_adapters_[1].setJoint_index(1); motor_adapters_[1].setPort(PCAN_DEV1); + + /*KFE*/ motor_adapters_[2].setIdx(2); motor_adapters_[2].setSign(1); motor_adapters_[2].setLeg_index(0); + motor_adapters_[2].setJoint_index(2); motor_adapters_[2].setPort(PCAN_DEV1); + + // LH + /*HAA*/ motor_adapters_[3].setIdx(9); motor_adapters_[3].setSign(1); motor_adapters_[3].setLeg_index(1); + motor_adapters_[3].setJoint_index(0); motor_adapters_[3].setPort(PCAN_DEV3); + + /*HFE*/ motor_adapters_[4].setIdx(7); motor_adapters_[4].setSign(1); motor_adapters_[4].setLeg_index(1); + motor_adapters_[4].setJoint_index(1); motor_adapters_[4].setPort(PCAN_DEV3); + + /*KFE*/ motor_adapters_[5].setIdx(8); motor_adapters_[5].setSign(1); motor_adapters_[5].setLeg_index(1); + motor_adapters_[5].setJoint_index(2); motor_adapters_[5].setPort(PCAN_DEV3); + + // RF + /*HAA*/ motor_adapters_[6].setIdx(6); motor_adapters_[6].setSign(1); motor_adapters_[6].setLeg_index(2); + motor_adapters_[6].setJoint_index(0); motor_adapters_[6].setPort(PCAN_DEV2); + + /*HFE*/ motor_adapters_[7].setIdx(4); motor_adapters_[7].setSign(-1); motor_adapters_[7].setLeg_index(2); + motor_adapters_[7].setJoint_index(1); motor_adapters_[7].setPort(PCAN_DEV2); + + /*KFE*/ motor_adapters_[8].setIdx(5); motor_adapters_[8].setSign(-1); motor_adapters_[8].setLeg_index(2); + motor_adapters_[8].setJoint_index(2); motor_adapters_[8].setPort(PCAN_DEV2); + + // RH + /*HAA*/ motor_adapters_[9].setIdx(12); motor_adapters_[9].setSign(-1); motor_adapters_[9].setLeg_index(3); + motor_adapters_[9].setJoint_index(0); motor_adapters_[9].setPort(PCAN_DEV4); + + /*HFE*/ motor_adapters_[10].setIdx(10); motor_adapters_[10].setSign(-1); motor_adapters_[10].setLeg_index(3); + motor_adapters_[10].setJoint_index(1); motor_adapters_[10].setPort(PCAN_DEV4); + + /*KFE*/ motor_adapters_[11].setIdx(11); motor_adapters_[11].setSign(-1); motor_adapters_[11].setLeg_index(3); + motor_adapters_[11].setJoint_index(2); motor_adapters_[11].setPort(PCAN_DEV4); + + // The default ID for the power_dist is '32' + motor_adapters_[32].setIdx(32); motor_adapters_[32].setPort(PCAN_DEV3); + + + /* ------------------------ TX STOP PACKAGE ------------------------------ */ + _stop.ID = 0x00; + _stop.MSGTYPE = PCAN_MESSAGE_BRS | PCAN_MESSAGE_EXTENDED | PCAN_MESSAGE_FD; + _stop.DLC = 7; + _stop.DATA[0] = 0x01; // Write uint8 (0x00) | Write 1 register (0x01) + _stop.DATA[1] = 0x00; // Register to write: MODE + _stop.DATA[2] = 0x00; // Value to write: STOPPED MODE + _stop.DATA[3] = 0x1F; // Read floats (0x1C) | Read 3 registers (0x03) + _stop.DATA[4] = 0x01; // Starting register: POSITION, VELOCITY, TORQUE + _stop.DATA[5] = 0x1F; // Read floats (0x1C) | Read 3 registers (0x03) + _stop.DATA[6] = 0x0D; // Starting register: VOLTAGE, TEMPERATURE, FAULT + + /* ------------------------- TX ZERO PACKAGE ------------------------------ */ + _zero.ID = 0x00; + _zero.MSGTYPE = PCAN_MESSAGE_BRS | PCAN_MESSAGE_EXTENDED | PCAN_MESSAGE_FD; + _zero.DLC = 9; // 12 bytes ... ex : 0db0029a99193e for a zero pos = 0.15 + _zero.DATA[0] = 0x0D; // write float (0x0C) | write 1 register (0x01) + _zero.DATA[1] = 0xB0; // Register to write: 0x130(REZERO) + _zero.DATA[2] = 0x02; + _zero.DATA[7] = 0x1F; // Read floats (0x1C) | Read 3 registers (0x03) + _zero.DATA[8] = 0x01; // Starting register: POSITION, VELOCITY, TORQUE + _zero.DATA[9] = 0x1F; // Read floats (0x1C) | Read 3 registers (0x03) + _zero.DATA[10] = 0x0D;// Starting register: VOLTAGE, TEMPERATURE, FAULT + _zero.DATA[11] = 0x50;// pad unused bytes to 0x50 + +/* --------------------------TX POS PACKAGE -------------------------------*/ + moteus_tx_msg.ID = 0x00; + moteus_tx_msg.MSGTYPE = PCAN_MESSAGE_BRS | PCAN_MESSAGE_EXTENDED | PCAN_MESSAGE_FD; + moteus_tx_msg.DLC = 14; // 12 = 24 bytes, 13 = 32 bytes, 14 = 48 bytes, 15 = 64 bytes + moteus_tx_msg.DATA[0] = 0x01; // WRITE_REGISTERS - Type.INT8 1 registers + moteus_tx_msg.DATA[1] = 0x00; // Starting at reg 0x000(MODE) + moteus_tx_msg.DATA[2] = 0x0A; // Reg 0x000(MODE) = 10(POSITION) + + moteus_tx_msg.DATA[3] = 0x0C; // WRITE_REGISTERS - Type FLOAT + moteus_tx_msg.DATA[4] = 0x0A; // 5 registers + moteus_tx_msg.DATA[5] = 0x20; // Starting at reg 0x020 POSITION + moteus_tx_msg.DATA[6] = _comm_position; // FLOAT + moteus_tx_msg.DATA[10] = _comm_velocity; + moteus_tx_msg.DATA[14] = _comm_fftorque; + moteus_tx_msg.DATA[18] = _comm_kp; + moteus_tx_msg.DATA[22] = _comm_kd; + moteus_tx_msg.DATA[26] = _comm_maxtorque; + moteus_tx_msg.DATA[30] = _comm_stop_mode; + moteus_tx_msg.DATA[34] = _comm_watchdog_timeout; + moteus_tx_msg.DATA[38] = _comm_target_vel; + moteus_tx_msg.DATA[42] = _comm_target_acc; + + moteus_tx_msg.DATA[46] = 0x1F; // READ_REGISTERS - FLOAT + moteus_tx_msg.DATA[47] = 0x01; // Starting at reg 0x001(POSITION) + moteus_tx_msg.DATA[48] = 0x1F; // READ_REGISTERS - FLOAT + moteus_tx_msg.DATA[49] = 0x0D; // Starting at reg 0x00d(VOLTAGE) + moteus_tx_msg.DATA[50] = 0x50; // Padding a NOP byte (moteus protocol) + moteus_tx_msg.DATA[51] = 0x50; + moteus_tx_msg.DATA[52] = 0x50; + moteus_tx_msg.DATA[53] = 0x50; + moteus_tx_msg.DATA[54] = 0x50; + moteus_tx_msg.DATA[55] = 0x50; + moteus_tx_msg.DATA[56] = 0x50; + moteus_tx_msg.DATA[57] = 0x50; + moteus_tx_msg.DATA[58] = 0x50; + moteus_tx_msg.DATA[59] = 0x50; + moteus_tx_msg.DATA[60] = 0x50; + moteus_tx_msg.DATA[61] = 0x50; + moteus_tx_msg.DATA[62] = 0x50; + moteus_tx_msg.DATA[63] = 0x50; + + /* ------------------- TX POWER BOARD PACKAGE -----------------------------*/ + _power_board_tx_msg.ID = 0x00; + _power_board_tx_msg.MSGTYPE = PCAN_MESSAGE_BRS | PCAN_MESSAGE_EXTENDED | PCAN_MESSAGE_FD; + _power_board_tx_msg.DLC = 8; // 8 = 8 bytes; 9 = 12 bytes + _power_board_tx_msg.DATA[0] = 0x05; // write 1 int8 register + _power_board_tx_msg.DATA[1] = 0x03; // register 3 = Lock Time + _power_board_tx_msg.DATA[2] = 0x4E; // timing 20s + _power_board_tx_msg.DATA[3] = 0x20; + _power_board_tx_msg.DATA[4] = 0x17; // READ_REGISTERS - INT16, 3 registers + _power_board_tx_msg.DATA[5] = 0x00; // Starting register: 0x000 STATE, FAULT CODE, SWITCH STATUS + _power_board_tx_msg.DATA[6] = 0x17; // READ_REGISTERS - INT16, 3 registers + _power_board_tx_msg.DATA[7] = 0x10; // Starting register: 0x010 Output Voltage, Output Current, Temperature + + //-------------------------------------------------------------------------------------------- +} + +YloTwoPcanToMoteus::~YloTwoPcanToMoteus() +{ +} + +sensor_msgs::Imu YloTwoPcanToMoteus::ylo3_imu; + +bool YloTwoPcanToMoteus::security_switch(){ // read Gpio port state. + if (mraa_gpio_read(YloTwoPcanToMoteus::btnPin) == -1){ + ROS_INFO("ERROR IN MRAA LIB WITH GPIO !!! \n---> See YloTwoPcanToMoteus.cpp into security_switch function."); + return true; // GPIO board not ready or button pressed + } + if (mraa_gpio_read(YloTwoPcanToMoteus::btnPin) == 1){ + //ROS_INFO("Working OK."); + return true; + } + _comm_maxtorque = 0.0; + ROS_INFO("SECUTITY SWITCH PRESSED !!! \n---> Motors are stopped now !!!."); + return false; +} + + +bool YloTwoPcanToMoteus::Can_reset(){ + for (unsigned int p = 0; p < 4; ++p){ + //reset ports + Status = CAN_Reset(pcanPorts_[p]); + CAN_GetErrorText(Status, 0, strMsg); + if(Status){std::cout << "Error: can't reset_buffer. " << pcanPorts_[p] << " port. Status = " << strMsg << std::endl; + return(false);} + } + return(true); +} + + +bool YloTwoPcanToMoteus::Can_init(){ + for (unsigned int p = 0; p < 4; ++p){ + // open ports + do{ Status = CAN_InitializeFD(pcanPorts_[p], BitrateFD); usleep(10); } + while(Status != PCAN_ERROR_OK); + } + return(true); +} + + +void YloTwoPcanToMoteus::subscribeToImuData() { + ros::NodeHandle nh; + ros::Subscriber imu_sub = nh.subscribe("/imu/data", 1, &YloTwoPcanToMoteus::imuCallback, this); + ros::Rate rate(100); // 100 hz + while (ros::ok()) { + ros::spinOnce(); + rate.sleep(); + } +} + + +void YloTwoPcanToMoteus::imuCallback(const sensor_msgs::Imu::ConstPtr& imu_msg) { + ylo3_imu = *imu_msg; +} + + +sensor_msgs::Imu YloTwoPcanToMoteus::getYlo3Imu() { + return ylo3_imu; +} + + +bool YloTwoPcanToMoteus::send_moteus_stop_order(int id, int port){ + _stop.ID = 0x8000 | id; + Status = CAN_WriteFD(port, &_stop); + usleep(100); + CAN_GetErrorText(Status, 0, strMsg); + if(Status != PCAN_ERROR_OK){std::cout << "Error: can't stop motor " << id << " Status = " << strMsg << std::endl; + return(false);} + return(true); +} + +// send a position order, using stop replacement/update mode, with vel and acc targets. +bool YloTwoPcanToMoteus::send_to_moteus(int id, int port, float pos, float vel, float fftorque, float kp, float kd, float target_vel, float target_accel){ + _comm_position = pos; + _comm_velocity = NAN; + _comm_fftorque = fftorque; // in radians + _comm_kp = kp; + _comm_kd = kd; + _comm_stop_mode = NAN; + _comm_watchdog_timeout = 10; // new mode : https://jpieper.com/2023/08/22/new-hold-position-watchdog-timeout-mode-for-moteus/ + _comm_target_vel = target_vel; + _comm_target_acc = target_accel; + + moteus_tx_msg.ID = 0x8000 | id; + memcpy(&moteus_tx_msg.DATA[MSGTX_ADDR_POSITION], &_comm_position, sizeof(float)); + memcpy(&moteus_tx_msg.DATA[MSGTX_ADDR_VELOCITY], &_comm_velocity, sizeof(float)); + memcpy(&moteus_tx_msg.DATA[MSGTX_ADDR_FFTORQUE], &_comm_fftorque, sizeof(float)); + memcpy(&moteus_tx_msg.DATA[MSGTX_ADDR_KP], &_comm_kp, sizeof(float)); + memcpy(&moteus_tx_msg.DATA[MSGTX_ADDR_KD], &_comm_kd, sizeof(float)); + + memcpy(&moteus_tx_msg.DATA[MSGTX_ADDR_MAXTORQUE], &_comm_maxtorque, sizeof(float)); + memcpy(&moteus_tx_msg.DATA[MSGTX_ADDR_STOP_MODE], &_comm_stop_mode, sizeof(float)); + memcpy(&moteus_tx_msg.DATA[MSGTX_ADDR_WATCHDOG_], &_comm_watchdog_timeout, sizeof(float)); + memcpy(&moteus_tx_msg.DATA[MSGTX_ADDR_TARG_VEL], &_comm_target_vel, sizeof(float)); + memcpy(&moteus_tx_msg.DATA[MSGTX_ADDR_TARG_ACC], &_comm_target_acc, sizeof(float)); + + //std::cout << "write" << std::endl; + //std::copy(std::begin(moteus_tx_msg.DATA), std::end(moteus_tx_msg.DATA), std::ostream_iterator(std::cout, " ")); + //std::cout << "" << std::endl; + + do{ Status = CAN_WriteFD(port, &moteus_tx_msg);} + while(Status != PCAN_ERROR_OK); + return(true); +} + +// send a position order, using stop replacement/update mode, with vel and acc targets. +bool YloTwoPcanToMoteus::send_to_moteus(int id, int port, float pos, float vel, float fftorque, float kp, float kd){ + _comm_position = pos; + _comm_velocity = vel; + _comm_fftorque = fftorque; // in radians + _comm_kp = kp; + _comm_kd = kd; + _comm_stop_mode = NAN; + _comm_watchdog_timeout = 10; + _comm_target_vel = NAN; + _comm_target_acc = NAN; + moteus_tx_msg.ID = 0x8000 | id; + + memcpy(&moteus_tx_msg.DATA[MSGTX_ADDR_POSITION], &_comm_position, sizeof(float)); + memcpy(&moteus_tx_msg.DATA[MSGTX_ADDR_VELOCITY], &_comm_velocity, sizeof(float)); + memcpy(&moteus_tx_msg.DATA[MSGTX_ADDR_FFTORQUE], &_comm_fftorque, sizeof(float)); + memcpy(&moteus_tx_msg.DATA[MSGTX_ADDR_KP], &_comm_kp, sizeof(float)); + memcpy(&moteus_tx_msg.DATA[MSGTX_ADDR_KD], &_comm_kd, sizeof(float)); + memcpy(&moteus_tx_msg.DATA[MSGTX_ADDR_MAXTORQUE], &_comm_maxtorque, sizeof(float)); + memcpy(&moteus_tx_msg.DATA[MSGTX_ADDR_STOP_MODE], &_comm_stop_mode, sizeof(float)); + memcpy(&moteus_tx_msg.DATA[MSGTX_ADDR_WATCHDOG_], &_comm_watchdog_timeout, sizeof(float)); + memcpy(&moteus_tx_msg.DATA[MSGTX_ADDR_TARG_VEL], &_comm_target_vel, sizeof(float)); + memcpy(&moteus_tx_msg.DATA[MSGTX_ADDR_TARG_ACC], &_comm_target_acc, sizeof(float)); + + do{ Status = CAN_WriteFD(port, &moteus_tx_msg);} + while(Status != PCAN_ERROR_OK); + return(true); +} + +bool YloTwoPcanToMoteus::read_moteus_RX_queue(int id, int port, float& position, float& velocity, float& torque, float& voltage, float& temperature, float& fault){ + moteus_rx_msg.ID = 0x8000 | id; + + do{ Status = CAN_ReadFD(port,&moteus_rx_msg, NULL); + usleep(10); } + while(Status != PCAN_ERROR_OK); // is return frame received ? + + if(Status != PCAN_ERROR_QRCVEMPTY){ // rx queue feeded. + memcpy(&_position, &moteus_rx_msg.DATA[MSGRX_ADDR_POSITION], sizeof(float)); + memcpy(&_velocity, &moteus_rx_msg.DATA[MSGRX_ADDR_VELOCITY], sizeof(float)); + memcpy(&_torque, &moteus_rx_msg.DATA[MSGRX_ADDR_TORQUE], sizeof(float)); + memcpy(&_voltage, &moteus_rx_msg.DATA[MSGRX_ADDR_VOLTAGE], sizeof(int8_t)); + memcpy(&_temperature, &moteus_rx_msg.DATA[MSGRX_ADDR_TEMPERATURE], sizeof(int8_t)); + memcpy(&_fault, &moteus_rx_msg.DATA[MSGRX_ADDR_FAULT], sizeof(int8_t)); + position = _position; + velocity = _velocity; + torque = _torque; + voltage = _voltage; + temperature = _temperature; + fault = _fault; + + //std::cout << "read" << std::endl; + //std::copy(std::begin(moteus_tx_msg.DATA), std::end(moteus_tx_msg.DATA), std::ostream_iterator(std::cout, " ")); + //std::cout << "" << std::endl; + + return true;} + else{ + std::cout << "### RX queue is empty for ID " << id << "." << std::endl; + return false; + } +} + +bool YloTwoPcanToMoteus::read_moteus_RX_queue(int id, int port, float& voltage, float& temperature, float& fault){ + moteus_rx_msg.ID = 0x8000 | id; + + do{ Status = CAN_ReadFD(port,&moteus_rx_msg, NULL); + usleep(10); } + while(Status != PCAN_ERROR_OK); // is return frame received ? + + if(Status != PCAN_ERROR_QRCVEMPTY){ // rx queue feeded. + memcpy(&_voltage, &moteus_rx_msg.DATA[MSGRX_ADDR_VOLTAGE], sizeof(int8_t)); + memcpy(&_temperature, &moteus_rx_msg.DATA[MSGRX_ADDR_TEMPERATURE], sizeof(int8_t)); + memcpy(&_fault, &moteus_rx_msg.DATA[MSGRX_ADDR_FAULT], sizeof(int8_t)); + voltage = _voltage; + temperature = _temperature; + fault = _fault; + + //std::cout << "----------> RX frame: " << id << std::endl; + //std::copy(std::begin(moteus_rx_msg.DATA), std::end(moteus_rx_msg.DATA), std::ostream_iterator(std::cout, " ")); + //std::cout << "" << std::endl; + // a décrypter avec : /home/ylo2/Documents/decode_Moteus_can_frame.py + + return true;} + else{ + std::cout << "### RX queue is empty for ID " << id << "." << std::endl; + return false; + } +} + +/* ZERO - Set Output Nearest + When sent, this causes the servo to select a whole number of internal motor rotations + so that the final position is as close to the given position as possible*/ +bool YloTwoPcanToMoteus::send_moteus_zero_order(int id, int port, float zero_position){ + _zero.ID = 0x8000 | id; + _comm_position = zero_position; + memcpy(&_zero.DATA[3], &_comm_position, sizeof(float)); + Status = CAN_WriteFD(port,&_zero); + usleep(200); + CAN_GetErrorText(Status, 0, strMsg); + if(Status == PCAN_ERROR_OK){ + return true;} + else{ + ROS_INFO("--ERROR IN WRITING : send_moteus_zero_order()--"); + return false;} +} + +/* POWER BOARD */ +/* WRITE */ +bool YloTwoPcanToMoteus::send_power_board_order(){ + int ids = 32; + int port = PCAN_DEV3; + _power_board_tx_msg.ID = 0x8000 | ids; + //std::copy(std::begin(_power_board_tx_msg.DATA), std::end(_power_board_tx_msg.DATA), std::ostream_iterator(std::cout, " ")); + //std::cout << "" << std::endl; + Status = CAN_WriteFD(port, &_power_board_tx_msg); + usleep(200); + CAN_GetErrorText(Status, 0, strMsg); + if(Status == PCAN_ERROR_OK){ + return(true);} + else{ + std::cout << "error into send_power_board_order(). Status = " << strMsg << std::endl; + return false;} +} + + +/* READ TODO */ +bool YloTwoPcanToMoteus::read_power_board_RX_queue(float& state, float& fault_code, float& switch_status, float& out_volt, float& out_curr, float& board_temp){ + int ids = 32; + int port = PCAN_DEV3; + _power_board_rx_msg.ID = 0x8000 | ids; + Status = CAN_ReadFD(port,&_power_board_rx_msg, NULL); // read can port + usleep(50); + //std::copy(std::begin(_power_board_rx_msg.DATA), std::end(_power_board_rx_msg.DATA), std::ostream_iterator(std::cout, " ")); + //std::cout << "" << std::endl; + CAN_GetErrorText(Status, 0, strMsg); + std::cout << "status " << Status << std::endl; + if(Status != PCAN_ERROR_QRCVEMPTY){ // rx queue feeded. + + //memcpy(&_state, &_power_board_rx_msg.DATA[MSGPBRX_ADDR_STATE], sizeof(float)); + //memcpy(&_fault_code, &_power_board_rx_msg.DATA[MSGPBRX_ADDR_FAULT_CODE], sizeof(float)); + //memcpy(&_switch_status, &_power_board_rx_msg.DATA[MSGPBRX_ADDR_SWITCH_STATUS], sizeof(float)); + memcpy(&_out_volt, &_power_board_rx_msg.DATA[MSGPBRX_ADDR_OUT_VOLTAGE], sizeof(float)); + memcpy(&_out_curr, &_power_board_rx_msg.DATA[MSGPBRX_ADDR_OUT_CURRENT], sizeof(float)); + memcpy(&_board_temp, &_power_board_rx_msg.DATA[MSGPBRX_ADDR_TEMPERATURE], sizeof(float)); + state = _state; + fault_code = _fault_code; + switch_status = _switch_status; + out_volt = _out_volt; + out_curr = _out_curr; + board_temp = _board_temp; + + return true; + } + else { + std::cout << "error into read_power_board_RX_queue(). Status = " << strMsg << std::endl; + return false; } +} + +bool YloTwoPcanToMoteus::peak_fdcan_board_initialization(){ + if(!YloTwoPcanToMoteus::Can_init()){ // run and check the return of the function + all_moteus_controllers_ok = false; + ROS_INFO("--PEAK BOARD ERROR - can't send Initialization frame to can port--"); + return false;} + + usleep(200); + + if(!YloTwoPcanToMoteus::Can_reset()){ + all_moteus_controllers_ok = false; + ROS_INFO("--PEAK BOARD ERROR - can't send reset frame to can port--"); + return false;} + + usleep(200); + + ROS_INFO("--PEAK INITIALIZATION AND RESET---> OK--"); + stop_motors(); + usleep(200); + + all_moteus_controllers_ok = true; + return true; +} + +bool YloTwoPcanToMoteus::stop_motors(){ + for(unsigned int i=0; i<12; ++i){ + auto ids = YloTwoPcanToMoteus::motor_adapters_[i].getIdx(); + int port = YloTwoPcanToMoteus::motor_adapters_[i].getPort(); + // send a stop order, to avoid damages, and query its values. + if(!YloTwoPcanToMoteus::send_moteus_stop_order(ids, port)){ + all_moteus_controllers_ok = false; + ROS_INFO("-- PEAK BOARD ERROR - can't send Stop_command to id %d --", ids); + return false;} + } + all_moteus_controllers_ok = true; + ROS_INFO("--CONTROLLERS STOPPED & RESET ----> OK--"); + usleep(200); + return true; + +} + +bool YloTwoPcanToMoteus::check_initial_ground_pose(){ + // startup. + int count = 0; // check zero for all 12 motors + while(count != 12){ + // --- LOOPING WITH THE 12 MOTORS UNTIL SUCCESS--- + count = 0; + for(unsigned int i=0; i<12; ++i){ + auto ids = YloTwoPcanToMoteus::motor_adapters_[i].getIdx(); + int port = YloTwoPcanToMoteus::motor_adapters_[i].getPort(); + auto target_joint_position = sit_down_joints_pose[i]; + + // --- SENDING ZERO COMMAND --- + if(!YloTwoPcanToMoteus::send_moteus_zero_order(ids, port, target_joint_position)){ + ROS_INFO("--- Error in send_moteus_zero_order() process. ---"); + can_error = true; + return false;} + usleep(200); + + // --- QUERYING VALUES --- + if(!YloTwoPcanToMoteus::read_moteus_RX_queue(ids, port, RX_pos, RX_vel, RX_tor, RX_volt, RX_temp, RX_fault)){ + ROS_INFO("--- Error in read_moteus_RX_queue() process, on id %d. ---", ids); + can_error = true; + return false;} + usleep(200); + + // --- CHECKING JOINT STARTUP ANGLE --- + float diff = std::abs(std::abs(RX_pos) - std::abs(target_joint_position)); + if(diff > std::abs(calibration_error)){ + ROS_INFO("--Ylo2 joint ID : %d ; pos : %f ; target : %f ; diff : %f", ids, RX_pos, target_joint_position, diff); + is_calibrated = false; + count -=1; + } + usleep(200); + count +=1; + } + usleep(200); + } + + ROS_INFO("--ROBOT ZERO CALIBRATION ---------> OK--"); + std::cout << ("") << std::endl; + return true; +} diff --git a/robot_driver/robot_driver.yaml b/robot_driver/robot_driver.yaml index 56d889a99..48e0ad958 100644 --- a/robot_driver/robot_driver.yaml +++ b/robot_driver/robot_driver.yaml @@ -19,3 +19,23 @@ robot_driver: high_pass_b: [0.062500000000000, 0] high_pass_c: [0.031912805108981, -0.028979795079613] high_pass_d: [0.0009996017974875904] + + # Extended Kalman filter estimator coefficients + joint_state_max_time: 20 # Milliseconds until we disregard last joint state message + update_rate: 200 # 200 in the original, reduced to 100 for testing + bias_x: 0.10 # actual measurement of bias_acc x is 0.083616044 + bias_y: -0.156 # actual measurement of bias_acc y is 0.060205998 + bias_z: 0.0 # actual measurement of bias_acc z is 0.00 + bias_r: 0.00000 # actual measurement of bias_gyro r is 0.00009 + bias_p: 0.00000 # actual measurement of bias_gyro p is 0.00003 + bias_w: 0.00000 # actual measurement of bias_gyro y is 0.00002 + na: 0.01 # noise in accelerometer 0.01 + ng: 0.01 # noise in gyro 0.01 + ba: 0.01 # bias in accelerometer 0.01 + bg: 0.001 # bias in gyro 0.001 + nf: 0.001 # noise in feet 0.001 + nfk: 0.001 # noise in forward kinematics 0.001 + ne: 0.001 # noise in encoder 0.001 + P0: 3 # Initial Value of Covariance Matrix 3 + contact_w: 1000 # Contact weight 1000 + thresh_out: 1000 #Norm Measurement Rejection Threshold, (Check for Outliers) diff --git a/robot_driver/setup_deps.sh b/robot_driver/setup_deps.sh index e69de29bb..ce8048aa1 100755 --- a/robot_driver/setup_deps.sh +++ b/robot_driver/setup_deps.sh @@ -0,0 +1,2 @@ +sudo apt install -y ros-noetic-gazebo-ros-pkgs -y + diff --git a/robot_driver/src/controllers/inertia_estimation_controller.cpp b/robot_driver/src/controllers/inertia_estimation_controller.cpp new file mode 100644 index 000000000..25a8969d0 --- /dev/null +++ b/robot_driver/src/controllers/inertia_estimation_controller.cpp @@ -0,0 +1,166 @@ +#include "robot_driver/controllers/inertia_estimation_controller.h" + +InertiaEstimationController::InertiaEstimationController() {} + +bool InertiaEstimationController::computeLegCommandArray( + const quad_msgs::RobotState &robot_state_msg, + quad_msgs::LegCommandArray &leg_command_array_msg, + quad_msgs::GRFArray &grf_array_msg) { + if ((last_local_plan_msg_ == NULL) || + ((ros::Time::now() - last_local_plan_msg_->header.stamp).toSec() >= + 0.1)) { + return false; + } else { + leg_command_array_msg.leg_commands.resize(num_feet_); + + // Define vectors for joint positions and velocities + Eigen::VectorXd joint_positions(3 * num_feet_), + joint_velocities(3 * num_feet_), foot_positions(3 * num_feet_), + foot_velocities(3 * num_feet_), body_state(12); + quad_utils::vectorToEigen(robot_state_msg.joints.position, joint_positions); + quad_utils::vectorToEigen(robot_state_msg.joints.velocity, + joint_velocities); + quad_utils::multiFootStateMsgToEigen(robot_state_msg.feet, foot_positions, + foot_velocities); + body_state = quad_utils::bodyStateMsgToEigen(robot_state_msg.body); + + // Define vectors for state positions and velocities + Eigen::VectorXd state_positions(3 * num_feet_ + 6), + state_velocities(3 * num_feet_ + 6); + state_positions << joint_positions, body_state.head(6); + state_velocities << joint_velocities, body_state.tail(6); + + // Initialize variables for ff and fb + Eigen::VectorXd tau_array(3 * num_feet_), + tau_swing_leg_array(3 * num_feet_); + + // Get reference state and grf from local plan or traj + grf messages + ros::Time t_first_state = last_local_plan_msg_->states.front().header.stamp; + double t_now = (ros::Time::now() - last_local_plan_msg_->state_timestamp) + .toSec(); // Use time of state - RECOMMENDED + // double t_now = (ros::Time::now() - last_local_plan_time_).toSec(); // Use + // time of plan receipt double t_now = (ros::Time::now() - + // t_first_state).toSec(); // Use time of first state in plan + + if ((t_now < + (last_local_plan_msg_->states.front().header.stamp - t_first_state) + .toSec()) || + (t_now > + (last_local_plan_msg_->states.back().header.stamp - t_first_state) + .toSec())) { + ROS_ERROR("ID node couldn't find the correct ref state!"); + } + + // Interpolate the local plan to get the reference state and ff GRF + for (int i = 0; i < last_local_plan_msg_->states.size() - 1; i++) { + if ((t_now >= + (last_local_plan_msg_->states[i].header.stamp - t_first_state) + .toSec()) && + (t_now < + (last_local_plan_msg_->states[i + 1].header.stamp - t_first_state) + .toSec())) { + double t_interp = + (t_now - + (last_local_plan_msg_->states[i].header.stamp - t_first_state) + .toSec()) / + (last_local_plan_msg_->states[i + 1].header.stamp.toSec() - + last_local_plan_msg_->states[i].header.stamp.toSec()); + + // Linearly interpolate between states + quad_utils::interpRobotState(last_local_plan_msg_->states[i], + last_local_plan_msg_->states[i + 1], + t_interp, ref_state_msg_); + + // ZOH on GRFs + grf_array_msg = last_local_plan_msg_->grfs[i]; + + break; + } + } + + // Declare plan and state data as Eigen vectors + Eigen::VectorXd ref_body_state(12), grf_array(3 * num_feet_), + ref_foot_positions(3 * num_feet_), ref_foot_velocities(3 * num_feet_), + ref_foot_acceleration(3 * num_feet_); + + // Load plan and state data from messages + quad_utils::multiFootStateMsgToEigen( + ref_state_msg_.feet, ref_foot_positions, ref_foot_velocities, + ref_foot_acceleration); + grf_array = quad_utils::grfArrayMsgToEigen(grf_array_msg); + if (last_grf_array_.norm() >= 1e-3) { + grf_array = grf_exp_filter_const_ * grf_array.array() + + (1 - grf_exp_filter_const_) * last_grf_array_.array(); + quad_utils::eigenToGRFArrayMsg(grf_array, ref_state_msg_.feet, + grf_array_msg); + } + + // Load contact mode + std::vector contact_mode(num_feet_); + for (int i = 0; i < num_feet_; i++) { + contact_mode[i] = ref_state_msg_.feet.feet[i].contact; + } + + // Compute joint torques + quadKD_->computeInverseDynamics(state_positions, state_velocities, + ref_foot_acceleration, grf_array, + contact_mode, tau_array); + + // Convert gains to eigen vectors for easier math + Eigen::VectorXd swing_kp_cart_eig, swing_kd_cart_eig, + swing_cart_fb(3 * num_feet_); + quad_utils::vectorToEigen(swing_kp_cart_, swing_kp_cart_eig); + quad_utils::vectorToEigen(swing_kd_cart_, swing_kd_cart_eig); + + // Compute PD feedback in cartesian space + Eigen::MatrixXd jacobian(3 * num_feet_, state_positions.size()); + swing_cart_fb = swing_kp_cart_eig.replicate<4, 1>().cwiseProduct( + ref_foot_positions - foot_positions) + + swing_kd_cart_eig.replicate<4, 1>().cwiseProduct( + ref_foot_velocities - foot_velocities); + + // Transform PD into joint space + quadKD_->getJacobianBodyAngVel(state_positions, jacobian); + swing_cart_fb = + jacobian.block(0, 0, 3 * num_feet_, 3 * num_feet_).transpose() * + swing_cart_fb; + + double t = ros::Time::now().toSec(); + + for (int i = 0; i < num_feet_; ++i) { + leg_command_array_msg.leg_commands.at(i).motor_commands.resize(3); + + leg_command_array_msg.leg_commands.at(i) + .motor_commands.at(0) + .pos_setpoint = (0.2*sin(4*t) + +0.4*cos(15*sin(2.6*t)))*(sin(5.4*t) < 0.8) + +(sin(5.4*t) >= 0.8)*(0.5*sin(0.7*t)); + leg_command_array_msg.leg_commands.at(i) + .motor_commands.at(1) + .pos_setpoint = (-1.0*cos(6*t)+0.3 + -0.5*cos(20*sin(2.2*t)))*(sin(6.5*t) < 0.8) + +(sin(6.5*t) >= 0.8)*(sin(0.9*t)+0.5); + leg_command_array_msg.leg_commands.at(i) + .motor_commands.at(2) + .pos_setpoint = (-0.8*cos(10*t)+1.3 + -0.5*cos(25*sin(1.4*t)))*(sin(7*t) < 0.8) + +(sin(7*t) >= 0.8)*(0.7*sin(1.1*t)+0.7); + + for (int j = 0; j < 3; ++j) { + leg_command_array_msg.leg_commands.at(i) + .motor_commands.at(j) + .vel_setpoint = 0; + leg_command_array_msg.leg_commands.at(i) + .motor_commands.at(j) + .torque_ff = 0; + leg_command_array_msg.leg_commands.at(i).motor_commands.at(j).kp = + swing_kp_.at(j); + leg_command_array_msg.leg_commands.at(i).motor_commands.at(j).kd = + swing_kd_.at(j); + } + } + + last_grf_array_ = grf_array; + return true; + } +} diff --git a/robot_driver/src/controllers/underbrush_inverse_dynamics.cpp b/robot_driver/src/controllers/underbrush_inverse_dynamics.cpp new file mode 100644 index 000000000..dee51dbb8 --- /dev/null +++ b/robot_driver/src/controllers/underbrush_inverse_dynamics.cpp @@ -0,0 +1,422 @@ +#include "robot_driver/controllers/underbrush_inverse_dynamics.h" + +UnderbrushInverseDynamicsController::UnderbrushInverseDynamicsController() { + force_mode_ = {0, 0, 0, 0}; + last_mode_ = {0, 0, 0, 0}; + + double t_now = ros::Time::now().toSec(); + t_switch_ = {t_now, t_now, t_now, t_now}; + t_LO_ = {t_now, t_now, t_now, t_now}; + t_TD_ = {t_now, t_now, t_now, t_now}; +} + +void UnderbrushInverseDynamicsController::updateBodyForceEstimate( + const quad_msgs::BodyForceEstimate::ConstPtr &msg) { + last_body_force_estimate_msg_ = msg; +} + +void UnderbrushInverseDynamicsController::setUnderbrushParams( + double retract_vel, double tau_push, double tau_contact_start, + double tau_contact_end, double min_switch, double t_down, double t_up) { + retract_vel_ = retract_vel; + tau_push_ = tau_push; + tau_contact_start_ = tau_contact_start; + tau_contact_end_ = tau_contact_end; + min_switch_ = min_switch; + t_down_ = t_down; + t_up_ = t_up; +} + +bool UnderbrushInverseDynamicsController::computeLegCommandArray( + const quad_msgs::RobotState &robot_state_msg, + quad_msgs::LegCommandArray &leg_command_array_msg, + quad_msgs::GRFArray &grf_array_msg) { + if ((last_local_plan_msg_ == NULL || last_body_force_estimate_msg_ == NULL) || + ((ros::Time::now() - last_local_plan_msg_->header.stamp).toSec() >= + 0.1)) { + return false; + } else { + leg_command_array_msg.leg_commands.resize(num_feet_); + + // Define vectors for joint positions and velocities + Eigen::VectorXd joint_positions(3 * num_feet_), + joint_velocities(3 * num_feet_), foot_positions(3 * num_feet_), + foot_velocities(3 * num_feet_), body_state(12); + quad_utils::vectorToEigen(robot_state_msg.joints.position, joint_positions); + quad_utils::vectorToEigen(robot_state_msg.joints.velocity, + joint_velocities); + quad_utils::multiFootStateMsgToEigen(robot_state_msg.feet, foot_positions, + foot_velocities); + body_state = quad_utils::bodyStateMsgToEigen(robot_state_msg.body); + + // Define vectors for state positions and velocities + Eigen::VectorXd state_positions(3 * num_feet_ + 6), + state_velocities(3 * num_feet_ + 6); + state_positions << joint_positions, body_state.head(6); + state_velocities << joint_velocities, body_state.tail(6); + + // Initialize variables for ff and fb + quad_msgs::RobotState ref_underbrush_msg, ref_abad_msg; + Eigen::VectorXd tau_array(3 * num_feet_), + tau_swing_leg_array(3 * num_feet_); + + // Get reference state and grf from local plan or traj + grf messages + ros::Time t_first_state = last_local_plan_msg_->states.front().header.stamp; + double t_now = (ros::Time::now() - last_local_plan_msg_->state_timestamp) + .toSec(); // Use time of state - RECOMMENDED + double t_now2 = ros::Time::now().toSec(); + + if ((t_now < + (last_local_plan_msg_->states.front().header.stamp - t_first_state) + .toSec()) || + (t_now > + (last_local_plan_msg_->states.back().header.stamp - t_first_state) + .toSec())) { + ROS_ERROR("ID node couldn't find the correct ref state!"); + } + + int all_TD = 0; // end looping when all next touchdowns have been found + + // Interpolate the local plan to get the reference state and ff GRF + for (int i = 0; i < last_local_plan_msg_->states.size() - 1; i++) { + if ((t_now >= + (last_local_plan_msg_->states[i].header.stamp - t_first_state) + .toSec()) && + (t_now < + (last_local_plan_msg_->states[i + 1].header.stamp - t_first_state) + .toSec())) { + double t_interp = + (t_now - + (last_local_plan_msg_->states[i].header.stamp - t_first_state) + .toSec()) / + (last_local_plan_msg_->states[i + 1].header.stamp.toSec() - + last_local_plan_msg_->states[i].header.stamp.toSec()); + + // Linearly interpolate between states + quad_utils::interpRobotState(last_local_plan_msg_->states[i], + last_local_plan_msg_->states[i + 1], + t_interp, ref_state_msg_); + + // ref_state_msg = last_local_plan_msg_->states[i]; + grf_array_msg = last_local_plan_msg_->grfs[i]; + + // Don't switch immediately after beginning swing + for (int j = 0; j < num_feet_; j++) { + if (last_local_plan_msg_->states[i].feet.feet.at(j).contact && + !last_local_plan_msg_->states[i + 1].feet.feet.at(j).contact) { + // t_switch_.at(j) = + // last_local_plan_msg_->states[i+1].header.stamp.toSec(); + t_LO_.at(j) = + last_local_plan_msg_->states[i + 1].header.stamp.toSec(); + } + } + + // break; + + } else if (t_now < last_local_plan_msg_->states[i + 1] + .header.stamp.toSec()) { // find next touchdowns + if (all_TD == pow(2.0, num_feet_) - + 1) { // terminate when all next touchdowns are found + break; + } + for (int j = 0; j < num_feet_; j++) { + if (!(all_TD & (1 << j)) && + i > 0) { // touchdown not yet found for this foot + if (ref_state_msg_.feet.feet.at(j).contact) { + all_TD = + all_TD | + (1 << j); // foot is in stance, don't need next touchdown + } else { + if (last_local_plan_msg_->states[i].feet.feet.at(j).contact) { + t_TD_.at(j) = + last_local_plan_msg_->states[i].header.stamp.toSec(); + all_TD = all_TD | (1 << j); // next touchdown found + } + } + } + } + } + } + + double foot_x_err, foot_y_err, foot_z_err, foot_horz_err, foot_z_hip; + double foot_vx, foot_vy, foot_vz; + + // Underbrush swing leg motion + ref_underbrush_msg = ref_state_msg_; + for (int i = 0; i < 4; i++) { + if (!ref_state_msg_.feet.feet.at(i).contact) { + for (int j = 0; j < last_local_plan_msg_->states.size() - 1; j++) { + if (t_now < last_local_plan_msg_->states[j].header.stamp.toSec() && + bool(last_local_plan_msg_->states[j].feet.feet.at(i).contact)) { + ref_underbrush_msg.feet.feet.at(i).position.x = + robot_state_msg.feet.feet.at(i).position.x; + ref_underbrush_msg.feet.feet.at(i).position.y = + robot_state_msg.feet.feet.at(i).position.y; + ref_underbrush_msg.feet.feet.at(i).position.z = + robot_state_msg.feet.feet.at(i).position.z; + + // foot cartesian distances from desired footfall + foot_x_err = + last_local_plan_msg_->states[j].feet.feet.at(i).position.x - + robot_state_msg.feet.feet.at(i).position.x; + foot_y_err = + last_local_plan_msg_->states[j].feet.feet.at(i).position.y - + robot_state_msg.feet.feet.at(i).position.y; + foot_z_err = + last_local_plan_msg_->states[j].feet.feet.at(i).position.z - + robot_state_msg.feet.feet.at(i).position.z; + foot_z_hip = robot_state_msg.feet.feet.at(i).position.z - + robot_state_msg.body.pose.position.z; + foot_horz_err = foot_x_err; + + // cartesian foot velocity commands + foot_vx = 10.0 * foot_x_err; + foot_vx = + abs(foot_vx) > 4.0 ? (foot_vx > 0 ? 1 : -1) * 4.0 : foot_vx; + foot_vy = 10.0 * foot_y_err; + foot_vy = + abs(foot_vy) > 4.0 ? (foot_vy > 0 ? 1 : -1) * 4.0 : foot_vy; + foot_vz = 10.0 * foot_z_err; + if (foot_horz_err > 0.02 && t_TD_.at(i) - t_now2 > t_down_) { + // don't put the foot down unless it's close to the right x, y + // position or there's no time left + foot_vz = 1.0 / (100 * (foot_horz_err - 0.02) + 1) * foot_vz; + } + if (foot_z_hip > -0.05) { + // foot is too high above hip; singularity problems + foot_vx = 1 / (50 * (foot_z_hip + 0.05) + 1) * foot_vx; + foot_vy = 1 / (50 * (foot_z_hip + 0.05) + 1) * foot_vy; + foot_vz += -10.0 * (foot_z_hip + 0.05); + } + foot_vz = + abs(foot_vz) > 4.0 ? (foot_vz > 0 ? 1 : -1) * 4.0 : foot_vz; + + ref_underbrush_msg.feet.feet.at(i).velocity.x = foot_vx; + ref_underbrush_msg.feet.feet.at(i).velocity.y = foot_vy; + ref_underbrush_msg.feet.feet.at(i).velocity.z = foot_vz; + + ref_underbrush_msg.feet.feet.at(i).acceleration.x = 0; + ref_underbrush_msg.feet.feet.at(i).acceleration.y = 0; + ref_underbrush_msg.feet.feet.at(i).acceleration.z = 0; + + break; + } + } + } + } + quad_utils::ikRobotState(*quadKD_, ref_underbrush_msg); + + // Compute abad joint IK + ref_abad_msg = ref_underbrush_msg; + for (int i = 0; i < 4; i++) { + if (!ref_state_msg_.feet.feet.at(i).contact) { + for (int j = 0; j < last_local_plan_msg_->states.size() - 1; j++) { + if (t_now < last_local_plan_msg_->states[j].header.stamp.toSec() && + bool(last_local_plan_msg_->states[j].feet.feet.at(i).contact)) { + ref_abad_msg.feet.feet.at(i).position.x = + last_local_plan_msg_->states[j].feet.feet.at(i).position.x; + ref_abad_msg.feet.feet.at(i).position.y = + last_local_plan_msg_->states[j].feet.feet.at(i).position.y; + ref_abad_msg.feet.feet.at(i).position.z = + last_local_plan_msg_->states[j].feet.feet.at(i).position.z; + break; + } + } + } + } + quad_utils::ikRobotState(*quadKD_, ref_abad_msg); + for (int i = 0; i < num_feet_; ++i) { + if (!ref_state_msg_.feet.feet.at(i).contact) { + ref_underbrush_msg.joints.position.at(3 * i + 0) = + ref_abad_msg.joints.position.at(3 * i + 0); + ref_underbrush_msg.joints.velocity.at(3 * i + 0) = 0; + } + } + + for (int i = 0; i < num_feet_; ++i) { + // Limit the joint velocities computed by inverse kinematics + for (int j = 0; j < 3; ++j) { + if (ref_underbrush_msg.joints.velocity.at(3 * i + j) > retract_vel_) { + ref_underbrush_msg.joints.velocity.at(3 * i + j) = retract_vel_; + } + if (ref_underbrush_msg.joints.velocity.at(3 * i + j) < -retract_vel_) { + ref_underbrush_msg.joints.velocity.at(3 * i + j) = -retract_vel_; + } + } + + // Push the joints out of bad configurations + if (robot_state_msg.joints.position.at(3 * i + 2) < 0.3) { + ref_underbrush_msg.joints.velocity.at(3 * i + 2) += + -20 * (robot_state_msg.joints.position.at(3 * i + 2) - 0.3); + } + if (robot_state_msg.joints.position.at(3 * i + 1) < -0.5) { + ref_underbrush_msg.joints.velocity.at(3 * i + 1) += + -20 * (robot_state_msg.joints.position.at(3 * i + 1) + 0.5); + } + + // Limit the joint velocities again + for (int j = 0; j < 3; ++j) { + if (ref_underbrush_msg.joints.velocity.at(3 * i + j) > retract_vel_) { + ref_underbrush_msg.joints.velocity.at(3 * i + j) = retract_vel_; + } + if (ref_underbrush_msg.joints.velocity.at(3 * i + j) < -retract_vel_) { + ref_underbrush_msg.joints.velocity.at(3 * i + j) = -retract_vel_; + } + } + } + + ref_state_msg_ = ref_underbrush_msg; + + // Declare plan and state data as Eigen vectors + Eigen::VectorXd ref_body_state(12), grf_array(3 * num_feet_), + ref_foot_positions(3 * num_feet_), ref_foot_velocities(3 * num_feet_), + ref_foot_acceleration(3 * num_feet_); + + // Load plan and state data from messages + quad_utils::multiFootStateMsgToEigen( + ref_state_msg_.feet, ref_foot_positions, ref_foot_velocities, + ref_foot_acceleration); + grf_array = quad_utils::grfArrayMsgToEigen(grf_array_msg); + if (last_grf_array_.norm() >= 1e-3) { + grf_array = grf_exp_filter_const_ * grf_array.array() + + (1 - grf_exp_filter_const_) * last_grf_array_.array(); + quad_utils::eigenToGRFArrayMsg(grf_array, ref_state_msg_.feet, + grf_array_msg); + } + + // Load contact mode + std::vector contact_mode(num_feet_); + for (int i = 0; i < num_feet_; i++) { + contact_mode[i] = ref_state_msg_.feet.feet[i].contact; + } + + // Compute joint torques + quadKD_->computeInverseDynamics(state_positions, state_velocities, + ref_foot_acceleration, grf_array, + contact_mode, tau_array); + + for (int i = 0; i < num_feet_; ++i) { + leg_command_array_msg.leg_commands.at(i).motor_commands.resize(3); + + if (contact_mode[i]) { + // Stance phase + for (int j = 0; j < 3; ++j) { + int joint_idx = 3 * i + j; + + leg_command_array_msg.leg_commands.at(i) + .motor_commands.at(j) + .pos_setpoint = ref_state_msg_.joints.position.at(joint_idx); + leg_command_array_msg.leg_commands.at(i) + .motor_commands.at(j) + .vel_setpoint = ref_state_msg_.joints.velocity.at(joint_idx); + leg_command_array_msg.leg_commands.at(i) + .motor_commands.at(j) + .torque_ff = tau_array(joint_idx); + + leg_command_array_msg.leg_commands.at(i).motor_commands.at(j).kp = + stance_kp_.at(j); + leg_command_array_msg.leg_commands.at(i).motor_commands.at(j).kd = + stance_kd_.at(j); + } + } else { + // Swing phase + + // Switch swing modes + if (force_mode_.at(i) && (t_now2 - t_switch_.at(i) > min_switch_) && + (last_body_force_estimate_msg_->joint_torques.at(3 * i + 2) < + tau_contact_end_) && + t_TD_.at(i) - t_now2 >= t_down_) { + // leg is not obstructed anymore: stop retracting and extend + last_mode_.at(i) = 0; + force_mode_.at(i) = 0; + t_switch_.at(i) = t_now2; + } else if (!force_mode_.at(i) && + (t_now2 - t_switch_.at(i) > min_switch_) && + (last_body_force_estimate_msg_->joint_torques.at(3 * i + 2) > + tau_contact_start_ || + last_body_force_estimate_msg_->joint_torques.at(3 * i + 1) > + tau_contact_start_ && + t_now2 - t_LO_.at(i) > t_up_)) { + // leg is now obstructed: retract it over the obstruction + force_mode_.at(i) = 1; + t_switch_.at(i) = t_now2; + } + + if (force_mode_.at(i) && + t_TD_.at(i) - t_now2 < t_down_) { // insufficient time left in + // stance; put the foot down) + force_mode_.at(i) = 0; + t_switch_.at(i) = t_now2; + last_mode_.at(i) = 1; + } + + if (t_now2 - t_LO_.at(i) < t_up_) { + if (last_mode_.at(i)) { + force_mode_.at(i) = 1; // retain previous mode + t_switch_.at(i) = t_LO_.at(i); + } + } + + if (!force_mode_.at(i)) { + // Usual swing mode + for (int j = 0; j < 3; ++j) { + int joint_idx = 3 * i + j; + + leg_command_array_msg.leg_commands.at(i) + .motor_commands.at(j) + .pos_setpoint = ref_state_msg_.joints.position.at(joint_idx); + leg_command_array_msg.leg_commands.at(i) + .motor_commands.at(j) + .vel_setpoint = ref_state_msg_.joints.velocity.at(joint_idx); + leg_command_array_msg.leg_commands.at(i) + .motor_commands.at(j) + .torque_ff = 0; + + leg_command_array_msg.leg_commands.at(i).motor_commands.at(j).kp = + swing_kp_.at(j); + leg_command_array_msg.leg_commands.at(i).motor_commands.at(j).kd = + swing_kd_.at(j); + } + } else { + // Obstructed swing mode + for (int j = 0; j < 3; ++j) { + leg_command_array_msg.leg_commands.at(i) + .motor_commands.at(j) + .pos_setpoint = 0; + leg_command_array_msg.leg_commands.at(i) + .motor_commands.at(j) + .torque_ff = 0; + leg_command_array_msg.leg_commands.at(i).motor_commands.at(j).kp = + 0; + leg_command_array_msg.leg_commands.at(i).motor_commands.at(j).kd = + swing_kd_.at(j); + } + + leg_command_array_msg.leg_commands.at(i) + .motor_commands.at(0) + .pos_setpoint = ref_state_msg_.joints.position.at(3 * i + 0); + leg_command_array_msg.leg_commands.at(i) + .motor_commands.at(0) + .vel_setpoint = ref_state_msg_.joints.velocity.at(3 * i + 0); + leg_command_array_msg.leg_commands.at(i).motor_commands.at(0).kp = + swing_kp_.at(0); + + leg_command_array_msg.leg_commands.at(i) + .motor_commands.at(1) + .vel_setpoint = -retract_vel_; + + leg_command_array_msg.leg_commands.at(i) + .motor_commands.at(2) + .vel_setpoint = 0; + leg_command_array_msg.leg_commands.at(i).motor_commands.at(2).kd = 0; + leg_command_array_msg.leg_commands.at(i) + .motor_commands.at(2) + .torque_ff = -tau_push_; + } + } + } + + last_grf_array_ = grf_array; + return true; + } +} diff --git a/robot_driver/src/estimators/comp_filter_estimator.cpp b/robot_driver/src/estimators/comp_filter_estimator.cpp index cda8f96ce..71015702d 100644 --- a/robot_driver/src/estimators/comp_filter_estimator.cpp +++ b/robot_driver/src/estimators/comp_filter_estimator.cpp @@ -40,11 +40,11 @@ void CompFilterEstimator::init(ros::NodeHandle& nh) { bool CompFilterEstimator::updateOnce( quad_msgs::RobotState& last_robot_state_msg_) { ros::Time state_timestamp = ros::Time::now(); + ROS_INFO_STREAM("Timestamp" << state_timestamp.toSec()); last_robot_state_msg_.body.twist.angular = last_imu_msg_.angular_velocity; last_robot_state_msg_.joints = last_joint_state_msg_; last_joint_state_msg_.header.stamp = state_timestamp; last_imu_msg_.header.stamp = state_timestamp; - // Check if mocap data was received if (last_mocap_msg_ == NULL) { ROS_WARN_THROTTLE(1, "No body pose (mocap) recieved"); diff --git a/robot_driver/src/estimators/ekf_estimator.cpp b/robot_driver/src/estimators/ekf_estimator.cpp index 7ace6584b..15effe2b5 100644 --- a/robot_driver/src/estimators/ekf_estimator.cpp +++ b/robot_driver/src/estimators/ekf_estimator.cpp @@ -4,9 +4,578 @@ EKFEstimator::EKFEstimator() {} void EKFEstimator::init(ros::NodeHandle& nh) { nh_ = nh; - std::cout << "EKF Estimator Initiated" << std::endl; + + // Load rosparams from parameter server + std::string joint_encoder_topic, imu_topic, contact_topic, local_plan_topic, + grf_topic, state_estimate_topic, ground_truth_topic; + + nh_.param("topics/state/joints", joint_encoder_topic, + "/state/joints"); + nh_.param("topics/state/imu", imu_topic, "/state/imu"); + // nh_.param("topics/control/grfs", grf_topic, "/control/grfs"); + nh_.param("topics/state/estimate", state_estimate_topic, + "/state/estimate"); + + nh_.param("topics/contact_mode", contact_topic, "/contact_mode"); + // nh_.param("topic/state/ground_truth", ground_truth_topic, + // "/state/ground_truth"); + quad_utils::loadROSParam(nh_, "topics/state/ground_truth", + ground_truth_topic); + quad_utils::loadROSParam(nh_, "topics/control/grfs", grf_topic); + quad_utils::loadROSParam(nh_, "topics/local_plan", local_plan_topic); + // Load Update Rate, Joint State Time + quad_utils::loadROSParam(nh_, "/robot_driver/update_rate", update_rate_); + quad_utils::loadROSParam(nh_, "/robot_driver/joint_state_max_time", + joint_state_msg_time_diff_max_); + quad_utils::loadROSParamDefault(nh_, "robot_driver/is_hardware", is_hardware_, + true); + + // Load initial IMU bias from robot_driver yaml + quad_utils::loadROSParam(nh_, "/robot_driver/bias_x", bias_x_); + quad_utils::loadROSParam(nh_, "/robot_driver/bias_y", bias_y_); + quad_utils::loadROSParam(nh_, "/robot_driver/bias_z", bias_z_); + quad_utils::loadROSParam(nh_, "/robot_driver/bias_r", bias_r_); + quad_utils::loadROSParam(nh_, "/robot_driver/bias_p", bias_p_); + quad_utils::loadROSParam(nh_, "/robot_driver/bias_w", bias_w_); + + // Load noise terms from robot_driver yaml + quad_utils::loadROSParam(nh_, "/robot_driver/na", na_); + quad_utils::loadROSParam(nh_, "/robot_driver/ng", ng_); + quad_utils::loadROSParam(nh_, "/robot_driver/ba", ba_); + quad_utils::loadROSParam(nh_, "/robot_driver/bg", bg_); + quad_utils::loadROSParam(nh_, "/robot_driver/nf", nf_); + quad_utils::loadROSParam(nh_, "/robot_driver/nfk", nfk_); + quad_utils::loadROSParam(nh_, "/robot_driver/ne", ne_); + quad_utils::loadROSParam(nh_, "/robot_driver/P0", P0_); + quad_utils::loadROSParam(nh_, "/robot_driver/contact_w", contact_w_); + quad_utils::loadROSParam(nh_, "/robot_driver/thresh_out", thresh_out); + quad_utils::loadROSParamDefault(nh_, "robot_driver/is_hardware", is_hardware_, + true); + + // Setup subs + grf_sub_ = nh_.subscribe(grf_topic, 1, &EKFEstimator::grfCallback, this); + contact_sub_ = + nh_.subscribe(contact_topic, 1, &EKFEstimator::contactCallback, this); + local_plan_sub_ = + nh_.subscribe(local_plan_topic, 1, &EKFEstimator::localPlanCallback, this, + ros::TransportHints().tcpNoDelay(true)); + + // In Sim, Grab IMU, Joint Encoders from Gazebo + if (!is_hardware_) { + imu_sub_ = nh_.subscribe(imu_topic, 1, &EKFEstimator::imuCallback, this); + joint_encoder_sub_ = nh_.subscribe( + joint_encoder_topic, 1, &EKFEstimator::jointEncoderCallback, this); + state_ground_truth_sub_ = nh_.subscribe( + ground_truth_topic, 1, &EKFEstimator::groundtruthCallback, this); + } + + // QuadKD class + quadKD_ = std::make_shared(); + ROS_INFO_STREAM("Initialized EKF Estimator"); +} + +bool EKFEstimator::updateOnce(quad_msgs::RobotState& last_robot_state_msg_) { + // ROS_INFO_STREAM("Estiamted State before update:" << last_robot_state_msg_); + ros::Time state_timestamp = ros::Time::now(); + // std::cout << "Here"<< std::endl; + ROS_INFO_STREAM("State Timestamp" << state_timestamp.toSec()); + // std::cout << "After"<< std::endl; + // ROS_INFO_STREAM("State Timestamp" << state_timestamp); + if (is_hardware_) { + // ROS_INFO_STREAM("Makes it Here in Update Once"); + last_robot_state_msg_.joints = last_joint_state_msg_; + last_joint_state_msg_.header.stamp = state_timestamp; + last_imu_msg_.header.stamp = state_timestamp; + // ROS_INFO_STREAM("Populates Message"); + } + // std::cout << "1"<< std::endl; + // Define Initial State, Preallocated Space for State Vectors + X0 = Eigen::VectorXd::Zero(num_state); + + // set noise + this->setNoise(); + if (initialized) { + // std::cout << "4"<< std::endl; + setInitialState(last_robot_state_msg_); + // std::cout << "5"<< std::endl; + // ROS_INFO_STREAM(last_robot_state_msg_); + quad_utils::fkRobotState(*quadKD_, last_robot_state_msg_); + // std::cout << "6"<< std::endl; + quad_utils::updateStateHeaders(last_robot_state_msg_, state_timestamp, + "map", 0); + // std::cout << "7"<< std::endl; + } + + // ROS_INFO_STREAM(last_imu_msg_); + // Run Step Once to Calculate Change in State Once Local Planner Starts + if (last_local_plan_msg_ != nullptr) { + if (initialized) { + // Set Start Time on Initialization + last_time = ros::Time::now(); + setInitialState(last_robot_state_msg_); + // Initialize Filter + P = P0_ * Eigen::MatrixXd::Identity(num_cov, num_cov); + X0 << last_robot_state_msg_.body.pose.position.x, + last_robot_state_msg_.body.pose.position.y, + last_robot_state_msg_.body.pose.position.z, + last_robot_state_msg_.body.twist.linear.x, + last_robot_state_msg_.body.twist.linear.y, + last_robot_state_msg_.body.twist.linear.z, + last_robot_state_msg_.feet.feet[0].position.x, + last_robot_state_msg_.feet.feet[0].position.y, + last_robot_state_msg_.feet.feet[0].position.z, + last_robot_state_msg_.feet.feet[1].position.x, + last_robot_state_msg_.feet.feet[1].position.y, + last_robot_state_msg_.feet.feet[1].position.z, + last_robot_state_msg_.feet.feet[2].position.x, + last_robot_state_msg_.feet.feet[2].position.y, + last_robot_state_msg_.feet.feet[2].position.z, + last_robot_state_msg_.feet.feet[3].position.x, + last_robot_state_msg_.feet.feet[3].position.y, + last_robot_state_msg_.feet.feet[3].position.z; + X = X0; + X_pre = X0; + last_X = X0; + initialized = false; + } + // std::cout << "2"<< std::endl; + auto new_state_est = this->StepOnce(); + // std::cout << "3"<< std::endl; + last_robot_state_msg_ = new_state_est; + quad_utils::updateStateHeaders(last_robot_state_msg_, state_timestamp, + "map", 0); + // std::cout << "4"<< std::endl; + last_robot_state_msg_.joints = last_joint_state_msg_; + last_joint_state_msg_.header.stamp = state_timestamp; + // std::cout << "5"<< std::endl; + } + ROS_INFO_STREAM(last_robot_state_msg_); + return true; +} + +void EKFEstimator::setInitialState( + quad_msgs::RobotState& last_robot_state_msg_) { + // body + // Grab this Directly from the IMU + last_robot_state_msg_.body.pose.orientation.w = 1.0; + last_robot_state_msg_.body.pose.orientation.x = 0.0; + last_robot_state_msg_.body.pose.orientation.y = 0.0; + last_robot_state_msg_.body.pose.orientation.z = 0.0; + + last_robot_state_msg_.body.pose.position.x = 0.0; + last_robot_state_msg_.body.pose.position.y = 0.0; + last_robot_state_msg_.body.pose.position.z = 0.27; + + last_robot_state_msg_.body.twist.linear.x = 0; + last_robot_state_msg_.body.twist.linear.y = 0; + last_robot_state_msg_.body.twist.linear.z = 0; + + last_robot_state_msg_.body.twist.angular.x = 0; + last_robot_state_msg_.body.twist.angular.y = 0; + last_robot_state_msg_.body.twist.angular.z = 0; + + return; +} + +void EKFEstimator::groundtruthCallback( + const quad_msgs::RobotState::ConstPtr& msg) { + last_robot_state_msg_ = *msg; +} + +void EKFEstimator::jointEncoderCallback( + const sensor_msgs::JointState::ConstPtr& msg) { + last_joint_state_msg_ = *msg; +} + +void EKFEstimator::imuCallback(const sensor_msgs::Imu::ConstPtr& msg) { + last_imu_msg_ = *msg; +} + +void EKFEstimator::contactCallback( + const quad_msgs::ContactMode::ConstPtr& msg) { + last_contact_msg_ = msg; +} + +void EKFEstimator::grfCallback(const quad_msgs::GRFArray::ConstPtr& msg) { + last_grf_msg_ = msg; +} + +void EKFEstimator::localPlanCallback( + const quad_msgs::RobotPlan::ConstPtr& msg) { + last_local_plan_msg_ = msg; +} + +void EKFEstimator::setX(Eigen::VectorXd Xin) { X = Xin; } + +void EKFEstimator::setlast_X(Eigen::VectorXd Xin) { last_X = Xin; } + +void EKFEstimator::setP(Eigen::MatrixXd Pin) { P = Pin; } + +Eigen::VectorXd EKFEstimator::getX() { return X; } + +Eigen::VectorXd EKFEstimator::getlastX() { return last_X; } + +Eigen::VectorXd EKFEstimator::getX_pre() { return X_pre; } + +quad_msgs::RobotState EKFEstimator::StepOnce() { + // Record start time of function, used in verifying messages are not out of + // date and in timing function + ros::Time start_time = ros::Time::now(); + + // Create skeleton message to send out + quad_msgs::RobotState new_state_est; + + // calculate dt + double dt = (start_time - last_time).toSec(); + last_time = start_time; + // std::cout << "this is dt" << dt << std::endl; + + /// Collect and Process Data + // IMU reading linear acceleration + Eigen::VectorXd fk = Eigen::VectorXd::Zero(3); + // IMU reading angular acceleration + Eigen::VectorXd wk = Eigen::VectorXd::Zero(3); + // IMU orientation (w, x, y, z) + Eigen::Quaterniond qk(1, 0, 0, 0); + // if there is good imu data: read data from bag file + this->readIMU(last_imu_msg_, fk, wk, qk); + // qk.normalize(); + // Joint data reading 3 joints * 4 legs + Eigen::VectorXd jk = Eigen::VectorXd::Zero(12); + Eigen::VectorXd vk = Eigen::VectorXd::Zero(12); + this->readJointEncoder(last_joint_state_msg_, jk, vk); + std::vector jkVector(jk.data(), jk.data() + jk.rows() * jk.cols()); + /// Prediction Step + // std::cout << "this is X before" << X.transpose() << std::endl; + this->predict(dt, fk, wk, qk); + // std::cout << "this is X predict" << X_pre.transpose() << std::endl; + + // for testing prediction step + // X = X_pre; + // P = P_pre; + // last_X = X; + + /// Update Step + this->update(jk, fk, vk, wk); // Uncomment for Update Step + // std::cout << "this is X update" << X.transpose() << std::endl; + + // last_X = X; + + /// publish new message + new_state_est.header.stamp = ros::Time::now(); + + // body + // Grab this Directly from the IMU + new_state_est.body.pose.orientation.w = qk.w(); + new_state_est.body.pose.orientation.x = qk.x(); + new_state_est.body.pose.orientation.y = qk.y(); + new_state_est.body.pose.orientation.z = qk.z(); + + new_state_est.body.pose.position.x = X[0]; + new_state_est.body.pose.position.y = X[1]; + new_state_est.body.pose.position.z = X[2]; + + new_state_est.body.twist.linear.x = X[3]; + new_state_est.body.twist.linear.y = X[4]; + new_state_est.body.twist.linear.z = X[5]; + + // joint + new_state_est.joints.header.stamp = ros::Time::now(); + // '8', '0', '1', '9', '2', '3', '10', '4', '5', '11', '6', '7' + new_state_est.joints.name = {"8", "0", "1", "9", "2", "3", + "10", "4", "5", "11", "6", "7"}; + new_state_est.joints.position = jkVector; + new_state_est.joints.velocity = {0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0}; + new_state_est.joints.effort = {0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0}; + + // feet + + return new_state_est; +} + +void EKFEstimator::predict(const double& dt, const Eigen::VectorXd& fk, + const Eigen::VectorXd& wk, + const Eigen::Quaterniond& qk) { + // Generate Conversion Matrix to Rotate the Body Frame IMU into World Frame + // Double Check if this needs a transpose or not + // Eigen::Matrix3d C1 = (qk.toRotationMatrix()).transpose(); + C1 = (qk.toRotationMatrix()); + + // q = Eigen::VectorXd::Zero(4); + // q << qk.w(), qk.x(), qk.y(), qk.z(); + // // //get better C matrix: + // double angle = q.norm(); + // Eigen::Vector3d axis; + + // if (angle == 0) { + // axis = Eigen::VectorXd::Zero(3); + // } else { + // axis = q / angle; + // } + // Eigen::Vector3d q_xyz = sin(angle / 2) * axis; + // double q_w = cos(angle / 2); + // Eigen::Quaterniond q1(q_w, q_xyz[0], q_xyz[1], q_xyz[2]); + + // Eigen::Matrix3d C = q1.toRotationMatrix().transpose(); + + // Collect states info from previous state vector + // Segment Syntax .segment(start_index, number of values) + Eigen::VectorXd r = last_X.segment(0, 3); + Eigen::VectorXd v = last_X.segment(3, 3); + Eigen::VectorXd p = last_X.segment(10, 12); + // Generate Linearized Dynamics Matrix (18 x 18) + F = Eigen::MatrixXd::Identity(num_cov, num_cov); + F.block<3, 3>(0, 3) = dt * Eigen::MatrixXd::Identity(3, 3); + + // Generate a Process Prediction X (18 x 1) + X_pre = Eigen::VectorXd::Zero(num_state); + P_pre = Eigen::MatrixXd::Zero(num_state, num_state); + // Generate Estimation State Transition (18 x 3) + Eigen::MatrixXd B(num_state, 3); + B.setZero(); + B.block<3, 3>(0, 0) = + 0.5 * dt * dt * + Eigen::MatrixXd::Identity(3, 3); // Add acceleration into the process + B.block<3, 3>(3, 0) = dt * Eigen::MatrixXd::Identity(3, 3); + + // Generate Estimation State Transition Noise (18 x 18) + Q = Eigen::MatrixXd::Identity(num_state, num_state); + Q.block<3, 3>(0, 0) = na_ * dt / 20.0 * Eigen::MatrixXd::Identity(3, 3); + Q.block<3, 3>(3, 3) = + na_ * dt * 9.81 / 20.0 * Eigen::MatrixXd::Identity(3, 3); + + // Resolve for Q depending on contact states + for (int i = 0; i < num_feet; ++i) { + if ((*last_grf_msg_).contact_states[i]) { + Q.block<3, 3>(3 * i + 6, 3 * i + 6) = + (1.0) * nf_ * dt * Eigen::MatrixXd::Identity(3, 3); + } else { + Q.block<3, 3>(3 * i + 6, 3 * i + 6) = + (1.0 + contact_w_) * nf_ * dt * Eigen::MatrixXd::Identity(3, 3); + } + } + + // Generate Control Input U + // May Need to add a Rotation Matrix to Compensate for Frames Here + g = Eigen::Vector3d(0, 0, -9.81); + u = C1 * fk + g; + + // Solve for Process Prediction State and Covariance + X_pre = (F * last_X) + B * u; // (18 x 1) + P_pre = (F * P * F.transpose()) + Q; // (18 x 18) +} + +void EKFEstimator::update(const Eigen::VectorXd& jk, const Eigen::VectorXd& fk, + const Eigen::VectorXd& vk, + const Eigen::VectorXd& wk) { + // debug for update step, set the predicted state to be ground truth: + // Preallocate Space and Generate C + Eigen::MatrixXd C(num_measure, num_state); + C.setZero(); + for (int i = 0; i < num_feet; ++i) { + C.block<3, 3>(3 * i, 0) = -Eigen::MatrixXd::Identity(3, 3); + C.block<3, 3>(3 * i, 6 + 3 * i) = Eigen::MatrixXd::Identity(3, 3); + C.block<3, 3>(12 + 3 * i, 3) = Eigen::MatrixXd::Identity(3, 3); + C(24 + i, 8 + 3 * i) = 1.0; + } + + // Preallocate Space and Generate R + R = Eigen::MatrixXd::Identity(num_measure, num_measure); + for (int i = 0; i < num_feet; ++i) { + if ((*last_grf_msg_).contact_states[i]) { + R.block<3, 3>(3 * i, 3 * i) = na_ * Eigen::MatrixXd::Identity(3, 3); + R.block<3, 3>(12 + 3 * i, 12 + 3 * i) = + na_ * Eigen::MatrixXd::Identity(3, 3); + R(24 + i, 24 + i) = nf_; + } else { + R.block<3, 3>(3 * i, 3 * i) = + na_ * (1.0 + contact_w_) * Eigen::MatrixXd::Identity(3, 3); + R.block<3, 3>(12 + 3 * i, 12 + 3 * i) = + na_ * (1.0 + contact_w_) * Eigen::MatrixXd::Identity(3, 3); + R(24 + i, 24 + i) = nf_ * (1.0 + contact_w_); + } + } + + // Generate Measurement y from Kinematics, Extract Process Pos and Vel + y = Eigen::VectorXd::Zero(num_measure); + // Eigen::VectorXd r_pre = X_pre.segment(0,3); + // Eigen::VectorXd v_pre = X_pre.segment(3, 3); + Eigen::VectorXd r_pre = last_X.segment(0, 3); + Eigen::VectorXd v_pre = last_X.segment(3, 3); + + // Update Measurement Attempt #2 + // Rewritten to Account for Rotation Matrix of IMU Frame + Eigen::VectorXd joint_state(num_state); + Eigen::VectorXd joint_velocity(num_state); + Eigen::VectorXd joint_velocities(num_state); + Eigen::VectorXd rbs(6); + Eigen::MatrixXd jacobian = Eigen::MatrixXd::Zero(12, 18); + joint_state << jk, r_pre, v_pre; + // joint_velocity << vk, v_pre, C1*fk; + rbs << r_pre, v_pre; + + // Solve for Linear Foot Velocities in the Body Frame + quadKD_->getJacobianBodyAngVel(joint_state, jacobian); + Eigen::VectorXd lin_foot_vel; + + // Method for Joint Velocity + // joint_velocities = + // math_utils::sdlsInv(jacobian.leftCols(12)) * + // (vk - jacobian.rightCols(6) * rbs.tail(6)); + + lin_foot_vel = jacobian.leftCols(12) * vk; + // lin_foot_vel = jacobian*joint_velocity; + + for (int i = 0; i < num_feet; ++i) { + // Solve for Foot Relative Positions + Eigen::Vector3d joint_state_i; + Eigen::Vector3d toe_body_pos; + toe_body_pos.setZero(); + joint_state_i = jk.segment(3 * i, 3); + quadKD_->bodyToFootFKBodyFrame(i, joint_state_i, toe_body_pos); + y.segment(3 * i, 3) = C1 * (toe_body_pos); + + // Solve for Foot Heights + y(24 + i) = (1.0 - (*last_grf_msg_).contact_states[i]) * + (r_pre(2) + toe_body_pos(2)) + + (*last_grf_msg_).contact_states[i] * 0; + + // Solve for Foot Relative Velocties + Eigen::VectorXd leg_v(3); + Eigen::MatrixXd acc; + + acc = calcSkewsym(wk); + leg_v = -(lin_foot_vel.segment(3 * i, 3)) - (acc * toe_body_pos); + y.segment(12 + 3 * i, 3) = + (*last_grf_msg_).contact_states[i] * C1 * leg_v + + last_X.segment(3, 3) * (1.0 - (*last_grf_msg_).contact_states[i]); + } + + // Solve for Error between Measured Y Residual and Process Residual + error_y = y - (C * X_pre); + // ROS_INFO_STREAM("Innovation Norm" << error_y.norm()); + // ROS_INFO("Worked"); + // Skip Update if the Innovation is too High + if (error_y.norm() < thresh_out) { + S = C * P_pre * C.transpose() + R; + // ROS_INFO_STREAM("This is S" << S); + S = 0.5 * + (S + + S.transpose()); // Ensure that the Innovation Covariance is Symmetric + Serror_y = S.fullPivHouseholderQr().solve(error_y); + // ROS_INFO_STREAM("This is S Error" << Serror_y); + // EKF Filter Equations, Solve for Kalman Gain + + X = X_pre + P_pre * C.transpose() * Serror_y; + SC = S.fullPivHouseholderQr().solve(C); + P = P_pre - P_pre * C.transpose() * SC * P_pre; + P = 0.5 * + (P + P.transpose()); // Ensure that the Covariance Matrix is Symmetric + + // Shuo Method to Reduce Positional Drift (Try) + if (P.block<2, 2>(0, 0).determinant() > 1e-6) { + P.block<2, 16>(0, 2).setZero(); + P.block<16, 2>(2, 0).setZero(); + P.block<2, 2>(0, 0) /= 10.0; + } + } else { + X = X_pre; + P = P_pre; + } + last_X = X; +} + +Eigen::VectorXd EKFEstimator::quaternionDynamics(const Eigen::VectorXd& wdt, + const Eigen::VectorXd& q2v) { + Eigen::VectorXd output = Eigen::VectorXd::Zero(4); + + double angle = wdt.norm(); + Eigen::Vector3d axis; + if (angle == 0) { + axis = Eigen::VectorXd::Zero(3); + } else { + axis = wdt / angle; + } + Eigen::Vector3d q_xyz = sin(angle / 2) * axis; + double q_w = cos(angle / 2); + Eigen::Quaterniond q1(q_w, q_xyz[0], q_xyz[1], q_xyz[2]); + + Eigen::Quaterniond q2(q2v[0], q2v[1], q2v[2], q2v[3]); + q1.normalize(); + q2.normalize(); + Eigen::Quaterniond q3 = q1 * q2; + q3.normalize(); + output << q3.w(), q3.x(), q3.y(), q3.z(); + return output; +} + +Eigen::MatrixXd EKFEstimator::calcSkewsym(const Eigen::VectorXd& w) { + Eigen::MatrixXd output = Eigen::MatrixXd::Zero(3, 3); + output << 0, -w(2), w(1), w(2), 0, -w(0), -w(1), w(0), 0; + return output; +} + +Eigen::MatrixXd EKFEstimator::calcRodrigues(const double& dt, + const Eigen::VectorXd& w, + const int& sub) { + Eigen::MatrixXd output = Eigen::MatrixXd::Identity(3, 3); + Eigen::VectorXd wdt = dt * w; + double ang = wdt.norm(); + Eigen::VectorXd axis; + if (ang == 0) { + axis = Eigen::VectorXd::Zero(3); + } else { + axis = wdt / ang; + } + Eigen::MatrixXd w_cap = calcSkewsym(axis); + switch (sub) { + case 0: + output = output + sin(ang) * w_cap + (1 - cos(ang)) * (w_cap * w_cap); + break; + + case 1: + if (ang == 0) { + break; + } else { + output = output + (1 - cos(ang)) * (w_cap / ang) + + (ang - sin(ang)) * (w_cap * w_cap) / ang; + break; + } + + case 2: + if (ang == 0) { + break; + } else { + output = output + (ang - sin(ang)) * (w_cap / (ang * ang)) + + (((cos(ang) - 1) + (pow(ang, 2) / 2)) / (ang * ang)) * + (w_cap * w_cap); + break; + } + + case 3: + if (ang == 0) { + break; + } else { + output = output + + (cos(ang) - 1 + (pow(ang, 2) / 2)) / (pow(ang, 3)) * w_cap + + ((sin(ang) - ang + (pow(ang, 3) / 6)) / (pow(ang, 3)) * + (w_cap * w_cap)); + break; + } + default: + break; + } + output = pow(dt, sub) * output; + return output; } -bool EKFEstimator::updateOnce(quad_msgs::RobotState& last_robot_state_msg) { - std::cout << "EKF Estimator Updated Once" << std::endl; +void EKFEstimator::setNoise() { + this->noise_acc = 0.001 * Eigen::MatrixXd::Identity(3, 3); + this->noise_gyro = 0.001 * Eigen::MatrixXd::Identity(3, 3); + this->bias_acc = 0.001 * Eigen::MatrixXd::Identity(3, 3); + this->bias_gyro = 0.001 * Eigen::MatrixXd::Identity(3, 3); + this->noise_feet = 0.001 * Eigen::MatrixXd::Identity(3, 3); + this->noise_fk = 0.001 * Eigen::MatrixXd::Identity(3, 3); + this->noise_encoder = 0.001; + return; } diff --git a/robot_driver/src/estimators/state_estimator.cpp b/robot_driver/src/estimators/state_estimator.cpp index 20c23c0a8..92cdb38e7 100644 --- a/robot_driver/src/estimators/state_estimator.cpp +++ b/robot_driver/src/estimators/state_estimator.cpp @@ -4,32 +4,32 @@ StateEstimator::StateEstimator() { quadKD_ = std::make_shared(); } -void StateEstimator::readIMU(const sensor_msgs::Imu::ConstPtr& last_imu_msg_, - Eigen::Vector3d& fk, Eigen::Vector3d& wk, +void StateEstimator::readIMU(const sensor_msgs::Imu& last_imu_msg_, + Eigen::VectorXd& fk, Eigen::VectorXd& wk, Eigen::Quaterniond& qk) { - if (last_imu_msg_ != NULL) { - fk << (*last_imu_msg_).linear_acceleration.x, - (*last_imu_msg_).linear_acceleration.y, - (*last_imu_msg_).linear_acceleration.z; + if (!last_imu_msg_.header.stamp.isZero()) { + fk << (last_imu_msg_).linear_acceleration.x, + (last_imu_msg_).linear_acceleration.y, + (last_imu_msg_).linear_acceleration.z; - wk << (*last_imu_msg_).angular_velocity.x, - (*last_imu_msg_).angular_velocity.y, - (*last_imu_msg_).angular_velocity.z; + wk << (last_imu_msg_).angular_velocity.x, + (last_imu_msg_).angular_velocity.y, (last_imu_msg_).angular_velocity.z; - qk.w() = (*last_imu_msg_).orientation.w; - qk.x() = (*last_imu_msg_).orientation.x; - qk.y() = (*last_imu_msg_).orientation.y; - qk.z() = (*last_imu_msg_).orientation.z; + qk.w() = (last_imu_msg_).orientation.w; + qk.x() = (last_imu_msg_).orientation.x; + qk.y() = (last_imu_msg_).orientation.y; + qk.z() = (last_imu_msg_).orientation.z; qk.normalize(); } } void StateEstimator::readJointEncoder( - const sensor_msgs::JointState::ConstPtr& last_joint_state_msg_, - Eigen::VectorXd& jk) { - if (last_joint_state_msg_ != NULL) { + const sensor_msgs::JointState& last_joint_state_msg_, Eigen::VectorXd& jk, + Eigen::VectorXd& vk) { + if (!last_joint_state_msg_.header.stamp.isZero()) { for (int i = 0; i < 12; i++) { - jk[i] = (*last_joint_state_msg_).position[i]; + jk[i] = (last_joint_state_msg_).position[i]; + vk[i] = (last_joint_state_msg_).velocity[i]; } } } diff --git a/robot_driver/src/hardware_interfaces/spirit_interface.cpp b/robot_driver/src/hardware_interfaces/spirit_interface.cpp index 4bdaca236..4b4da6102 100644 --- a/robot_driver/src/hardware_interfaces/spirit_interface.cpp +++ b/robot_driver/src/hardware_interfaces/spirit_interface.cpp @@ -1,93 +1,163 @@ +/* + * Copyright (C) 2023 Vincent FOUCAULT + * Author: Vincent FOUCAULT + * email: elpimous12@gmail.com + * + * This program is free software: you can redistribute it and/or modify + * it under the terms of the GNU Lesser General Public License as published by + * the Free Software Foundation, either version 3 of the License, or + * (at your option) any later version. + * + * This program is distributed in the hope that it will be useful, + * but WITHOUT ANY WARRANTY; without even the implied warranty of + * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the + * GNU Lesser General Public License for more details. + * + * You should have received a copy of the GNU Lesser General Public License + * along with this program. If not, see +*/ + #include "robot_driver/hardware_interfaces/spirit_interface.h" -SpiritInterface::SpiritInterface() {} +SpiritInterface::SpiritInterface(){} + +YloTwoPcanToMoteus command; // instance of class YloTwoPcanToMoteus + +void startImuThread(YloTwoPcanToMoteus* obj) { + obj->subscribeToImuData(); +} + +std::thread imuThread(startImuThread, &command); // create thread and run imu subscriber void SpiritInterface::loadInterface(int argc, char** argv) { - /// Ghost MBLink interface class - mblink_.start(argc, argv); - mblink_.rxstart(); - mblink_.setRetry("_UPST_ADDRESS", 255); - mblink_.setRetry("UPST_LOOP_DELAY", 1); + startup_routine(); + usleep(10000); + booting_motors(); + usleep(200); +} + +void SpiritInterface::unloadInterface(){ + imuThread.join(); // free imu thread before exit + ROS_INFO("--RELAXING MOTORS AND QUIT--------> OK--"); + usleep(2000); + command.stop_motors(); + ROS_INFO("--ALL MOTORS RELAXED--------------> OK--"); + usleep(5000); +} + +bool SpiritInterface::booting_motors(){ + if (is_up_flag == false){ + for (unsigned int j = 0; j<2; ++j){ + command.security_switch(); + for (size_t i = 0; i < 12; ++i) { + auto ids = command.motor_adapters_[i].getIdx(); + int port = command.motor_adapters_[i].getPort(); + auto joint_position = NAN; + auto joint_velocity = command._comm_velocity; + auto joint_fftorque = 0.01; + auto joint_kp = command._comm_kp; + auto joint_kd = command._comm_kd; + auto target_vel = 1.0; + auto target_accel = 0.8; + command.send_to_moteus(ids, port, joint_position, joint_velocity, joint_fftorque, joint_kp, joint_kd, target_vel, target_accel); + usleep(200); + } + } + ROS_INFO("--MOTORS UNDER VOLTAGE------------> OK--"); + return true; + } + return false; } -void SpiritInterface::unloadInterface() { mblink_.rxstop(); } +bool SpiritInterface::startup_routine() +{ + /* initialize GPIO pin, for security switch button */ + command.btnPin = mraa_gpio_init(BTN_PIN); + mraa_gpio_dir(command.btnPin, MRAA_GPIO_IN); + command.peak_fdcan_board_initialization(); + usleep(200); + command.check_initial_ground_pose(); + ROS_INFO("--STARTUP ROUTINE DONE------------> OK--"); + usleep(200); + return true; +} bool SpiritInterface::send( const quad_msgs::LegCommandArray& last_leg_command_array_msg, const Eigen::VectorXd& user_tx_data) { - int leg_command_heartbeat = 1; - - bool restart_flag = (user_tx_data[0] == 1); - - LimbCmd_t limbcmd[4]; - for (int i = 0; i < 4; ++i) { // For each leg - // std::cout << "leg = " << i << std::endl; - quad_msgs::LegCommand leg_command = - last_leg_command_array_msg.leg_commands.at(i); - - for (int j = 0; j < 3; ++j) { // For each joint - // std::cout << "joint = " << j << std::endl; - limbcmd[i].pos[j] = - leg_command_heartbeat * leg_command.motor_commands.at(j).pos_setpoint; - limbcmd[i].vel[j] = - leg_command_heartbeat * leg_command.motor_commands.at(j).vel_setpoint; - limbcmd[i].tau[j] = - leg_command_heartbeat * leg_command.motor_commands.at(j).torque_ff; - limbcmd[i].kp[j] = static_cast( - leg_command_heartbeat * leg_command.motor_commands.at(j).kp); - limbcmd[i].kd[j] = static_cast( - leg_command_heartbeat * leg_command.motor_commands.at(j).kd); - limbcmd[i].restart_flag = restart_flag; - } - } - - float data[58] = {0}; - memcpy(data, limbcmd, 4 * sizeof(LimbCmd_t)); - mblink_.sendUser(Eigen::Map >(data)); - return true; + //std::cout << "SEND FUNCTION" << std::endl; + /** SEND COMMANDS TO MOTEUS CONTROLLERS JOINTS. + * @brief Write moteus controllers datas : + * @param[out] joint_position + * @param[out] joint_velocity + * @param[out] joint_fftorque + * @param[out] joint_kp + * @param[out] joint_kd */ + + for (unsigned int jj=0; jj<12; ++jj) // for all 12 joints, + { + //std::cout << "joint = " << jj << std::endl; + auto ids = command.motor_adapters_[jj].getIdx(); // moteus controller id + int port = command.motor_adapters_[jj].getPort(); // select correct port on Peak canfd board + auto sign = command.motor_adapters_[jj].getSign(); // in case of joint reverse rotation + auto leg_index = command.motor_adapters_[jj].getLeg_index(); // for vector position feed + auto joint_index = command.motor_adapters_[jj].getJoint_index(); // for vector position feed + + joint_position = (sign*(last_leg_command_array_msg.leg_commands[leg_index].motor_commands[joint_index].pos_setpoint))/TWO_M_PI; + joint_velocity = (sign*(last_leg_command_array_msg.leg_commands[leg_index].motor_commands[joint_index].vel_setpoint))/TWO_M_PI; + joint_fftorque = sign*last_leg_command_array_msg.leg_commands[leg_index].motor_commands[joint_index].torque_ff; + + joint_kp = static_cast( last_leg_command_array_msg.leg_commands[leg_index].motor_commands[joint_index].kp); + joint_kd = static_cast( last_leg_command_array_msg.leg_commands[leg_index].motor_commands[joint_index].kd); + + command.send_to_moteus(ids, port, joint_position, joint_velocity, joint_fftorque, joint_kp, joint_kd); + usleep(120); + } + return true; } + bool SpiritInterface::recv(sensor_msgs::JointState& joint_state_msg, sensor_msgs::Imu& imu_msg, Eigen::VectorXd& user_rx_data) { - // Get the data and appropriate timestamp (this may be blocking) - MBData_t mbdata = mblink_.get(); - // Check if data exists - if (mbdata.empty()) { - return false; - } - - // Add the data corresponding to each joint - for (int i = 0; i < joint_names_.size(); i++) { - joint_state_msg.name[i] = joint_names_[i]; - joint_state_msg.position[i] = mbdata["joint_position"][joint_indices_[i]]; - joint_state_msg.velocity[i] = mbdata["joint_velocity"][joint_indices_[i]]; - - // Convert from current to torque using linear motor model - joint_state_msg.effort[i] = - kt_vec_[i] * mbdata["joint_current"][joint_indices_[i]]; - } - - // Transform from rpy to quaternion - geometry_msgs::Quaternion orientation_msg; - tf2::Quaternion quat_tf; - Eigen::Vector3f rpy; - quat_tf.setRPY(mbdata["imu_euler"][0], mbdata["imu_euler"][1], - mbdata["imu_euler"][2]); - tf2::convert(quat_tf, orientation_msg); - - // Load the data into the imu message - imu_msg.orientation = orientation_msg; - imu_msg.angular_velocity.x = mbdata["imu_angular_velocity"][0]; - imu_msg.angular_velocity.y = mbdata["imu_angular_velocity"][1]; - imu_msg.angular_velocity.z = mbdata["imu_angular_velocity"][2]; - - // I guess the acceleration is opposite - imu_msg.linear_acceleration.x = -mbdata["imu_linear_acceleration"][0]; - imu_msg.linear_acceleration.y = -mbdata["imu_linear_acceleration"][1]; - imu_msg.linear_acceleration.z = -mbdata["imu_linear_acceleration"][2]; + //std::cout << "RECEIVE FUNCTION" << std::endl; + /** RECEIVE JOINTS VALUES. + * @brief Read moteus controllers datas : + * @param[out] joint position + * @param[out] joint velocity + * @param[out] joint fftorque */ + + for (unsigned int jj=0; jj<12; ++jj) + { + // Reset values + float RX_pos = 0.0; + float RX_vel = 0.0; + float RX_tor = 0.0; + float RX_volt = 0.0; + float RX_temp = 0.0; + float RX_fault = 0.0; + auto ids = command.motor_adapters_[jj].getIdx(); + int port = command.motor_adapters_[jj].getPort(); + auto sign = command.motor_adapters_[jj].getSign(); + + command.read_moteus_RX_queue(ids, port, RX_pos, RX_vel, RX_tor, RX_volt, RX_temp, RX_fault); // query values; + usleep(10); + + joint_state_msg.name[jj] = joint_names_[jj]; + // servo inversions, and radians to turn/s (moteus protocol) + joint_state_msg.position[jj] = static_cast(sign*(RX_pos*TWO_M_PI)); // joint turns to radians + joint_state_msg.velocity[jj] = static_cast(sign*(RX_vel*TWO_M_PI)); // turns in radians / s + joint_state_msg.effort[jj] = static_cast(sign*RX_tor); // measured in N*m + usleep(200); + } - return true; -} + + // imu variable is correctly feeded according to quad_sdk need + // imu_msg.angular_velocity.xyz and imu_msg.linear_acceleration.xyz are OK + imu_msg = YloTwoPcanToMoteus::ylo3_imu; + //std::cout << imu_msg.orientation.x << std::endl; + + return true; +} \ No newline at end of file diff --git a/robot_driver/src/robot_driver.cpp b/robot_driver/src/robot_driver.cpp index ebb05e1c8..9a48b0fe6 100644 --- a/robot_driver/src/robot_driver.cpp +++ b/robot_driver/src/robot_driver.cpp @@ -1,4 +1,6 @@ #include "robot_driver/robot_driver.h" +// New comments to test David's stuff +// 10/06/2023 @ 15:25 RobotDriver::RobotDriver(ros::NodeHandle nh, int argc, char **argv) { nh_ = nh; @@ -10,7 +12,9 @@ RobotDriver::RobotDriver(ros::NodeHandle nh, int argc, char **argv) { robot_state_topic, trajectory_state_topic, local_plan_topic, leg_command_array_topic, control_mode_topic, remote_heartbeat_topic, robot_heartbeat_topic, single_joint_cmd_topic, mocap_topic, - control_restart_flag_topic; + control_restart_flag_topic, body_force_estimate_topic, + state_estimate_topic; + quad_utils::loadROSParamDefault(nh_, "robot_type", robot_name, std::string("spirit")); quad_utils::loadROSParam(nh_, "topics/state/imu", imu_topic); @@ -19,10 +23,13 @@ RobotDriver::RobotDriver(ros::NodeHandle nh, int argc, char **argv) { quad_utils::loadROSParam(nh_, "topics/state/ground_truth", robot_state_topic); quad_utils::loadROSParam(nh_, "topics/state/trajectory", trajectory_state_topic); + quad_utils::loadROSParam(nh_, "topics/state/estimate", state_estimate_topic); quad_utils::loadROSParam(nh_, "/topics/heartbeat/remote", remote_heartbeat_topic); quad_utils::loadROSParam(nh_, "topics/heartbeat/robot", robot_heartbeat_topic); + quad_utils::loadROSParam(nh_, "topics/body_force/joint_torques", + body_force_estimate_topic); quad_utils::loadROSParam(nh_, "topics/control/grfs", grf_topic); quad_utils::loadROSParam(nh_, "topics/control/joint_command", leg_command_array_topic); @@ -31,8 +38,7 @@ RobotDriver::RobotDriver(ros::NodeHandle nh, int argc, char **argv) { single_joint_cmd_topic); quad_utils::loadROSParam(nh_, "topics/control/restart_flag", control_restart_flag_topic); - quad_utils::loadROSParam(nh_, "topics/mocap", mocap_topic); - + quad_utils::loadROSParam(nh_, "/topics/mocap", mocap_topic); quad_utils::loadROSParamDefault(nh_, "robot_driver/is_hardware", is_hardware_, true); quad_utils::loadROSParamDefault(nh_, "robot_driver/controller", @@ -40,6 +46,8 @@ RobotDriver::RobotDriver(ros::NodeHandle nh, int argc, char **argv) { std::string("inverse_dynamics")); quad_utils::loadROSParamDefault(nh_, "robot_driver/estimator", estimator_id_, std::string("comp_filter")); + quad_utils::loadROSParamDefault(nh_, "robot_driver/ground_truth", + ground_truth_, std::string("sim")); quad_utils::loadROSParam(nh_, "/robot_driver/update_rate", update_rate_); quad_utils::loadROSParam(nh_, "/robot_driver/publish_rate", publish_rate_); quad_utils::loadROSParam(nh_, "/robot_driver/mocap_rate", mocap_rate_); @@ -78,6 +86,9 @@ RobotDriver::RobotDriver(ros::NodeHandle nh, int argc, char **argv) { single_joint_cmd_sub_ = nh_.subscribe(single_joint_cmd_topic, 1, &RobotDriver::singleJointCommandCallback, this); + body_force_estimate_sub_ = + nh_.subscribe(body_force_estimate_topic, 1, + &RobotDriver::bodyForceEstimateCallback, this); remote_heartbeat_sub_ = nh_.subscribe( remote_heartbeat_topic, 1, &RobotDriver::remoteHeartbeatCallback, this); control_restart_flag_sub_ = @@ -90,9 +101,12 @@ RobotDriver::RobotDriver(ros::NodeHandle nh, int argc, char **argv) { nh_.advertise(robot_heartbeat_topic, 1); trajectry_robot_state_pub_ = nh_.advertise(trajectory_state_topic, 1); + state_estimate_pub_ = + nh_.advertise(state_estimate_topic, 1); // Set up pubs and subs dependent on robot layer if (is_hardware_) { + // Initialize Subs and Pubs when testing on Hardware or Estimation in Sim ROS_INFO("Loading hardware robot driver"); mocap_sub_ = nh_.subscribe(mocap_topic, 1000, &RobotDriver::mocapCallback, this, ros::TransportHints().tcpNoDelay(true)); @@ -102,10 +116,13 @@ RobotDriver::RobotDriver(ros::NodeHandle nh, int argc, char **argv) { joint_state_pub_ = nh_.advertise(joint_state_topic, 1); } else { + // Grab Robot State From Sim ROS_INFO("Loading sim robot driver"); robot_state_sub_ = nh_.subscribe(robot_state_topic, 1, &RobotDriver::robotStateCallback, this, ros::TransportHints().tcpNoDelay(true)); + joint_state_pub_ = + nh_.advertise(joint_state_topic, 1); } // Initialize kinematics object @@ -113,7 +130,7 @@ RobotDriver::RobotDriver(ros::NodeHandle nh, int argc, char **argv) { // Initialize hardware interface if (is_hardware_) { - if (robot_name == "spirit") { + if (robot_name == "spirit" || robot_name == "spirit_rotors") { hardware_interface_ = std::make_shared(); } else { ROS_ERROR_STREAM("Invalid robot name " << robot_name @@ -127,11 +144,13 @@ RobotDriver::RobotDriver(ros::NodeHandle nh, int argc, char **argv) { // Start sitting control_mode_ = SIT; + // initialized_ = SIT; remote_heartbeat_received_time_ = std::numeric_limits::max(); last_state_time_ = std::numeric_limits::max(); // Initialize timing last_robot_state_msg_.header.stamp = ros::Time::now(); + // state_estimate_.header.stamp = ros::Time::now(); t_pub_ = ros::Time::now(); // Initialize state and control data structures @@ -141,14 +160,18 @@ RobotDriver::RobotDriver(ros::NodeHandle nh, int argc, char **argv) { // Initialize state and control strucutres initStateControlStructs(); - // Initialize state estimator object + // Initialize state estimator object "Comp Filter" initStateEstimator(); + ROS_INFO_STREAM("Init Robot Driver Completed"); } void RobotDriver::initStateEstimator() { if (estimator_id_ == "comp_filter") { + ROS_INFO("Comp Filter"); state_estimator_ = std::make_shared(); + ekf_estimator_ = std::make_shared(); } else if (estimator_id_ == "ekf_filter") { + ROS_INFO("EKF Filter"); state_estimator_ = std::make_shared(); } else { ROS_ERROR_STREAM("Invalid estimator id " << estimator_id_ @@ -157,7 +180,13 @@ void RobotDriver::initStateEstimator() { } if (state_estimator_ != nullptr) { + // Always Initialize the Defualt State Estimator state_estimator_->init(nh_); + + if (estimator_id_ == "comp_filter" && is_hardware_ == false) { + // In Sim, Initialize both the comp filter and ekf for comparison + ekf_estimator_->init(nh_); + } } } @@ -168,6 +197,26 @@ void RobotDriver::initLegController() { leg_controller_ = std::make_shared(); } else if (controller_id_ == "joint") { leg_controller_ = std::make_shared(); + } else if (controller_id_ == "underbrush") { + leg_controller_ = std::make_shared(); + double retract_vel, tau_push, tau_contact_start, tau_contact_end, + min_switch, t_down, t_up; + quad_utils::loadROSParam(nh_, "/underbrush_swing/retract_vel", retract_vel); + quad_utils::loadROSParam(nh_, "/underbrush_swing/tau_push", tau_push); + quad_utils::loadROSParam(nh_, "/underbrush_swing/tau_contact_start", + tau_contact_start); + quad_utils::loadROSParam(nh_, "/underbrush_swing/tau_contact_end", + tau_contact_end); + quad_utils::loadROSParam(nh_, "/underbrush_swing/min_switch", min_switch); + quad_utils::loadROSParam(nh_, "/underbrush_swing/t_down", t_down); + quad_utils::loadROSParam(nh_, "/underbrush_swing/t_up", t_up); + UnderbrushInverseDynamicsController *c = + dynamic_cast( + leg_controller_.get()); + c->setUnderbrushParams(retract_vel, tau_push, tau_contact_start, + tau_contact_end, min_switch, t_down, t_up); + } else if (controller_id_ == "inertia_estimation") { + leg_controller_ = std::make_shared(); } else { ROS_ERROR_STREAM("Invalid controller id " << controller_id_ << ", returning nullptr"); @@ -241,26 +290,29 @@ void RobotDriver::mocapCallback( Eigen::Vector3d pos; quad_utils::pointMsgToEigen(msg->pose.position, pos); - // Record time diff between messages - ros::Time t_now = ros::Time::now(); - double t_diff_mocap_msg = - (msg->header.stamp - last_mocap_msg_->header.stamp).toSec(); - double t_mocap_ros_latency = (t_now - msg->header.stamp).toSec(); - last_mocap_time_ = t_now; - - // If time diff between messages < mocap dropout threshould then - // apply filter - if (abs(t_diff_mocap_msg - 1.0 / mocap_rate_) < mocap_dropout_threshold_) { - if (CompFilterEstimator *c = - dynamic_cast(state_estimator_.get())) { - c->mocapCallBackHelper(msg, pos); + if (last_mocap_msg_ != NULL) { + // Record time diff between messages + ros::Time t_now = ros::Time::now(); + double t_diff_mocap_msg = + (msg->header.stamp - last_mocap_msg_->header.stamp).toSec(); + + // If time diff between messages < mocap dropout threshould then + // apply filter + if (abs(t_diff_mocap_msg - 1.0 / mocap_rate_) < mocap_dropout_threshold_) { + if (CompFilterEstimator *c = + dynamic_cast(state_estimator_.get())) { + c->mocapCallBackHelper(msg, pos); + } + } else { + ROS_WARN_THROTTLE( + 0.1, + "Mocap time diff exceeds max dropout threshold, hold the last value"); } } else { ROS_WARN_THROTTLE( 0.1, "Mocap time diff exceeds max dropout threshold, hold the last value"); } - // Update our cached mocap position last_mocap_msg_ = msg; } @@ -270,6 +322,16 @@ void RobotDriver::robotStateCallback( last_robot_state_msg_ = *msg; } +void RobotDriver::bodyForceEstimateCallback( + const quad_msgs::BodyForceEstimate::ConstPtr &msg) { + if (controller_id_ == "underbrush") { + UnderbrushInverseDynamicsController *c = + reinterpret_cast( + leg_controller_.get()); + c->updateBodyForceEstimate(msg); + } +} + void RobotDriver::remoteHeartbeatCallback( const std_msgs::Header::ConstPtr &msg) { // Get the current time and compare to the message time @@ -308,6 +370,7 @@ void RobotDriver::checkMessagesForSafety() { } bool RobotDriver::updateState() { + // If Operating on Hardware if (is_hardware_) { // grab data from hardware bool fully_populated = hardware_interface_->recv( @@ -324,15 +387,51 @@ bool RobotDriver::updateState() { state_estimator_->loadMocapMsg(last_mocap_msg_); } - // update robot state using state estimator + // State information coming through sim subscribers, not hardware interface if (state_estimator_ != nullptr) { return state_estimator_->updateOnce(last_robot_state_msg_); } else { ROS_WARN_THROTTLE(1, "No state estimator is initialized"); return false; } - } else { - // State information coming through sim subscribers, not hardware interface + } else { // If Operating in Sim + if (state_estimator_ != nullptr) { + if (initialized_ == SIT && control_mode_ == READY) { + ROS_INFO_STREAM("Initialized"); + estimated_state_ = last_robot_state_msg_; + last_joint_state_msg_.position = last_robot_state_msg_.joints.position; + last_joint_state_msg_.velocity = last_robot_state_msg_.joints.velocity; + // ROS_INFO_STREAM(last_joint_state_msg_); + initialized_ = true; + } + if (estimator_id_ == "comp_filter") { + state_estimate_ = last_robot_state_msg_; + // Update State Estimate once GRF's are being Published + if (grf_array_msg_.vectors[0].x != 0 && control_mode_ == READY) { + last_joint_state_msg_.position = + last_robot_state_msg_.joints.position; + last_joint_state_msg_.velocity = + last_robot_state_msg_.joints.velocity; + ekf_estimator_->updateOnce(estimated_state_); + } + } + if (estimator_id_ == "ekf_filter") { + // Addded to make sure the robot can stand + if (initialized_ == SIT) { + state_estimate_ = last_robot_state_msg_; + } + if (grf_array_msg_.vectors[0].x != 0 && control_mode_ == READY) { + last_joint_state_msg_.position = + last_robot_state_msg_.joints.position; + last_joint_state_msg_.velocity = + last_robot_state_msg_.joints.velocity; + estimated_state_.joints = last_robot_state_msg_.joints; + // ROS_INFO_STREAM("Estiamted State before update:" << estimated_state_); + state_estimator_->updateOnce(estimated_state_); + state_estimate_ = estimated_state_; + } + } + } return true; } } @@ -342,6 +441,14 @@ void RobotDriver::publishState() { imu_pub_.publish(last_imu_msg_); joint_state_pub_.publish(last_joint_state_msg_); robot_state_pub_.publish(last_robot_state_msg_); + // state_estimate_pub_.publish(state_estimate_); + } else { + if (control_mode_ == READY) { + joint_state_pub_.publish(last_joint_state_msg_); + if (initialized_) { + // state_estimate_pub_.publish(estimated_state_); + } + } } } @@ -356,7 +463,7 @@ bool RobotDriver::updateControl() { // Check incoming messages to determine if we should enter safety mode checkMessagesForSafety(); - + // ROS_INFO_STREAM("8"); if (last_robot_state_msg_.header.stamp.toSec() == 0) { return false; } @@ -371,7 +478,7 @@ bool RobotDriver::updateControl() { // Initialize leg command message leg_command_array_msg_.leg_commands.resize(num_feet_); - + // ROS_INFO_STREAM("9"); // Enter state machine for filling motor command message if (control_mode_ == SAFETY) { for (int i = 0; i < num_feet_; ++i) { @@ -534,6 +641,7 @@ void RobotDriver::publishControl(bool is_valid) { leg_command_array_msg_.header.stamp = ros::Time::now(); leg_command_array_pub_.publish(leg_command_array_msg_); grf_array_msg_.header.stamp = leg_command_array_msg_.header.stamp; + // ROS_INFO_STREAM("Publishing grfs"); grf_pub_.publish(grf_array_msg_); // } @@ -568,22 +676,21 @@ void RobotDriver::spin() { while (ros::ok()) { // Collect new messages on subscriber topics and publish heartbeat ros::spinOnce(); - + // ROS_INFO_STREAM("Start Spin"); // Get the newest state information updateState(); - + // ROS_INFO_STREAM("After Update State"); // Compute the leg command and publish if valid bool is_valid = updateControl(); publishControl(is_valid); - - // // Publish state and heartbeat + // Publish state and heartbeat publishState(); + // ROS_INFO_STREAM("After Publish State"); publishHeartbeat(); // Enforce update rate r.sleep(); } - // Close the mblink connection if (is_hardware_) { hardware_interface_->unloadInterface(); diff --git a/setup.sh b/setup.sh index 644f10d54..b0d788bd8 100755 --- a/setup.sh +++ b/setup.sh @@ -4,7 +4,7 @@ echo "#########################" echo "Install base dependencies" echo "#########################" echo -sudo apt install -y doxygen libeigen3-dev python3-catkin-tools python-pip +sudo apt install -y doxygen libeigen3-dev python3-catkin-tools python3-pip pip install cpplint # Install all dependencies from package setup files @@ -29,4 +29,4 @@ echo "#######################" echo "Install all rosdeps" echo "#######################" echo -rosdep install --from-paths .. --ignore-src -r -y --rosdistro melodic +rosdep install --from-paths .. --ignore-src -r -y --rosdistro noetic