def genCommand(char, command):
"""Update the command according to the character entered by the user."""
if char == 'a':
command = CModelCommand();
command.rACT = 1
command.rGTO = 1
command.rSP = 255
command.rFR = 150
if char == 'r':
command = CModelCommand();
command.rACT = 0
if char == 'c':
command.rPR = 255
if char == 'o':
command.rPR = 0
#If the command entered is a int, assign this value to rPRA
try:
command.rPR = int(char)
if command.rPR > 255:
command.rPR = 255
if command.rPR < 0:
command.rPR = 0
except ValueError:
pass
if char == 'f':
command.rSP += 25
if command.rSP > 255:
command.rSP = 255
if char == 'l':
command.rSP -= 25
if command.rSP < 0:
command.rSP = 0
if char == 'i':
command.rFR += 25
if command.rFR > 255:
command.rFR = 255
if char == 'd':
command.rFR -= 25
if command.rFR < 0:
command.rFR = 0
return command
def genCommand(char, command):
"""Update the command according to the character entered by the user."""
if char == 'a':
command = CModelCommand()
command.rACT = 1
command.rGTO = 1
command.rSP = 255
command.rFR = 150
if char == 'r':
command = CModelCommand()
command.rACT = 0
if char == 'c':
command.rPR = 255
if char == 'o':
command.rPR = 0
#If the command entered is a int, assign this value to rPRA
try:
command.rPR = int(char)
if command.rPR > 255:
command.rPR = 255
if command.rPR < 0:
command.rPR = 0
except ValueError:
pass
if char == 'f':
command.rSP += 25
if command.rSP > 255:
command.rSP = 255
if char == 'l':
command.rSP -= 25
if command.rSP < 0:
command.rSP = 0
if char == 'i':
command.rFR += 25
if command.rFR > 255:
command.rFR = 255
if char == 'd':
command.rFR -= 25
if command.rFR < 0:
command.rFR = 0
return command
def _goto_position(self, pos, vel, force): """ Goto position with desired force and velocity @type pos: float @param pos: Gripper width in meters @type vel: float @param vel: Gripper speed in m/s @type force: float @param force: Gripper force in N """ command = CModelCommand() command.rACT = 1 command.rGTO = 1 command.rPR = int(np.clip((-230)/(self._max_gap - self._min_gap) * (pos - self._min_gap) + 230., 0, 230)) command.rSP = int(np.clip((255)/(self._max_speed - self._min_speed) * (vel - self._min_speed), 0, 255)) command.rFR = int(np.clip((255)/(self._max_force - self._min_force) * (force - self._min_force), 0, 255)) self._cmd_pub.publish(command)