def cmd_publish(self):
"""
Publish the commands to the arm using LCM.
This publishes the joint angles currently set in the Rexarm object
You need to call this function to command the arm.
"""
self.clamp()
msg = dynamixel_command_list_t()
msg.len = 2
for i in range(msg.len):
cmd = dynamixel_command_t()
cmd.utime = int(time.time() * 1e6)
cmd.position_radians = self.joint_angles[i]
cmd.speed = self.speed_multiplier * self.speed[i]
cmd.max_torque = self.torque_multiplier * self.max_torque[i]
msg.commands.append(cmd)
# print "self.speed_multiplier: ", self.speed_multiplier, " self.torque_multiplier: ", self.torque_multiplier
self.lc.publish("DXL_COMMAND_ARM1", msg.encode())
msg = dynamixel_command_list_t()
msg.len = 4
for i in range(2, 6):
cmd = dynamixel_command_t()
cmd.utime = int(time.time() * 1e6)
cmd.position_radians = self.joint_angles[i]
cmd.speed = self.speed_multiplier * self.speed[i]
cmd.max_torque = self.torque_multiplier * self.max_torque[i]
msg.commands.append(cmd)
# print "self.speed_multiplier: ", self.speed_multiplier, " self.torque_multiplier: ", self.torque_multiplier
self.lc.publish("DXL_COMMAND_ARM2", msg.encode())
def cmd_publish(self):
"""
Publish the commands to the arm using LCM.
NO NEED TO CHANGE.
You need to activelly call this function to command the arm.
You can uncomment the print statement to check commanded values.
"""
msg = dynamixel_command_list_t()
msg.len = 4
curr_time = []
self.clamp()
for i in range(msg.len):
cmd = dynamixel_command_t()
cmd.utime = int(time.time() * 1e6)
curr_time.append(cmd.utime)
cmd.position_radians = self.joint_angles[i]
# you SHOULD change this to contorl each joint speed separately
if self.speed[i] == 0:
cmd.speed = 0.1
else:
cmd.speed = self.speed[i]+0.03
#cmd.speed = 0.1
cmd.max_torque = self.max_torque
#print cmd.position_radians
msg.commands.append(cmd)
if self.save_data:
self.commandPosfp.writerow(self.joint_angles)
self.commandSpeedfp.writerow(self.speed)
self.commandTimefp.writerow(curr_time)
self.lc.publish("ARM_COMMAND",msg.encode())
def cmd_publish(self):
"""
Publish the commands to the arm using LCM.
You need to activelly call this function to command the arm.
You can uncomment the print statement to check commanded values.
"""
msg = dynamixel_command_list_t()
msg.len = 6
self.clamp()
for i in range(msg.len):
cmd = dynamixel_command_t()
cmd.utime = int(time.time() * 1e6)
cmd.position_radians = self.joint_angles[i]
# you SHOULD change this to contorl each joint speed separately
cmd.speed = self.speed
cmd.max_torque = self.max_torque
#print cmd.position_radians
msg.commands.append(cmd)
self.lc.publish("ARM_COMMAND", msg.encode())
def cmd_publish(self):
"""
Publish the commands to the arm using LCM.
NO NEED TO CHANGE.
You need to activelly call this function to command the arm.
You can uncomment the print statement to check commanded values.
"""
msg = dynamixel_command_list_t()
msg.len = 4
self.clamp()
for i in range(msg.len):
cmd = dynamixel_command_t()
cmd.utime = int(time.time() * 1e6)
cmd.position_radians = self.joint_angles[i]
cmd.speed = self.speed[i]
cmd.max_torque = self.max_torque
#print cmd.position_radians
msg.commands.append(cmd)
self.lc.publish("ARM_COMMAND",msg.encode())