mirror of
https://github.com/wassname/kair_algorithms_draft.git
synced 2026-09-09 11:25:10 +08:00
Add doctrings and change file directory
This commit is contained in:
kdh0429
Executable
+401
@@ -0,0 +1,401 @@
|
||||
#!/usr/bin/env python
|
||||
"""Read dynamixel state and publish through ROS. Also, control dynamixel position with subscribed joint command
|
||||
|
||||
- Author: DH Kim
|
||||
- Contact: kdh0429@snu.ac.kr
|
||||
"""
|
||||
|
||||
import numpy as np
|
||||
|
||||
# ROS Imports
|
||||
import rospy
|
||||
from config.dynamixel import config as cfg
|
||||
from dynamixel_sdk import (
|
||||
DXL_HIBYTE,
|
||||
DXL_HIWORD,
|
||||
DXL_LOBYTE,
|
||||
DXL_LOWORD,
|
||||
GroupBulkRead,
|
||||
GroupSyncWrite,
|
||||
PacketHandler,
|
||||
PortHandler,
|
||||
)
|
||||
from sensor_msgs.msg import JointState
|
||||
from std_msgs.msg import Float64MultiArray
|
||||
from utils import deg2rad, rad2deg, rpm2rad
|
||||
|
||||
|
||||
class DynamixelPositionControl(object):
|
||||
"""Dynamixel read & write class."""
|
||||
def __init__(self, cfg):
|
||||
# Dynamixel Setting
|
||||
rospy.loginfo("Dynamixel Position Controller Created")
|
||||
self.cfg = cfg
|
||||
self.portHandler = PortHandler(self.cfg["DEVICENAME"])
|
||||
self.packetHandler = PacketHandler(self.cfg["PROTOCOL_VERSION"])
|
||||
self.groupSyncWrite = GroupSyncWrite(
|
||||
self.portHandler,
|
||||
self.packetHandler,
|
||||
self.cfg["ADDR_GOAL_POSITION"],
|
||||
self.cfg["LEN_GOAL_POSITION"],
|
||||
)
|
||||
self.groupBulkReadPosition = GroupBulkRead(self.portHandler, self.packetHandler)
|
||||
self.groupBulkReadVelocity = GroupBulkRead(self.portHandler, self.packetHandler)
|
||||
self.groupBulkReadCurrent = GroupBulkRead(self.portHandler, self.packetHandler)
|
||||
# Port Open
|
||||
if self.portHandler.openPort():
|
||||
print("Succeeded to open the port")
|
||||
else:
|
||||
print("Failed to open the port")
|
||||
quit()
|
||||
|
||||
# Set port baudrate
|
||||
if self.portHandler.setBaudRate(self.cfg["BAUDRATE"]):
|
||||
print("Succeeded to change the baudrate")
|
||||
else:
|
||||
print("Failed to change the baudrate")
|
||||
quit()
|
||||
|
||||
self.packetHandler.write1ByteTxRx(
|
||||
self.portHandler, self.cfg["DXL1_ID"], self.cfg["ADDR_OP_MODE"], 3
|
||||
)
|
||||
self.packetHandler.write1ByteTxRx(
|
||||
self.portHandler, self.cfg["DXL2_ID"], self.cfg["ADDR_OP_MODE"], 3
|
||||
)
|
||||
self.packetHandler.write1ByteTxRx(
|
||||
self.portHandler, self.cfg["DXL3_ID"], self.cfg["ADDR_OP_MODE"], 3
|
||||
)
|
||||
self.packetHandler.write1ByteTxRx(
|
||||
self.portHandler, self.cfg["DXL4_ID"], self.cfg["ADDR_OP_MODE"], 3
|
||||
)
|
||||
|
||||
self.groupBulkReadPosition.addParam(
|
||||
self.cfg["DXL1_ID"],
|
||||
self.cfg["ADDR_PRESENT_POSITION"],
|
||||
self.cfg["LEN_PRESENT_POSITION"],
|
||||
)
|
||||
self.groupBulkReadPosition.addParam(
|
||||
self.cfg["DXL2_ID"],
|
||||
self.cfg["ADDR_PRESENT_POSITION"],
|
||||
self.cfg["LEN_PRESENT_POSITION"],
|
||||
)
|
||||
self.groupBulkReadPosition.addParam(
|
||||
self.cfg["DXL3_ID"],
|
||||
self.cfg["ADDR_PRESENT_POSITION"],
|
||||
self.cfg["LEN_PRESENT_POSITION"],
|
||||
)
|
||||
self.groupBulkReadPosition.addParam(
|
||||
self.cfg["DXL4_ID"],
|
||||
self.cfg["ADDR_PRESENT_POSITION"],
|
||||
self.cfg["LEN_PRESENT_POSITION"],
|
||||
)
|
||||
self.groupBulkReadVelocity.addParam(
|
||||
self.cfg["DXL1_ID"],
|
||||
self.cfg["ADDR_PRESENT_VELOCITY"],
|
||||
self.cfg["LEN_PRESENT_VELOCITY"],
|
||||
)
|
||||
self.groupBulkReadVelocity.addParam(
|
||||
self.cfg["DXL2_ID"],
|
||||
self.cfg["ADDR_PRESENT_VELOCITY"],
|
||||
self.cfg["LEN_PRESENT_VELOCITY"],
|
||||
)
|
||||
self.groupBulkReadVelocity.addParam(
|
||||
self.cfg["DXL3_ID"],
|
||||
self.cfg["ADDR_PRESENT_VELOCITY"],
|
||||
self.cfg["LEN_PRESENT_VELOCITY"],
|
||||
)
|
||||
self.groupBulkReadVelocity.addParam(
|
||||
self.cfg["DXL4_ID"],
|
||||
self.cfg["ADDR_PRESENT_VELOCITY"],
|
||||
self.cfg["LEN_PRESENT_VELOCITY"],
|
||||
)
|
||||
|
||||
self.groupBulkReadCurrent.addParam(
|
||||
self.cfg["DXL1_ID"],
|
||||
self.cfg["ADDR_PRESENT_CURRENT"],
|
||||
self.cfg["LEN_PRESENT_CURRENT"],
|
||||
)
|
||||
self.groupBulkReadCurrent.addParam(
|
||||
self.cfg["DXL2_ID"],
|
||||
self.cfg["ADDR_PRESENT_CURRENT"],
|
||||
self.cfg["LEN_PRESENT_CURRENT"],
|
||||
)
|
||||
self.groupBulkReadCurrent.addParam(
|
||||
self.cfg["DXL3_ID"],
|
||||
self.cfg["ADDR_PRESENT_CURRENT"],
|
||||
self.cfg["LEN_PRESENT_CURRENT"],
|
||||
)
|
||||
self.groupBulkReadCurrent.addParam(
|
||||
self.cfg["DXL4_ID"],
|
||||
self.cfg["ADDR_PRESENT_CURRENT"],
|
||||
self.cfg["LEN_PRESENT_CURRENT"],
|
||||
)
|
||||
|
||||
# Enable Dynamixel Torque
|
||||
self.packetHandler.write1ByteTxRx(
|
||||
self.portHandler,
|
||||
self.cfg["DXL1_ID"],
|
||||
self.cfg["ADDR_TORQUE_ENABLE"],
|
||||
self.cfg["TORQUE_ENABLE"],
|
||||
)
|
||||
self.packetHandler.write1ByteTxRx(
|
||||
self.portHandler,
|
||||
self.cfg["DXL2_ID"],
|
||||
self.cfg["ADDR_TORQUE_ENABLE"],
|
||||
self.cfg["TORQUE_ENABLE"],
|
||||
)
|
||||
self.packetHandler.write1ByteTxRx(
|
||||
self.portHandler,
|
||||
self.cfg["DXL3_ID"],
|
||||
self.cfg["ADDR_TORQUE_ENABLE"],
|
||||
self.cfg["TORQUE_ENABLE"],
|
||||
)
|
||||
self.packetHandler.write1ByteTxRx(
|
||||
self.portHandler,
|
||||
self.cfg["DXL4_ID"],
|
||||
self.cfg["ADDR_TORQUE_ENABLE"],
|
||||
self.cfg["TORQUE_ENABLE"],
|
||||
)
|
||||
|
||||
# ROS Publisher
|
||||
self.joint_states_pub = rospy.Publisher(
|
||||
"/open_manipulator/joint_states_real", JointState, queue_size=3
|
||||
)
|
||||
# ROS Subcriber
|
||||
self.joint_pos_command_sub = rospy.Subscriber(
|
||||
"/open_manipulator/joint_position/command",
|
||||
Float64MultiArray,
|
||||
self.joint_command_cb,
|
||||
)
|
||||
|
||||
self.joint_states = JointState()
|
||||
self.dxl_present_position = np.zeros(4)
|
||||
self.dxl_present_velocity = np.zeros(4)
|
||||
self.dxl_present_current = np.zeros(4)
|
||||
self.q_desired = np.zeros(4)
|
||||
self.dxl_goal_position = [
|
||||
[0, 0, 0, 0],
|
||||
[0, 0, 0, 0],
|
||||
[0, 0, 0, 0],
|
||||
[0, 0, 0, 0],
|
||||
]
|
||||
self.read_dxl()
|
||||
for i in range(4):
|
||||
self.dxl_goal_position[i] = [
|
||||
DXL_LOBYTE(DXL_LOWORD(int(self.dxl_present_position[i]))),
|
||||
DXL_HIBYTE(DXL_LOWORD(int(self.dxl_present_position[i]))),
|
||||
DXL_LOBYTE(DXL_HIWORD(int(self.dxl_present_position[i]))),
|
||||
DXL_HIBYTE(DXL_HIWORD(int(self.dxl_present_position[i]))),
|
||||
]
|
||||
|
||||
self.r = rospy.Rate(100)
|
||||
try:
|
||||
while not rospy.is_shutdown():
|
||||
self.read_dxl()
|
||||
self.write_dxl()
|
||||
self.r.sleep()
|
||||
except KeyboardInterrupt:
|
||||
self.packetHandler.write1ByteTxRx(
|
||||
self.portHandler,
|
||||
self.cfg["DXL1_ID"],
|
||||
self.cfg["ADDR_TORQUE_ENABLE"],
|
||||
self.cfg["TORQUE_DISABLE"],
|
||||
)
|
||||
self.packetHandler.write1ByteTxRx(
|
||||
self.portHandler,
|
||||
self.cfg["DXL2_ID"],
|
||||
self.cfg["ADDR_TORQUE_ENABLE"],
|
||||
self.cfg["TORQUE_DISABLE"],
|
||||
)
|
||||
self.packetHandler.write1ByteTxRx(
|
||||
self.portHandler,
|
||||
self.cfg["DXL3_ID"],
|
||||
self.cfg["ADDR_TORQUE_ENABLE"],
|
||||
self.cfg["TORQUE_DISABLE"],
|
||||
)
|
||||
self.packetHandler.write1ByteTxRx(
|
||||
self.portHandler,
|
||||
self.cfg["DXL4_ID"],
|
||||
self.cfg["ADDR_TORQUE_ENABLE"],
|
||||
self.cfg["TORQUE_DISABLE"],
|
||||
)
|
||||
|
||||
def joint_command_cb(self, joint_desired):
|
||||
""" Transform subscribed joint command to dynamixel byte information."""
|
||||
i = 0
|
||||
while i < 4:
|
||||
self.q_desired[i] = joint_desired.data[i]
|
||||
dxl_command = int(
|
||||
rad2deg(self.q_desired[i]) / self.cfg["DXL_RESOLUTION"]
|
||||
+ self.cfg["DXL_POS_OFFSET"]
|
||||
)
|
||||
if dxl_command > self.cfg["CW_LIMIT"]:
|
||||
dxl_command = self.cfg["CW_LIMIT"]
|
||||
elif dxl_command < self.cfg["CCW_LIMIT"]:
|
||||
dxl_command = self.cfg["CCW_LIMIT"]
|
||||
|
||||
self.dxl_goal_position[i] = [
|
||||
DXL_LOBYTE(DXL_LOWORD(dxl_command)),
|
||||
DXL_HIBYTE(DXL_LOWORD(dxl_command)),
|
||||
DXL_LOBYTE(DXL_HIWORD(dxl_command)),
|
||||
DXL_HIBYTE(DXL_HIWORD(dxl_command)),
|
||||
]
|
||||
i += 1
|
||||
|
||||
def read_dxl(self):
|
||||
""" Read dynamixel position, velocity, current value and publish through ROS."""
|
||||
self.groupBulkReadPosition.txRxPacket()
|
||||
|
||||
self.dxl_present_position[0] = self.groupBulkReadPosition.getData(
|
||||
self.cfg["DXL1_ID"],
|
||||
self.cfg["ADDR_PRESENT_POSITION"],
|
||||
self.cfg["LEN_PRESENT_POSITION"],
|
||||
)
|
||||
self.dxl_present_position[1] = self.groupBulkReadPosition.getData(
|
||||
self.cfg["DXL2_ID"],
|
||||
self.cfg["ADDR_PRESENT_POSITION"],
|
||||
self.cfg["LEN_PRESENT_POSITION"],
|
||||
)
|
||||
self.dxl_present_position[2] = self.groupBulkReadPosition.getData(
|
||||
self.cfg["DXL3_ID"],
|
||||
self.cfg["ADDR_PRESENT_POSITION"],
|
||||
self.cfg["LEN_PRESENT_POSITION"],
|
||||
)
|
||||
self.dxl_present_position[3] = self.groupBulkReadPosition.getData(
|
||||
self.cfg["DXL4_ID"],
|
||||
self.cfg["ADDR_PRESENT_POSITION"],
|
||||
self.cfg["LEN_PRESENT_POSITION"],
|
||||
)
|
||||
|
||||
self.groupBulkReadVelocity.txRxPacket()
|
||||
self.dxl_present_velocity[0] = self.groupBulkReadVelocity.getData(
|
||||
self.cfg["DXL1_ID"],
|
||||
self.cfg["ADDR_PRESENT_VELOCITY"],
|
||||
self.cfg["LEN_PRESENT_VELOCITY"],
|
||||
)
|
||||
self.dxl_present_velocity[1] = self.groupBulkReadVelocity.getData(
|
||||
self.cfg["DXL2_ID"],
|
||||
self.cfg["ADDR_PRESENT_VELOCITY"],
|
||||
self.cfg["LEN_PRESENT_VELOCITY"],
|
||||
)
|
||||
self.dxl_present_velocity[2] = self.groupBulkReadVelocity.getData(
|
||||
self.cfg["DXL3_ID"],
|
||||
self.cfg["ADDR_PRESENT_VELOCITY"],
|
||||
self.cfg["LEN_PRESENT_VELOCITY"],
|
||||
)
|
||||
self.dxl_present_velocity[3] = self.groupBulkReadVelocity.getData(
|
||||
self.cfg["DXL4_ID"],
|
||||
self.cfg["ADDR_PRESENT_VELOCITY"],
|
||||
self.cfg["LEN_PRESENT_VELOCITY"],
|
||||
)
|
||||
|
||||
self.groupBulkReadCurrent.txRxPacket()
|
||||
self.dxl_present_current[0] = self.groupBulkReadVelocity.getData(
|
||||
self.cfg["DXL1_ID"],
|
||||
self.cfg["ADDR_PRESENT_CURRENT"],
|
||||
self.cfg["LEN_PRESENT_CURRENT"],
|
||||
)
|
||||
self.dxl_present_current[1] = self.groupBulkReadVelocity.getData(
|
||||
self.cfg["DXL2_ID"],
|
||||
self.cfg["ADDR_PRESENT_CURRENT"],
|
||||
self.cfg["LEN_PRESENT_CURRENT"],
|
||||
)
|
||||
self.dxl_present_current[2] = self.groupBulkReadVelocity.getData(
|
||||
self.cfg["DXL3_ID"],
|
||||
self.cfg["ADDR_PRESENT_CURRENT"],
|
||||
self.cfg["LEN_PRESENT_CURRENT"],
|
||||
)
|
||||
self.dxl_present_current[3] = self.groupBulkReadVelocity.getData(
|
||||
self.cfg["DXL4_ID"],
|
||||
self.cfg["ADDR_PRESENT_CURRENT"],
|
||||
self.cfg["LEN_PRESENT_CURRENT"],
|
||||
)
|
||||
|
||||
for i in range(4):
|
||||
if self.dxl_present_velocity[i] > 2 ** (
|
||||
8 * self.cfg["ADDR_PRESENT_VELOCITY"] / 2
|
||||
):
|
||||
self.dxl_present_velocity[i] = self.dxl_present_velocity[i] - 2 ** (
|
||||
8 * self.cfg["ADDR_PRESENT_VELOCITY"]
|
||||
)
|
||||
if self.dxl_present_current[i] > 2 ** (
|
||||
8 * self.cfg["LEN_PRESENT_CURRENT"] / 2
|
||||
):
|
||||
self.dxl_present_current[i] = self.dxl_present_current[i] - 2 ** (
|
||||
8 * self.cfg["LEN_PRESENT_CURRENT"]
|
||||
)
|
||||
|
||||
q_current = [
|
||||
0.0,
|
||||
0.0,
|
||||
deg2rad(
|
||||
(self.dxl_present_position[0] - self.cfg["DXL_POS_OFFSET"])
|
||||
* self.cfg["DXL_RESOLUTION"]
|
||||
),
|
||||
deg2rad(
|
||||
(self.dxl_present_position[1] - self.cfg["DXL_POS_OFFSET"])
|
||||
* self.cfg["DXL_RESOLUTION"]
|
||||
),
|
||||
deg2rad(
|
||||
(self.dxl_present_position[2] - self.cfg["DXL_POS_OFFSET"])
|
||||
* self.cfg["DXL_RESOLUTION"]
|
||||
),
|
||||
deg2rad(
|
||||
(self.dxl_present_position[3] - self.cfg["DXL_POS_OFFSET"])
|
||||
* self.cfg["DXL_RESOLUTION"]
|
||||
),
|
||||
]
|
||||
qdot_current = [
|
||||
0.0,
|
||||
0.0,
|
||||
rpm2rad(self.dxl_present_velocity[0] * self.cfg["DXL_VELOCITY_RESOLUTION"]),
|
||||
rpm2rad(self.dxl_present_velocity[1] * self.cfg["DXL_VELOCITY_RESOLUTION"]),
|
||||
rpm2rad(self.dxl_present_velocity[2] * self.cfg["DXL_VELOCITY_RESOLUTION"]),
|
||||
rpm2rad(self.dxl_present_velocity[3] * self.cfg["DXL_VELOCITY_RESOLUTION"]),
|
||||
]
|
||||
motor_current = [
|
||||
0.0,
|
||||
0.0,
|
||||
self.dxl_present_current[0] * self.cfg["DXL_TO_CURRENT"],
|
||||
self.dxl_present_current[1] * self.cfg["DXL_TO_CURRENT"],
|
||||
self.dxl_present_current[2] * self.cfg["DXL_TO_CURRENT"],
|
||||
self.dxl_present_current[3] * self.cfg["DXL_TO_CURRENT"],
|
||||
]
|
||||
|
||||
self.joint_states.position = q_current
|
||||
self.joint_states.velocity = qdot_current
|
||||
self.joint_states.effort = motor_current
|
||||
|
||||
self.joint_states_pub.publish(self.joint_states)
|
||||
|
||||
def write_dxl(self):
|
||||
""" Write joint command to dynamixel."""
|
||||
self.groupSyncWrite.addParam(self.cfg["DXL1_ID"], self.dxl_goal_position[0])
|
||||
self.groupSyncWrite.addParam(self.cfg["DXL2_ID"], self.dxl_goal_position[1])
|
||||
self.groupSyncWrite.addParam(self.cfg["DXL3_ID"], self.dxl_goal_position[2])
|
||||
self.groupSyncWrite.addParam(self.cfg["DXL4_ID"], self.dxl_goal_position[3])
|
||||
|
||||
self.groupSyncWrite.txPacket()
|
||||
self.groupSyncWrite.clearParam()
|
||||
|
||||
def error_check(self, dxl_comm_result, dxl_error):
|
||||
""" Check dynamixel error."""
|
||||
if dxl_comm_result != self.cfg["COMM_SUCCESS"]:
|
||||
print("%s" % self.packetHandler.getTxRxResult(dxl_comm_result))
|
||||
elif dxl_error != 0:
|
||||
print("%s" % self.packetHandler.getRxPacketError(dxl_error))
|
||||
|
||||
|
||||
def main():
|
||||
rospy.init_node("dynamixel_read_write")
|
||||
|
||||
try:
|
||||
DynamixelPositionControl(cfg)
|
||||
except rospy.ROSInterruptException:
|
||||
pass
|
||||
|
||||
rospy.spin()
|
||||
|
||||
|
||||
if __name__ == "__main__":
|
||||
main()
|
||||
Reference in New Issue
Block a user