kemjensak / study_moveit-servo

moveit-ur

moveit_servo is copied from moveit_servo

ur5e_moveit_config is copied from ur5e_moveit_config

ur_robot_driver is copied from ur_robot_driver

Used execution(launch) files for pose tracking

1. pose_tracking_example.launch in moveit_servo/launch/

• This launch file runs pose_tracking_example node in moveit_servo.
• Corresponding executable file is pose_tracking_example.cpp(original).
• pose_tracking_example.cpp creates pose tracker(#103) and publishes target pose(#106, #149).
• Created tracker instance of PoseTracking class creates servo_ instance of Servo class(#67), subscribes target pose(#71,#239) and publishes stamped target twist(#75).
• And servo_ instance creates servo_calcs_ instance of ServoCalcs class(#79)
• servo_calcs_ instance subscribes stamped target twist published by tracker(#101)
• Finally, calculated command will be published in servo_calcs_ instance(#127) and subscribed by ur_robot_driver. (std_msgs/Float64MultiArray message type for ros_control JointGroupVelocityController)

2. ur5e_moveit_planning_execution.launch in ur5e_moveit_config/launch/

• ur5e_moveit_config package contains SRDF(ur5e.srdf), configuration files(config), launch files(launch) generated from robot's URDF model.
• This launch file loads config files and robot's srdf model.
• These files are needed for interface with MoveIt(move_group)

3. ur5e_bringup.launch in ur_robot_driver/launch/

• This launch file establishes connection with robot's control box

Target frame(e.g. EEF)에 대한 Twist 명령에 대응하는 Joint angular velocity 계산

• 상기 Pose tracking 설명에 따르면, moveit_servo최종적으로 servo_calcs_ 인스턴스가 생성됨.

• 생성 이후, ServoCalcs 클래스의 start 메소드가 실행(#305)되고, 메소드 내에서 동일 클래스의 mainCalcLoop 메소드를 호출(#203)하여 메소드 내의 while loop(#232)가 main loop로써 동작함.

• 실제 계산은 calculateSingleIteration 메소드(#248, #268)에서 호출하는 cartesianServoCalcs 메소드(#337, #432) 내에서 이뤄짐.

• #471~#498 에서 planning_frame(e.g. base_link)과 twist command 의 reference frame인 robot_link_command_frame(e.g. base_link, tool0)간의 TF를 이용하여 twist command의 reference frame을 planning_frame 과 일치하도록 변환함. 즉, commanding frame과 base frame을 일치시킴.

• Eigen의 x-dimensional vector variable(VectorXd)인 delta_x에 위 과정을 통해 변환된 twist command 대입.

• MatrixXd인 jacobian에 moveit_core의 getJacobian 메소드(#1269)를 통해 jacobian matrix 대입.

• Eigen 특이값 분해(SVD) 모듈의 JacobiSVD 클래스를 통해 thin $U$와 thin ${V}$(reduced SVD) matrix, singular value들을 구한다.(#527)

• 구한 singular value들은 asDiagonal 메소드를 통해 diagonal matrix인 $S$로 변환된다.(#529)

• $U,S,V$ 모두를 구했으므로, 아래 식과 같은 계산을 통해 pseudo inverse를 통한 jacobian inverse를 구한다.(#530) $$A = US{V^T},A^+ = VS^+U^T$$

• 이제 delta_x의 좌측에 위 과정을 통해 얻은 jacobian inverse인 pseudo_inverse를 곱한 결과가 joint의 angular velocity가 되며 이를 delta_theta_에 대입한다.(#532)

참고자료

MoveIt Servo Inverse Kinematics Improvements, 22/07/22

• 상기 jacobian pseudo inverse를 이용한 방법의 문제점으로 singularity에 근접한 경우 멈추는 문제 지적
• A primary focus of this research is redundancy resolution, in which robots with redundant degrees of freedom may utilize alternative joint configurations to achieve the same end-effector pose. By doing so, robots may be enabled to better avoid singularities, avoid collisions, and lower power consumption, to name a few benefits.
• MoveIT2 에서만 사용 가능