from cartesian_interface.pyci_all import *
import numpy as np
If errors occurred, maybe you forgot to include the CartesIO install path in your PYTHONPATH environment variable, such as:
export PYTHONPATH=$PYTHONPATH:/my/install/space/lib/python2.7/dist-packages
In a separate terminal, run the provided example launch file:
mon launch cartesian_interface coman.launch gui:=true
Note 1: you will need a roscore instance running in some other terminal
Note 2: if you did not install rosmon (recommended, sudo apt-get install ros-kinetic-rosmon),
you can use the standard roslaunch tool.
On success, you should see the IIT Coman robot in RViz.
You can now instantiate the CartesIO Python ROS Client, as follows
cli = pyci.CartesianInterfaceRos()
Some status information can be display by printing the returned object. You should see the roscpp node name where the client is running, plus a list of defined tasks.
print cli
The same information can be retrieved programmatically with the getTaskList method, returning a list of defined task names.
cli.getTaskList()
Let us now focus on one specific task, say left_hand, and retrieve some information about it.
task_name = 'left_hand'
larm = cli.getTask(task_name)
We can now get some generic task information such as the type, the name, the lambda value, etc...
print 'Task name is', larm.getName()
print 'Task type is', larm.getType()
print 'Task activation state is', larm.getActivationState()
print 'Task size is', larm.getSize()
print 'Task lambda is', larm.getLambda()
print 'Task active indices are', larm.getIndices()
print 'Task weight is: \n', larm.getWeight()
So far, this API is a generic one, applicable to all tasks regardless of their type (base class API).
Since we know that out right_hand task is a Cartesian task, we can also have access to the CartesianTask
specific API.
In C++ this would require an explicit dynamic_cast. In python, we get automatic down-casting, as we can immediately check by printing the task Python type:
print type(larm)
Let us get some Cartesian-specific information about the task:
print 'Task distal link is', larm.getDistalLink()
print 'Task base link is', larm.getBaseLink()
print 'Task control mode is', larm.getControlMode()
print 'Task state is', larm.getTaskState()
print 'Task current reference is \n', larm.getPoseReference()[0] # [0] for pose, [1] for velocity
Note: elements of SE3 (i.e. poses) are represented with the Affine3 type, which is a binding for Eigen::Affine3d.
The client API makes it easy to send a robot link to desired pose, possibly passing via
specified waypoints. By default, the commanded trajectory is a quintic spline stopping at
each waypoint. Internally, the client used the reach ROS action to command the motion.
Let us now send a target pose to our task. We will tell the hand to go 0.3m forward in 3.0 seconds.
cli.update() # to update the task curren pose reference
Tref, _, _ = larm.getPoseReference() # just return the pose ref, skip vel & acc
print Tref
Tref.translation_ref()[0] += 0.3
time = 3.0
larm.setPoseTarget(Tref, time)
larm.waitReachCompleted(10.0) # blocks till action is completed (or timeout has passed)
The robot in Rviz should move accordingly. Updating the client and retrieving the pose reference will confirm that the reference has infact advamced 0.3m along the x-axis. TBD check Com did not move!
cli.update()
Tref, _, _ = larm.getPoseReference() # just return the pose ref, skip vel & acc
print Tref
To specify waypoints, you must create a list of pyci.WayPoint objects, each containing a frame (of type Affine3) and the time relative to the start of the trajectory. For instance:
waypoints = [] # start from an empty list
# first waypoint
wp = pyci.WayPoint()
wp.frame.translation = [0.1, 0.4, 0]
wp.frame.quaternion = [0, 0, 0, 1]
wp.time = 2.0
waypoints.append(wp)
# second waypoint
wp = pyci.WayPoint()
wp.frame.translation = [0.4, 0.4, 0]
wp.frame.quaternion = [0, -1, 0, 1]
wp.time = 4.0
waypoints.append(wp)
# third waypoint
wp = pyci.WayPoint()
wp.frame.translation = [0.1, 0.2, 0]
wp.frame.quaternion = [0, 0, 0, 1]
wp.time = 6.0
waypoints.append(wp)
larm.setWayPoints(waypoints) # this sends the action goal
larm.waitReachCompleted(10.0) # blocks till action is completed (or timeout has passed)
As previously seen, each Task has an ActivationState property, which is an enum taking either the Enabled or Disabled value. Disabling a Task effectively removes it from the control problem. To see this, we can repeat the previous motion with the right hand task disabled.
rarm = cli.getTask('right_hand')
rarm.setActivationState(pyci.ActivationState.Disabled)
larm.setWayPoints(waypoints)
# blocks till action is completed (or timeout has passed)
The right hand does not keep its specified pose anymore (leading to self-collisions in this case). Let us re-enable the right hand task.
rarm.setActivationState(pyci.ActivationState.Enabled)
Very often, Cartesian references must be sent continuously, and our controller is supposed to track them in an
online fashion. This is done by publishing on the reference ROS topic. The Python Client library relieves us from the burden of opening all the required topics for all tasks, and directly provides this functionality through a uniform API. Moreover, ready-to-use interactive markers are generated as well, which are very useful to validate and tune a stack of tasks.
In the Rviz left panel, enable the InteractiveMarkers checkbox. A marker will appear close to the left hand. Expand the marker widget, and use the Update topic spinbox to change the marker topic to the one of right_hand.
Good! Now, right-click on the marker, and select Continuous ctrl. This will enable the publishing to the reference topic. If you move the marker around, you will see how the controller tries to track the required motion.
If you move the marker fast enough, you'll notice some delay in the tracking. This is due to two different reasons:
lambda feedback gainBy default, CartesIO applies velocity and acceleration limits to your references, as a form of safety feature. You can query the default values for such limits through the API:
rarm.getVelocityLimits() # linear part (m s^-1) and angular part (rad s^-1)
rarm.getAccelerationLimits() # linear part (m s^-2) and angular part (rad s^-2)
Let us set these numbers to crazy-high values to get rid of safety features! Tracking should now be more responsive.
rarm.setVelocityLimits(100, 100)
rarm.setAccelerationLimits(10000, 10000)
Still, the value of the feedback gain lambda can limit the tracking fidelity if set too low. Let's query the default value (which is set inside the problem_description):
rarm.getLambda()
Let's set it all the way to the max!
rarm.setLambda(1.0)
rarm.getLambda()
Now the tracking is veeery responsive, being limited only by the velocity limits, and by the loop frequency of the CartesIO ROS Server Node (defaults to 100 Hz)
Let us show how to do it through this simple code.
import math
import time
cli.update()
Tinit = Affine3()
Tinit.translation = [0.1, 0.3, 0]
Tinit.quaternion = [0, 0, 0, 1]
t = 0.0
dt = 0.01
period = 3.0
radius = 0.12
while t < 3*period:
delta_x = radius * math.cos(t*2.0*math.pi/period) - radius # + math.pi/2.0)
delta_y = radius * math.sin(t*2.0*math.pi/period)
Tref = Tinit.copy()
Tref.translation_ref()[0] += delta_x
Tref.translation_ref()[1] += delta_y
larm.setPoseReference(Tref) # this publishes the reference
t += dt
time.sleep(dt)