mc_rtc
mc_rtc
mc_rtc::Configuration general purpose configurationThis tutorial explains usage and concepts of the AdmittanceSample sample controller provided with the framework. This sample demonstrates the use of the mc_tasks::force::AdmittanceTask to regulate forces on JVRC1 hand while pushing against a wall.
This controller follows the following steps:
AdmittanceTask to establish contact20NSources for this sample are located in src/mc_control/samples/Admittance with the following main files:
To run the sample, you need a dynamic simulator in order to simulate the force sensors. This tutorial is intended to be used with mc_openrtm and Choreonoid, and the provided simulation file sim_mc_wall.cnoid. If you use another simulator, you will need to adapt the instructions, and create a scene with a wall 55cm away from the robot.
First, you need the following ~/.config/mc_rtc/mc_rtc.yaml file:
MainRobot: JVRC1
Enabled: AdmittanceSample
Timestep: 0.005
Then to run the sample, use:
$ (roscore &) # Ensure you have a roscore running (for rviz visualization)
$ cd /usr/local/share/hrpsys/samples/JVRC1
$ choreonoid sim_mc_wall.cnoid
$ roslaunch mc_rtc_ticker controler_display.launch
Then start the simulation by clicking on the green arrow in Choreonoid’s interface. You should see the following output:
The following graph depicts the results of the force tracking along the gripper’s z axis. Note the spike at t=6s corresponding to an impact between the gripper and the wall. About 1s later, the force target of -20N is realized, and tracked for a few seconds. Then the pressure is released.
Most of this sample is achieved by describing the states and task’s objective in the FSM configuration etc/AdmittanceSample.in.yaml. First, a transition map describes how the transitions between state occur.
transitions:
- [RightHandToWall, OK, RightHandPushAdmittance, Auto]
- [RightHandPushAdmittance, OK, RightHandKeepAdmittance, Auto]
- [RightHandKeepAdmittance, OK, UpdateWallFromKinematics, Auto]
- [UpdateWallFromKinematics, OK, RightHandReleaseAdmittance, Auto]
- [RightHandReleaseAdmittance, OK, RightHandMoveBack, Auto]
- [RightHandMoveBack, OK, StandingHalfSitting, Auto]
# Initial state
init: RightHandToWall
You should have easily recognized the main steps of the sample just by reading this transition map. Now let’s look at what the main states are doing.
During the whole experiment, we want the robot’s CoM to remain centered between the feet. This is achieved by using a mc_tasks::CoMTask. Additionally, we add an mc_tasks::OrientationTask on the robot’s chest to prevent it from moving too much and provide a more human-like motion. This state is used as the base state for all other states in the FSM, thus every state will inherit this behaviour.
# Keep the CoM centered between the feet, and the chest upright
StandingBase:
base: MetaTasks
tasks:
CoM:
type: com
above: [LeftFoot, RightFoot]
weight: 2000
stiffness: 5
KeepChest:
type: orientation
body: WAIST_R_S
weight: 100
stiffness: 1
Next, we want to move the hand to a location in front of the robot near the wall. To do so, we use the mc_tasks::BSplineTrajectoryTask, that provides a bspline trajectory parametrized in terms of waypoints in both position and orientation. The task controls a desired robot surface to follow this trajectory.
# Trajectory to bring hand close to the wall
RightHandToWall:
base: StandingBase
tasks:
# inherits CoM and KeepChest tasks from StandingBase state
# and adds a bspline_trajectory task
RightHandTrajectory:
type: bspline_trajectory
surface: RightGripper
weight: 1000
stiffness: 50
duration: 4
dimWeight: [1,1,1, 1, 0.5, 0.5]
displaySamples: 100
target:
translation: [0.45, -0.4, 1.1]
rotation: [1.57, 0, 1.57]
controlPoints: [[0.17, -0.5, 0.85]]
completion:
timeElapsed: true
Next, the RightHandPushAdmittance state uses the mc_tasks::AdmittanceTask to regulate the normal force on the hand and establish contact with the wall. The admittance task works by computing a desired end-effector velocity based on the error between the measured wrench (from force-torque sensor) and the desired wrench. The admittance coefficient regulates how fast the end-effector moves depending on the magnitude of the error along that axis. Note that in order for the task to track velocity, you want to configure it to have low-stiffness (position control) and high-damping (velocity control) along the directions where you wish to track forces. Also note that all wrenches are expressed in surface frame (here the RightGripper surface frame).
# Increase applied force until we reach 20N
RightHandPushAdmittance:
base: StandingBase
tasks:
RightHandAdmittance:
type: admittance
surface: RightGripper
# Tracks the forces along the normal axis of the gripper surface with an admittance coefficient of 0.001
# Note the high-damping and low stiffness on that axis.
# All other axis are position controlled and will keep the current gripper position as much as possible
admittance: [0,0,0,0,0,0.001]
stiffness: [10, 10, 10, 10, 10, 1]
damping: [6.3, 6.3, 6.3, 6.3, 6.3, 300]
maxVel:
linear: [0.2, 0.2, 0.5]
angular: [0.2, 0.2, 0.2]
# Target a desired force of 20N along the Z axis of the gripper surface
wrench:
force: [0, 0, -20]
couple: [0, 0, 0]
# Rotate the gripper such that the fingers point towards the wall
targetRotation: [1.57, 0, 1.57]
# The task will complete one a force of -20N has been reached. All other axes are ignored.
completion:
wrench:
force: [.nan, .nan, -20]
couple: [.nan, .nan, .nan]
The next state RightHandKeepAdmittance uses the ParallelState (mc_control::fsm::ParallelState) to keep the previous state running for a given duration. While the state is running, it’ll attempt to keep the applied force constant. Thus if you push the robot forward, the hand will move backwards to reduce the applied force, if you pull the robot, the opposite will occur.
The admittance task described above will cause the gripper to move towards the wall until the desired force has been reached. In our controller, we assumed the wall position to be at:
robots:
wall:
module: env/ground # reuse the ground model, but rotate it vertically to simulate a wall
init_pos:
# The wall position is put 5cm in front of choreonoid's one
translation: [0.50, 0.0, 0.0]
rotation: [0.0, -1.57, 0.0]
However, this expected position does not necessarily match the actual wall position: the robot might be starting from a slightly different configuration than expected. However, we know, thanks to the previous admittance states that the robot’s gripper has reached the wall. This information can be exploited to update our model’s wall position accordingly by exploiting the robot kinematics (and assuming non-slippage of the contacts). This is the role of the UpdateWall state implemented in C++. It reads the current estimated position of the robot’s fingertips, and updates the wall position such that the fingertips are in contact with it.
Note that this state uses the robot state observed by the mc_observers::KinematicInertialObserver to determine the actual hand position of the real robot (see the observers’ tutorial).
void UpdateWall::start(mc_control::fsm::Controller & ctl)
{
// [...] read YAML configuration parameters
// Get the estimated pose of the fingers of the JVRC1 robot
// As estimated by the state observation pipeline
const auto & bodyPose = ctl.realRobots().robot(rName).bodyPosW(bName);
// Move the wall position along the x axis to match the fingertip's position
auto posW = ctl.robot(moveRobotName).posW();
posW.translation().x() = bodyPose.translation().x();
ctl.robot(moveRobotName).posW(posW);
}
In the next state’s, we will use this updated wall position to ensure collision avoidance with the hand while going back to the initial posture.
The remainder of the FSM is dedicated to getting the robot back to its initial configuration. First, to ensure a smooth transition, we release the pressure on the hand with the RightHandReleaseAdmittance until it reaches close to zero force, and then move the hand 10cm backwards relative to its current position. Finally, we use the mc_control::fsm::HalfSittingState state to go back to the initial halfsitting posture. As we do not explicitly specify the hand motion, there is a risk that the hand could collide with the wall while going back to the half-sitting posture. To prevent this, we add a collision constraint between the hand and the wall:
RightHandMoveBack:
base: StandingBase
AddCollisionsAfter:
- r1: jvrc1
r2: wall
collisions:
- body1: R_WRIST_Y_S
body2: ground
iDist: 0.15
sDist: 0.05
damping: 0.0
tasks:
# [...]
For a more advanced example featuring stabilization based on the linear inverted pendulum model and external force compensation, please see the ExternalForces sample controller.