tuto using part4
It is not necessary to follow tutorial 1 as a prerequisite, since the MoveIt! config and the robot_description are provided. It is however recommended to have a look at the robot structure of the hand to understand the groups involved It is also not necessary to follow the first parts of tutorial 2 as this example is a stand-alone application
- Planning multi finger motion requires multiple goal states to be defined, one goal for each fingertip.
- One idea would be to virtually link several tips together to change the state as a set of tips.
- Interactive markers will be used for this purpose.
In the previous tutorial Interactive markers where used in RViz to move the end-effectors. This is a quick overview of how they work. The original tutorial to understand interactive markers is there
- Marker displayed in RVIZ, the user can interact with, changing one or more of these properties:
- position
- orientation
- menu entries
- Different types exist
- 6 DOF (fixed or relative)
- 3 DOF
- Quadracopter
- menu
- ... see the Basic Control Tutorial for all the types
- Structure
- A feedback topic is published by the display client to the marker server, each message holds the property changes of the marker (event)
- An update topic to modify the marker status from the server to the display client
- Processing the user action is done in a callback on incoming feedback messages
- Interfacing MoveIt with new interactive markers
- Instead of clicking on each single end-effector marker, the multifinger interactive marker server sends simulated clicks (feedback) to the tip markers.
- The multifinger interactive marker server also listens to tip markers update. The state (start or goal) currently controlled is stored in the update message:
rostopic echo /<rviznodename>/robot_interaction_interactive_marker_topic/update
header:
seq: 6
stamp:
secs: 0
nsecs: 0
frame_id: shadow_world_frame
client_id: /<rviznodename>/
marker_name: EE:goal_fftip
control_name: move
event_type: 5
pose:
position:
x: 0.0329998359084
y: -0.0099992249161
z: 0.438001036644
orientation:
x: 0.0
y: 0.0
z: 0.0
w: 1.0
menu_entry_id: 0
mouse_point:
x: 0.0388541817665
y: -0.0139917135239
z: 0.444140136242
mouse_point_valid: True
The code is in the package multifinger_planning_marker in the file multifinger_planning_marker.cpp
A callback function handles the messages coming on the feedback topic of the multifinger interactive marker server.
void processFeedback(const visualization_msgs::InteractiveMarkerFeedbackConstPtr &feedback )
{Each event type can be distinguished.
switch ( feedback->event_type )
{The main action occurs at mouse button up here.
case visualization_msgs::InteractiveMarkerFeedback::MOUSE_UP:First test if the marker is active and which state is controlled (the control_goal boolean is set in another function)
// if attached and planning is active (at least start or goal clicked)
if(attached && ee_active)
{
// check if start or goal is clicked, will move goal if both selected
std::string name_prefix;
if(control_goal)
name_prefix="EE:goal_";
else
name_prefix="EE:start_";Then, based on incoming information, create simulated click action on the planning scene markers
// Simulated a move action on the fftip planning interactive marker
visualization_msgs::InteractiveMarkerFeedback markerFeedBack;
markerFeedBack.marker_name=name_prefix+"fftip";
markerFeedBack.header.frame_id="palm";
markerFeedBack.control_name="move";
markerFeedBack.event_type=visualization_msgs::InteractiveMarkerFeedback::POSE_UPDATE;
// Copy the current info of our marker and
// add a positive offset for the planning marker to go to
markerFeedBack.client_id=feedback->client_id;
markerFeedBack.pose=feedback->pose;
markerFeedBack.pose.position.z+=0.010;
markerFeedBack.header.seq=feedback->header.seq;
// Fake the mouse pointing
markerFeedBack.mouse_point.x=0.0388541817665;
markerFeedBack.mouse_point.y=-0.0139917135239;
markerFeedBack.mouse_point.z=0.444140136242;
markerFeedBack.mouse_point_valid=true;and publish the simulated click action
moveit_marker_publisher.publish(markerFeedBack);Do the same for the second marker
markerFeedBack.marker_name=name_prefix+"thtip";
markerFeedBack.pose.position.z-=0.020;
// Publish the marker (will move the planning marker)
moveit_marker_publisher.publish(markerFeedBack);Then apply changes to the multifinger marker if any occured
server->applyChanges();A callback function is created to handle the incoming updates of the planning scene markers
void control_goalCb(const visualization_msgs::InteractiveMarkerUpdateConstPtr &update)
{Then extract the information of the update message, setting the control_goal variable and deactivating the marker if no state is currently active.
if (update->poses.size()!=0)
{
ee_active=true;
// use the updated planning markers to check if start or goal markers
// are currently active
if(update->poses[0].name.find("goal")!=std::string::npos)
control_goal=true;
else
control_goal=false;
}
else
{
ee_active=false;
}
}The menu has its own callback function, although a menu click is caught in the feedback too. A final callback function is added to handle the menu entry clicking
void attachCb(const visualization_msgs::InteractiveMarkerFeedbackConstPtr &feedback)
{
if(attached)
{
menu_handler.setCheckState(h_attach,MenuHandler::UNCHECKED);
attached=false;
}
else
{
menu_handler.setCheckState(h_attach,MenuHandler::CHECKED);
attached=true;
}
menu_handler.reApply( *server );
server->applyChanges();
}An interactive marker is composed of a marker and of controls. Initialization function for the marker
Marker makeBox( InteractiveMarker &msg )
{
Marker marker;
marker.type = Marker::CUBE;
marker.scale.x = msg.scale * 0.25;
marker.scale.y = msg.scale * 0.25;
marker.scale.z = msg.scale * 0.25;
marker.color.r = 0.5;
marker.color.g = 0.5;
marker.color.b = 0.5;
marker.color.a = 1.0;
return marker;
}Then initializing function for the controls
// Add control to the interactive marker
InteractiveMarkerControl& makeBoxControl( InteractiveMarker &msg )
{
InteractiveMarkerControl control;
control.always_visible = true;
control.markers.push_back( makeBox(msg) );
msg.controls.push_back( control );
return msg.controls.back();
}Then the real interactive marker construction with its properties
void make3DofMarker(std::string name, std::string frame_id)
{
InteractiveMarker int_marker;
int_marker.header.frame_id = "/"+frame_id;
// Initial position of the marker on the fftip initial pose.
int_marker.pose.position.x = 0.033;
int_marker.pose.position.y = 0.0;
int_marker.pose.position.z = 0.191;
int_marker.scale = 0.05;
int_marker.name = name+"_control";
int_marker.description = name+" 3-DOF Control";
// Insert a box and a control
makeBoxControl(int_marker);
InteractiveMarkerControl control;Custom control can be selected. Here translation only controls are added in a relative mode. The direction of the control defines which axis is controlled.
// Relative control from the previous state
control.orientation_mode = InteractiveMarkerControl::INHERIT;
// x axis control
control.orientation.w = 1;
control.orientation.x = 1;
control.orientation.y = 0;
control.orientation.z = 0;
control.name = "move_x";
control.interaction_mode = InteractiveMarkerControl::MOVE_AXIS;
// stack this control to the list of controls
int_marker.controls.push_back(control);
// z axis control
control.orientation.w = 1;
control.orientation.x = 0;
control.orientation.y = 1;
control.orientation.z = 0;
control.name = "move_z";
control.interaction_mode = InteractiveMarkerControl::MOVE_AXIS;
int_marker.controls.push_back(control);
// y axis control
control.orientation.w = 1;
control.orientation.x = 0;
control.orientation.y = 0;
control.orientation.z = 1;
control.name = "move_y";
control.interaction_mode = InteractiveMarkerControl::MOVE_AXIS;
int_marker.controls.push_back(control);The marker is inserted into the server
// Insert the marker on the server
server->insert(int_marker);
// Set the callback function to process events/messages received
server->setCallback(int_marker.name, &processFeedback);To simplify interaction, the menu is linked to a second marker.
// Create a menu marker
void makeMenuMarker( std::string name )
{
InteractiveMarker int_marker;
int_marker.header.frame_id = "palm";
int_marker.name = name;
// Place it at the back of the palm as a button.
int_marker.pose.position.x = 0.0;
int_marker.pose.position.y = 0.02;
int_marker.pose.position.z = 0.05;
int_marker.scale = 0.055;The menu is nothing more than a dedicated control mode.
control.interaction_mode = InteractiveMarkerControl::MENU;The remaining initialization is identical to the moveable marker
control.always_visible = true;
// Add a box to this marker
control.markers.push_back( makeBox( int_marker ) );
// Stack this control to the list of controls
int_marker.controls.push_back(control);
// Insert the marker on the server
server->insert(int_marker);
}However a menu_handler can maintain its own callback function
void initMenu()
{
h_attach = menu_handler.insert( "Attach ",&attachCb );
}In the main, the server is initialized
server.reset( new interactive_markers::InteractiveMarkerServer("interactive_finger_control","",false) );The access to the planning scene markers is created
moveit_marker_publisher = n.advertise<visualization_msgs::InteractiveMarkerFeedback>(
"/rviz_moveit_motion_planning_display/robot_interaction_interactive_marker_topic/feedback",2);
moveit_marker_subscriber = n.subscribe<visualization_msgs::InteractiveMarkerUpdate>(
"/rviz_moveit_motion_planning_display/robot_interaction_interactive_marker_topic/update",2,control_goalCb);A marker is instantiated
make3DofMarker("thff","palm");A menu marker is created and the menu_handler linked to it.
initMenu();
makeMenuMarker( "FingerControlOptions" );
menu_handler.apply( *server, "FingerControlOptions" );The changes must always be applied to the server
server->applyChanges();The loop basically waits for events/messages
ros::spin();- Compile the multifinger_planning_marker
- In your workspace, do catkin_make
- Starting MoveIt! demo
- Launch the MoveIt! demo for the Shadow Hand
roslaunch sr_moveit_hand_config demo.launch
3. Run the multifinger_planning_marker
rosrun multifinger_planning_marker multifinger_planning_marker
```
4. Add an interactive marker
* Use the "add" button to add interactive marker plugin to RVIZ
* Enable it and set its topic to interactive_finger_control/update
* You should see a new marker at the fftip.
5. Select the `first_finger_thumb group`, or the `shadow_hand` group to get both fftip and thtip effectors active
6. Activate the multifinger marker
* At the back of the palm, left click on the marker and activate the marker
[[media/images/tuto-using/multifinger_marker-test3.png]]
-
Set the goal and start states
- In the Planning Request panel, activate the Query Start State
- Move the 3-DOF marker (multifinger marker) to a position at mid-range between first finger and thumb. Note the update will occur on mouse release only.
- Only a small workspace is accessible to both fingertips separated by 20mm as set in our server.
- Do the same for the start state at a slightly different pose and reactivate both states to see the planning request
-
Plan the motion You should see a nice combined movement between start and goal states. if the movement is small, the separation between the fingers might even be close to a constant value, however nothing ensures that in joint-space planning.