hpp_tutorial  5.0.0
Tutorial for humanoid path planner platform.
Tutorial 4 - inverse kinematics

To run this tutorial, you need to install package ur_description.

To run the tutorial, open a terminal and open 3 tabs by typing CTRL+SHIFT+T twice. When the terminal is selected, you can select a tab by typing ALT-[1|2|3].

Starting

hppcorbaserver

In the first tab, type

hppcorbaserver

See package hpp-manipulation-corba for details.

Controlling via a python terminal

In the second tab, type

cd script
python -i tutorial_4.py

Script script/tutorial_4.py defines a path for an UR10 end-effector

Starting gepetto-gui

In the third tab, type

gepetto-gui

A window opens and is ready to display the scene containing the robot. The robot, environment and object will appear later.

Controlling via a python terminal

To display the scene, create a client to the viewer in the python terminal.

>>> v = vf.createViewer ()

The robot and environment should appear in the viewer. If the viewer window is black, select the window and hit space.

Explaining the script

The first part of the script below

  • creates the robot from a xacro file,
  • creates the problem solver and the viewer factory.
## Load robot from processing of a xacro file
Robot.urdfString = process_xacro\
("package://hpp_tutorial/urdf/ur10e.urdf.xacro",
"transmission_hw_interface:=hardware_interface/PositionJointInterface")
# Deactivate collision checking between consecutive links
Robot.srdfString = """
<robot name="ur10e">
<disable_collisions link1="shoulder_link"
link2="upper_arm_link" reason=""/>
<disable_collisions link1="upper_arm_link"
link2="forearm_link" reason=""/>
<disable_collisions link1="wrist_1_link"
link2="wrist_2_link" reason=""/>
<disable_collisions link1="wrist_2_link"
link2="wrist_3_link" reason=""/>
<disable_collisions link1="shoulder_link"
link2="forearm_link" reason=""/>
<disable_collisions link1="wrist_1_link"
link2="wrist_3_link" reason=""/>
<disable_collisions link1="base_link_inertia"
link2="shoulder_link" reason=""/>
<disable_collisions link1="forearm_link"
link2="wrist_1_link" reason=""/>
</robot>
"""
loadServerPlugin ("corbaserver", "manipulation-corba.so")
newProblem()
robot = Robot('robot', 'ur10e', rootJointType="anchor")
ps = ProblemSolver(robot)
vf = ViewerFactory(ps)

The following lines

  • create a gripper attached to the end-effector of the robot,
  • create two handles attached to the base of the robot.

These grippers and handles will ease the creation of pose constraints for the end-effector.

## Add a gripper to the robot
robot.client.manipulation.robot.addGripper\
('ur10e/wrist_3_link', 'ur10e/gripper', [0,0,.1,0.5,0.5,0.5,-0.5], 0.1)
## Create two handles
robot.client.manipulation.robot.addHandle\
('ur10e/base_link', 'handle1', [.8, -.4, .5, 0, 0, 0, 1], .1, 6*[True])
robot.client.manipulation.robot.addHandle\
('ur10e/base_link', 'handle2', [.8, .4, .5, 0, 0, 0, 1], .1, 6*[True])

Then two grasp constraints are created to defined initial and goal pose of the gripper

## Create grasp constraints
robot.client.manipulation.problem.createGrasp\
("ur10e/gripper grasps handle1", "ur10e/gripper", "handle1")
robot.client.manipulation.problem.createGrasp\
("ur10e/gripper grasps handle2", "ur10e/gripper", "handle2")

Configurations satisfying the pose constraints are computed by creating a constraint graph with two nodes and no edge.

## Create a constraint graph with one node for each grasp
cg = ConstraintGraph(robot, "graph")
cg.createNode(["ur10e/gripper grasps handle1", "ur10e/gripper grasps
handle2"])
cg.addConstraints(node = "ur10e/gripper grasps handle1", constraints = \
Constraints(numConstraints = ["ur10e/gripper grasps handle1"]))
cg.addConstraints(node = "ur10e/gripper grasps handle2", constraints = \
Constraints(numConstraints = ["ur10e/gripper grasps handle2"]))
cg.initialize()

We compute the initial and goal configurations by numerical inverse kinematics.

## Generate one configuration satisfying each constraint
q0 = 6*[0.]
res, q1, err = cg.applyNodeConstraints("ur10e/gripper grasps handle1", q0)
res, q2, err = cg.applyNodeConstraints("ur10e/gripper grasps handle2", q0)
# Check that configurations are collision free
res, msg = robot.isConfigValid(q1)
assert(res)
res, msg = robot.isConfigValid(q2)
assert(res)

We create several CORBA objects:

  • the current manipulation planning problem cmp,
  • the robot stored in this problem crobot,
  • a steering method csm of type hpp::manipulation::steeringMethod::EndEffectorTrajectory
## Create an EndEffectorTrajectory steering method
cmp = wd(ps.client.basic.problem.getProblem())
crobot = wd(cmp.robot())
cproblem = wd(ps.client.basic.problem.createProblem(crobot))
csm =
wd(ps.client.basic.problem.createSteeringMethod("EndEffectorTrajectory",
cproblem))

Then we create a ContraintSet containing an empty ConfigProjector that we pass to the problem. We set csm as the steering method of the problem. Note that the last line passes the ContraintSet of the problem to the steering method. The order is important here since at construction the problem is given an empty ContraintSet and setting the steering method of the problem passes the ContraintSet of the problem to the steering method.

cs = wd(ps.client.basic.problem.createConstraintSet(crobot,
"sm-constraints")) cp =
wd(ps.client.basic.problem.createConfigProjector(crobot, "cp", 1e-4, 40))
cs.addConstraint(cp)
cproblem.setConstraints(cs)
cproblem.setSteeringMethod(csm)

We need to create a time varying constraint for the end-effector. For that, we create a new grasp between the gripper and the first handle. Note that we cannot use the previously created identical grasp, since the comparison type of this one should be Equality.

# Create a new grasp constraint for the steering method right hand side
# The previously created one has EqualToZero as comparison types.
robot.client.manipulation.problem.createGrasp \
("end-effector-tc", "ur10e/gripper", "handle1")
# Set comparison type to Equality
ps.setConstantRightHandSide("end-effector-tc", False)

We insert this constraint into the ConfigProjector of the problem (and thus of the steering method)

tc = wd(ps.client.basic.problem.getConstraint("end-effector-tc"))
cp.add(tc, 0)

We pass this constraint to the steering method as the trajectory constraint. Note that from a mathematical point of view, the trajectory constraint is a mapping from the robot configuration space \(\mathcal{C}\) to \(SE(3)\) defined by

\begin{equation} tc(\mathbf{q}) = g^{-1}(\mathbf{q})h_1 \end{equation}

where \(g(\mathbf{q})\in SE(3)\) is the pose of the gripper in configuration \(\mathbf{q}\) and \(h_1\in SE(3)\) is the pose of handle1.

csm.trajectoryConstraint(tc)

We now need to build the right hand side of the constraint as a linear interpolation between the initial and final values. For that we evaluate the fonction \(tc\) of the constraint at the initial and goal configurations, we create a path in SE(3) linking these two values and we give this path to the steering method to define the time-varying right hand side.

From a mathematical point of view, \(\mathbf{p}\) is a mapping from an interval \([0,T]\) to \(SE(3)\) such that

\begin{eqnarray} \mathbf{p}(0) &=& tc(\mathbf{q}_1) \\ \mathbf{p}(T) &=& tc(\mathbf{q}_2) \\ \end{eqnarray}

The constraint applied along the path computed by the steering method is thus:

\begin{equation} \forall t\in[0,T],\ \ \mathbf{tc}(\mathbf{q}(t)) = \mathbf{p}(t) \end{equation}

# Get right hand side for q1 and q2
rhs1 = tc.function().value(q1)
rhs2 = tc.function().value(q2)
# Create linear path for end-effector
p = wd(csm.makePiecewiseLinearTrajectory([rhs1, rhs2], 6*[1.]))
# Set this path as the time-varying right hand side of the constraint
csm.trajectory(p, True)

We can now call the steering method between the initial and goal configurations and insert this path in the ProblemSolver in order to make it visible in gepetto-gui.

## Call steering method
p1 = wd(csm.call(q1,q2))
if p1:
ps.client.basic.problem.addPath(p1.asVector())

After connecting and refreshing gepetto-gui, you should be able to display the path. Notice that the path is discontinuous.

To get a continuous path, we need to use the EndEffectorTrajectory path planner .

## Using EndEffectorTrajectory path planner
cdistance = wd(cproblem.getDistance())
croadmap = wd(ps.client.basic.problem.createRoadmap(cdistance, crobot))
cplanner = wd(ps.client.basic.problem.createPathPlanner(
"EndEffectorTrajectory", cproblem, croadmap))
cplanner.setNRandomConfig(0)
cplanner.maxIterations(1)
cplanner.setNDiscreteSteps(20)
cproblem.setInitConfig(q1)
cproblem.addGoalConfig(q2)
p2 = wd(cplanner.solve())
if p2:
ps.client.basic.problem.addPath(p2)

We set the number of random configurations to 0 in order to force the planner to start from q1. Otherwise, in case of failure to plan a continuous path starting from q1, the planner would generate random initial configurations that satisfy the constraints at the beginning. To be consistent, we set the maximal number of iterations to 1 since new iterations would simply retry to start from q1. We set the number of steps at which a configuration is computed for the corresponding pose of the end effector to 20.

Notice that the path satisfies the end-effector time-varying constraint, but does not end at \(\mathbf{q}_2\) since the final configuration is completely determined by the initial one.