def __init__(self, ids, offsets):
self.helper = DxlHelper("preset.json", verbosity="detailed")
self.ids = ids
self.motors = {}
self.motor_offsets = offsets
for id in self.ids:
self.motors.__setitem__(id, self.helper.get_motor(id))
def main(self):
# /home/sujong/catkin_ws/src/dataset_autostation/script/
helper = DxlHelper(
"/home/sujongkim/dataset_ws/src/dataset-autostation/script/my_preset.json"
)
motor_id = 1
motor = helper.get_motor(motor_id)
motor.set_torque(True)
# _max = 4294967295
rospy.logwarn("Before the loop")
rate = rospy.Rate(30) # Hz
# Loop
counter = 0
offset_deg = 2
for x in self.angle_set:
# unit_i
self.i = x
counter += 1
print("\n# is {} [loop counter: {}]".format(self.i, counter))
t_cur = rospy.get_time()
# move
target = self.deg2dxlunit((self.i * self.unit_deg) + offset_deg)
motor.set_goal_position(target)
# wait
self.wait_for_target(motor, target)
# initialize done flag to (False)
self.init_flags()
# Wait for all done
while not rospy.is_shutdown():
if self.is_all_done():
break
rate.sleep()
dt = rospy.get_time() - t_cur
rospy.logwarn("dt is {}".format(dt))
motor.set_torque(False)
print("Shooting is done!!!")
rospy.signal_shutdown("Shooting is done!!!")
#!/usr/bin/env python
# -*- coding: utf-8 -*-
from dynamixel_helper import DxlHelper
helper = DxlHelper("preset/example_0.json")
motor_id = 0
motor = helper.get_motor(motor_id)
motor.set_torque(True)
print("--------------------")
dxl_unit, _ = motor.get_present_position()
print(motor.set_goal_position((dxl_unit + 2048) % 4096))
print("--------------------")
#!/usr/bin/env python
# -*- coding: utf-8 -*-
from dynamixel_helper import DxlHelper
def msg(motor_id, goal, position):
return "[ID:%03d] GoalPos:%03d PresPos:%03d" % (motor_id, goal, position)
helper = DxlHelper("preset/example_2.json")
boundaries = [1500, 2500]
position = [0, 0]
number = 2
motors = [helper.get_motor(i) for i in range(number)]
motors[0].set_torque(True)
motors[1].set_torque(True)
print("--------------------")
count = 0
while count < 5:
print("\nLoop {}".format(count))
goal = boundaries[count % 2]
motors[0].set_goal_position(goal)
motors[1].set_goal_position(goal)
while True:
# -*- coding: utf-8 -*-
import os
import time
# base_dir = os.path.dirname(os.path.abspath(__file__))
activate_this = os.path.expanduser(
'~/virtualenv/dxlhelper_py2test/bin/activate_this.py')
execfile(activate_this, dict(__file__=activate_this))
# import os
from dynamixel_helper import DxlHelper
# current_dir = os.path.dirname(os.path.abspath(__file__))
# helper = DxlHelper(current_dir + "/preset/2motor_2port.json", verbosity='detailed')
helper = DxlHelper("auto.json")
# helper = DxlHelper("auto.json", verbosity='detailed')
motor = helper.get_motor('joint_1')
motor.set_torque(False)
motor.set_operating_mode(3) # position mode
motor.set_torque(True)
dxl_unit, _ = motor.get_present_position()
v = 1000
print(motor.set_goal_position((dxl_unit + v) % 4096))
motor.set_torque(False)
motor.set_operating_mode(1) # velocity mode
motor.set_torque(True)
class Arm:
"""
This class creates an instance of a complete daisy chained dynamixel arm from one USB port.
This class gives access to easily manipulate each motor of the dynamixel arm easily and can be used in the ArmPositionController class to control the arm using inverse kinematics.
:param int[] ids: An array of 5 dynamixel ids that corrospond the the ids of the 5 dynamixel arm motors
:param offsets: A dictionary of <id: offset> where offset in the offset in degrees of the motor id
:type offsets: dict<int: float>
"""
def __init__(self, ids, offsets):
self.helper = DxlHelper("preset.json", verbosity="detailed")
self.ids = ids
self.motors = {}
self.motor_offsets = offsets
for id in self.ids:
self.motors.__setitem__(id, self.helper.get_motor(id))
def set_torque(self, motors, value):
"""
This method sets the torque of one or more motors
:param int[] motors: The array of motor ids to change the torque of
:param bool value: The value of the torque to be changed to
"""
for motor in motors:
self.motors[motor].set_torque(value)
def theta_to_pos(self, theta):
"""
This method converts from theta in degrees to a dynamixel position
:param theta: The value in degrees to be converted
:type theta: float or int
:return: The theta value converted to dynamixel poistion
:rtype: int or float
"""
# print(int((theta/360)*4096))
return int((theta / 360) * 4096)
def set_angle(self, id, theta):
"""
This method sets a dynamixel motor to the angle specified
:param int id: The id of the dynamixel motor
:param theta: The angle to set the motor to
:type theta: float or int
"""
position = self.theta_to_pos(theta + self.motor_offsets[id])
self.motors[id].set_goal_position(position)
def set_angles(self, angles, delay=0):
"""
This method sets the angles of mulitple dynamixel motors
:param angles: A dictionary in the format <id: angle>
:type angles: dict<int: int>
"""
for id, theta in angles.items():
position = self.theta_to_pos(theta + self.motor_offsets[id])
self.motors[id].set_goal_position(position)
sleep(delay)
def read_write_position(self, motor, js_value):
"""
This method reads the position of a motor and increments the position based on the js_value
:param int motor: The id of the motor
:param float js_value: The value to increment the motor position by
"""
dxl_unit, res = self.motors[motor].get_present_position()
if (dxl_unit + js_value) >= 4096:
return
self.motors[motor].set_goal_position(int(dxl_unit + js_value) % 4096)
def move(self, motor, axis, scaler):
"""
This method moves a motor based on a joystick value
:param int motor: The id of the motor
:param float axis: The value of an axis of a joystick
:param float scalar: The value to multiply the axis value by (speed)
"""
value = axis * scaler
self.read_write_position(motor, value)
def reboot(self, motor):
"""
This method reboots a dynamixel motor
:param int motor: The id of the motor
"""
self.motors[motor].reboot()
sleep(1)
def reboot_all(self):
"""
This method reboots all the motors in the arm
"""
for id, motor in self.motors.items():
motor.reboot()
sleep(3)