forked from xtdrone/XTDrone
update robotic arm
This commit is contained in:
parent
7a82693469
commit
b94018d4ea
|
@ -0,0 +1,111 @@
|
|||
#!/usr/bin/env python3
|
||||
# encoding: utf-8
|
||||
# 4自由度机械臂逆运动学:给定相应的坐标(X,Y,Z),以及俯仰角,计算出每个关节转动的角度
|
||||
# 2020/07/20 Aiden
|
||||
import logging
|
||||
from math import *
|
||||
|
||||
# CRITICAL, ERROR, WARNING, INFO, DEBUG
|
||||
logging.basicConfig(level=logging.ERROR)
|
||||
logger = logging.getLogger(__name__)
|
||||
|
||||
class IK:
|
||||
# 舵机从下往上数
|
||||
# 公用参数,即4自由度机械臂的连杆参数
|
||||
l1 = 6.10 #机械臂底盘中心到第二个舵机中心轴的距离6.10cm
|
||||
l2 = 10.16 #第二个舵机到第三个舵机的距离10.16cm
|
||||
l3 = 9.64 #第三个舵机到第四个舵机的距离9.64cm
|
||||
l4 = 0.00 #这里不做具体赋值,根据初始化时的选择进行重新赋值
|
||||
|
||||
# 气泵款特有参数
|
||||
l5 = 4.70 #第四个舵机到吸嘴正上方的距离4.70cm
|
||||
l6 = 4.46 #吸嘴正上方到吸嘴的距离4.46cm
|
||||
alpha = degrees(atan(l6 / l5)) #计算l5和l4的夹角
|
||||
|
||||
def __init__(self, arm_type): #根据不同款的夹持器,适配参数
|
||||
self.arm_type = arm_type
|
||||
if self.arm_type == 'pump': #如果是气泵款机械臂
|
||||
self.l4 = sqrt(pow(self.l5, 2) + pow(self.l6, 2)) #第四个舵机到吸嘴作为第四个连杆
|
||||
elif self.arm_type == 'arm':
|
||||
self.l4 = 16.65 #第四个舵机到机械臂末端的距离16.6cm, 机械臂末端是指爪子完全闭合时
|
||||
|
||||
def setLinkLength(self, L1=l1, L2=l2, L3=l3, L4=l4, L5=l5, L6=l6):
|
||||
# 更改机械臂的连杆长度,为了适配相同结构不同长度的机械臂
|
||||
self.l1 = L1
|
||||
self.l2 = L2
|
||||
self.l3 = L3
|
||||
self.l4 = L4
|
||||
self.l5 = L5
|
||||
self.l6 = L6
|
||||
if self.arm_type == 'pump':
|
||||
self.l4 = sqrt(pow(self.l5, 2) + pow(self.l6, 2))
|
||||
self.alpha = degrees(atan(self.l6 / self.l5))
|
||||
|
||||
def getLinkLength(self):
|
||||
# 获取当前设置的连杆长度
|
||||
if self.arm_type == 'pump':
|
||||
return {"L1":self.l1, "L2":self.l2, "L3":self.l3, "L4":self.l4, "L5":self.l5, "L6":self.l6}
|
||||
else:
|
||||
return {"L1":self.l1, "L2":self.l2, "L3":self.l3, "L4":self.l4}
|
||||
|
||||
def getRotationAngle(self, coordinate_data, Alpha):
|
||||
# 给定指定坐标和俯仰角,返回每个关节应该旋转的角度,如果无解返回False
|
||||
# coordinate_data为夹持器末端坐标,坐标单位cm, 以元组形式传入,例如(0, 5, 10)
|
||||
# Alpha为夹持器与水平面的夹角,单位度
|
||||
|
||||
# 设夹持器末端为P(X, Y, Z), 坐标原点为O, 原点为云台中心在地面的投影, P点在地面的投影为P_
|
||||
# l1与l2的交点为A, l2与l3的交点为B,l3与l4的交点为C
|
||||
# CD与PD垂直,CD与z轴垂直,则俯仰角Alpha为DC与PC的夹角, AE垂直DP_, 且E在DP_上, CF垂直AE,且F在AE上
|
||||
# 夹角表示:例如AB和BC的夹角表示为ABC
|
||||
X, Y, Z = coordinate_data
|
||||
if self.arm_type == 'pump':
|
||||
Alpha -= self.alpha
|
||||
#求底座旋转角度
|
||||
theta6 = degrees(atan2(Y, X))
|
||||
|
||||
P_O = sqrt(X*X + Y*Y) #P_到原点O距离
|
||||
CD = self.l4 * cos(radians(Alpha))
|
||||
PD = self.l4 * sin(radians(Alpha)) #当俯仰角为正时,PD为正,当俯仰角为负时,PD为负
|
||||
AF = P_O - CD
|
||||
CF = Z - self.l1 - PD
|
||||
AC = sqrt(pow(AF, 2) + pow(CF, 2))
|
||||
if round(CF, 4) < -self.l1:
|
||||
logger.debug('高度低于0, CF(%s)<l1(%s)', CF, -self.l1)
|
||||
return False
|
||||
if self.l2 + self.l3 < round(AC, 4): #两边之和小于第三边
|
||||
logger.debug('不能构成连杆结构, l2(%s) + l3(%s) < AC(%s)', self.l2, self.l3, AC)
|
||||
return False
|
||||
|
||||
#求theat4
|
||||
cos_ABC = round(-(pow(AC, 2)- pow(self.l2, 2) - pow(self.l3, 2))/(2*self.l2*self.l3), 4) #余弦定理
|
||||
if abs(cos_ABC) > 1:
|
||||
logger.debug('不能构成连杆结构, abs(cos_ABC(%s)) > 1', cos_ABC)
|
||||
return False
|
||||
ABC = acos(cos_ABC) #反三角算出弧度
|
||||
theta4 = 180.0 - degrees(ABC)
|
||||
|
||||
#求theta5
|
||||
CAF = acos(AF / AC)
|
||||
cos_BAC = round((pow(AC, 2) + pow(self.l2, 2) - pow(self.l3, 2))/(2*self.l2*AC), 4) #余弦定理
|
||||
if abs(cos_BAC) > 1:
|
||||
logger.debug('不能构成连杆结构, abs(cos_BAC(%s)) > 1', cos_BAC)
|
||||
return False
|
||||
if CF < 0:
|
||||
zf_flag = -1
|
||||
else:
|
||||
zf_flag = 1
|
||||
theta5 = degrees(CAF * zf_flag + acos(cos_BAC))
|
||||
|
||||
#求theta3
|
||||
theta3 = Alpha - theta5 + theta4
|
||||
if self.arm_type == 'pump':
|
||||
theta3 += self.alpha
|
||||
print(theta3, theta4, theta5, theta6)
|
||||
return [theta3/180*3.14, theta4/180*3.14, theta5/180*3.14, theta6/180*3.14]
|
||||
#return {"theta3":theta3, "theta4":theta4, "theta5":theta5, "theta6":theta6} # 有解时返回角度字典
|
||||
|
||||
if __name__ == '__main__':
|
||||
ik = IK('arm')
|
||||
ik.setLinkLength(L1=ik.l1 + 0.89, L4=ik.l4 - 0.3)
|
||||
print('连杆长度:', ik.getLinkLength())
|
||||
print(ik.getRotationAngle((0, 0, ik.l1 + ik.l2 + ik.l3 + ik.l4), 90))
|
|
@ -1,47 +0,0 @@
|
|||
# -*- encoding: utf-8 -*-
|
||||
import socket
|
||||
import time
|
||||
import kinematics
|
||||
import LeArm
|
||||
import sys
|
||||
IP = '192.168.149.2' #填写服务器端的IP地址
|
||||
port = 40005 #端口号必须一致
|
||||
s = socket.socket(socket.AF_INET, socket.SOCK_STREAM)
|
||||
last_target_x = 0
|
||||
last_target_y = 0
|
||||
last_target_z = 0
|
||||
try:
|
||||
s.connect((IP,port))
|
||||
except Exception as e:
|
||||
print('server not find or not open')
|
||||
sys.exit(0)
|
||||
LeArm.setServo(1, 500, 20)
|
||||
LeArm.setServo(3, 800, 20)
|
||||
LeArm.setServo(4, 900, 20)
|
||||
LeArm.setServo(5, 800, 20)
|
||||
while True:
|
||||
trigger = "Ready"
|
||||
s.sendall(trigger.encode())
|
||||
data = s.recv(1024)
|
||||
data = data.decode()
|
||||
print('recieved:',data)
|
||||
target = data.split(',')
|
||||
target_x = int(float(target[0]))
|
||||
target_y = int(float(target[1]))
|
||||
target_z = int(float(target[2]))
|
||||
if (target_x-last_target_x)**2+(target_y-last_target_y)**2+(target_z-last_target_z)**2 < 90000:
|
||||
continue
|
||||
LeArm.setServo(1, 500, 20)
|
||||
LeArm.setServo(3, 800, 20)
|
||||
LeArm.setServo(4, 900, 20)
|
||||
LeArm.setServo(5, 800, 20)
|
||||
time.sleep(0.33)
|
||||
kinematics.ki_move(target_x,target_y,target_z,20)
|
||||
time.sleep(0.33)
|
||||
LeArm.setServo(1, 1300, 20)
|
||||
time.sleep(0.5)
|
||||
last_target_x = target_x
|
||||
last_target_y = target_y
|
||||
last_target_z = target_z
|
||||
s.close()
|
||||
|
|
@ -7,8 +7,9 @@ Panels:
|
|||
- /Global Options1
|
||||
- /Status1
|
||||
- /TF1
|
||||
- /TF1/Frames1
|
||||
Splitter Ratio: 0.5
|
||||
Tree Height: 274
|
||||
Tree Height: 250
|
||||
- Class: rviz/Selection
|
||||
Name: Selection
|
||||
- Class: rviz/Tool Properties
|
||||
|
@ -57,37 +58,95 @@ Visualization Manager:
|
|||
Enabled: true
|
||||
Frame Timeout: 15
|
||||
Frames:
|
||||
All Enabled: true
|
||||
All Enabled: false
|
||||
arm_base_link:
|
||||
Value: true
|
||||
base_link:
|
||||
Value: false
|
||||
base_link_frd:
|
||||
Value: false
|
||||
base_link_inertia:
|
||||
Value: false
|
||||
camera_link:
|
||||
Value: true
|
||||
d400_aligned_depth_to_color_frame:
|
||||
Value: true
|
||||
d400_color_frame:
|
||||
Value: true
|
||||
d400_color_optical_frame:
|
||||
Value: true
|
||||
d400_depth_frame:
|
||||
Value: true
|
||||
d400_depth_optical_frame:
|
||||
Value: true
|
||||
d400_link:
|
||||
Value: true
|
||||
t265_link:
|
||||
Value: true
|
||||
t265_odom_frame:
|
||||
Value: true
|
||||
t265_pose_frame:
|
||||
Value: true
|
||||
tag_3:
|
||||
Value: true
|
||||
forearm_link:
|
||||
Value: false
|
||||
gripper_base_link:
|
||||
Value: false
|
||||
imu_link:
|
||||
Value: false
|
||||
left_finger_link:
|
||||
Value: false
|
||||
left_inner_knuckle_link:
|
||||
Value: false
|
||||
left_knuckle_link:
|
||||
Value: false
|
||||
map:
|
||||
Value: false
|
||||
map_ned:
|
||||
Value: false
|
||||
odom:
|
||||
Value: false
|
||||
odom_ned:
|
||||
Value: false
|
||||
right_finger_link:
|
||||
Value: false
|
||||
right_inner_knuckle_link:
|
||||
Value: false
|
||||
right_knuckle_link:
|
||||
Value: false
|
||||
rotor_0:
|
||||
Value: false
|
||||
rotor_1:
|
||||
Value: false
|
||||
rotor_2:
|
||||
Value: false
|
||||
rotor_3:
|
||||
Value: false
|
||||
shoulder_link:
|
||||
Value: false
|
||||
upper_arm_link:
|
||||
Value: false
|
||||
wrist_1_link:
|
||||
Value: false
|
||||
Marker Scale: 1
|
||||
Name: TF
|
||||
Show Arrows: true
|
||||
Show Axes: true
|
||||
Show Names: true
|
||||
Tree:
|
||||
base_link:
|
||||
arm_base_link:
|
||||
shoulder_link:
|
||||
upper_arm_link:
|
||||
forearm_link:
|
||||
wrist_1_link:
|
||||
gripper_base_link:
|
||||
left_inner_knuckle_link:
|
||||
left_finger_link:
|
||||
{}
|
||||
left_knuckle_link:
|
||||
{}
|
||||
right_inner_knuckle_link:
|
||||
right_finger_link:
|
||||
{}
|
||||
right_knuckle_link:
|
||||
{}
|
||||
base_link_frd:
|
||||
{}
|
||||
base_link_inertia:
|
||||
{}
|
||||
camera_link:
|
||||
tag_3:
|
||||
{}
|
||||
imu_link:
|
||||
{}
|
||||
rotor_0:
|
||||
{}
|
||||
rotor_1:
|
||||
{}
|
||||
rotor_2:
|
||||
{}
|
||||
rotor_3:
|
||||
{}
|
||||
Update Interval: 0
|
||||
Value: true
|
||||
|
@ -131,7 +190,7 @@ Visualization Manager:
|
|||
Views:
|
||||
Current:
|
||||
Class: rviz/Orbit
|
||||
Distance: 10
|
||||
Distance: 2.1567115783691406
|
||||
Enable Stereo Rendering:
|
||||
Stereo Eye Separation: 0.05999999865889549
|
||||
Stereo Focal Distance: 1
|
||||
|
@ -154,12 +213,12 @@ Visualization Manager:
|
|||
Window Geometry:
|
||||
Displays:
|
||||
collapsed: false
|
||||
Height: 1056
|
||||
Height: 1025
|
||||
Hide Left Dock: false
|
||||
Hide Right Dock: false
|
||||
Image:
|
||||
collapsed: false
|
||||
QMainWindow State: 000000ff00000000fd00000004000000000000035300000398fc0200000009fb0000001200530065006c0065006300740069006f006e00000001e10000009b0000005c00fffffffb0000001e0054006f006f006c002000500072006f007000650072007400690065007302000001ed000001df00000185000000a3fb000000120056006900650077007300200054006f006f02000001df000002110000018500000122fb000000200054006f006f006c002000500072006f0070006500720074006900650073003203000002880000011d000002210000017afb000000100044006900730070006c00610079007301000000270000019d000000c900fffffffb0000002000730065006c0065006300740069006f006e00200062007500660066006500720200000138000000aa0000023a00000294fb00000014005700690064006500530074006500720065006f02000000e6000000d2000003ee0000030bfb0000000c004b0069006e0065006300740200000186000001060000030c00000261fb0000000a0049006d00610067006501000001ca000001f50000001600ffffff000000010000010f00000398fc0200000003fb0000001e0054006f006f006c002000500072006f00700065007200740069006500730100000041000000780000000000000000fb0000000a00560069006500770073010000002700000398000000a400fffffffb0000001200530065006c0065006300740069006f006e010000025a000000b200000000000000000000000200000490000000a9fc0100000001fb0000000a00560069006500770073030000004e00000080000002e100000197000000030000073f0000003efc0100000002fb0000000800540069006d006501000000000000073f000002eb00fffffffb0000000800540069006d00650100000000000004500000000000000000000002d10000039800000004000000040000000800000008fc0000000100000002000000010000000a0054006f006f006c00730100000000ffffffff0000000000000000
|
||||
QMainWindow State: 000000ff00000000fd00000004000000000000035300000363fc0200000009fb0000001200530065006c0065006300740069006f006e00000001e10000009b0000005c00fffffffb0000001e0054006f006f006c002000500072006f007000650072007400690065007302000001ed000001df00000185000000a3fb000000120056006900650077007300200054006f006f02000001df000002110000018500000122fb000000200054006f006f006c002000500072006f0070006500720074006900650073003203000002880000011d000002210000017afb000000100044006900730070006c006100790073010000003d00000185000000c900fffffffb0000002000730065006c0065006300740069006f006e00200062007500660066006500720200000138000000aa0000023a00000294fb00000014005700690064006500530074006500720065006f02000000e6000000d2000003ee0000030bfb0000000c004b0069006e0065006300740200000186000001060000030c00000261fb0000000a0049006d00610067006501000001c8000001d80000001600ffffff000000010000010f00000363fc0200000003fb0000001e0054006f006f006c002000500072006f00700065007200740069006500730100000041000000780000000000000000fb0000000a00560069006500770073010000003d00000363000000a400fffffffb0000001200530065006c0065006300740069006f006e010000025a000000b200000000000000000000000200000490000000a9fc0100000001fb0000000a00560069006500770073030000004e00000080000002e100000197000000030000073d0000003efc0100000002fb0000000800540069006d006501000000000000073d000002eb00fffffffb0000000800540069006d00650100000000000004500000000000000000000002cf0000036300000004000000040000000800000008fc0000000100000002000000010000000a0054006f006f006c00730100000000ffffffff0000000000000000
|
||||
Selection:
|
||||
collapsed: false
|
||||
Time:
|
||||
|
@ -168,6 +227,6 @@ Window Geometry:
|
|||
collapsed: false
|
||||
Views:
|
||||
collapsed: false
|
||||
Width: 1855
|
||||
X: 65
|
||||
Y: 24
|
||||
Width: 1853
|
||||
X: 67
|
||||
Y: 27
|
||||
|
|
|
@ -5,91 +5,118 @@ import rospy
|
|||
from tf2_ros import TransformListener, Buffer
|
||||
from geometry_msgs.msg import Twist, PoseStamped, TransformStamped
|
||||
from pyquaternion import Quaternion
|
||||
import socket
|
||||
import time
|
||||
import sys
|
||||
import moveit_commander
|
||||
# from InverseKinematics import IK
|
||||
from kinematics import kinematic_analysis
|
||||
import math
|
||||
|
||||
def local_pose_callback(data):
|
||||
global local_pose, z_cnt
|
||||
if abs(local_pose.pose.position.z - data.pose.position.z) < 0.005:
|
||||
z_cnt = z_cnt + 1
|
||||
else:
|
||||
z_cnt = 0
|
||||
# if abs(local_pose.pose.position.z - data.pose.position.z) < 0.005:
|
||||
# z_cnt = z_cnt + 1
|
||||
# else:
|
||||
# z_cnt = 0
|
||||
local_pose = data
|
||||
|
||||
if __name__ == '__main__':
|
||||
vehicle_type = sys.argv[1]
|
||||
vehicle_id = sys.argv[2]
|
||||
rospy.init_node(vehicle_type+'_'+vehicle_id+'_arm_control')
|
||||
# ik = IK('arm')
|
||||
# ik.setLinkLength(L1=ik.l1 + 0.89, L4=ik.l4 - 0.3)
|
||||
# print('link length: ', ik.getLinkLength())
|
||||
moveit_commander.roscpp_initialize(sys.argv)
|
||||
arm = moveit_commander.MoveGroupCommander('arm')
|
||||
gripper = moveit_commander.MoveGroupCommander('gripper')
|
||||
# 设置机械臂运动的允许误差值
|
||||
arm.set_goal_joint_tolerance(0.001)
|
||||
gripper.set_goal_joint_tolerance(0.001)
|
||||
# 设置允许的最大速度和加速度
|
||||
arm.set_max_acceleration_scaling_factor(1)
|
||||
arm.set_max_velocity_scaling_factor(1)
|
||||
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.785, 0.785, 0, 0]
|
||||
arm.set_joint_value_target(joint_positions)
|
||||
arm.go()
|
||||
rospy.sleep(1)
|
||||
|
||||
#控制机械抓张开大角度
|
||||
grasp_angle = 0.7
|
||||
gripper.set_joint_value_target([grasp_angle,-grasp_angle,grasp_angle,-grasp_angle,grasp_angle,-grasp_angle])
|
||||
gripper.go()
|
||||
rospy.sleep(1)
|
||||
|
||||
cmd_vel_enu = Twist()
|
||||
local_pose = PoseStamped()
|
||||
time_cnt = 0
|
||||
z_cnt = 0
|
||||
rospy.Subscriber(vehicle_type+'_'+vehicle_id+"/mavros/vision_pose/pose", PoseStamped, local_pose_callback)
|
||||
cmd_vel_pub = rospy.Publisher('/xtdrone/'+vehicle_type+'_'+vehicle_id+'/cmd_vel_flu', Twist, queue_size=2)
|
||||
rospy.Subscriber(vehicle_type+'_'+vehicle_id+"/mavros/vision_pose/pose", PoseStamped, local_pose_callback)
|
||||
tfBuffer = Buffer()
|
||||
tflistener = TransformListener(tfBuffer)
|
||||
rate = rospy.Rate(20)
|
||||
rate = rospy.Rate(1)
|
||||
|
||||
IP = "192.168.149.2"
|
||||
port = 40005
|
||||
s = socket.socket(socket.AF_INET, socket.SOCK_STREAM)
|
||||
s.bind((IP,port))
|
||||
s.listen(1)
|
||||
print('listen at port :',port)
|
||||
conn,addr = s.accept()
|
||||
print('connected by',addr)
|
||||
Kp_xy = 2
|
||||
land_vel = 0.1
|
||||
|
||||
|
||||
Kp_xy = 0.5
|
||||
land_vel = 0.3
|
||||
|
||||
arm_x_bias = -0.0434
|
||||
arm_y_bias = 0.2155
|
||||
arm_z_bias = 0.02
|
||||
arm_x_bias = 0
|
||||
arm_y_bias = 0
|
||||
arm_z_bias = 0
|
||||
|
||||
while not rospy.is_shutdown():
|
||||
try:
|
||||
tfstamped = tfBuffer.lookup_transform('camera_link', 'tag_0', rospy.Time(0))
|
||||
except:
|
||||
try:
|
||||
tfstamped = tfBuffer.lookup_transform('camera_link', 'tag_1', rospy.Time(0))
|
||||
except:
|
||||
try:
|
||||
tfstamped = tfBuffer.lookup_transform('camera_link', 'tag_2', rospy.Time(0))
|
||||
except:
|
||||
try:
|
||||
tfstamped = tfBuffer.lookup_transform('camera_link', 'tag_3', rospy.Time(0))
|
||||
except:
|
||||
try:
|
||||
tfstamped = tfBuffer.lookup_transform('camera_link', 'tag_4', rospy.Time(0))
|
||||
except:
|
||||
try:
|
||||
tfstamped = tfBuffer.lookup_transform('camera_link', 'tag_5', rospy.Time(0))
|
||||
# # tfstamped = tfBuffer.lookup_transform('arm_base_link', 'tag_0', rospy.Time(0))
|
||||
l0 = tfBuffer.lookup_transform('arm_base_link', 'wrist_1_link', rospy.Time(0))
|
||||
# # l1 = tfBuffer.lookup_transform('shoulder_link', 'upper_arm_link', rospy.Time(0))
|
||||
# # l2 = tfBuffer.lookup_transform('upper_arm_link', 'forearm_link', rospy.Time(0))
|
||||
# # l3 = tfBuffer.lookup_transform('forearm_link', 'wrist_1_link', rospy.Time(0))
|
||||
print(l0.transform.translation.x)
|
||||
print(l0.transform.translation.y)
|
||||
print(l0.transform.translation.z)
|
||||
# # print(math.sqrt(l0.transform.translation.x**2+l0.transform.translation.y**2+l0.transform.translation.z**2))
|
||||
# # print(math.sqrt(l1.transform.translation.x**2+l1.transform.translation.y**2+l1.transform.translation.z**2))
|
||||
# # print(math.sqrt(l2.transform.translation.x**2+l2.transform.translation.y**2+l2.transform.translation.z**2))
|
||||
# # print(math.sqrt(l3.transform.translation.x**2+l3.transform.translation.y**2+l3.transform.translation.z**2))
|
||||
# # print('******')
|
||||
# continue
|
||||
except:
|
||||
rate.sleep()
|
||||
continue
|
||||
# print(z_cnt)
|
||||
if z_cnt < 20:
|
||||
cmd_vel_enu.linear.x = Kp_xy * (tfstamped.transform.translation.y)
|
||||
cmd_vel_enu.linear.y = Kp_xy* (tfstamped.transform.translation.x)
|
||||
cmd_vel_enu.linear.z = - land_vel
|
||||
cmd_vel_pub.publish(cmd_vel_enu)
|
||||
# if z_cnt < 20:
|
||||
# cmd_vel_enu.linear.x = Kp_xy * (tfstamped.transform.translation.y)
|
||||
# cmd_vel_enu.linear.y = - Kp_xy* (tfstamped.transform.translation.z)
|
||||
# cmd_vel_enu.linear.z = - land_vel
|
||||
# cmd_vel_pub.publish(cmd_vel_enu)
|
||||
# else:
|
||||
# target_x = (tfstamped.transform.translation.z + arm_x_bias) * 100
|
||||
# target_y = (-tfstamped.transform.translation.y + arm_y_bias) * 100
|
||||
# target_z = (tfstamped.transform.translation.x + arm_z_bias) * 100
|
||||
# print((target_x, target_y, target_z))
|
||||
target_x = 1250
|
||||
target_y = 0
|
||||
target_z = 1500
|
||||
# theta3, theta4, theta5, theta6 = ik.getRotationAngle((target_x, target_y, target_z), 90)
|
||||
# theta3, theta4, theta5, theta6 = ik.getRotationAngle((0, 0, ik.l1 + ik.l2 + ik.l3 + ik.l4), 90)
|
||||
alpha_list = []
|
||||
for alp in range(90, -90, -1):#遍历爪子与水平面的夹角,在-25到-65求解,其他范围夹取物体效果不好
|
||||
if kinematic_analysis(target_x, target_y, target_z, alp):
|
||||
alpha_list.append(alp)
|
||||
if len(alpha_list) > 0:
|
||||
if target_y > 2150:
|
||||
best_alpha = max(alpha_list)
|
||||
else:
|
||||
data = conn.recv(1024)
|
||||
data = data.decode()
|
||||
if not data:
|
||||
continue
|
||||
target_x = (tfstamped.transform.translation.x + arm_x_bias) * 1e4
|
||||
target_y = (tfstamped.transform.translation.y + arm_y_bias) * 1e4
|
||||
target_z = (tfstamped.transform.translation.z + arm_z_bias) * 1e4
|
||||
if data == 'Ready':
|
||||
print('recieved message:',data)
|
||||
send =str(target_x) +',' + str(target_y) + ',' + str(target_z)
|
||||
conn.sendall(send.encode())
|
||||
time.sleep(1)
|
||||
data = 'Not ready'
|
||||
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]
|
||||
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)
|
||||
# 控制机械臂完成运动
|
||||
arm.go()
|
||||
rate.sleep()
|
||||
|
||||
conn.close()
|
||||
s.close()
|
|
@ -0,0 +1,108 @@
|
|||
#!/usr/bin/env python3
|
||||
# coding:utf8
|
||||
# 机械臂运动学:给定相应的坐标(X,Y,Z),机械臂运行到相应的位置
|
||||
#
|
||||
from math import *
|
||||
import time
|
||||
|
||||
targetX = 0
|
||||
targetY = 0
|
||||
targetZ = 4000
|
||||
lastX = 0
|
||||
lastY = 0
|
||||
lastZ = 4000
|
||||
|
||||
l0 = 535.023363976 # 底盘到第二个舵机中心轴的距离9.6cm
|
||||
l1 = 405.587228596 # 第二个舵机到第三 个舵机的距离10.5cm
|
||||
l2 = 1092.62299079 # 第三个舵机到第四 个舵机的距离8.8cm
|
||||
l3 = 885.868465589 # 第四个舵机到机械臂(张开后)最高点的距离16.5cm
|
||||
|
||||
|
||||
alpha = 0 # 第四个舵机的仰角
|
||||
# theta3 = 0 # ID3舵机角度值
|
||||
# theta4 = 0 # ID4舵机角度值
|
||||
# theta5 = 0 # ID5舵机角度值
|
||||
# theta6 = 0 # ID6舵机角度值
|
||||
|
||||
|
||||
def kinematic_analysis(x, y, z, Alpha):
|
||||
#x,y,z为爪子张开时末端点的坐标,alpha为爪子与水平面的夹角
|
||||
#其他详细的说明请参照同目录下的机械臂标注图
|
||||
global alpha
|
||||
global l0, l1, l2, l3
|
||||
if x == 0:
|
||||
theta6 = 90.0
|
||||
elif x < 0:
|
||||
theta6 = atan(y / x)
|
||||
theta6 = 180 + (theta6 * 180.0/pi)
|
||||
else:
|
||||
theta6 = atan(y / x)
|
||||
theta6 = theta6 * 180.0 / pi
|
||||
y = sqrt(x*x + y*y) # x,y坐标的斜边
|
||||
y = y-l3 * cos(Alpha*pi/180.0) # 求出 y总 - y3 = y2 + y1
|
||||
z = z-l0-l3*sin(Alpha*pi/180.0) # 求出z1 + z2
|
||||
if z < -l0:
|
||||
return False
|
||||
if sqrt(y*y + z*z) > (l1 + l2):
|
||||
return False
|
||||
aaa = -(y*y+z*z-l1*l1-l2*l2)/(2*l1*l2)
|
||||
if aaa > 1 or aaa < -1:
|
||||
return False
|
||||
theta4 = acos(aaa) # 算出弧度
|
||||
theta4 = 180.0 - theta4 * 180.0 / pi # 转化角度
|
||||
if theta4 > 135.0 or theta4 < -135.0:
|
||||
return False
|
||||
alpha = acos(y / sqrt(y * y + z * z))
|
||||
bbb = (y*y+z*z+l1*l1-l2*l2)/(2*l1*sqrt(y*y+z*z))#余弦定理
|
||||
if bbb > 1 or bbb < -1:
|
||||
return False
|
||||
if z < 0:
|
||||
zf_flag = -1
|
||||
else:
|
||||
zf_flag = 1
|
||||
theta5 = alpha * zf_flag + acos(bbb)
|
||||
theta5 = theta5 * 180.0 / pi
|
||||
if theta5 > 180.0 or theta5 < 0:
|
||||
return False
|
||||
|
||||
theta3 = Alpha - theta5 + theta4
|
||||
if theta3 > 90.0 or theta3 < -90.0:
|
||||
return False
|
||||
return theta3, theta4, theta5, theta6 # 运行成功返回数据
|
||||
|
||||
|
||||
# 计算平均数
|
||||
def averagenum(num):
|
||||
nsum = 0
|
||||
for i in range(len(num)):
|
||||
nsum += num[i]
|
||||
return nsum / len(num)
|
||||
|
||||
|
||||
def ki_move(x, y, z, movetime):#x,y,z为给定坐标,movetime为舵机转动时间
|
||||
alpha_list = []
|
||||
for alp in range(-25, -65, -1):#遍历爪子与水平面的夹角,在-25到-65求解,其他范围夹取物体效果不好
|
||||
if kinematic_analysis(x, y, z, alp):
|
||||
alpha_list.append(alp)
|
||||
if len(alpha_list) > 0:
|
||||
if y > 2150:
|
||||
best_alpha = max(alpha_list)
|
||||
else:
|
||||
best_alpha = min(alpha_list)
|
||||
theta3, theta4, theta5, theta6 = kinematic_analysis(x, y, z, best_alpha)
|
||||
print(theta3, theta4, theta5, theta6)
|
||||
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)
|
||||
pwm_3 = int(2000.0 * theta3 / 180.0 + 1500.0)
|
||||
time.sleep(movetime / 1000.0)
|
||||
return True
|
||||
else:
|
||||
return False
|
||||
|
||||
|
||||
if __name__ == '__main__':
|
||||
try:
|
||||
ki_move(1250,0,200,1000)
|
||||
except Exception as e:
|
||||
print(e)
|
|
@ -0,0 +1,22 @@
|
|||
import rospy
|
||||
from gazebo_msgs.msg import LinkStates
|
||||
from geometry_msgs.msg import Twist, Pose, TransformStamped
|
||||
|
||||
def link_states_callback(msg):
|
||||
try:
|
||||
box_id = msg.name.index("box::base_link")
|
||||
camera_id = msg.name.index("iris_0::gripper_base_link")
|
||||
box_pose = msg.pose[box_id]
|
||||
camera_pose = msg.pose[camera_id]
|
||||
print('box: ', box_pose)
|
||||
print('camera: ', camera_pose)
|
||||
except ValueError:
|
||||
pass
|
||||
|
||||
rospy.init_node('test')
|
||||
rospy.Subscriber("/gazebo/link_states", LinkStates, link_states_callback)
|
||||
|
||||
rate = rospy.Rate(20)
|
||||
|
||||
while not rospy.is_shutdown():
|
||||
rate.sleep()
|
Loading…
Reference in New Issue