add logo

README.en.md

@@ -1,4 +1,4 @@
-# XTDrone
+<img src="./images/logo.jpg" width="256"  />
 
 <div id="sidebar"><a href="./README.md" target="_blank"><font color=#0000FF size=5px >[中文版]<font></center><a></div>
 

README.md

@@ -1,4 +1,4 @@
-# XTDrone
+<img src="./images/logo.jpg" width="256"  />
 
 <div id="sidebar"><a href="./README.en.md" target="_blank"><font color=#0000FF size=5px >[ENGLISH]<font></center><a></div>
 

communication/multirotor_communication.py

@@ -135,7 +135,7 @@ class Communication:
         if self.hover_flag == 0:
             self.coordinate_frame = 8
             self.motion_type = 1     
-            self.target_motion = self.construct_target(vx=msg.linear.x,vy=msg.linear.y,vz=msg.linear.z,yaw = -math.pi/2)       
+            self.target_motion = self.construct_target(vx=msg.linear.x,vy=msg.linear.y,vz=msg.linear.z,yaw = msg.angular.z)       
  
     def cmd_vel_enu_callback(self, msg):
         self.hover_state_transition(msg.linear.x, msg.linear.y, msg.linear.z, msg.angular.z)

control/dev/arm/arm_control.py

@@ -39,7 +39,7 @@ if __name__ == '__main__':
     gripper.set_max_acceleration_scaling_factor(1)
     gripper.set_max_velocity_scaling_factor(1)
     # 控制机械臂先回到特定位置
-    joint_positions = [0, 1.57, -0.785, -0.785, 0]
+    joint_positions = [0, 0, 0.785, 0, 0]
     #joint_positions = [0, 0.785, 0.785, 0, 0]
     arm.set_joint_value_target(joint_positions)
     arm.go()
@@ -51,6 +51,42 @@ if __name__ == '__main__':
     gripper.go()
     rospy.sleep(1)
 
+    # 获取终端link的名称
+    end_effector_link = arm.get_end_effector_link()
+
+    # 设置目标位置所使用的参考坐标系
+    reference_frame = 'arm_base_link'
+    arm.set_pose_reference_frame(reference_frame)
+
+    # 当运动规划失败后，允许重新规划
+    arm.allow_replanning(True)
+
+    # 设置机械臂工作空间中的目标位姿，位置使用x、y、z坐标描述，
+    # 姿态使用四元数描述，基于base_link坐标系
+    target_pose = PoseStamped()
+    target_pose.header.frame_id = reference_frame
+    target_pose.header.stamp = rospy.Time.now()     
+    target_pose.pose.position.x = 0.0636649477824
+    target_pose.pose.position.y = -0.00552153656686
+    target_pose.pose.position.z = 0.26141858437
+    target_pose.pose.orientation.x = 0.70692
+    target_pose.pose.orientation.y = 0.0
+    target_pose.pose.orientation.z = 0.0
+    target_pose.pose.orientation.w = 0.70729
+
+    # 设置机器臂当前的状态作为运动初始状态
+    arm.set_start_state_to_current_state()
+    
+    # 设置机械臂终端运动的目标位姿
+    arm.set_pose_target(target_pose, end_effector_link)
+    
+    # 规划运动路径
+    traj = arm.plan()
+    
+    # 按照规划的运动路径控制机械臂运动
+    arm.execute(traj)
+    rospy.sleep(1)
+
     cmd_vel_enu = Twist()   
     local_pose = PoseStamped()
     time_cnt = 0
@@ -112,8 +148,9 @@ if __name__ == '__main__':
             else:
                 best_alpha = min(alpha_list)
         theta3, theta4, theta5, theta6 = kinematic_analysis(target_x, target_y, target_z, best_alpha)
-        joint_positions = [theta6/180*3.14, 1.57-theta5/180*3.14, -theta4/180*3.14, theta3/180*3.14, 0]
+        print(theta3, theta4, theta5, theta6)
-        joint_positions[1] = joint_positions[1] + 0.785
+        joint_positions = [theta6/180*3.14, theta5/180*3.14, theta4/180*3.14, -theta3/180*3.14, 0]
+        joint_positions[1] = joint_positions[1] - 0.785
         joint_positions[2] = joint_positions[2] + 0.785
         print(joint_positions)
         arm.set_joint_value_target(joint_positions)

control/dev/arm/kinematics.py

@@ -60,14 +60,14 @@ def kinematic_analysis(x, y, z, Alpha):
         zf_flag = -1
     else:
         zf_flag = 1
-    theta5 = alpha * zf_flag + acos(bbb)
+    theta5 = alpha * zf_flag - acos(bbb)
     theta5 = theta5 * 180.0 / pi
     if theta5 > 180.0 or theta5 < 0:
         return False
 
-    theta3 = Alpha - theta5 + theta4
+    theta3 = theta5 + theta4 - Alpha
-    if theta3 > 90.0 or theta3 < -90.0:
+    # if theta3 > 90.0 or theta3 < -90.0:
-        return False
+    #     return False
     return theta3, theta4, theta5, theta6     # 运行成功返回数据
 
 
@@ -90,7 +90,7 @@ def ki_move(x, y, z, movetime):#x，y，z为给定坐标，movetime为舵机转
         else:
             best_alpha = min(alpha_list)
         theta3, theta4, theta5, theta6 = kinematic_analysis(x, y, z, best_alpha)
-        print(theta3, theta4, theta5, theta6)
+        print(theta3, theta4, theta5, theta6, best_alpha)
         pwm_6 = int(2000.0 * theta6 / 180.0 + 500.0)
         pwm_5 = int(2000.0 * (90.0 - theta5) / 180.0 + 1500.0)
         pwm_4 = int(2000.0 * (135.0 - theta4) / 270.0 + 500.0)
@@ -103,6 +103,6 @@ def ki_move(x, y, z, movetime):#x，y，z为给定坐标，movetime为舵机转
 
 if __name__ == '__main__':
      try:
-        ki_move(1250,0,200,1000)
+        ki_move(1250,0,1500,1000)
      except Exception as e:
          print(e)

control/dev/arm/moveit_ik_demo.py

@@ -0,0 +1,95 @@
+#!/usr/bin/env python
+# -*- coding: utf-8 -*-
+
+# Copyright 2019 Wuhan PS-Micro Technology Co., Itd.
+#
+# Licensed under the Apache License, Version 2.0 (the "License");
+# you may not use this file except in compliance with the License.
+# You may obtain a copy of the License at
+#
+#     http://www.apache.org/licenses/LICENSE-2.0
+#
+# Unless required by applicable law or agreed to in writing, software
+# distributed under the License is distributed on an "AS IS" BASIS,
+# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+# See the License for the specific language governing permissions and
+# limitations under the License.
+
+import rospy, sys
+import moveit_commander
+from geometry_msgs.msg import PoseStamped, Pose
+
+
+class MoveItIkDemo:
+    def __init__(self):
+        # 初始化move_group的API
+        moveit_commander.roscpp_initialize(sys.argv)
+        
+        # 初始化ROS节点
+        rospy.init_node('moveit_ik_demo')
+                
+        # 初始化需要使用move group控制的机械臂中的arm group
+        arm = moveit_commander.MoveGroupCommander('manipulator')
+                
+        # 获取终端link的名称
+        end_effector_link = arm.get_end_effector_link()
+                        
+        # 设置目标位置所使用的参考坐标系
+        reference_frame = 'base_link'
+        arm.set_pose_reference_frame(reference_frame)
+                
+        # 当运动规划失败后，允许重新规划
+        arm.allow_replanning(True)
+        
+        # 设置位置(单位：米)和姿态（单位：弧度）的允许误差
+        arm.set_goal_position_tolerance(0.001)
+        arm.set_goal_orientation_tolerance(0.01)
+       
+        # 设置允许的最大速度和加速度
+        arm.set_max_acceleration_scaling_factor(0.5)
+        arm.set_max_velocity_scaling_factor(0.5)
+
+        # 控制机械臂先回到初始化位置
+        arm.set_named_target('home')
+        arm.go()
+        rospy.sleep(1)
+               
+        # 设置机械臂工作空间中的目标位姿，位置使用x、y、z坐标描述，
+        # 姿态使用四元数描述，基于base_link坐标系
+        target_pose = PoseStamped()
+        target_pose.header.frame_id = reference_frame
+        target_pose.header.stamp = rospy.Time.now()     
+        target_pose.pose.position.x = 0.2593
+        target_pose.pose.position.y = 0.0636
+        target_pose.pose.position.z = 0.1787
+        target_pose.pose.orientation.x = 0.70692
+        target_pose.pose.orientation.y = 0.0
+        target_pose.pose.orientation.z = 0.0
+        target_pose.pose.orientation.w = 0.70729
+        
+        # 设置机器臂当前的状态作为运动初始状态
+        arm.set_start_state_to_current_state()
+        
+        # 设置机械臂终端运动的目标位姿
+        arm.set_pose_target(target_pose, end_effector_link)
+        
+        # 规划运动路径
+        traj = arm.plan()
+        
+        # 按照规划的运动路径控制机械臂运动
+        arm.execute(traj)
+        rospy.sleep(1)
+
+        # 控制机械臂回到初始化位置
+        arm.set_named_target('home')
+        arm.go()
+
+        # 关闭并退出moveit
+        moveit_commander.roscpp_shutdown()
+        moveit_commander.os._exit(0)
+
+if __name__ == "__main__":
+    MoveItIkDemo()
+
+    
+