aah98
/
XTDrone
forked from xtdrone/XTDrone
1
0
Fork 0
Code Issues Pull Requests Releases Wiki Activity

update robotic arm

This commit is contained in:
robin_shaun 2021-10-07 22:14:16 +08:00
parent 7a82693469
commit b94018d4ea
6 changed files with 420 additions and 140 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

111
control/dev/arm/InverseKinematics.py Executable file
View File

@ -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))

47
control/dev/arm/arm.py
View File

@ -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()

119
control/dev/arm/arm.rviz
View File

@ -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:
camera_link:
tag_3:
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:
{}
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

153
control/dev/arm/arm_control.py
View File

@ -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))
# # 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:
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))
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)
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'
rate.sleep()
conn.close()
s.close()
continue
# 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:
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()

108
control/dev/arm/kinematics.py Normal file
View File

@ -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)

22
control/dev/arm/test.py Normal file
View File

@ -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()