No artigo sobre a exploração do eezy bot arm mk2 com o ros moveit com base em mensagens FollowJointTrajectoryAction, ficou em aberto o controlo da pinça ou garra (gripper, claw?).
Pois esse artigo refere-se a controlo do posicionamento do end effector, e não ao controlo da acção do end effector propriamente dito.
Sobre o assunto não encontrei propriamente muita informação mas no entanto, destaco os seguintes:
- https://answers.ros.org/question/307638/how-to-grasp/
- https://answers.ros.org/question/227576/how-to-configure-the-gripper_action_controller-correctly/
Os dois artigos acima forneceram as informações importantes para configurar a parte do ros moveit, ou seja as alterações ao ficheiro config/controllers.yaml descritas mais abaixo.
A outra fonte de inspiração, que se aplicou às alterações do node eba_arm_control.py de modo a implementar o action server para o GripperCommand, foi um ficheiro do nxdefiant, que reproduzo na integra no meu repositório, e está disponivel no seguinte endereço:
https://github.com/inaciose/roscode/blob/master/ros/moveit-trajectory-griper-action-server-node.py
Deste script extrai apenas as partes necessárias ao controlo da pinça. Note-se que o script também implementa o feedback e o cancel, quer no GripperCommand quer no FollowJointTrajectory.
Alterações ao pacote ebamk2_moveit_config
Ao ficheiro config/controllers.yaml anteriormente criado no pacote de configuração do moveit para o braço eezybotarm mk2, cujo conteúdo original era o seguinte:
controller_manager_ns: /
controller_list:
- name: arm_controller
action_ns: follow_joint_trajectory
type: FollowJointTrajectory
joints: [joint_1, joint_2, joint_3, joint_6]
Foi removida a joint_6 da lista dos joints para o FollowJointTrajectory, e adicionadas as seguintes linhas que definem o controlador para a pinça (gripper) do robot.
- name: gripper_controller
action_ns: gripper_action
type: GripperCommand
joints: [joint_6]
action_monitor_rate: 20
goal_tolerance: 0.002
max_effort: 100
stall_velocity_threshold: 0.001
stall_timeout: 1.0
Após as alterações, o conteúdo do ficheiro config/controllers.yaml ficou conforme apresentado abaixo.
controller_manager_ns: /
controller_list:
- name: arm_controller
action_ns: follow_joint_trajectory
type: FollowJointTrajectory
joints: [joint_1, joint_2, joint_3]
- name: gripper_controller
action_ns: gripper_action
type: GripperCommand
joints: [joint_6]
action_monitor_rate: 20
goal_tolerance: 0.002
max_effort: 100
stall_velocity_threshold: 0.001
stall_timeout: 1.0
Com esta alteração o moveit passa a publicar mensagens nos tópicos: gripper_controller/gripper_action/goal.
Se o nosso node nao implementar o action server para estas mensagens o moveit suspende o uso do controller.
Nessa situação aparecem no terminal as seguintes mensagens de aviso e erro
[ WARN] [1590357841.286752284]: Waiting for gripper_controller/gripper_action to come up [ERROR] [1590357847.287084048]: Action client not connected: gripper_controller/gripper_action
Esta mensagem deixa de aparecer quando o nosso node, no meu caso o eba_arm_control.py, responde adequadamente.
Alterações ao eba_arm_control.py
As alterações ao node não foram muito grandes, e encontram-se resumidas abaixo.
Incluir a importação das bibliotecas de mensagens usadas.
# gripper from control_msgs.msg import GripperCommandAction, GripperCommandActionResult, GripperCommandFeedback
def __init__(self):
No metodo de inicialização da classe foi adicionado o código que declara e inicializa o actionlib server.
# Set up action GripperCommandAction
self.action_server_gripper = actionlib.SimpleActionServer("/gripper_controller/gripper_action",
GripperCommandAction,
self.do_action_gripper_callback, False)
self.action_server_gripper.start()
def do_action_gripper_callback(self, goal):
Este método é novo e é o responsavel pelo processamento das mensagens recebidas. Ou seja abre e fecha a pinça e devolve o resultado.
def do_action_gripper_callback(self, goal):
print ('Received gripper goal %.2f'%goal.command.position)
# adjust position and commit to hardware
self.c(goal.command.position)
self.publish_servos_state(self.robot_joints)
# Action done, either canceled or position reached
self._gripper_result.status = 3 # 3 = GoalStatus.SUCCEEDED
self._gripper_result.result.position = goal.command.position
self._gripper_result.result.stalled = False
self._gripper_result.result.reached_goal = True
self.action_server_gripper.set_succeeded(self._gripper_result.result)
print('gripper move complete')
Para histórico, foi feita uma cópia do node eba_arm_control.py sem as alterações para contemplar o gripper, para o seguinte ficheiro: eba_arm_follow_trajectory.py.
Considerações finais sobre grasp, gripper e afins
Apesar de ter chegado a uma implementação, ela não é completamente satisfatória.
Em primeiro lugar porque, conforme artigo indicado abaixo, pelo que percebi não existe um interface para o GripperCommand no rviz do moveit.
https://answers.ros.org/question/313637/openclose-end-effector-with-moveit-rviz/
Por outro lado, parece que existe mais que um tipo de solução para este tipo de problema.
Conforme o link seguinte, pode ser usadas soluções baseadas no, pick and place pipeline, moveit_grasps e no moveit_tasks_constructor.
https://answers.ros.org/question/350706/open-gripper-using-grippercommand-in-pick-and-place-pipeline/
Provavelmente voltarei mais tarde a este assunto