Ask Your Question
1

set_max_velocity_scaling_factor in a cartesian path

asked 2017-03-28 15:53:37 -0500

shzargar gravatar image

updated 2017-03-30 17:28:12 -0500

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 ...
(more)
edit retag flag offensive close merge delete

Comments

Could you update your question with a self-contained code so that we can run it on our computers? I want to see if I can reproduce the issue but imports and variables are not known with the current snippet so that I can't.

130s gravatar image130s ( 2017-03-30 14:56:08 -0500 )edit

1 Answer

Sort by ยป oldest newest most voted
2

answered 2017-03-31 10:13:27 -0500

v4hn gravatar image

The plain and simple answer to this question is:

This doesn't work because nobody required and/or implemented it until now. The GetCartesianPath.srv ROS service offered by the move_group does not support a max_velocity_scaling_factor in its request section and the MoveGroupCommander uses that service to compute the cartesian trajectory.

So either you have to implement support for this in there, or you have to live with the full-speed trajectory you receive and re-scale the time parameterization afterwards.

edit flag offensive delete link more

Comments

Would be really helpful to have code snippets for that here. Related: https://answers.ros.org/question/2889...

fvd gravatar imagefvd ( 2018-09-19 23:32:43 -0500 )edit

Your Answer

Please start posting anonymously - your entry will be published after you log in or create a new account.

Add Answer

Question Tools

3 followers

Stats

Asked: 2017-03-28 15:53:37 -0500

Seen: 687 times

Last updated: Mar 31 '17