ROS Resources: Documentation | Support | Discussion Forum | Index | Service Status | ros @ Robotics Stack Exchange
Ask Your Question
0

moveit python api

asked 2022-10-18 15:43:48 -0500

akumar3.1428 gravatar image

updated 2023-06-18 10:06:15 -0500

lucasw gravatar image

I am writing a code to move a robot arm using MoveIt Tutorials and moveit_commander.move_group.MoveGroupCommander Class Reference . However facing some problem in understanding that how can I move the robot at a specific (x,y,z) from base. My arm is moving but not to the specific place like x=50cm, y=50cm, z=50cm , and there is error

Fail: ABORTED: No motion plan found. No execution attempted.

whenever I am trying different position even if it is near to the robot. I think I need to know how to put input in meters from base coordinate frame and there is some relation between set_goal_tolerance(val) and pose_goal.position for reference I am attaching my code

import sys
import copy
import rospy
import moveit_commander
import moveit_msgs.msg
import geometry_msgs.msg
from math import pi
from std_msgs.msg import String
from moveit_commander.conversions import pose_to_list


moveit_commander.roscpp_initialize(sys.argv)
rospy.init_node('move_group_python_interface_tutorial',anonymous=True)

robot = moveit_commander.RobotCommander()
scene = moveit_commander.PlanningSceneInterface()
_commander = moveit_commander.move_group.MoveGroupCommander('xarm{}'.format(6))
display_trajectory_publisher = rospy.Publisher('/move_group/display_planned_path',moveit_msgs.msg.DisplayTrajectory,queue_size=20)



planning_frame = _commander.get_planning_frame()
print("============ Reference frame: %s" % planning_frame)
eef_link = _commander.get_end_effector_link()
print("============ End effector: %s" % eef_link)


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

print("============ Printing robot state")
print(robot.get_current_state())
print ("")


current_pose = _commander.get_current_pose(end_effector_link='link6').pose

print(f"The current pose is {current_pose}")

pose_goal = geometry_msgs.msg.Pose()

pose_goal.orientation.w = 1
pose_goal.position.x = 1
pose_goal.position.y = 1
pose_goal.position.z = 1

print(pose_goal)
_commander.set_pose_target(pose_goal)
_commander.set_goal_tolerance(1)

rospy.loginfo("Planning Trajectory to Pose 1")
plan1 = _commander.plan()
rospy.loginfo("Done Planning Trajectory to Pose 1")
rospy.loginfo("Executing Trajectory to Pose 1")

success = _commander.go(wait=True)

rospy.loginfo("Done Executing Trajectory to Pose 1")
_commander.stop()

_commander.clear_pose_targets()
edit retag flag offensive close merge delete

Comments

Are you sure that the goal pose is reachable?

ravijoshi gravatar image ravijoshi  ( 2022-10-19 08:42:48 -0500 )edit

I tried

pose_goal = geometry_msgs.msg.Pose()

pose_goal.orientation.w = 1.000000
pose_goal.position.x = 1.000000
pose_goal.position.y = 1.000000
pose_goal.position.z = 1.000000

print(pose_goal)
_commander.set_pose_target(pose_goal)
# _commander.set_planning_time(10)
_commander.set_goal_tolerance(1)

and this configuration were working, however when I changes the value pose_goal.position.x = 1.000000 to pose_goal.position.x = 1.200000 it's failing. In addition to this the position that I am getting in the above described values are 54 cm in x,y and z from the base. I want to know how can I make sure it moves to a point exactly where I want it to be, like for example 1,1,1 should be 1meter in x,y,z

akumar3.1428 gravatar image akumar3.1428  ( 2022-10-19 08:50:38 -0500 )edit

1 Answer

Sort by ยป oldest newest most voted
0

answered 2022-10-19 12:23:22 -0500

akumar3.1428 gravatar image

I resolved the issue the problem with my code was that I was giving

> pose_goal.orientation.w = value
> pose_goal.position.x = value
> pose_goal.position.y = value
> pose_goal.position.z = value

however, I should also give the quaternions as the planning scene was colliding with the end-effector without it and hence oit was failing to provide a path. In addition to this I also change set_goal_tolerance(1e-6)

for reference this is new values

pose_goal.position.x = 0.6281073954848061 
pose_goal.position.y = -0.09639226578995838
pose_goal.position.z =  0.6397201159890544


pose_goal.orientation.x = 0.1375399725278269
pose_goal.orientation.y = 0.13221897080143274
pose_goal.orientation.z = 0.6228175168854516
pose_goal.orientation.w = 0.7587484697697222

for trouble shooting I used moveIt to plan motion and use get_current_pose(end_effector_link='link6').pose to get the position value of the end-effector. I also found out that my values are in meters.

edit flag offensive delete link more

Question Tools

1 follower

Stats

Asked: 2022-10-18 15:43:48 -0500

Seen: 162 times

Last updated: Oct 19 '22