def __init__(self, serial_port='/dev/ttyUSB0'):
self.dynamixel = xm430.USB2Dynamixel_Device(serial_port)
self.p1 = xm430.Robotis_Servo2(self.dynamixel, 1,
series="XM") #inner gripper
self.p2 = xm430.Robotis_Servo2(self.dynamixel, 2,
series="XM") #outer gripper
self.p3 = xm430.Robotis_Servo2(self.dynamixel, 3,
series="XM") #outer gripper
self.p4 = xm430.Robotis_Servo2(self.dynamixel, 4,
series="XM") #linear actuator
self.pitch = 1.98 #9./5.*1.05*1.02 # found from experiment
self.pgc_linear_zero = 3383
return
def __init__(self, serial_port='/dev/ttyUSB0'):
self.dynamixel = xm430.USB2Dynamixel_Device(serial_port)
self.p1 = xm430.Robotis_Servo2(self.dynamixel, 1,
series="XM") #inner gripper
self.p2 = xm430.Robotis_Servo2(self.dynamixel, 2,
series="XM") #outer gripper
self.p3 = xm430.Robotis_Servo2(self.dynamixel, 3,
series="XM") #outer gripper
self.p4 = xm430.Robotis_Servo2(self.dynamixel, 4,
series="XM") #linear actuator
self.pitch = rospy.get_param("pitch", 1.98)
self.pgc_linear_zero = rospy.get_param("pgc_linear_zero", 3325)
self.og_open_motor_pos_ = rospy.get_param("og_open_motor_pos_", 1024)
return
def __init__(self):
name = rospy.get_name()
serial_port = rospy.get_param(name + "/serial_port", "/dev/ttyUSB0")
rospy.loginfo("Starting up on serial port: " + serial_port)
self.setScrew_motor_id = rospy.get_param(name + "/setScrew_motor_id",
75)
self.dynamixel = xm430.USB2Dynamixel_Device(serial_port,
baudrate=57600)
self.p1 = xm430.Robotis_Servo2(self.dynamixel,
self.setScrew_motor_id,
series="XM")
self._feedback = o2as_msgs.msg.ToolsCommandFeedback()
self._result = o2as_msgs.msg.ToolsCommandResult()
#define the action
self._action_name = "setScrew_tools_action"
self._action_server = actionlib.SimpleActionServer(
self._action_name,
o2as_msgs.msg.ToolsCommandAction,
execute_cb=self.action_callback,
auto_start=False)
self._action_server.start()
rospy.loginfo('Action server ' + str(self._action_name) + " started.")
return
def __init__(self):
name = rospy.get_name()
serial_port = rospy.get_param(name + "/serial_port",
"/dev/for_docker/blacktool")
rospy.loginfo("Starting up on serial port: " + serial_port)
self.setScrew_motor_id = rospy.get_param(name + "/setScrew_motor_id",
75)
self.peg_motor_id = rospy.get_param(name + "/peg_motor_id", 1)
self.m10_nut_motor_id = rospy.get_param(name + "/m10_nut_motor_id", 2)
self.m6_nut_motor_id = rospy.get_param(name + "/m6_nut_motor_id", 3)
self.dynamixel = xm430.USB2Dynamixel_Device(serial_port,
baudrate=57600)
self.p1 = xm430.Robotis_Servo2(self.dynamixel,
self.peg_motor_id,
series="XM") #Peg (big nut??)
# self.p2 = xm430.Robotis_Servo2( self.dynamixel, self.m10_nut_motor_id, series = "XM" ) #Big nut
# self.p3 = xm430.Robotis_Servo2( self.dynamixel, self.m6_nut_motor_id, series = "XM" ) #small nut
self.p75 = xm430.Robotis_Servo2(self.dynamixel,
self.setScrew_motor_id,
series="XM") # set screw
self._feedback = o2as_msgs.msg.ToolsCommandFeedback()
self._result = o2as_msgs.msg.ToolsCommandResult()
#define the action
self._action_name = "nut_tools_action"
self._action_server = actionlib.SimpleActionServer(
self._action_name,
o2as_msgs.msg.ToolsCommandAction,
execute_cb=self.action_callback,
auto_start=False)
self._action_server.start()
rospy.loginfo('Action server ' + str(self._action_name) + " started.")
return
def __init__(self):
name = rospy.get_name()
serial_port = rospy.get_param(name + "/serial_port", "/dev/for_docker/gripper")
rospy.loginfo("Starting up on serial port: " + serial_port)
self.dynamixel = xm430.USB2Dynamixel_Device( serial_port, baudrate = 57600 )
self.p1 = xm430.Robotis_Servo2( self.dynamixel, 1, series = "XM" ) #inner gripper
# self.p2 = xm430.Robotis_Servo2( self.dynamixel, 2, series = "XM" ) #outer gripper
self.inner_gripper_motor_pos = -1
# self.outer_gripper_motor_pos = -1
self.inner_gripper_read_current_position()
# self.outer_gripper_read_current_position()
self.inner_motor_pos_pub = rospy.Publisher('o2as_precision_gripper/inner_gripper_motor_pos', std_msgs.msg.Int32, queue_size=1)
self.inner_motor_closed_pub = rospy.Publisher('o2as_precision_gripper/inner_gripper_fully_closed', std_msgs.msg.Bool, queue_size=1)
self.inner_motor_opened_pub = rospy.Publisher('o2as_precision_gripper/inner_gripper_fully_opened', std_msgs.msg.Bool, queue_size=1)
### Careful: The "fully opened" check is not very reliable, because of the bending of the fingertips.
### When grasping large washers from the inside, the gripper is fully opened.
self.timer = rospy.Timer(rospy.Duration(0.05), self.publish_status)
#read the parameters
self._feedback = o2as_msgs.msg.PrecisionGripperCommandFeedback()
self._result = o2as_msgs.msg.PrecisionGripperCommandResult()
self.outer_force = rospy.get_param(name + "/outer_force", 30)
self.inner_force = rospy.get_param(name + "/inner_force", 7)
self.grasping_inner_force = rospy.get_param(name + "/grasping_inner_force", 5)
self.outer_open_position = rospy.get_param(name + "/outer_open_position", 1123)
self.outer_close_position = rospy.get_param(name + "/outer_close_position", 50240)
self.speed_limit = rospy.get_param(name + "/speed_limit", 10)
self.inner_open_motor_position = rospy.get_param(name + "/inner_open_motor_position", 2500)
self.inner_close_motor_position = rospy.get_param(name + "/inner_close_motor_position", 2222)
self.inner_slight_open = rospy.get_param(name + "/inner_slight_open", 70)
self.inner_opensilghtly_motor_position = rospy.get_param(name + "/inner_opensilghtly_motor_position", 2200)
rospy.loginfo("inner_open_motor_position = " + str(self.inner_open_motor_position))
# Define the action
self._action_name = "precision_gripper_action"
self._action_server = actionlib.SimpleActionServer(self._action_name, o2as_msgs.msg.PrecisionGripperCommandAction, execute_cb=self.action_callback, auto_start = False)
self._action_server.start()
rospy.loginfo('Action server '+ str(self._action_name)+" started.")
return
import lib_robotis_xm430 as xm430
import sys
import time
pitch=1.98#9./5.*1.05*1.02#found from experiment
#%%
#joshua
#20180620
dynamixel1 = xm430.USB2Dynamixel_Device( '/dev/ttyUSB0' )
#xm.find_servos(dyn)
#%%
p1 = xm430.Robotis_Servo2( dynamixel1, 1, series = "XM" )#inner gripper
#%%
p2 = xm430.Robotis_Servo2( dynamixel1, 2, series = "XM" )#outer gripper
#%%
p3 = xm430.Robotis_Servo2( dynamixel1, 3, series = "XM" )#outer gripper
#%%
p4 = xm430.Robotis_Servo2( dynamixel1, 4, series = "XM" )#linear actuator
#%%
#outer gripper related functions
def outer_gripper_close_new(goal_position):
try:
#%%
p2.set_operating_mode("currentposition")
p3.set_operating_mode("currentposition")
#%%
p2.set_current(150)#0.3Nm
p3.set_current(150)
i2=p2.read_current_position()
i3=p3.read_current_position()
i_avg=int((i2+i3)/2)
while i_avg<goal_position: