set_max_velocity_scaling_factor in a cartesian path

edit

Hi,

For a Nextage open on Ubuntu 14 and Ros Indigo , I want to control de velocity in a Cartesian path using waypoints and compute_cartesian_path. I used set_max_velocity_scaling_factor, but it is not working. As well, it is not giving any error, I change the factor but the robot is running always with the same speed.

I tried set_max_velocity_scaling_factor in a joint path and it is working. It seems that is not working for Cartesian pathes.

Here is my phyton code:

#!/usr/bin/env python
# @file     nextage_moveit_waypoint.py   #
# @brief    Nextage Move it demo program #
# @author   Amir                 #
# @date     2017/01/26                   #
from hironx_ros_bridge.ros_client import ROS_Client
# This should come earlier than later import.
# See http://code.google.com/p/rtm-ros-robotics/source/detail?r=6773
from nextage_ros_bridge import nextage_client

from hrpsys import rtm
import argparse

import sys
import moveit_commander
import copy
import rospy
import geometry_msgs.msg
import unittest
import moveit_msgs.msg

def main():
    rospy.init_node("moveit_command_sender")

    robot = moveit_commander.RobotCommander()

    print "=" * 10, " Robot Groups:"
    print robot.get_group_names()

    print "=" * 10, " Printing robot state"
    print robot.get_current_state()
    print "=" * 10 

    rarm = moveit_commander.MoveGroupCommander("right_arm")
    larm = moveit_commander.MoveGroupCommander("left_arm")
    both= moveit_commander.MoveGroupCommander("botharms")
    rhand= moveit_commander.MoveGroupCommander("right_hand")
    lhand= moveit_commander.MoveGroupCommander("left_hand")
    lartor= moveit_commander.MoveGroupCommander("left_arm_torso")
    larm.set_max_velocity_scaling_factor(.01)   


    print "=" * 15, " Right arm ", "=" * 15
    print "=" * 10, " Reference frame: %s" % rarm.get_planning_frame()
    print "=" * 10, " Reference frame: %s" % rarm.get_end_effector_link()

    print "=" * 15, " Left ight arm ", "=" * 15
    print "=" * 10, " Reference frame: %s" % larm.get_planning_frame()
    print "=" * 10, " Reference frame: %s" % larm.get_end_effector_link()

    #Right Arm Initial Pose
    rarm_initial_pose = rarm.get_current_pose().pose
    print "=" * 10, " Printing Right Hand initial pose: "
    print rarm_initial_pose

    #Light Arm Initial Pose
    larm_initial_pose = larm.get_current_pose().pose    
    print "=" * 10, " Printing Left Hand initial pose: "
    print larm_initial_pose

    print robot1.get_hand_version()
    robot1.handlight_r(True)
    robot1.handlight_l(True)
    ## Cartesian Paths
    ## ^^^^^^^^^^^^^^^
    ## You can plan a cartesian path directly by specifying a list of waypoints 
    ## for the end-effector to go through.
    waypoints = []

    # start with the current pose
    waypoints.append(larm.get_current_pose().pose)

    # first orient gripper and move forward (+x)
    wpose = geometry_msgs.msg.Pose()
    #wpose.orientation.w = 1.0
    wpose.position.x = waypoints[0].position.x + 0.15
    wpose.position.y = waypoints[0].position.y
    wpose.position.z = waypoints[0].position.z
    wpose.orientation.x = 0.0
    wpose.orientation.y = -0.707
    wpose.orientation.z = 0.0
    wpose.orientation.w = 0.707

    waypoints.append(copy.deepcopy(wpose))

    # second move up
    wpose.position.z += 0.2
    waypoints.append(copy.deepcopy(wpose))

    # third move to the side
    wpose.position.y -= 0.2
    waypoints.append(copy.deepcopy(wpose))

    wpose.position.x = waypoints[0].position.x
    wpose.position.y = waypoints[0].position.y
    wpose.position.z = waypoints[0].position.z
    wpose.orientation.x = 0.0
    wpose.orientation.y = -0.707
    wpose.orientation.z = 0.0
    wpose.orientation.w = 0.707
    waypoints.append(copy.deepcopy(wpose))
    print waypoints

    ## We want the cartesian path to be interpolated at a resolution of 1 cm
    ## which is why we will specify 0.01 as the eef_step in cartesian
    ## translation.  We will specify the jump threshold as 0.0, effectively
    ## disabling it.
    larm.set_max_velocity_scaling_factor(0.01)  
    (plan1, fraction ...