void testJacobian(std::string group_name, std::string base_link, std::string tip_link) { SCOPED_TRACE(group_name + ": " + base_link + " to " + tip_link); srand ( time(NULL) ); // initialize random seed: rdf_loader::RDFLoader model_loader("robot_description"); robot_model::RobotModelPtr kinematic_model; kinematic_model.reset(new robot_model::RobotModel(model_loader.getURDF(),model_loader.getSRDF())); robot_state::RobotStatePtr kinematic_state; kinematic_state.reset(new robot_state::RobotState(kinematic_model)); kinematic_state->setToDefaultValues(); const moveit::core::JointModelGroup* joint_state_group = kinematic_state->getRobotModel()->getJointModelGroup(group_name); std::string link_name = tip_link; std::vector<double> joint_angles(7,0.0); geometry_msgs::Point ref_position; Eigen::MatrixXd jacobian; Eigen::Vector3d point(0.0,0.0,0.0); kinematic_state->setJointGroupPositions(joint_state_group, &joint_angles[0]); ASSERT_TRUE(kinematic_state->getJacobian(joint_state_group, kinematic_state->getRobotModel()->getLinkModel(link_name),point,jacobian)); KDL::Tree tree; if (!kdl_parser::treeFromUrdfModel(*model_loader.getURDF(), tree)) { ROS_ERROR("Could not initialize tree object"); } KDL::Chain kdl_chain; std::string base_frame(base_link); std::string tip_frame(tip_link); if (!tree.getChain(base_frame, tip_frame, kdl_chain)) { ROS_ERROR("Could not initialize chain object"); } KDL::ChainJntToJacSolver kdl_solver(kdl_chain); KDL::Jacobian jacobian_kdl(7); KDL::JntArray q_in(7); EXPECT_TRUE(kdl_solver.JntToJac(q_in,jacobian_kdl) >= 0); unsigned int NUM_TESTS = 10000; for(unsigned int i=0; i < NUM_TESTS; i++) { for(int j=0; j < 7; j++) { q_in(j) = gen_rand(-M_PI,M_PI); joint_angles[j] = q_in(j); } EXPECT_TRUE(kdl_solver.JntToJac(q_in,jacobian_kdl) >= 0); kinematic_state->setJointGroupPositions(joint_state_group, &joint_angles[0]); EXPECT_TRUE(kinematic_state->getJacobian(joint_state_group, kinematic_state->getRobotModel()->getLinkModel(link_name), point, jacobian)); for(unsigned int k=0; k < 6; k++) { for(unsigned int m=0; m < 7; m++) { EXPECT_FALSE(NOT_NEAR(jacobian_kdl(k,m),jacobian(k,m),1e-10)); } } } }
TEST(JacobianSolver, solver) { srand ( time(NULL) ); // initialize random seed: rdf_loader::RDFLoader model_loader("robot_description"); robot_model::RobotModelPtr kinematic_model; kinematic_model.reset(new robot_model::RobotModel(model_loader.getURDF(),model_loader.getSRDF())); robot_state::RobotStatePtr kinematic_state; kinematic_state.reset(new robot_state::RobotState(kinematic_model)); kinematic_state->setToDefaultValues(); robot_state::JointStateGroup* joint_state_group = kinematic_state->getJointStateGroup("right_arm"); std::string link_name = "r_wrist_roll_link"; std::vector<double> joint_angles(7,0.0); geometry_msgs::Point ref_position; Eigen::MatrixXd jacobian; Eigen::Vector3d point(0.0,0.0,0.0); joint_state_group->setVariableValues(joint_angles); ASSERT_TRUE(joint_state_group->getJacobian(link_name,point,jacobian)); KDL::Tree tree; if (!kdl_parser::treeFromUrdfModel(*model_loader.getURDF(), tree)) { ROS_ERROR("Could not initialize tree object"); } KDL::Chain kdl_chain; std::string base_frame("torso_lift_link"); std::string tip_frame("r_wrist_roll_link"); if (!tree.getChain(base_frame, tip_frame, kdl_chain)) { ROS_ERROR("Could not initialize chain object"); } KDL::ChainJntToJacSolver kdl_solver(kdl_chain); KDL::Jacobian jacobian_kdl(7); KDL::JntArray q_in(7); EXPECT_TRUE(kdl_solver.JntToJac(q_in,jacobian_kdl) >= 0); unsigned int NUM_TESTS = 1000000; for(unsigned int i=0; i < NUM_TESTS; i++) { for(int j=0; j < 7; j++) { q_in(j) = gen_rand(-M_PI,M_PI); joint_angles[j] = q_in(j); } EXPECT_TRUE(kdl_solver.JntToJac(q_in,jacobian_kdl) >= 0); joint_state_group->setVariableValues(joint_angles); EXPECT_TRUE(joint_state_group->getJacobian(link_name,point,jacobian)); for(unsigned int k=0; k < 6; k++) { for(unsigned int m=0; m < 7; m++) { EXPECT_FALSE(NOT_NEAR(jacobian_kdl(k,m),jacobian(k,m),1e-10)); } } } }
/* handle a GIMBAL_REPORT message */ void AP_Mount_MAVLink::handle_gimbal_report(mavlink_channel_t chan, mavlink_message_t *msg) { // just save it for future processing and reporting to GCS for now mavlink_msg_gimbal_report_decode(msg, &_gimbal_report); Vector3f delta_angles(_gimbal_report.delta_angle_x, _gimbal_report.delta_angle_y, _gimbal_report.delta_angle_z); Vector3f delta_velocity(_gimbal_report.delta_velocity_x, _gimbal_report.delta_velocity_y, _gimbal_report.delta_velocity_z); Vector3f joint_angles(_gimbal_report.joint_roll, _gimbal_report.joint_pitch, _gimbal_report.joint_yaw); _ekf.RunEKF(_gimbal_report.delta_time, delta_angles, delta_velocity, joint_angles); /* we have two different gimbal control algorithms. One does tilt control only, but has better control characteristics. The other does roll/tilt/yaw, but has worset control characteristics */ #if TILT_CONTROL_ONLY Vector3f rateDemand = gimbal_update_control2(_angle_ef_target_rad, _gimbal_report.delta_time, delta_angles, delta_velocity, joint_angles); #else Vector3f rateDemand = gimbal_update_control1(_angle_ef_target_rad, _gimbal_report.delta_time, delta_angles, delta_velocity, joint_angles); #endif // for now send a zero gyro bias update and incorporate into the // demanded rates Vector3f gyroBias(0,0,0); // send the gimbal control message mavlink_msg_gimbal_control_send(chan, msg->sysid, msg->compid, rateDemand.x, rateDemand.y, rateDemand.z, // demanded rates gyroBias.x, gyroBias.y, gyroBias.z); }