Clopema robot (package)

This package contains a common interface to CloPeMa robot implemented as library in both C++ and Python. This interface extends the MoveGroup class and MoveGroupCommander and should acts as a middle layer for the CloPeMa developers using the robot.

Scripts and nodes

The clopema_robot package contains simple scripts to control the robot motion, grippers and monitor its state. All script should have comprehensible help, that can be invoked by passing -h as an argument. Here are some examples:

$ rosrun clopema_robot -h                                    # Show help message
$ rosrun clopema_robot -r 1 -s 0                             # Close gripper on first arms
$ rosrun clopema_robot                                       # Show state of the robot
$ rosrun clopema_robot                                     # Move robot into home possition
$ rosrun clopema_robot -- 1 0 0.3 1.1 - 1.2           # Move robot to position defined by joint values
$ rosrun clopema_robot -- 1 0 0.3 1.1 - 1.2           # Move robot to position defined by joint values
$ rosrun clopema_robot -l r1_ee -- 0 0 0 0 0 1 0           # Move robot to position defined by pose
$ rosrun clopema_robot save test.txt                     # Save current pose into file
$ rosrun clopema_robot exec test.txt                     # Execute save state
$ rosrun clopema_robot                                     # Set robot servo power off
$ rosrun clopema_robot file_name                  # Execute trajectory saved as yaml file

The ClopemaRobotCommander interface

The interface extends the MoveGroup and MoveGroupCommander API for C++ and Python respectively. So one can just replace the MoveGroup or MoveGroupCommander with the ClopemaRobotComander and everything will work fine.


clopema_robot::ClopemaRobotCommander robot([group_name="arms"])     -- C++
robot = ClopemaRobotCommander([group_name="arms"])                  -- Python
  • group_name - Name of the planning group used for move_group or MoveGroupCommander initialization. The clopema description defines r1_arm, r2_arm, arms, ext, r1_xtion, r2_xtion.

Set servo power off

robot.setServoPowerOff([force=false])                               -- C++
robot.set_servo_power_off([force=False])                            -- Python
  • force -

Set robot speed

robot.setRobotSpeed(speed)                                          -- C++
robot.set_robot_speed(speed)                                        -- Python
  • speed - The speed to be set. The value can go from 0 to 1 and is then multiplied by 10000 in order to get the speed in motoman units. Note that the upper speed limit is defined by the I001 variable on the teach-pendant.

Get robot speed

float speed = robot.getRobotSpeed()                                 -- C++
speed = robot.get_robot_speed()                                     -- Python
  • speed - Robot speed.

Get robot IO

Reads controllers universal IO registers. Note that the proper address can be read from the teach-pendant under IN/OUT > UNIVERSAL INPUT or IN/OUT > UNIVERSAL OUTPUT, the the first value is the name of the IO and the second (#10010 for example) is the address.

bool val = robot.getRobotIO(unsigned int address)                   -- C++
val = robot.get_robot_io(address)                                   -- Python
  • address - IO address

Set robot IO

Writes into controllers universal IO registers.

robot.setRobotIO(unsigned int address, bool value)                  -- C++
robot.set_robot_io(address, value)                                  -- Pyhton
  • address - IO address
  • value - IO value True or False

Set gripper state

Set state (open or close) of the gripper. This function, due to wrong implementation of the actionlib, require the node to be initialised.

robot.setGripperState(gripper_action, state, [wait=false])          -- C++
robot.set_gripper_state(gripper_action, state, [wait=False])        -- Python
  • gripper_action - Which robot the state will be set to. Possible values are ACTION_R1_GRIPPER and ACTION_R1_GRIPPER
  • state - What state the gripper will be set to. Possible values are GRIPPER_OPEN and GRIPPER_CLOSE
  • wait - Wait for the end of the action, true or false.

Get Inverse Kinematic

rs = robot.get_ik(pose, link, links, poses, robot_state=None, constraints=None) -- Python
sucess = compute_ik(links, poses, robot_state::Ptr, group="")       -- C++ 
  • pose - Pose for single link
  • link - Link name for single link
  • poses - list of poses
  • links - list of end effector links
  • robot_state - Start state for the inverse kinematic (optional)
  • constraints - A moveit_msgs/Constraints that IK must obey (optional)

In Python use pose and link for single arm IK and, poses and links for dual arm IK.

  • Robot State

Get Robot State

Note that Python version of the ClopemaRobotController will return only the RobotState message as opposed to the C++ version. The C++ version of the MoveGroup uses a RobotState class as a wrapper for the RobotState message.

rs = robot.get_current_state()                                     -- Python
rs = robot.getCurrentState();                                      -- C++

Plan between joint states

rt = plan_between_joint_states(js1, js2)                       -- Python


- js1, js2 - Joint states


- trajectory or None