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) = larm.compute_cartesian_path(
waypoints, # waypoints to follow
0.01, # eef_step
0.0) # jump_threshold
print "============ Waiting while RVIZ displays plan3..."
print rospy.Time.now()
rospy.sleep(1)
print larm.get_known_constraints()
larm.set_max_velocity_scaling_factor(0.01)
larm.execute(plan1)
robot1.handlight_r(False)
robot1.handlight_l(False)
if __name__ == '__main__':
parser = argparse.ArgumentParser(description='hiro command line interpreters')
parser.add_argument('--host', help='corba name server hostname')
parser.add_argument('--port', help='corba name server port number')
parser.add_argument('--modelfile', help='robot model file nmae')
parser.add_argument('--robot', help='robot modlule name (RobotHardware0 for real robot, Robot()')
parser.add_argument('--dio_ver', help="Version of digital I/O. Only users "
"whose robot was shipped before Aug 2014 need to "
"define this, and the value should be '0.4.2'.")
args, unknown = parser.parse_known_args()
print args
print unknown
if args.host:
rtm.nshost = args.host
if args.port:
rtm.nsport = args.port
if not args.robot:
args.robot = "RobotHardware0" if args.host else "HiroNX(Robot)0"
if not args.modelfile:
args.modelfile = "/opt/jsk/etc/NEXTAGE/model/main.wrl" if args.host else ""
print args.host
print rtm.nshost
# support old style format
if len(unknown) >= 2:
args.robot = unknown[0]
args.modelfile = unknown[1]
robot1 = nxc = nextage_client.NextageClient()
robot1.init(robotname=args.robot, url=args.modelfile) #This launch hrpsys
robot1.set_hand_version('0.4.2')
#ROS Client.
#ros = ROS_Client()
try:
main()
except rospy.ROSInterruptException:
pass
I used set_max_velocity_scaling_factor here before compute_cartesian_path and execute, but I checked it once before each command and it doesn change anything.
Thanks
↧