Comments (3)
Heya, could you give a minimal example for a python script I can execute with a ur5 to reproduce the issue?
This is related to ros-planning/moveit_ros#622 which was never merged into indigo, but I feel like that's not the whole story here.
from moveit_commander.
v4hn,
My apologies for the delay; however, the file you requested is included below.
#!/usr/bin/env python
"""
moveit_cartesian_path.py - Version 0.1 2016-07-28
Based on the R. Patrick Goebel's moveit_cartesian_demo.py demo code.
Plan and execute a Cartesian path for the end-effector.
Created for the Pi Robot Project: http://www.pirobot.org
Copyright (c) 2014 Patrick Goebel. All rights reserved.
This program is free software; you can redistribute it and/or modify
it under the terms of the GNU General Public License as published by
the Free Software Foundation; either version 2 of the License, or
(at your option) any later version.5
This program is distributed in the hope that it will be useful,
but WITHOUT ANY WARRANTY; without even the implied warranty of
MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
GNU General Public License for more details at:
http://www.gnu.org/licenses/gpl.html
"""
import rospy, sys
import moveit_commander
from geometry_msgs.msg import Pose
from copy import deepcopy
class MoveItCartesianPath:
def __init__(self):
rospy.init_node("moveit_cartesian_path", anonymous=False)
rospy.loginfo("Starting node moveit_cartesian_path")
rospy.on_shutdown(self.cleanup)
# Initialize the move_group API
moveit_commander.roscpp_initialize(sys.argv)
# Initialize the move group for the ur5_arm
self.arm = moveit_commander.MoveGroupCommander('manipulator')
# Get the name of the end-effector link
end_effector_link = self.arm.get_end_effector_link()
# Set the reference frame for pose targets
reference_frame = "/base_link"
# Set the ur5_arm reference frame accordingly
self.arm.set_pose_reference_frame(reference_frame)
# Allow replanning to increase the odds of a solution
self.arm.allow_replanning(True)
# Allow some leeway in position (meters) and orientation (radians)
self.arm.set_goal_position_tolerance(0.01)
self.arm.set_goal_orientation_tolerance(0.1)
# Get the current pose so we can add it as a waypoint
start_pose = self.arm.get_current_pose(end_effector_link).pose
# Initialize the waypoints list
waypoints = []
# Set the first waypoint to be the starting pose
# Append the pose to the waypoints list
waypoints.append(start_pose)
wpose = deepcopy(start_pose)
# Set the next waypoint to the right 0.5 meters
wpose.position.y -= 0.5
waypoints.append(deepcopy(wpose))
fraction = 0.0
maxtries = 100
attempts = 0
# Set the internal state to the current state
self.arm.set_start_state_to_current_state()
# Plan the Cartesian path connecting the waypoints
while fraction < 1.0 and attempts < maxtries:
(plan, fraction) = self.arm.compute_cartesian_path (waypoints, 0.01, 0.0, True)
# Increment the number of attempts
attempts += 1
# Print out a progress message
if attempts % 10 == 0:
rospy.loginfo("Still trying after " + str(attempts) + " attempts...")
# If we have a complete plan, execute the trajectory
if fraction == 1.0:
rospy.loginfo("Path computed successfully. Moving the arm.")
self.arm.execute(plan)
rospy.loginfo("Path execution complete.")
else:
rospy.loginfo("Path planning failed with only " + str(fraction) + " success after " + str(maxtries) + " attempts.")
def cleanup(self):
rospy.loginfo("Stopping the robot")
# Stop any current arm movement
self.arm.stop()
#Shut down MoveIt! cleanly
rospy.loginfo("Shutting down Moveit!")
moveit_commander.roscpp_shutdown()
moveit_commander.os._exit(0)
if __name__ == "__main__":
try:
MoveItCartesianPath()
except KeyboardInterrupt:
print "Shutting down MoveItCartesianPath node."
Thank you for your help, as well as, your time and attention to this matter. Have a great rest of the week. God bless.
Very Respectfully,
CMobley7
from moveit_commander.
This is github, not an office. No need for that much etiquette.
I just merged #29 ,
you can use this new method to add velocity/effort data to the plan you
you get back from cartesian planning like that:
self.robot= moveit_commander.RobotCommander()
self.arm= self.robot.right_arm
...
self.arm.compute_cartesian_path(waypoints, 0.01, 0.0, True)
plan= self.arm.retime_trajectory(self.robot.get_current_state(), plan, 1.0)
This solves your problem on the user side.
The question remains whether we want have velocities/accelerations added in the cartesian
path planning automatically. I added a pull-request for this here.
from moveit_commander.
Related Issues (20)
- MoveGroupCommander.get_current_joint_values() does not work as described HOT 1
- Floating point parsing assumes US decimal convention. HOT 2
- core dump whenever exiting Python MoveIt script HOT 8
- Add python interface to set trajectory constraints also with constraints msg on a move_group
- No equivalent to C++ group.setSupportSurfaceName() in Python API HOT 4
- core dump in move_group.py, line 455, in pick HOT 12
- .deb installing to the wrong place? HOT 4
- Python GIL not released before entering the C++ library
- joint value out of bounds with continuous joints
- Segmentation fault on wrong or no robot model HOT 1
- Python module locations on Jade HOT 1
- setNumPlanningAttempts missing from python api HOT 4
- setMaxVelocityScalingFactor missing from Python api HOT 2
- Add methods to PlanningSceneInterface already present on C++ version HOT 1
- Release new indigo version HOT 1
- Update Query Start / Goal State HOT 3
- can't import moveit_commander HOT 1
- Broken link HOT 5
- Add Python interface for RobotState class
Recommend Projects
-
React
A declarative, efficient, and flexible JavaScript library for building user interfaces.
-
Vue.js
🖖 Vue.js is a progressive, incrementally-adoptable JavaScript framework for building UI on the web.
-
Typescript
TypeScript is a superset of JavaScript that compiles to clean JavaScript output.
-
TensorFlow
An Open Source Machine Learning Framework for Everyone
-
Django
The Web framework for perfectionists with deadlines.
-
Laravel
A PHP framework for web artisans
-
D3
Bring data to life with SVG, Canvas and HTML. 📊📈🎉
-
Recommend Topics
-
javascript
JavaScript (JS) is a lightweight interpreted programming language with first-class functions.
-
web
Some thing interesting about web. New door for the world.
-
server
A server is a program made to process requests and deliver data to clients.
-
Machine learning
Machine learning is a way of modeling and interpreting data that allows a piece of software to respond intelligently.
-
Visualization
Some thing interesting about visualization, use data art
-
Game
Some thing interesting about game, make everyone happy.
Recommend Org
-
Facebook
We are working to build community through open source technology. NB: members must have two-factor auth.
-
Microsoft
Open source projects and samples from Microsoft.
-
Google
Google ❤️ Open Source for everyone.
-
Alibaba
Alibaba Open Source for everyone
-
D3
Data-Driven Documents codes.
-
Tencent
China tencent open source team.
from moveit_commander.