def move_robot():
print "Starting rospy and moveit..."
moveit_commander.roscpp_initialize(sys.argv)
rospy.init_node('move_group_py')
robot = moveit_commander.RobotCommander()
scene = moveit_commander.PlanningSceneInterface()
group1 = moveit_commander.MoveGroupCommander("both_arms")
display_trajectory_publisher = rospy.Publisher('/move_group/display_planned_path',moveit_msgs.msg.DisplayTrajectory, queue_size=1)
group1.allow_replanning(True)
group1.set_planning_time(10.0)
print "Waiting for RViz"
rospy.sleep(10)
print "Initializing..."
print "Reference frame: %s" %group1.get_planning_frame()
print "Generating plan..."
group_values = group1.get_current_joint_values()
group_values[1] = 0
group_values[2] = 0
group1.set_joint_value_target(group_values)
plan1 = group1.plan()
rospy.sleep(5)
print "Executing..."
plan1 = group1.go()
def starting_pose():
rospy.sleep(20)
moveit_commander.roscpp_initialize(sys.argv)
rospy.init_node('move_to_starting_pose',
anonymous=True)
robot = moveit_commander.RobotCommander()
scene = moveit_commander.PlanningSceneInterface()
group = moveit_commander.MoveGroupCommander("arm_gp")
display_trajectory_publisher = rospy.Publisher(
'/move_group/display_planned_path',
moveit_msgs.msg.DisplayTrajectory)
print "============ Waiting for RVIZ..."
pose_target = geometry_msgs.msg.Pose()
pose_target.orientation.x = 0.0
pose_target.orientation.y = -1.0
pose_target.orientation.z = 0.0
pose_target.orientation.w = 0.0
pose_target.position.x = 0.3
pose_target.position.y = 0.0
pose_target.position.z = 0.38
group.set_pose_target(pose_target)
group.go(wait=True)
## When finished shut down moveit_commander.
moveit_commander.roscpp_shutdown()
def init():
global robot
global scene
global left_arm
global right_arm
global left_gripper
global right_gripper
robot = None
scene = None
left_arm = None
right_arm = None
left_gripper = None
right_gripper = None
gc.collect()
#Initialize moveit_commander
moveit_commander.roscpp_initialize(sys.argv)
#Initialize both arms
robot = moveit_commander.RobotCommander()
scene = moveit_commander.PlanningSceneInterface()
left_arm = moveit_commander.MoveGroupCommander('left_arm')
right_arm = moveit_commander.MoveGroupCommander('right_arm')
left_arm.set_planner_id('RRTConnectkConfigDefault')
left_arm.set_planning_time(20)
right_arm.set_planner_id('RRTConnectkConfigDefault')
right_arm.set_planning_time(20)
def __init__(self):
self.state = "INITIALIZING"
moveit_commander.roscpp_initialize(sys.argv)
#Start a node
rospy.init_node('moveit_node')
#Initialize subscriber
self.sub = rospy.Subscriber("game_state", String, self.state_machine)
#Initialize both arms
self.robot = moveit_commander.RobotCommander()
self.scene = moveit_commander.PlanningSceneInterface()
self.left_arm = moveit_commander.MoveGroupCommander('left_arm')
self.right_arm = moveit_commander.MoveGroupCommander('right_arm')
self.left_arm.set_planner_id('RRTConnectkConfigDefault')
self.left_arm.set_planning_time(10)
self.right_arm.set_planner_id('RRTConnectkConfigDefault')
self.right_arm.set_planning_time(10)
#Set up the grippers
self.left_gripper = baxter_gripper.Gripper('left')
self.right_gripper = baxter_gripper.Gripper('right')
#Calibrate the gripper (other commands won't work unless you do this first)
print('Calibrating...')
self.right_gripper.calibrate()
rospy.sleep(0.5)
#Initialize class variables
self.all_pieces = [0,1,2,3,4,5,6,7,8,9,13,14,15,16,20]
self.available_pieces = self.all_pieces
self.state = "PLAY"
def setUp(self):
# create a action client of move group
self._move_client = SimpleActionClient('move_group', MoveGroupAction)
self._move_client.wait_for_server()
moveit_commander.roscpp_initialize(sys.argv)
group_name = moveit_commander.RobotCommander().get_group_names()[0]
group = moveit_commander.MoveGroupCommander(group_name)
# prepare a joint goal
self._goal = MoveGroupGoal()
self._goal.request.group_name = group_name
self._goal.request.max_velocity_scaling_factor = 0.1
self._goal.request.max_acceleration_scaling_factor = 0.1
self._goal.request.start_state.is_diff = True
goal_constraint = Constraints()
joint_values = [0.1, 0.2, 0.3, 0.4, 0.5, 0.6]
joint_names = group.get_active_joints()
for i in range(len(joint_names)):
joint_constraint = JointConstraint()
joint_constraint.joint_name = joint_names[i]
joint_constraint.position = joint_values[i]
joint_constraint.weight = 1.0
goal_constraint.joint_constraints.append(joint_constraint)
self._goal.request.goal_constraints.append(goal_constraint)
self._goal.planning_options.planning_scene_diff.robot_state.is_diff = True
def followCartTrajMoveit(self, x_vec, dt):
moveit_commander.roscpp_initialize(sys.argv)
group = moveit_commander.MoveGroupCommander("left_arm")
waypoints = []
waypoints.append(group.get_current_pose().pose)
for i in range(len(x_vec)):
#Make sure quaternion is normalized
x_vec[i][3:7] = x_vec[i][3:7] / la.norm(x_vec[i][3:7])
wpose = geometry_msgs.msg.Pose()
wpose.position.x = x_vec[i][0]
wpose.position.y = x_vec[i][1]
wpose.position.z = x_vec[i][2]
wpose.orientation.x = x_vec[i][3]
wpose.orientation.y = x_vec[i][4]
wpose.orientation.z = x_vec[i][5]
wpose.orientation.w = x_vec[i][6]
waypoints.append(copy.deepcopy(wpose))
(plan, fraction) = group.compute_cartesian_path(
waypoints, # waypoints to follow
dt, # eef_step
0.0) # jump_threshold
print "fraction: ", fraction
print "plan: ", plan
group.execute(plan)
def __init__(self):
## First initialize moveit_commander and rospy.
moveit_commander.roscpp_initialize(sys.argv)
rospy.init_node('move_piece_interface',
anonymous=True)
## Instantiate a RobotCommander object. This object is an interface to
## the robot as a whole.
self.robot = moveit_commander.RobotCommander()
## Instantiate a PlanningSceneInterface object. This object is an interface
## to the world surrounding the robot.
self.scene = moveit_commander.PlanningSceneInterface()
## Instantiate a MoveGroupCommander object. This object is an interface
## to one group of joints. In this case the group is the joints in the left
## arm. This interface can be used to plan and execute motions on the left
## arm.
self.left_group = moveit_commander.MoveGroupCommander("left_arm")
self.right_group = moveit_commander.MoveGroupCommander("right_arm")
#self.listener = tf.TransformListener()
#self.listener.waitForTransform("/base", 'left_gripper', rospy.Time(0), rospy.Duration(3.0));
#self._limb = baxter_interface.Limb('left')
self._left_gripper = baxter_interface.Gripper('left')
self._right_gripper = baxter_interface.Gripper('right')
self.gripper_close()
def launch_script():
print "====== l_leg_inverse_ik.py started ========"
print "======= Start initializing move commander for l_leg_inverse ==========="
moveit_commander.roscpp_initialize(sys.argv)
rospy.init_node('l_leg_inverse_ik', anonymous=True)
lleg_group = moveit_commander.MoveGroupCommander("l_leg_inverse")
lleg_group.set_planner_id("RRTConnectkConfigDefault")
print "\n =========== Some basic informations of Move Commander=================="
print "Current Pose of end effector %s %s" %(lleg_group.get_end_effector_link(), lleg_group.get_current_pose())
lleg_group.set_end_effector_link("pelvis")
print "The end effector is: %s" %lleg_group.get_end_effector_link()
current_pose = lleg_group.get_current_pose()
random_target = lleg_group.get_random_pose()
print "==============================================================="
print "Random pose set: %s" %random_target
lleg_group.set_pose_target(random_target)
print "==============================================================="
print "Planning"
lleg_group.plan()
print "==============================================================="
print "Running"
lleg_group.go(wait=True)
print "==============================================================="
def move_group_python_interface_tutorial():
## First initialize moveit_commander and rospy.
print "============ Starting tutorial setup"
moveit_commander.roscpp_initialize(sys.argv)
rospy.init_node('move_group_python_interface_tutorial',
anonymous=True)
## Instantiate a RobotCommander object. This object is an interface to
## the robot as a whole.
robot = moveit_commander.RobotCommander()
## Instantiate a PlanningSceneInterface object. This object is an interface
## to the world surrounding the robot.
scene = moveit_commander.PlanningSceneInterface()
## Instantiate a MoveGroupCommander object. This object is an interface
## to one group of joints. In this case the group is the joints in the left
## arm. This interface can be used to plan and execute motions on the left
## arm.
group = moveit_commander.MoveGroupCommander("arm")
## Sometimes for debugging it is useful to print the entire state of the
print "============ Printing robot end-effector pose"
#print group.get_current_pose()
print group.get_current_pose().pose.position.x, group.get_current_pose().pose.position.y, group.get_current_pose().pose.position.z
print group.get_current_rpy()
## When finished shut down moveit_commander.
moveit_commander.roscpp_shutdown()
def __init__(self):
# Initialize the move_group API
moveit_commander.roscpp_initialize(sys.argv)
rospy.init_node('subtask_grasp')
rospy.on_shutdown(self.shutdown)
self.grasp_srv = rospy.Service('subtask_grasp', rp_grasp, self.graspSrvCallback)
def __init__(self):
rospy.init_node("moveit_cartesian_path", anonymous=False)
rospy.loginfo("Starting node moveit_cartesian_path")
rospy.on_shutdown(self.cleanup)
moveit_commander.roscpp_initialize(sys.argv)
self._arm = moveit_commander.MoveGroupCommander('manipulator')
rospy.loginfo("End effector: " + self._arm.get_end_effector_link())
self._arm.set_pose_reference_frame("/base")
# 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)
self._joint_order = ('shoulder_pan_joint',
'shoulder_lift_joint',
'elbow_joint',
'wrist_1_joint',
'wrist_2_joint',
'wrist_3_joint')
self._joint_sub = rospy.Subscriber("/joint_states", JointState, self.cb_js)
self._last_check = 0
self._tcp_location = None
self._ur_kin = ur5()
def main():
global left_arm
global left_gripper
#Initialize moveit_commander
moveit_commander.roscpp_initialize(sys.argv)
#Start a node
rospy.init_node('moveit_interface')
#Set up Baxter Arms
robot = moveit_commander.RobotCommander()
scene = moveit_commander.PlanningSceneInterface()
left_arm = moveit_commander.MoveGroupCommander('left_arm')
left_arm.set_planner_id('RRTConnectkConfigDefault')
left_arm.set_planning_time(10)
#Calibrate the end effector on the left arm
left_gripper = baxter_gripper.Gripper('left')
print('Calibrating...')
left_gripper.calibrate()
rospy.sleep(2.0)
print('Ready to go')
#make subscriber to position topic
rospy.Subscriber('new_position', Pose, move_arm)
#make subscriber to gripper topic
rospy.Subscriber('gripper_control', Bool, actuate_gripper)
rospy.spin()
def __init__(self):
rospy.loginfo("Initializing ChipBehaviour")
moveit_commander.roscpp_initialize(sys.argv)
robot = moveit_commander.RobotCommander()
scene = moveit_commander.PlanningSceneInterface()
self.group = moveit_commander.MoveGroupCommander("right_arm_torso")
self.pose_pub = rospy.Publisher('/arm_pointing_pose',
geometry_msgs.msg.PoseStamped,
queue_size=1)
self.tts_ac = SimpleActionClient('/tts', TtsAction)
self.tts_ac.wait_for_server()
self.point_head_ac = SimpleActionClient('/head_controller/point_head_action',
PointHeadAction)
self.point_head_ac.wait_for_server(rospy.Duration(10.0))
self.faces_sub = rospy.Subscriber('/pal_face/faces',
FaceDetections,
self.faces_cb,
queue_size=1)
def listener():
# In ROS, nodes are uniquely named. If two nodes with the same
# node are launched, the previous one is kicked off. The
# anonymous=True flag means that rospy will choose a unique
# name for our 'listener' node so that multiple listeners can
# run simultaneously.
moveit_commander.roscpp_initialize(sys.argv)
rospy.init_node('listener', anonymous=True)
global robot
global scene
global arm
global gripper
robot = moveit_commander.RobotCommander()
scene = moveit_commander.PlanningSceneInterface()
arm = moveit_commander.MoveGroupCommander("arm")
gripper = moveit_commander.MoveGroupCommander("gripper")
rospy.Subscriber("/pick_pose", Point, callback)
# sub = rospy.Subscriber("/clicked_point", geometry_msgs.msg.PoseStamped, callback)
# spin() simply keeps python from exiting until this node is stopped
rospy.spin()
def InitializeMoveItCommander():
moveit_commander.roscpp_initialize(sys.argv)
# Instantiate a RobotCommander object. This object is an interface to
# the robot as a whole.
robot = moveit_commander.RobotCommander()
rospy.sleep(1)
## Instantiate a PlanningSceneInterface object. This object is an interface
## to the world surrounding the robot.
global scene
scene = moveit_commander.PlanningSceneInterface()
rospy.sleep(1)
## Instantiate a MoveGroupCommander object. This object is an interface
## to one group of joints. In this case the group is the joints in the left
## arm. This interface can be used to plan and execute motions on the left
## arm.
global group
group = MoveGroupCommander("left_arm")
global left_gripper
left_gripper = baxter_interface.Gripper('left')
left_gripper.calibrate()
rospy.sleep(2)
def __init__(self):
# Initialize the move_group API
moveit_commander.roscpp_initialize(sys.argv)
rospy.init_node('moveit_demo')
# Initialize the MoveIt! commander for the right arm
arm = moveit_commander.MoveGroupCommander('arm_with_torso')
arm.set_end_effector_link('wrist_roll_link')
# Get the name of the end-effector link
end_effector_link = arm.get_end_effector_link()
# Set the reference frame for pose targets
reference_frame = 'base_link'
# Set the right arm reference frame accordingly
arm.set_pose_reference_frame(reference_frame)
# Allow replanning to increase the odds of a solution
arm.allow_replanning(True)
# Allow some leeway in position (meters) and orientation (radians)
arm.set_goal_position_tolerance(0.01)
arm.set_goal_orientation_tolerance(0.05)
# Set the start pose. This particular pose has the gripper oriented horizontally
# 0.75 meters above the base and 0.72 meters in front of the robot.
target_pose = PoseStamped()
target_pose.header.frame_id = reference_frame
target_pose.header.stamp = rospy.Time.now()
target_pose.pose.position.x = 0.609889
target_pose.pose.position.y = 0.207002
target_pose.pose.position.z = 1.10677
target_pose.pose.orientation.x = 0.0
target_pose.pose.orientation.y = 0.0
target_pose.pose.orientation.z = 0.0
target_pose.pose.orientation.w = 1.0
# Set the start state to the current state
arm.set_start_state_to_current_state()
# Set the goal pose of the end effector to the stored pose
arm.set_pose_target(target_pose, end_effector_link)
# Plan the trajectory to the goal
traj = arm.plan()
# Execute the planned trajectory
arm.execute(traj)
# Pause for a couple of seconds
rospy.sleep(2)
# Shut down MoveIt cleanly
moveit_commander.roscpp_shutdown()
# Exit MoveIt
moveit_commander.os._exit(0)
def __init__(self):
smach.State.__init__(self, outcomes=['done'],
input_keys=['goal_pose'])
moveit_commander.roscpp_initialize(sys.argv)
## Instantiate a RobotCommander object. This object is an interface to
## the robot as a whole.
self.robot = moveit_commander.RobotCommander()
## Instantiate a PlanningSceneInterface object. This object is an interface
## to the world surrounding the robot.
rospy.loginfo('Instantiate PlanningSceneInterface')
self.scene = moveit_commander.PlanningSceneInterface()
## Instantiate a MoveGroupCommander object. This object is an interface
## to one group of joints. In this case the group is the joints in the left
## arm. This interface can be used to plan and execute motions on the left
## arm.
rospy.loginfo('Instantiate MoveGroupCommander')
self.group = moveit_commander.MoveGroupCommander("manipulator")
## We create this DisplayTrajectory publisher which is used below to publish
## trajectories for RVIZ to visualize.
rospy.loginfo('Create display_trajectory_publisher')
self.display_trajectory_publisher = rospy.Publisher(
'/move_group/display_planned_path',
moveit_msgs.msg.DisplayTrajectory)
def move_arm():
print "Starting tutorial setup"
moveit_commander.roscpp_initialize(sys.argv)
rospy.init_node('move_arm_node')
print "move_arm_node should be initialized"
print "Initializing Robot Commander"
robot = moveit_commander.RobotCommander()
print "Initializing Planning Scene Interface"
scene = moveit_commander.PlanningSceneInterface()
print "Let's move left arm first"
group = moveit_commander.MoveGroupCommander("left_arm")
print "Create publisher to display_planned_path"
display_trajectory_publisher = rospy.Publisher('/move_group/display_planned_path',moveit_msgs.msg.DisplayTrajectory)
robot_pose = geometry_msgs.msg.Pose()
robot_pose.orientation.x = 1
robot_pose.position.x = 0.7
robot_pose.position.y = 0.4
robot_pose.position.z = 0.3
group.set_pose_target(robot_pose)
plan1 = group.plan()
rospy.sleep(5)
group.go(wait=True)
moveit_commander.roscpp_shutdown()
print "Finishing"
def move_group_python_interface_tutorial():
global group_left
global group_right
global p
print "============ Starting tutorial setup"
moveit_commander.roscpp_initialize(sys.argv)
#rospy.init_node('move_group_python_interface_tutorial',anonymous=True)
## Instantiate a RobotCommander object. Interface to the robot.
robot = moveit_commander.RobotCommander()
## Instantiate a PlanningSceneInterface object. Interface to the world.
scene = moveit_commander.PlanningSceneInterface()
## Instantiate a MoveGroupCommander object. Interface to groups of joints.
group_left = moveit_commander.MoveGroupCommander("left_arm")
#group_right = moveit_commander.MoveGroupCommander("right_arm")
## We create this DisplayTrajectory publisher which is used below to publish
## trajectories for RVIZ to visualize.
display_trajectory_publisher = rospy.Publisher(
'/move_group/display_planned_path',
moveit_msgs.msg.DisplayTrajectory,
queue_size=20)
## Wait for RVIZ to initialize. This sleep is ONLY to allow Rviz to come up.
print "============ Waiting for RVIZ..."
#rospy.sleep(2)
print "============ Starting tutorial "
## We can get the name of the reference frame for this robot
print "============ Reference frame: %s" % group_left.get_planning_frame()
## We can also print the name of the end-effector link for this group
print "============ End effector: %s" % group_left.get_end_effector_link()
## We can get a list of all the groups in the robot
print "============ Robot Groups:"
print robot.get_group_names()
## Sometimes for debugging it is useful to print the entire state of the
## robot.
print "============ Printing robot state"
print robot.get_current_state()
print "============"
p = PlanningSceneInterface("base_link")
p.addBox("table",0.2 ,1.25 ,0.55 ,0.65 ,0.775 ,0.275)
p.addCylinder("redCylinder",0.3,0.03,0.65,0.7,0.7)
p.addCylinder("blueCylinder",0.3,0.03,0.65,0.8,0.7)
p.addCylinder("greenCylinder",0.3,0.03,0.65,0.9,0.7)
p.addBox("obstacle1_1", 0.6, 0.2, 0.4, 0.65, 0.45, 0.95)
p.addBox("obstacle1_2", 0.45, 0.01, 0.50, 0.3, 0.4, 0.3)
print group_left.get_planning_time()
group_left.set_planning_time(15)
print group_left.get_planning_time()
group_left.set_num_planning_attempts(20)
'''p.addBox("obstacle1_1", 0.15, 0.1, 0.3, 0.6, 0.4, 0.7)#llega hasta 0.85
p.addBox("obstacle1_2", 0.25, 0.1, 0.70, 0.35, 0.4, 0.35)
#p.addBox("obstacle1_3", 0.75, 0.8, 0.3, 0.6, 0.4, 1.32)
p.addBox("obstacle1_3", 0.75, 0.8, 0.1, 0.6, 0.4, 1.18)
#p.addBox("obstacle1_3_1", 0.3, 0.1, 0.04, 0.225, 0.4, 1.09)
p.addBox("obstacle1_1_bis", 0.3, 0.1, 0.4, 0.6, 0.00, 0.7)'''
#p.addBox("obstacle1_2_bis", 0.25, 0.1, 0.1, 0.35, 0.4, 1.12)
#p.addBox("obstacle1_11", 0.2, 0.1, 0.65, 0.6, 0.4, 1.175)
#p.addBox("obstacle1_21", 0.25, 0.1, 0.65, 0.35, 0.4, 1.275)
'''#PRUEBA 1
def __init__(self):
# rospy.sleep(10) # wait for other files to be launched successfully
moveit_commander.roscpp_initialize(sys.argv)
rospy.init_node('cw3_q1')
self.tf_buffer = tf2_ros.Buffer()
self.tf_listener = tf2_ros.TransformListener(self.tf_buffer)
# self.touch = [0,0,0,0]
# rospy.Subscriber("/contacts/lh_ff/distal", ContactsState, self.call_ff)
# rospy.Subscriber("/contacts/lh_mf/distal", ContactsState, self.call_mf)
# rospy.Subscriber("/contacts/lh_rf/distal", ContactsState, self.call_rf)
# rospy.Subscriber("/contacts/lh_th/distal", ContactsState, self.call_th)
# # Robot contains the entire state of the robot (iiwa and shadow hand)
# robot = moveit_commander.RobotCommander()
# # We can get a list of all the groups in the robot
# print('============ Robot Groups:')
# print('{}\n\n'.format(robot.get_group_names()))
self.iiwa_group = moveit_commander.MoveGroupCommander('hand_iiwa')
self.hand_group = moveit_commander.MoveGroupCommander('sr_hand')
rospy.sleep(1)
# object tracking
[all_object_name,all_object_pose] = self.get_object_position()
# create a service proxy for add and remove objects to allowed collision matrix
add_to_acm = rospy.ServiceProxy('/add_object_acm', ChangeCollisionObject)
remove_from_acm = rospy.ServiceProxy('/remove_object_acm', ChangeCollisionObject)
# open the hand
self.hand_open()
for i in range(0,3):
# i = 2 # object no.
# move the iiwa to just above the object
self.move_iiwa_to_object(all_object_pose,i)
# add object to allowed collision matrix
add_to_acm(all_object_name[i])
# close the hand (grasping)
self.hand_close()
# hold the grasping position for 10 seconds
rospy.sleep(10)
# open the hand (releasing)
self.hand_open()
# remove object from allowed collision matrix
remove_from_acm(all_object_name[i])
moveit_commander.roscpp_shutdown()
rospy.loginfo("Task finished")
def move_group_python_interface_tutorial():
global group_left
global group_right
global p
print "============ Starting tutorial setup"
moveit_commander.roscpp_initialize(sys.argv)
#rospy.init_node('move_group_python_interface_tutorial',anonymous=True)
## Instantiate a RobotCommander object. Interface to the robot.
robot = moveit_commander.RobotCommander()
## Instantiate a PlanningSceneInterface object. Interface to the world.
scene = moveit_commander.PlanningSceneInterface()
## Instantiate a MoveGroupCommander object. Interface to groups of joints.
group_left = moveit_commander.MoveGroupCommander("left_arm")
#group_right = moveit_commander.MoveGroupCommander("right_arm")
## We create this DisplayTrajectory publisher which is used below to publish
## trajectories for RVIZ to visualize.
display_trajectory_publisher = rospy.Publisher(
'/move_group/display_planned_path',
moveit_msgs.msg.DisplayTrajectory,
queue_size=20)
## Wait for RVIZ to initialize. This sleep is ONLY to allow Rviz to come up.
print "============ Waiting for RVIZ..."
#rospy.sleep(2)
print "============ Starting tutorial "
## We can get the name of the reference frame for this robot
print "============ Reference frame: %s" % group_left.get_planning_frame()
## We can also print the name of the end-effector link for this group
print "============ End effector: %s" % group_left.get_end_effector_link()
## We can get a list of all the groups in the robot
print "============ Robot Groups:"
print robot.get_group_names()
## Sometimes for debugging it is useful to print the entire state of the
## robot.
print "============ Printing robot state"
print robot.get_current_state()
print "============"
p = PlanningSceneInterface("base_link")
p.addBox("table",0.2 ,1.25 ,0.35 ,0.6 ,0.775 ,0.175)
p.addCylinder("redCylinder",0.3,0.03,0.6,0.7,0.5)
p.addCylinder("blueCylinder",0.3,0.03,0.6,0.8,0.5)
p.addCylinder("greenCylinder",0.3,0.03,0.6,0.9,0.5)
p.addBox("obstacle1_1", 0.4, 0.01, 0.5, 0.6, 0.55, 0.6)
p.addBox("obstacle1_2", 0.4, 0.01, 0.2, 0.6, 0.55, 0.95)#cambiar a 0.25 despues
p.addBox("obstacle1_3", 0.4, 0.01, 0.2, 0.6, 0.55, 1.15)
#p.addBox("obstacle1_1", 0.25, 0.01, 0.2, 0.6, 0.3, 0.45) #Estos estados para obstacle1 OK
#p.addBox("obstacle1_2", 0.65, 0.01, 0.2, 0.6, 0.3, 0.65)
#p.addBox("obstacle1_3", 0.65, 0.01, 0.2, 0.6, 0.3, 0.85)
#p.addBox("obstacle2_1", 1, 0.01, 0.2, 0.6, 0.6, 0.45)
#p.addBox("obstacle2_2", 0.25, 0.01, 0.2, 0.6, 0.6, 0.65)
#.addBox("obstacle2_3", 0.95, 0.01, 0.2, 0.6, 0.6, 0.85)
#p.addBox("obstacle2_4", 0.475, 0.01, 0.3, 0.2625, 0.4, 0.3)
p.addBox("obstacle2_1", 0.15, 0.01, 0.45, 0.6, 0.3, 0.6)
## Planning to a Pose goal
## We can plan a motion for this group to a desired pose for the
## end-effector
'''print "============ Running code from ~/ws_moveit"
def kinect_controller():
moveit_commander.roscpp_initialize(sys.argv)
rospy.init_node('robot_state_publisher', anonymous=True,)
rate = rospy.Rate(10)
# Instantiate a RobotCommander object.
robotc = moveit_commander.RobotCommander()
groupc = moveit_commander.MoveGroupCommander("Kinect2_Target")
groupc.set_planner_id("RRTConnectkConfigDefault")
# We create this publisher for the desired pose of the Kinect2 Sensor
pose_publisher = rospy.Publisher("desired_pose", PoseStamped, queue_size=5)
# Create a suscrber to read the trajectory published by kinect_planner.py
rospy.Subscriber("planned_path", RobotTrajectory, callback)
print "=============== Current Pose ==========="
print groupc.get_current_pose();
print "========= Setting initial pose ================"
group_variable_values = groupc.get_current_joint_values()
## Defining a Pose goal
pose_target = PoseStamped()
pose_target.header.frame_id = 'base_footprint'
pose_target.pose.orientation.x = 0
pose_target.pose.orientation.y = 0
pose_target.pose.orientation.z = 0
pose_target.pose.orientation.w = 1
pose_target.pose.position.x = 1.3
pose_target.pose.position.y = -0.2
pose_target.pose.position.z = 1.17
while not rospy.is_shutdown():
global fresh_data
# Publishe desired pose
pose_publisher.publish(pose_target)
fresh_data = false
# If no trajectory is received, sleep
while fresh_data == false:
rate.sleep()
# Read the trajectory published by kinect_planner.py
#plan = set_robot_trajectory_msg(trajectory1,group_variable_values)
plan_target = groupc.plan(trajectory1)
groupc.go(wait=True)
print "============ Waiting while the plan is executed"
rate.sleep()
# Display current joint values
group_variable_values = groupc.get_current_joint_values()
print "============ Joint values: ", group_variable_values
def init():
#Wake up Baxter
baxter_interface.RobotEnable().enable()
rospy.sleep(0.25)
print "Baxter is enabled"
print "Intitializing clients for services"
global ik_service_left
ik_service_left = rospy.ServiceProxy(
"ExternalTools/left/PositionKinematicsNode/IKService",
SolvePositionIK)
global ik_service_right
ik_service_right = rospy.ServiceProxy(
"ExternalTools/right/PositionKinematicsNode/IKService",
SolvePositionIK)
global obj_loc_service
obj_loc_service = rospy.ServiceProxy(
"object_location_service", ObjLocation)
global stopflag
stopflag = False
#Taken from the MoveIt Tutorials
moveit_commander.roscpp_initialize(sys.argv)
robot = moveit_commander.RobotCommander()
global scene
scene = moveit_commander.PlanningSceneInterface()
#Activate Left Arm to be used with MoveIt
global left_group
left_group = MoveGroupCommander("left_arm")
left_group.set_goal_position_tolerance(0.01)
left_group.set_goal_orientation_tolerance(0.01)
global right_group
right_group = MoveGroupCommander("right_arm")
pose_right = Pose()
pose_right.position = Point(0.793, -0.586, 0.329)
pose_right.orientation = Quaternion(1.0, 0.0, 0.0, 0.0)
request_pose(pose_right, "right", right_group)
global left_gripper
left_gripper = baxter_interface.Gripper('left')
left_gripper.calibrate()
print left_gripper.parameters()
global end_effector_subs
end_effector_subs = rospy.Subscriber("/robot/end_effector/left_gripper/state", EndEffectorState, end_effector_callback)
rospy.sleep(1)
global pubpic
pubpic = rospy.Publisher('/robot/xdisplay', Image, latch=True, queue_size=1)
rospy.sleep(1)
def publish_states():
moveit_commander.roscpp_initialize(sys.argv)
joint_states = JointState()
rospy.init_node('Testing_node')
joint_states.name = ['r1joint_1', 'r1joint_2', 'r1joint_3', 'r1joint_4', 'r1joint_5', 'r1joint_6']
joint_states.position = [1.2,1,1,-0.2,-0.2,-0.2]
joint_states.header.frame_id = '/base_link'
set_joints(joint_states)
def main():
moveit_commander.roscpp_initialize(sys.argv)
rospy.init_node('moveit', anonymous=True)
rospy.Subscriber("point", Point, callback)
rospy.loginfo("Starting moveit")
try:
rospy.spin()
except KeyboardInterrupt:
print("Shutting down")
def go():
node_name = 'mico_planner'
group_name = 'arm'
planner_name = 'RRTstarkConfigDefault'
ee_link_name = 'mico_link_endeffector'
roscpp_initialize(sys.argv)
rospy.init_node(node_name, anonymous=True)
acHan = ActionHandler(group_name, planner_name, ee_link_name)
rospy.sleep(1)
files = []
lant = -1
loc = -1
ori = -1
#get input
has_input = False
while(not has_input):
inp = raw_input("Enter Lantern to pick, and location to place (Format = Lantern# Location# Orientation#) > ")
inp = inp.split(" ")
if len(inp) == 3:
for n in inp:
if (n == "0") or (n == "1") or (n == "2") or (n == "3") or (n == "4"):
has_input = True
else:
has_input = False
break
lant = inp[0]
loc = inp[1]
ori = inp[2]
#get files
files.append("StartupPlan")
files.append("Release")
files.append("TransEmpty_" + str(lant) + "_" + str(ori))
files.append("Grasp_80")
files.append("TransLoadedUp_" + str(lant) + "_" + str(ori))
files.append("TransLoadedDown_" + str(loc) + "_" + str(ori))
files.append("Grasp_30")
files.append("TransEmptyRetract_" + str(loc) + "_" + str(ori))
files.append("Release")
for f in files:
t = open("plans/" + str(f), "r")
runFiles(t, acHan)
t.close()
def initialize_moveit(self):
print "============ Initializing Moveit"
moveit_commander.roscpp_initialize(sys.argv)
#rospy.init_node('baxter3_moveit',anonymous=True)
## Instantiate a RobotCommander object. This object is an interface to
## the robot as a whole.
self.robot = moveit_commander.RobotCommander()
## Instantiate a PlanningSceneInterface object. This object is an interface
## to the world surrounding the robot.
self.scene = moveit_commander.PlanningSceneInterface()
print "============ Waiting for RVIZ..."
rospy.sleep(3)
print "============ Starting tutorial "
print "============ Adding collision objects..."
# Add in objects
# Table
p = PoseStamped()
p.header.frame_id = self.robot.get_planning_frame()
p.pose.position.x = 0.8
p.pose.position.y = 0.025
p.pose.position.z = -0.6
p.pose.orientation.x = 1
self.scene.add_box("table", p, (0.762, 1.25, 1))
# Box 1
p = PoseStamped()
p.header.frame_id = self.robot.get_planning_frame()
p.pose.position.x = 0.6
p.pose.position.y = 0.025
p.pose.position.z = -0.22
p.pose.orientation.x = 1
self.scene.add_box("box1", p, (0.25, 0.2, 0.08))
# Box 2
p = PoseStamped()
p.header.frame_id = self.robot.get_planning_frame()
p.pose.position.x = 1
p.pose.position.y = 0.025
p.pose.position.z = -0.22
p.pose.orientation.x = 1
self.scene.add_box("box2", p, (0.25, 0.2, 0.08))
print "============"
self.group_left = moveit_commander.MoveGroupCommander("left_arm")
self.group_left.set_goal_position_tolerance(0.01)
self.group_left.set_goal_orientation_tolerance(0.01)
self.group_right = moveit_commander.MoveGroupCommander("right_arm")
self.group_right.set_goal_position_tolerance(0.01)
self.group_right.set_goal_orientation_tolerance(0.01)
## We create this DisplayTrajectory publisher which is used below to publish trajectories for RVIZ to visualize.
display_trajectory_publisher = rospy.Publisher('/move_group/display_planned_path',moveit_msgs.msg.DisplayTrajectory, queue_size=10)
def __init__(self):
rospy.init_node('cruimanager')
moveit_commander.roscpp_initialize(sys.argv)
# Pull all params off param server
self.analyze_grasp_topic = rospy.get_param("analyze_grasp_topic")
self.execute_grasp_topic = rospy.get_param("execute_grasp_topic")
self.run_recognition_topic = rospy.get_param("run_recognition_topic")
self.grasp_approach_tran_frame = rospy.get_param("grasp_approach_tran_frame")
self.world_frame = rospy.get_param("world_frame")
self.arm_move_group_name = rospy.get_param("arm_move_group_name")
self.gripper_move_group_name = rospy.get_param("gripper_move_group_name")
self.analyzer_planner_id = rospy.get_param("analyzer_planner_id")
self.executor_planner_id = rospy.get_param("executor_planner_id")
self.allowed_analyzing_time = rospy.get_param("allowed_analyzing_time")
self.allowed_execution_time = rospy.get_param("allowed_execution_time")
self.grasping_controller = graspit_moveit_controller.MoveitPickPlaceInterface(
arm_name=self.arm_move_group_name,
gripper_name=self.gripper_move_group_name,
grasp_approach_tran_frame=self.grasp_approach_tran_frame,
analyzer_planner_id=self.analyzer_planner_id,
execution_planner_id=self.executor_planner_id,
allowed_analyzing_time=self.allowed_analyzing_time,
allowed_execution_time=self.allowed_execution_time
)
self.scene = moveit_commander.PlanningSceneInterface()
self.block_recognition_client = block_recognition.BlockRecognitionClient()
self.world_manager_client = world_manager.world_manager_client.WorldManagerClient()
self.tf_listener = tf.TransformListener()
# Start grasp analyzer action server
self._analyze_grasp_as = actionlib.SimpleActionServer(self.analyze_grasp_topic,
graspit_msgs.msg.CheckGraspReachabilityAction,
execute_cb=self._analyze_grasp_reachability_cb,
auto_start=False)
self._analyze_grasp_as.start()
# Start grasp execution action server
self._execute_grasp_as = actionlib.SimpleActionServer(self.execute_grasp_topic,
graspit_msgs.msg.GraspExecutionAction,
execute_cb=self._execute_grasp_cb,
auto_start=False)
self._execute_grasp_as.start()
# Start object recognition action server
self._run_recognition_as = actionlib.SimpleActionServer(self.run_recognition_topic,
graspit_msgs.msg.RunObjectRecognitionAction,
execute_cb=self._run_recognition_cb,
auto_start=False)
self._run_recognition_as.start()
rospy.loginfo(self.__class__.__name__ + " is inited")
def __init__(self):
moveit_commander.roscpp_initialize(sys.argv)
# initialize MoveIt
self.robot = moveit_commander.RobotCommander()
self.scene = moveit_commander.PlanningSceneInterface()
self.leftGroup = moveit_commander.MoveGroupCommander("left_arm")
self.rightGroup = moveit_commander.MoveGroupCommander("right_arm")
self.display_trajectory_publisher = rospy.Publisher('/move_group/display_planned_path',
moveit_msgs.msg.DisplayTrajectory)
def __init__(self):
rospy.init_node('xm_arm_moveit_ik_test', anonymous=True)
moveit_commander.roscpp_initialize(sys.argv)
xm_arm = moveit_commander.MoveGroupCommander('xm_arm')
end_effector_link = xm_arm.get_end_effector_link()
reference_frame = 'base_footprint'
xm_arm.set_pose_reference_frame(reference_frame)
xm_arm.allow_replanning(True)
xm_arm.set_goal_position_tolerance(5)
xm_arm.set_goal_orientation_tolerance(5)
xm_arm.set_planner_id("RRTConnectkConfigDefault")
xm_arm.set_named_target('initial')
xm_arm.go()
rospy.sleep(2)
print "-------------------- Test Arm Moveit IK ----------------------"
print "Input the position of object(reference frame is base's center)"
print "Such as [object/o x y z x y z w] "
while not rospy.is_shutdown():
print ">>> ",
keyboard_cmd = raw_input().split(" ")
try:
if keyboard_cmd[0] == "object" or keyboard_cmd[0] == "o":
target_pose = PoseStamped()
target_pose.header.frame_id = reference_frame
target_pose.header.stamp = rospy.Time.now()
target_pose.pose.position.x = float(keyboard_cmd[1])
target_pose.pose.position.y = float(keyboard_cmd[2])
target_pose.pose.position.z = float(keyboard_cmd[3])
target_pose.pose.orientation.x = float(keyboard_cmd[4])
target_pose.pose.orientation.y = float(keyboard_cmd[5])
target_pose.pose.orientation.z = float(keyboard_cmd[6])
target_pose.pose.orientation.w = float(keyboard_cmd[7])
print ("position[x y z] %s, %s, %s" % (keyboard_cmd[1],
keyboard_cmd[2],
keyboard_cmd[3]))
print "orientation[x y z w] %s, %s, %s, %s" % (
keyboard_cmd[4],
keyboard_cmd[5],
keyboard_cmd[6],
keyboard_cmd[7])
xm_arm.set_start_state_to_current_state()
xm_arm.set_pose_target(target_pose, end_effector_link)
trajectory = xm_arm.plan()
xm_arm.execute(trajectory)
rospy.sleep(1)
elif keyboard_cmd[0] == "exit" or keyboard_cmd[0] == "e":
exit()
except Exception as exce:
print "Error!", exce
moveit_commander.roscpp_shutdown()
moveit_commander.os._exit(0)
def __init__(self):
# Initialize API for movo_group
moveit_commander.roscpp_initialize(sys.argv)
# Initialize Node of ROS
rospy.init_node('zhiyang_attached_camera_demo')
# Define Scene Object
scene = PlanningSceneInterface()
rospy.sleep(1)
# Initialize arm group from moveit group to control manipulator
rarm = MoveGroupCommander('right_arm')
larm = MoveGroupCommander('left_arm')
# Get end effector link name
r_end_effector_link = rarm.get_end_effector_link()
l_end_effector_link = larm.get_end_effector_link()
# Set tolerance of position(m) and orentation(rad)
rarm.set_goal_position_tolerance(0.01)
rarm.set_goal_orientation_tolerance(0.05)
larm.set_goal_position_tolerance(0.01)
larm.set_goal_orientation_tolerance(0.05)
# Allow replanning
rarm.allow_replanning(True)
rarm.set_planning_time(10)
larm.allow_replanning(True)
larm.set_planning_time(10)
# Let arm go back to home position
# arm.set_named_target('home')
# arm.go()
# Remove other attached object in before Scene run
# scene.remove_attached_object(end_effector_link, 'tool')
# scene.remove_world_object('table')
# scene.remove_world_object('target')
# Set height of table
table_ground = 0.6
# Set 3d size of table and tool
table_size = [0.3, 0.7, 0.01]
tool_size = [0.15, 0.15, 0.06]
# Set orentation and position of tool
p1 = PoseStamped()
p1.header.frame_id = r_end_effector_link
# p.pose.position.x = tool_size[0] / 2.0 - 0.025
# p.pose.position.y = -0.015
# p.pose.position.z = 0.0
p1.pose.position.x = -0.07
p1.pose.position.y = 0.0
p1.pose.position.z = 0.08
p1.pose.orientation.x = 0
p1.pose.orientation.y = 0
p1.pose.orientation.z = 0
p1.pose.orientation.w = 1
p2 = PoseStamped()
p2.header.frame_id = l_end_effector_link
p2.pose.position.x = -0.07
p2.pose.position.y = 0.0
p2.pose.position.z = 0.08
p2.pose.orientation.x = 0
p2.pose.orientation.y = 0
p2.pose.orientation.z = 0
p2.pose.orientation.w = 1
# Make tool attact to end effector
scene.attach_box(r_end_effector_link, 'Camera1', p1, tool_size)
scene.attach_box(l_end_effector_link, 'Camera2', p2, tool_size)
# Add table to Scene
table_pose = PoseStamped()
table_pose.header.frame_id = 'base_link'
table_pose.pose.position.x = 0.75
table_pose.pose.position.y = 0.0
table_pose.pose.position.z = table_ground + table_size[2] / 2.0
table_pose.pose.orientation.w = 1.0
scene.add_box('table', table_pose, table_size)
rospy.sleep(2)
# Update current state (position and orentation)
rarm.set_start_state_to_current_state()
larm.set_start_state_to_current_state()
# Set target (rad)
joint_positions = [0.827228546495185, 0.29496592875743577, 0.29496592875743577, 1.1185644936946095, -0.7987583317769674, -0.18950024740190782, 0.11752152218233858]
rarm.set_joint_value_target(joint_positions)
larm.set_joint_value_target(joint_positions)
# Let arm go
rarm.go()
larm.go()
rospy.sleep(1)
# Let arm go back to initial state
# arm.set_named_target('home')
# arm.go()
moveit_commander.roscpp_shutdown()
moveit_commander.os._exit(0)
def main():
#Main Function that initializes the node
moveit_commander.roscpp_initialize(sys.argv)
rospy.init_node('elir_move_group', anonymous=True)
x = MoveGroupElir()
def __init__(self):
moveit_commander.roscpp_initialize(sys.argv)
current_robot_ns = 'robot1'
self.robot = moveit_commander.RobotCommander(current_robot_ns+'/robot_description',current_robot_ns)
# Init moveit group
self.group = moveit_commander.MoveGroupCommander('robot', current_robot_ns+'/robot_description',current_robot_ns)
self.arm_group = moveit_commander.MoveGroupCommander('arm',current_robot_ns+'/robot_description',current_robot_ns)
self.base_group = moveit_commander.MoveGroupCommander('base', current_robot_ns+'/robot_description',current_robot_ns)
self.scene = moveit_commander.PlanningSceneInterface()
self.sce = moveit_python.PlanningSceneInterface('odom', current_robot_ns)
self.pub_co = rospy.Publisher('/'+current_robot_ns+'/collision_object', CollisionObject, queue_size=100)
self.msg_print = SetBoolRequest()
self.request_fk = rospy.ServiceProxy('/'+current_robot_ns+'/compute_fk', GetPositionFK)
self.pub_co = rospy.Publisher('/'+current_robot_ns+'/collision_object',CollisionObject,queue_size=10)
sub_waypoint_status = rospy.Subscriber('/'+current_robot_ns+'/execute_trajectory/status', GoalStatusArray, self.waypoint_execution_cb)
sub_movegroup_status = rospy.Subscriber('/'+current_robot_ns+'/move_group/status', GoalStatusArray, self.move_group_execution_cb)
rospy.Subscriber('/'+current_robot_ns+"/joint_states", JointState, self.further_ob_printing)
# Initialize extruder
print_request = rospy.ServiceProxy('/'+current_robot_ns+'/set_extruder_printing', SetBool)
self.extruder_publisher = rospy.Publisher('/'+current_robot_ns+'/set_extruder_rate',Float32,queue_size=20)
# switch the extruder ON, control via set_extruder_rate
msg_print = SetBoolRequest()
msg_print.data=True
print_request(msg_print)
self.re_position_x = []
self.re_position_y = []
self.waypoint_execution_status = 0
self.move_group_execution_status = 0
self.further_printing_number = 0
self.pre_further_printing_number = 0
# initial printing number
self._printing_number = 0
self._further_printing_number = 0
self.future_printing_status = False
self.current_printing_pose = None
self.previous_printing_pose = None
self.target_list = None
self.group.allow_looking(1)
self.group.allow_replanning(1)
self.group.set_planning_time(10)
# Initialize time record
self.travel_time = 0
self.planning_time = 0
self.printing_time = 0
# Initialize extruder
msg_extrude = Float32()
msg_extrude = 0
self.extruder_publisher.publish(msg_extrude)
def __init__(self):
# 初始化move_group的API
moveit_commander.roscpp_initialize(sys.argv)
# 初始化ROS节点
rospy.init_node('moveit_cartesian_demo', anonymous=True)
# 是否需要使用笛卡尔空间的运动规划
cartesian = rospy.get_param('~cartesian', True)
# 初始化需要使用move group控制的机械臂中的arm group
arm = MoveGroupCommander('arm')
# 当运动规划失败后，允许重新规划
arm.allow_replanning(True)
# 设置目标位置所使用的参考坐标系
arm.set_pose_reference_frame('base_link')
# 设置位置(单位：米)和姿态（单位：弧度）的允许误差
arm.set_goal_position_tolerance(0.01)
arm.set_goal_orientation_tolerance(0.1)
# 获取终端link的名称
end_effector_link = arm.get_end_effector_link()
# 控制机械臂运动到之前设置的“forward”姿态
arm.set_named_target('forward')
arm.go()
# 获取当前位姿数据最为机械臂运动的起始位姿
start_pose = arm.get_current_pose(end_effector_link).pose
# 初始化路点列表
waypoints = []
# 将初始位姿加入路点列表
if cartesian:
waypoints.append(start_pose)
# 设置第二个路点数据，并加入路点列表
# 第二个路点需要向后运动0.2米，向右运动0.2米
wpose = deepcopy(start_pose)
#wpose.position.x -= 0.05
wpose.position.y += 0.1
#wpose.position.z += 0.2
if cartesian:
waypoints.append(deepcopy(wpose))
else:
arm.set_pose_target(wpose)
arm.go()
rospy.sleep(1)
# 设置第三个路点数据，并加入路点列表
#wpose.position.x -= 0.05
wpose.position.y += 0.15
#wpose.position.z += 0.15
if cartesian:
waypoints.append(deepcopy(wpose))
else:
arm.set_pose_target(wpose)
arm.go()
rospy.sleep(1)
# 设置第四个路点数据，回到初始位置，并加入路点列表
if cartesian:
waypoints.append(deepcopy(start_pose))
else:
arm.set_pose_target(start_pose)
arm.go()
rospy.sleep(1)
if cartesian:
fraction = 0.0 #路径规划覆盖率
maxtries = 100 #最大尝试规划次数
attempts = 0 #已经尝试规划次数
# 设置机器臂当前的状态作为运动初始状态
arm.set_start_state_to_current_state()
# 尝试规划一条笛卡尔空间下的路径，依次通过所有路点
while fraction < 1.0 and attempts < maxtries:
(plan, fraction) = arm.compute_cartesian_path(
waypoints, # waypoint poses，路点列表
0.01, # eef_step，终端步进值
0.0, # jump_threshold，跳跃阈值
True) # avoid_collisions，避障规划
# 尝试次数累加
attempts += 1
# 打印运动规划进程
if attempts % 10 == 0:
rospy.loginfo("Still trying after " + str(attempts) +
" attempts...")
# 如果路径规划成功（覆盖率100%）,则开始控制机械臂运动
if fraction == 1.0:
rospy.loginfo("Path computed successfully. Moving the arm.")
arm.execute(plan)
rospy.loginfo("Path execution complete.")
# 如果路径规划失败，则打印失败信息
else:
rospy.loginfo("Path planning failed with only " +
str(fraction) + " success after " +
str(maxtries) + " attempts.")
# 控制机械臂回到初始化位置
arm.set_named_target('home')
arm.go()
rospy.sleep(1)
# 关闭并退出moveit
moveit_commander.roscpp_shutdown()
moveit_commander.os._exit(0)
def __init__(self):
# 初始化move_group的API
moveit_commander.roscpp_initialize(sys.argv)
# 初始化ROS节点
rospy.init_node('moveit_obstacles_demo')
# 初始化场景对象
scene = PlanningSceneInterface()
# 创建一个发布场景变化信息的发布者
self.scene_pub = rospy.Publisher('planning_scene',
PlanningScene,
queue_size=5)
# 创建一个存储物体颜色的字典对象
self.colors = dict()
# 等待场景准备就绪
rospy.sleep(1)
# 初始化需要使用move group控制的机械臂中的arm group
arm = MoveGroupCommander('aubo10')
# 获取终端link的名称
end_effector_link = arm.get_end_effector_link()
# 设置位置(单位：米)和姿态（单位：弧度）的允许误差
arm.set_goal_position_tolerance(0.01)
arm.set_goal_orientation_tolerance(0.05)
# 当运动规划失败后，允许重新规划
arm.allow_replanning(True)
# 设置目标位置所使用的参考坐标系
reference_frame = 'base_link'
arm.set_pose_reference_frame(reference_frame)
# 设置每次运动规划的时间限制：5s
arm.set_planning_time(5)
# 设置场景物体的名称
table_id = 'table'
box1_id = 'box1'
box2_id = 'box2'
# 移除场景中之前运行残留的物体
scene.remove_world_object(table_id)
scene.remove_world_object(box1_id)
scene.remove_world_object(box2_id)
rospy.sleep(1)
# 控制机械臂先回到初始化位置
# target_pose = PoseStamped()
# target_pose.header.frame_id = reference_frame
# target_pose.pose.position.x = 0.0
# target_pose.pose.position.y = 0.0
# target_pose.pose.position.z = 0.0
# target_pose.pose.orientation.w = 1.0
# # 控制机械臂运动到目标位置
# arm.set_pose_target(target_pose, end_effector_link)
# arm.go()
# rospy.sleep(2)
# 设置桌面的高度
table_ground = 1.4
# 设置table、box1和box2的三维尺寸
table_size = [0.2, 0.7, 0.01]
box1_size = [0.1, 0.05, 0.05]
box2_size = [0.05, 0.05, 0.15]
# 将三个物体加入场景当中
table_pose = PoseStamped()
table_pose.header.frame_id = reference_frame
table_pose.pose.position.x = 0.8
table_pose.pose.position.y = -0.9
table_pose.pose.position.z = table_ground + table_size[2] / 2.0
table_pose.pose.orientation.w = 1.0
scene.add_box(table_id, table_pose, table_size)
# box1_pose = PoseStamped()
# box1_pose.header.frame_id = reference_frame
# box1_pose.pose.position.x = 0.8
# box1_pose.pose.position.y = -1.5
# box1_pose.pose.position.z = table_ground + table_size[2] + box1_size[2] / 2.0
# box1_pose.pose.orientation.w = 1.0
# scene.add_box(box1_id, box1_pose, box1_size)
# box2_pose = PoseStamped()
# box2_pose.header.frame_id = reference_frame
# box2_pose.pose.position.x = 0.8
# box2_pose.pose.position.y = -0.1
# box2_pose.pose.position.z = table_ground + table_size[2] + box2_size[2] / 2.0
# box2_pose.pose.orientation.w = 1.0
# scene.add_box(box2_id, box2_pose, box2_size)
# 将桌子设置成红色，两个box设置成橙色
self.setColor(table_id, 0.8, 0, 0, 1.0)
# self.setColor(box1_id, 0.8, 0.4, 0, 1.0)
# self.setColor(box2_id, 0.8, 0.4, 0, 1.0)
# 将场景中的颜色设置发布
self.sendColors()
# 设置机械臂的运动目标位置，位于桌面之上两个盒子之间
target_pose = PoseStamped()
target_pose.header.frame_id = reference_frame
target_pose.pose.position.x = 0.8
target_pose.pose.position.y = -0.7
target_pose.pose.position.z = 1.5 #table_pose.pose.position.z + table_size[2] + 0.05
target_pose.pose.orientation.w = 1.0
# 控制机械臂运动到目标位置
arm.set_pose_target(target_pose, end_effector_link)
arm.go()
rospy.sleep(2)
# # 设置机械臂的运动目标位置，进行避障规划
# target_pose2 = PoseStamped()
# target_pose2.header.frame_id = reference_frame
# target_pose2.pose.position.x = 0.8
# target_pose2.pose.position.y = -0.7
# target_pose2.pose.position.z = 1.5#table_pose.pose.position.z + table_size[2] + 0.05
# target_pose2.pose.orientation.w = 1.0
# # 控制机械臂运动到目标位置
# arm.set_pose_target(target_pose2, end_effector_link)
# arm.go()
# rospy.sleep(2)
# target_pose = PoseStamped()
# target_pose.header.frame_id = reference_frame
# target_pose.pose.position.x = 0.0
# target_pose.pose.position.y = 0.0
# target_pose.pose.position.z = 0.0
# target_pose.pose.orientation.w = 1.0
# # 控制机械臂运动到目标位置
# arm.set_pose_target(target_pose, end_effector_link)
# arm.go()
# 关闭并退出moveit
moveit_commander.roscpp_shutdown()
moveit_commander.os._exit(0)
def __init__(self):
'''
Constructor of the MoveitObjectHandler class.
'''
moveit_commander.roscpp_initialize(sys.argv)
self.scene = moveit_commander.PlanningSceneInterface()
def __init__(self):
self.bridge = cv_bridge.CvBridge()
self.image_sub = rospy.Subscriber('/ur5/usbcam/image_raw', Image,
self.image_callback)
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
wpose = deepcopy(start_pose)
# Set the next waypoint to the right 0.5 meters
wpose.position.x = -0.2
wpose.position.y = -0.2
wpose.position.z = 0.3
waypoints.append(deepcopy(wpose))
wpose.position.x = 0.1052
wpose.position.y = -0.4271
wpose.position.z = 0.4005
wpose.orientation.x = 0.4811
wpose.orientation.y = 0.4994
wpose.orientation.z = -0.5121
wpose.orientation.w = 0.5069
waypoints.append(deepcopy(wpose))
if np.sqrt((wpose.position.x-start_pose.position.x)**2+(wpose.position.x-start_pose.position.x)**2 \
+(wpose.position.x-start_pose.position.x)**2)<0.1:
rospy.loginfo(
"Warnig: target position overlaps with the initial position!")
# self.arm.set_pose_target(wpose)
# Set the internal state to the current state
self.arm.set_start_state_to_current_state()
# Plan the Cartesian path connecting the waypoints
"""moveit_commander.move_group.MoveGroupCommander.compute_cartesian_path(
self, waypoints, eef_step, jump_threshold, avoid_collisios= True)
Compute a sequence of waypoints that make the end-effector move in straight line segments that follow the
poses specified as waypoints. Configurations are computed for every eef_step meters;
The jump_threshold specifies the maximum distance in configuration space between consecutive points
in the resultingpath. The return value is a tuple: a fraction of how much of the path was followed,
the actual RobotTrajectory.
"""
plan, fraction = self.arm.compute_cartesian_path(
waypoints, 0.01, 0.0, True)
# plan = self.arm.plan()
# If we have a complete plan, execute the trajectory
if 1 - fraction < 0.2:
rospy.loginfo("Path computed successfully. Moving the arm.")
num_pts = len(plan.joint_trajectory.points)
rospy.loginfo("\n# intermediate waypoints = " + str(num_pts))
self.arm.execute(plan)
rospy.loginfo("Path execution complete.")
else:
rospy.loginfo("Path planning failed")
def __init__(self):
# Initialize the move_group API
moveit_commander.roscpp_initialize(sys.argv)
# Initialize the ROS node
rospy.init_node('moveit_demo', anonymous=True)
GRIPPER_OPEN = [0.05]
GRIPPER_CLOSED = [-0.03]
GRIPPER_NEUTRAL = [0.01]
# Connect to the right_arm move group
right_arm = moveit_commander.MoveGroupCommander('right_arm')
# Connect to the right_gripper move group
right_gripper = moveit_commander.MoveGroupCommander('right_gripper')
# Get the name of the end-effector link
end_effector_link = right_arm.get_end_effector_link()
# Display the name of the end_effector link
rospy.loginfo("The end effector link is: " + str(end_effector_link))
# Set a small tolerance on joint angles
right_arm.set_goal_joint_tolerance(0.001)
right_gripper.set_goal_joint_tolerance(0.001)
# Start the arm target in "resting" pose stored in the SRDF file
right_arm.set_named_target('resting')
# Plan a trajectory to the goal configuration
traj = right_arm.plan()
# Execute the planned trajectory
right_arm.execute(traj)
# Pause for a moment
rospy.sleep(1)
# Set the gripper target to neutal position using a joint value target
right_gripper.set_joint_value_target(GRIPPER_NEUTRAL)
# Plan and execute the gripper motion
right_gripper.go()
rospy.sleep(1)
# Set target joint values for the arm: joints are in the order they appear in
# the kinematic tree.
joint_positions = [-0.0867, -1.274, 0.02832, 0.0820, -1.273, -0.003]
# Set the arm's goal configuration to the be the joint positions
right_arm.set_joint_value_target(joint_positions)
# Plan and execute the motion
right_arm.go()
rospy.sleep(1)
# Save this configuration for later
right_arm.remember_joint_values('saved_config', joint_positions)
# Close the gripper as if picking something up
right_gripper.set_joint_value_target(GRIPPER_CLOSED)
right_gripper.go()
rospy.sleep(1)
# Set the arm target to the named "straight_out" pose stored in the SRDF file
right_arm.set_named_target('straight_forward')
# Plan and execute the motion
right_arm.go()
rospy.sleep(1)
# Set the goal configuration to the named configuration saved earlier
right_arm.set_named_target('saved_config')
# Plan and execute the motion
right_arm.go()
rospy.sleep(1)
# Open the gripper as if letting something go
right_gripper.set_joint_value_target(GRIPPER_OPEN)
right_gripper.go()
rospy.sleep(1)
# Return the arm to the named "resting" pose stored in the SRDF file
right_arm.set_named_target('resting')
right_arm.go()
rospy.sleep(1)
# Return the gripper target to neutral position
right_gripper.set_joint_value_target(GRIPPER_NEUTRAL)
right_gripper.go()
rospy.sleep(1)
# Cleanly shut down MoveIt
moveit_commander.roscpp_shutdown()
# Exit the script
moveit_commander.os._exit(0)
def __init__(self):
# Initialize the move_group API
moveit_commander.roscpp_initialize(sys.argv)
rospy.init_node('moveit_demo')
# Construct the initial scene object
scene = PlanningSceneInterface()
self.scene_pub = rospy.Publisher('planning_scene', PlanningScene)
# Create a dictionary to hold object colors
self.colors = dict()
# Pause for the scene to get ready
rospy.sleep(1)
# Initialize the move group
both_arms = moveit_commander.MoveGroupCommander('both_arms')
right_arm = moveit_commander.MoveGroupCommander('right_arm')
left_arm = moveit_commander.MoveGroupCommander('left_arm')
left_gripper = MoveGroupCommander('left_gripper')
# Get the name of the end-effector link
right_eef = right_arm.get_end_effector_link()
left_eef = left_arm.get_end_effector_link()
print "============ Printing end-effector link"
print right_eef
print left_eef
# Set the reference frame for pose targets
right_reference_frame = right_arm.get_planning_frame()
left_reference_frame = left_arm.get_planning_frame()
# Set the right arm reference frame accordingly
#right_arm.set_pose_reference_frame(reference_frame)
# Allow replanning to increase the odds of a solution
right_arm.allow_replanning(True)
# Allow some leeway in position (meters) and orientation (radians)
right_arm.set_goal_position_tolerance(0.01)
right_arm.set_goal_orientation_tolerance(0.01)
left_arm.allow_replanning(True)
# Allow some leeway in position (meters) and orientation (radians)
left_arm.set_goal_position_tolerance(0.01)
left_arm.set_goal_orientation_tolerance(0.01)
# Allow 5 seconds per planning attempt
left_arm.set_planning_time(5)
right_arm.set_planning_time(5)
object1_id = 'object1'
# Remove leftover objects from a previous run
scene.remove_world_object(object1_id)
rospy.sleep(2)
right_arm.set_named_target('right_ready')
right_arm.go()
left_arm.set_named_target('left_ready')
left_arm.go()
left_gripper.set_joint_value_target(GRIPPER_OPEN)
left_gripper.go()
rospy.sleep(1)
# attach_box
# Set the length, width and height of the object to attach
object1_size = [0.09, 0.057, 0.001]
# Create a pose for the tool relative to the end-effector
object1_p = PoseStamped()
object1_p.header.frame_id = right_reference_frame
object1_p.pose.position.x = 0.77
object1_p.pose.position.y = -0.27
object1_p.pose.position.z = -0.21
object1_p.pose.orientation.w = 1
scene.add_box(object1_id, object1_p, object1_size)
self.setColor(object1_id, 0.8, 0.4, 0, 1.0)
self.sendColors()
right_arm.set_start_state_to_current_state()
left_arm.set_start_state_to_current_state()
# target pose
target_pose1 = PoseStamped()
target_pose1.header.frame_id = right_reference_frame
target_pose1.header.stamp = rospy.Time.now()
target_pose1.pose.position.x = 0.77
target_pose1.pose.position.y = -0.27
target_pose1.pose.position.z = -0.14
target_pose1.pose.orientation.x = 0.500166303975
target_pose1.pose.orientation.y = 0.865326245156
target_pose1.pose.orientation.z = -0.0155702691449
target_pose1.pose.orientation.w = 0.0283147405248
right_arm.set_pose_target(target_pose1, right_eef)
right_arm.go()
rospy.sleep(1)
# Attach the tool to the end-effector
scene.attach_box(right_eef, object1_id, object1_p, object1_size)
target_pose2 = PoseStamped()
target_pose2.header.frame_id = right_reference_frame
target_pose2.header.stamp = rospy.Time.now()
target_pose2.pose.position.x = 0.632
target_pose2.pose.position.y = -0.125
target_pose2.pose.position.z = 0.08
target_pose2.pose.orientation.x = 0.2392
target_pose2.pose.orientation.y = -0.9708
target_pose2.pose.orientation.z = -0.0137
target_pose2.pose.orientation.w = 0.0103
target_pose3 = PoseStamped()
target_pose3.header.frame_id = left_reference_frame
target_pose3.header.stamp = rospy.Time.now()
target_pose3.pose.position.x = 0.632
target_pose3.pose.position.y = 0.026
target_pose3.pose.position.z = 0.013
target_pose3.pose.orientation.x = -0.021
target_pose3.pose.orientation.y = 0.017
target_pose3.pose.orientation.z = -0.999
target_pose3.pose.orientation.w = 0.031
both_arms.set_pose_target(target_pose2, right_eef)
both_arms.set_pose_target(target_pose3, left_eef)
both_arms.go()
rospy.sleep(3)
#scene.remove_attached_object(right_eef, 'tool')
object1_p = left_arm.get_current_pose(left_eef)
object1_p.pose.position.y -= 0.12
object1_p.pose.orientation.x = 0
object1_p.pose.orientation.y = 0
object1_p.pose.orientation.z = 0.707
object1_p.pose.orientation.w = 0.707
scene.remove_attached_object(right_eef, object1_id)
scene.attach_box(left_eef, object1_id, object1_p, object1_size)
rospy.sleep(1)
left_gripper.set_joint_value_target(GRIPPER_CLOSE)
left_gripper.go()
target_pose4 = PoseStamped()
target_pose4.header.frame_id = right_reference_frame
target_pose4.header.stamp = rospy.Time.now()
target_pose4.pose.position.x = 0.58241
target_pose4.pose.position.y = -0.30286
target_pose4.pose.position.z = 0.05471
target_pose4.pose.orientation.x = 0.23931
target_pose4.pose.orientation.y = -0.97080
target_pose4.pose.orientation.z = -0.01346
target_pose4.pose.orientation.w = 0.01003
target_pose5 = PoseStamped()
target_pose5.header.frame_id = left_reference_frame
target_pose5.header.stamp = rospy.Time.now()
target_pose5.pose.position.x = 0.843
target_pose5.pose.position.y = 0.438
target_pose5.pose.position.z = 0.105
target_pose5.pose.orientation.x = -0.01386
target_pose5.pose.orientation.y = -0.02950
target_pose5.pose.orientation.z = -0.70489
target_pose5.pose.orientation.w = 0.70856
#right_arm.set_pose_target(target_pose4, right_eef)
both_arms.set_pose_target(target_pose4, right_eef)
both_arms.set_pose_target(target_pose5, left_eef)
both_arms.go()
rospy.sleep(2)
# Shut down MoveIt cleanly
moveit_commander.roscpp_shutdown()
# Exit MoveIt
moveit_commander.os._exit(0)
joint_goal[1] = joint_goal_list[1]
joint_goal[2] = joint_goal_list[2]
joint_goal[3] = joint_goal_list[3]
joint_goal[4] = joint_goal_list[4]
joint_goal[5] = joint_goal_list[5]
#제어시작
move_group.go(joint_goal, wait=False)
if __name__=='__main__':
signal.signal(signal.SIGINT, signal_handler)
#GROUP_NAME_GRIPPER = "NAME OF GRIPPER"
roscpp_initialize(sys.argv)
rospy.init_node('control_Husky_UR3', anonymous=True)
robot = RobotCommander()
scene = PlanningSceneInterface()
##모바일 파트 관련 변수 선언
x = 0.0
y = 0.0
theta = 0.0
## 매니퓰레이터 변수 선언
def move_demo():
moveit_commander.roscpp_initialize(sys.argv)
rospy.init_node('moveit_personal_demo', anonymous=True)
robot = moveit_commander.RobotCommander()
scene = moveit_commander.PlanningSceneInterface()
group = moveit_commander.MoveGroupCommander("left_arm")
display_trajectory_publisher = rospy.Publisher(
'/move_group/display_planned_path',
moveit_msgs.msg.DisplayTrajectory,
queue_size=20)
rospy.sleep(10) #allow RVIZ to initialize??
print("============ Reference frame: %s" % group.get_planning_frame())
print("=========== Generating plan 1")
pose_target = geometry_msgs.msg.Pose()
pose_target.orientation.w = 1.0
pose_target.position.x = 0.7
pose_target.position.y = -0.05
pose_target.position.z = 1.1
group.set_pose_target(pose_target)
plan1 = group.plan()
print("========= Waiting while RVIZ displays play1...")
rospy.sleep(10)
collision_object = moveit_msgs.msg.CollisionObject()
collision_object.header.frame_id = group.get_planning_frame()
collision_object.id = 'box1'
# primitive = shape_msgs.msg.SolidPrimitive()
# primitive.type = primitive.BOX
# primitive.dimensions.append(0.4)
# primitive.dimensions.append(0.1)
# primitive.dimensions.append(0.4)
# collision_object.primitives.append(primitive)
# collision_object.primitive_poses.append(box_pose)
# collision_object.operation = collision_object.ADD
# collision_objects = list(collision_objects)
# print("================= Trying to add box1")
# box1_pose = geometry_msgs.msg.Pose()
# box1_pose.orientation.w = 1.0
# box1_pose.position.x = 0.5
# box1_pose.position.y = 0.2
# box1_pose.position.z = 0.3
# box1_pose_stamped = geometry_msgs.msg.PoseStamped()
# box1_pose_stamped.pose = box1_pose
# box1_pose_stamped.header = ""
# scene.add_box("box1", box1_pose)
# rospy.sleep(10)
moveit_commander.roscpp_shutdown()
def __init__(self):
super(DiffCoplusBaxterExperiments, self).__init__()
## BEGIN_SUB_TUTORIAL setup
##
## First initialize `moveit_commander`_ and a `rospy`_ node:
moveit_commander.roscpp_initialize(sys.argv)
rospy.init_node('DiffCoplusBaxterExperiments', anonymous=True)
## Instantiate a `RobotCommander`_ object. Provides information such as the robot's
## kinematic model and the robot's current joint states
robot = moveit_commander.RobotCommander()
## Instantiate a `PlanningSceneInterface`_ object. This provides a remote interface
## for getting, setting, and updating the robot's internal understanding of the
## surrounding world:
scene = moveit_commander.PlanningSceneInterface()
## Instantiate a `MoveGroupCommander`_ object. This object is an interface
## to a planning group (group of joints). In this tutorial the group is the primary
## arm joints in the Panda robot, so we set the group's name to "panda_arm".
## If you are using a different robot, change this value to the name of your robot
## arm planning group.
## This interface can be used to plan and execute motions:
group_name = "left_arm"
move_group = moveit_commander.MoveGroupCommander(group_name)
## Create a `DisplayTrajectory`_ ROS publisher which is used to display
## trajectories in Rviz:
display_trajectory_publisher = rospy.Publisher(
'/move_group/display_planned_path',
moveit_msgs.msg.DisplayTrajectory,
queue_size=20)
robot_state_publisher = rospy.Publisher('/robot/joint_states',
JointState,
queue_size=1)
self.pos = [0 for _ in range(7)]
self.state = JointState()
self.state.name = [
'left_s0', 'left_s1', 'left_e0', 'left_e1', 'left_w0', 'left_w1',
'left_w2'
]
self.state.position = self.pos
self.state_pub_thread = threading.Thread(target=self.pub_state)
## BEGIN_SUB_TUTORIAL basic_info
##
## Getting Basic Information
## ^^^^^^^^^^^^^^^^^^^^^^^^^
# We can get the name of the reference frame for this robot:
planning_frame = move_group.get_planning_frame()
print("============ Planning frame: %s" % planning_frame)
# We can also print the name of the end-effector link for this group:
eef_link = move_group.get_end_effector_link()
print("============ End effector link: %s" % eef_link)
# We can get a list of all the groups in the robot:
group_names = robot.get_group_names()
print("============ Available Planning Groups:",
robot.get_group_names())
# Sometimes for debugging it is useful to print the entire state of the
# robot:
print("============ Printing robot state")
print(robot.get_current_state())
print("")
## END_SUB_TUTORIAL
# Misc variables
self.box_name = ''
self.robot = robot
self.scene = scene
self.move_group = move_group
self.display_trajectory_publisher = display_trajectory_publisher
self.robot_state_publisher = robot_state_publisher
self.planning_frame = planning_frame
self.eef_link = eef_link
self.group_names = group_names
self.state_pub_thread.start()
def __init__(self):
# Initialize the move_group API
moveit_commander.roscpp_initialize(sys.argv)
# Initialize the ROS node
rospy.init_node('moveit_demo', anonymous=True)
# Connect to the right_arm move group
right_arm = moveit_commander.MoveGroupCommander('arm')
# Get the name of the end-effector link
end_effector_link = right_arm.get_end_effector_link()
# Display the name of the end_effector link
rospy.loginfo("The end effector link is: " + str(end_effector_link))
# Set a small tolerance on joint angles
right_arm.set_goal_tolerance(0.0001)
right_arm.set_named_target('right')
right_arm.go()
rospy.sleep(1)
'''
start_pose = right_arm.get_current_pose().pose
start_pose.position.x = 0.4
start_pose.position.y = 0
#start_pose.position.z = 0.4
start_pose.orientation.x = 0.707106781186547
start_pose.orientation.y = 0
start_pose.orientation.z = 0
start_pose.orientation.w = 0.707106781186547
right_arm.set_pose_target(start_pose)
right_arm.go()
# Pause for a moment
rospy.sleep(1)
'''
plan = RobotTrajectory()
joint_trajectory = JointTrajectory()
joint_trajectory.header.frame_id = 'base_link'
#joint_trajectory.header.stamp = rospy.Time.now()
joint_trajectory.joint_names = [
'base_to_armA', 'armA_to_armB', 'armB_to_armC', 'armC_to_armD'
]
# Output
with open("/home/jyk/Desktop/pvt.txt", 'r') as f:
for line in f.readlines():
cont = line.rstrip('\r\n').replace(' ', '').split(',')
point = JointTrajectoryPoint()
point.positions = map(float, cont[0:4])
point.velocities = map(float, cont[4:8])
point.time_from_start = rospy.Duration(float(cont[8]))
joint_trajectory.points.append(deepcopy(point))
plan.joint_trajectory = joint_trajectory
#print type(plan)
#print plan
right_arm.set_start_state_to_current_state()
right_arm.execute(plan)
#rospy.sleep(3)
'''
# Return the arm to the named "resting" pose stored in the SRDF file
right_arm.set_named_target('right')
right_arm.go()
rospy.sleep(1)
'''
# Cleanly shut down MoveIt
moveit_commander.roscpp_shutdown()
# Exit the script
moveit_commander.os._exit(0)
def main():
try:
#rospy.init_node('avalos_limb_py')
#moveit_commander.roscpp_initialize(sys.argv)
moveit_commander.roscpp_initialize(sys.argv)
rospy.init_node('move_group_python_interface_tutorial', anonymous=True)
robot = moveit_commander.RobotCommander()
scene = moveit_commander.PlanningSceneInterface()
group_name = 'right_arm'
group = moveit_commander.MoveGroupCommander(group_name)
display_trajectory_publisher = rospy.Publisher(
'/move_group/display_planned_path',
moveit_msgs.msg.DisplayTrajectory,
queue_size=20)
# We can get the name of the reference frame for this robot:
planning_frame = group.get_planning_frame()
#frecuency for Sawyer robot
f = 100
rate = rospy.Rate(f)
# add gripper
if (False):
gripper = intera_interface.Gripper('right_gripper')
gripper.calibrate()
gripper.open()
moveit_robot_state = RobotState()
joint_state = JointState()
joint_state.header = Header()
joint_state.header.stamp = rospy.Time.now()
joint_state.name = [
'right_j0', 'right_j1', 'right_j2', 'right_j3', 'right_j4',
'right_j5', 'right_j6'
]
#Define topic
pub = rospy.Publisher('/robot/limb/right/joint_command',
JointCommand,
queue_size=10)
# Class to record
#data=Data()
#Class limb to acces information sawyer
limb = Limb()
limb.move_to_neutral()
group.clear_pose_targets()
group.set_start_state_to_current_state()
# We can get the joint values from the group and adjust some of the values:
pose_goal = geometry_msgs.msg.Pose()
# Orientation
pose_goal.orientation.x = -1
pose_goal.orientation.y = 0.0
pose_goal.orientation.z = 0.0
pose_goal.orientation.w = 0.0
q0 = np.array([])
q1 = np.array([])
q2 = np.array([])
q3 = np.array([])
q4 = np.array([])
q5 = np.array([])
q6 = np.array([])
# Cartesian position
pose_goal.position.x = -0.1
pose_goal.position.y = 0.35
pose_goal.position.z = 0.05
pose_goal = Pose(Point(-0.1, 0.6, 0.05), Quaternion(-1, 0, 0, 0))
group.set_pose_target(pose_goal)
a = group.plan()
points = a.joint_trajectory.points
n = len(points)
joint_state.position = [
points[n - 1].positions[0], points[n - 1].positions[1],
points[n - 1].positions[2], points[n - 1].positions[3],
points[n - 1].positions[4], points[n - 1].positions[5],
points[n - 1].positions[6]
]
moveit_robot_state.joint_state = joint_state
group.set_start_state(moveit_robot_state)
for i in range(n):
q0 = np.append(q0, points[i].positions[0])
q1 = np.append(q1, points[i].positions[1])
q2 = np.append(q2, points[i].positions[2])
q3 = np.append(q3, points[i].positions[3])
q4 = np.append(q4, points[i].positions[4])
q5 = np.append(q5, points[i].positions[5])
q6 = np.append(q6, points[i].positions[6])
print "q000", q0
# Cartesian position
pose_goal.position.x = 0.43
pose_goal.position.y = -0.4
pose_goal.position.z = 0.2
group.set_pose_target(pose_goal)
a = group.plan()
points = a.joint_trajectory.points
n = len(points)
joint_state.position = [
points[n - 1].positions[0], points[n - 1].positions[1],
points[n - 1].positions[2], points[n - 1].positions[3],
points[n - 1].positions[4], points[n - 1].positions[5],
points[n - 1].positions[6]
]
moveit_robot_state.joint_state = joint_state
group.set_start_state(moveit_robot_state)
for i in range(
n - 1): # Si se repite un numero en posicion entra en un bug
q0 = np.append(q0, points[i + 1].positions[0])
q1 = np.append(q1, points[i + 1].positions[1])
q2 = np.append(q2, points[i + 1].positions[2])
q3 = np.append(q3, points[i + 1].positions[3])
q4 = np.append(q4, points[i + 1].positions[4])
q5 = np.append(q5, points[i + 1].positions[5])
q6 = np.append(q6, points[i + 1].positions[6])
#'''
q = np.array([q0, q1, q2, q3, q4, q5, q6])
print "q001", q0
print q[0]
#return 0
alfa = 0.5
start = time.time()
opt = Opt_avalos(q, f, alfa)
v_time = opt.full_time()
j, v, a, jk = generate_path_cub(q, v_time, f)
ext = len(j[0, :])
end = time.time()
print('Process Time:', end - start)
print ext
#save_matrix(j,"data_p.txt",f)
#save_matrix(v,"data_v.txt",f)
#save_matrix(a,"data_a.txt",f)
#save_matrix(jk,"data_y.txt",f)
v_jk = sqrt(np.mean(np.square(jk)))
print("Opt Time:", v_time)
print("Min Time:", m_time)
#print('Optimizacion:',opt.result())
print('Costo Tiempo:', len(j[0]) / float(100.0))
print('Costo Jerk:', v_jk)
# Position init
#raw_input('Iniciar_CT_initial_position?')
#limb.move_to_joint_positions({"right_j6": ik1[6],"right_j5": ik1[5], "right_j4": ik1[4], "right_j3": ik1[3], "right_j2":
#ik1[2],"right_j1": ik1[1],"right_j0": ik1[0]})
raw_input('Iniciar_CT_execute?')
#Build message
my_msg = JointCommand()
# POSITION_MODE
my_msg.mode = 4
my_msg.names = [
"right_j0", "right_j1", "right_j2", "right_j3", "right_j4",
"right_j5", "right_j6"
]
#data.writeon(str(alfa)+"trayectoria.txt")
print("Control por trayectoria iniciado.")
#time.sleep(0.25)
for n in xrange(ext):
my_msg.position = [
j[0][n], j[1][n], j[2][n], j[3][n], j[4][n], j[5][n], j[6][n]
]
my_msg.velocity = [
v[0][n], v[1][n], v[2][n], v[3][n], v[4][n], v[5][n], v[6][n]
]
my_msg.acceleration = [
a[0][n], a[1][n], a[2][n], a[3][n], a[4][n], a[5][n], a[6][n]
]
pub.publish(my_msg)
rate.sleep()
print("Control por trayectoria terminado.")
time.sleep(0.25)
#data.writeoff()
print("Programa terminado.")
except rospy.ROSInterruptException:
rospy.logerr('Keyboard interrupt detected from the user.')
def __init__(self):
# Name this node, it must be unique
rospy.init_node('move_arm', anonymous=False)
rospy.on_shutdown(
self.cleanup
) # Set rospy to execute a shutdown function when exiting
self.ready_for_goal = 0
self.ct = 0
# Set up ROS subscriber callback routines
rospy.Subscriber("/move_group/status",
GoalStatusArray,
self.cb_stat,
queue_size=1)
rospy.Subscriber("/move_arm/goal", Twist, self.cb_goal, queue_size=1)
#rospy.Subscriber("/move_arm/joint_states",JointState, self.cb_joint, queue_size=1)
# Set up ROS publishers
self.joint_pub = rospy.Publisher("/move_arm/joint_states",
JointState,
queue_size=1)
rospy.set_param('arm_prefix', 'ur5_arm_')
rospy.set_param('reference_frame', '/base_link')
rospy.loginfo("Moving arm to desired position")
rospy.sleep(1)
# Initialize the move_group API
moveit_commander.roscpp_initialize(sys.argv)
robot = moveit_commander.RobotCommander()
scene = moveit_commander.PlanningSceneInterface()
# Initialize the move group for the ur5_arm
self.arm = moveit_commander.MoveGroupCommander("ur5_arm")
# Get the name of the end-effector link
self.end_effector_link = self.arm.get_end_effector_link()
rospy.loginfo(self.end_effector_link)
# Initialize Necessary Variables
reference_frame = rospy.get_param("~reference_frame", "/base_link")
#reference_frame = "ee_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.0001)
self.arm.set_goal_orientation_tolerance(0.001)
# Set the target pose from the input
self.target_pose = PoseStamped()
self.target_pose.header.frame_id = reference_frame
# Get current joint position to use for planning
rospy.loginfo("Initializing motion planning server.")
rospy.loginfo("Listening for <Twist> published on </move_arm/goal>")
def __init__(self):
"""
Setup ROS communication between the Kinect and Baxter
:return:
"""
# Initialise variables
self.rgb_img = None
self.depth_img = None
self.marker_box = None
self.marker_center_x = None
self.marker_center_y = None
self.marker_depth = None
self.bridge = CvBridge()
# First initialize moveit_commander and rospy.
print "============ Initialising Baxter"
moveit_commander.roscpp_initialize(sys.argv)
rospy.init_node('can_node', anonymous=True)
self.robot = moveit_commander.RobotCommander()
self.scene = moveit_commander.PlanningSceneInterface()
self.left_arm = moveit_commander.MoveGroupCommander("left_arm")
self.right_arm = moveit_commander.MoveGroupCommander("right_arm")
self.right_arm_navigator = Navigator('right')
# Setup grippers
self.right_gripper = baxter_interface.Gripper('right')
# Setup the table in the scene
scene = PlanningSceneInterface()
self.scene_pub = rospy.Publisher(
'/move_group/monitored_planning_scene', PlanningScene)
table_id = 'table'
scene.remove_world_object(table_id)
rospy.sleep(1)
table_ground = -0.3
table_size = [4.0, 4.6, 0.1]
table_pose = PoseStamped()
table_pose.header.frame_id = self.robot.get_planning_frame()
table_pose.pose.position.x = 0.7
table_pose.pose.position.y = 0.0
table_pose.pose.position.z = table_ground + table_size[2] / 2.0
table_pose.pose.orientation.w = 1.0
scene.add_box(table_id, table_pose, table_size)
# Create a dictionary to hold object colors
self.colors = dict()
self.setColor(table_id, 0.8, 0, 0, 1.0)
self.sendColors()
self.display_trajectory_publisher = rospy.Publisher(
'/move_group/display_planned_path',
moveit_msgs.msg.DisplayTrajectory,
queue_size=10)
# We can get a list of all the groups in the robot
print "============ Right Pose:"
print self.right_arm.get_current_pose()
print "============ Left Pose:"
print self.left_arm.get_current_pose()
# Setup the subscribers and publishers
self.rgb_sub = rospy.Subscriber("/camera/rgb/image_rect_color", Image,
self.callback_rgb)
self.points_sub = rospy.Subscriber("/camera/depth_registered/points",
PointCloud2, self.callback_points)
self.display_trajectory_publisher = rospy.Publisher(
'/move_group/display_planned_path',
moveit_msgs.msg.DisplayTrajectory,
queue_size=5)
self.screen_pub = rospy.Publisher('robot/xdisplay',
Image,
latch=True,
queue_size=10)
self.right_hand_range_pub = rospy.Subscriber(
"/robot/range/right_hand_range/state",
Range,
self.callback_range,
queue_size=1)
self.left_cam_sub = rospy.Subscriber("/cameras/left_hand_camera/image",
Image, self.callback_left_hand)
def move_group_python_interface_tutorial():
## BEGIN_TUTORIAL
##
## Setup
## ^^^^^
## CALL_SUB_TUTORIAL imports
##
## First initialize moveit_commander and rospy.
print "============ Starting tutorial setup"
moveit_commander.roscpp_initialize(sys.argv)
rospy.init_node('move_group_python_interface_tutorial',
anonymous=True)
## Instantiate a RobotCommander object. This object is an interface to
## the robot as a whole.
robot = moveit_commander.RobotCommander()
## Instantiate a PlanningSceneInterface object. This object is an interface
## to the world surrounding the robot.
scene = moveit_commander.PlanningSceneInterface()
## Instantiate a MoveGroupCommander object. This object is an interface
## to one group of joints. In this case the group is the joints in the left
## arm. This interface can be used to plan and execute motions on the left
## arm.
group = moveit_commander.MoveGroupCommander("left_arm")
## We create this DisplayTrajectory publisher which is used below to publish
## trajectories for RVIZ to visualize.
display_trajectory_publisher = rospy.Publisher(
'/move_group/display_planned_path',
moveit_msgs.msg.DisplayTrajectory,
queue_size=20)
## Wait for RVIZ to initialize. This sleep is ONLY to allow Rviz to come up.
print "============ Waiting for RVIZ..."
rospy.sleep(10)
print "============ Starting tutorial "
## Getting Basic Information
## ^^^^^^^^^^^^^^^^^^^^^^^^^
##
## We can get the name of the reference frame for this robot
print "============ Reference frame: %s" % group.get_planning_frame()
## We can also print the name of the end-effector link for this group
print "============ Reference frame: %s" % group.get_end_effector_link()
## We can get a list of all the groups in the robot
print "============ Robot Groups:"
print robot.get_group_names()
## Sometimes for debugging it is useful to print the entire state of the
## robot.
print "============ Printing robot state"
print robot.get_current_state()
print "============"
## Planning to a Pose goal
## ^^^^^^^^^^^^^^^^^^^^^^^
## We can plan a motion for this group to a desired pose for the
## end-effector
print "============ Generating plan 1"
pose_target = geometry_msgs.msg.Pose()
pose_target.orientation.w = 1.0
pose_target.position.x = 0.7
pose_target.position.y = -0.05
pose_target.position.z = 1.1
group.set_pose_target(pose_target)
## Now, we call the planner to compute the plan
## and visualize it if successful
## Note that we are just planning, not asking move_group
## to actually move the robot
plan1 = group.plan()
print "============ Waiting while RVIZ displays plan1..."
rospy.sleep(5)
## You can ask RVIZ to visualize a plan (aka trajectory) for you. But the
## group.plan() method does this automatically so this is not that useful
## here (it just displays the same trajectory again).
print "============ Visualizing plan1"
display_trajectory = moveit_msgs.msg.DisplayTrajectory()
display_trajectory.trajectory_start = robot.get_current_state()
display_trajectory.trajectory.append(plan1)
display_trajectory_publisher.publish(display_trajectory);
print "============ Waiting while plan1 is visualized (again)..."
rospy.sleep(5)
## Moving to a pose goal
## ^^^^^^^^^^^^^^^^^^^^^
##
## Moving to a pose goal is similar to the step above
## except we now use the go() function. Note that
## the pose goal we had set earlier is still active
## and so the robot will try to move to that goal. We will
## not use that function in this tutorial since it is
## a blocking function and requires a controller to be active
## and report success on execution of a trajectory.
# Uncomment below line when working with a real robot
# group.go(wait=True)
# Use execute instead if you would like the robot to follow
# the plan that has already been computed
# group.execute(plan1)
## Planning to a joint-space goal
## ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^
##
## Let's set a joint space goal and move towards it.
## First, we will clear the pose target we had just set.
group.clear_pose_targets()
## Then, we will get the current set of joint values for the group
group_variable_values = group.get_current_joint_values()
print "============ Joint values: ", group_variable_values
## Now, let's modify one of the joints, plan to the new joint
## space goal and visualize the plan
group_variable_values[0] = 1.0
group.set_joint_value_target(group_variable_values)
plan2 = group.plan()
print "============ Waiting while RVIZ displays plan2..."
rospy.sleep(5)
## 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(group.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.1
wpose.position.y = waypoints[0].position.y
wpose.position.z = waypoints[0].position.z
waypoints.append(copy.deepcopy(wpose))
# second move down
wpose.position.z -= 0.10
waypoints.append(copy.deepcopy(wpose))
# third move to the side
wpose.position.y += 0.05
waypoints.append(copy.deepcopy(wpose))
## 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.
(plan3, fraction) = group.compute_cartesian_path(
waypoints, # waypoints to follow
0.01, # eef_step
0.0) # jump_threshold
print "============ Waiting while RVIZ displays plan3..."
rospy.sleep(5)
# Uncomment the line below to execute this plan on a real robot.
# group.execute(plan3)
## Adding/Removing Objects and Attaching/Detaching Objects
## ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^
## First, we will define the collision object message
collision_object = moveit_msgs.msg.CollisionObject()
## When finished shut down moveit_commander.
moveit_commander.roscpp_shutdown()
## END_TUTORIAL
print "============ STOPPING"
def main():
try:
rospy.loginfo("Creating Demo Planning Scene")
scene = ExtendedPlanningSceneInterface()
rospy.sleep(
1
) # ----- Not having this delay sometimes caused failing to create some boxes
for config in IRLab_workspace:
rospy.loginfo("-- Creating object: {}..".format(config['name']))
success = scene.add_box(**config)
rospy.loginfo(
"------ {}".format("success" if success else "FAILED!"))
rospy.loginfo("Created Demo Planning Scene.")
except rospy.ROSInterruptException:
return
except KeyboardInterrupt:
return
if __name__ == '__main__':
rospy.init_node('simple_scene_creator', anonymous=True)
moveit_commander.roscpp_initialize(sys.argv)
main()
def __init__(self):
# Initialize ros and moveit
moveit_commander.roscpp_initialize(sys.argv)
# Initialize moveit commander
self.arm = moveit_commander.MoveGroupCommander('arm')
self.scene = moveit_commander.PlanningSceneInterface()
# Initialize arm effector link
self.end_effector_link = self.arm.get_end_effector_link()
# Initialize reference frame
self.reference_frame = 'world'
self.arm.set_pose_reference_frame(self.reference_frame)
# Initialize moveit parameters
self.arm.allow_replanning(True)
self.arm.set_goal_position_tolerance(0.001)
self.arm.set_goal_orientation_tolerance(0.001)
self.arm.set_max_velocity_scaling_factor(1.0)
#self.set_max_velocity_scaling_factor (0.2)
#############添加水平障碍############
rospy.sleep(1)
base_table_id = 'base_table'
self.scene.remove_world_object(base_table_id)
base_table_size = [3, 3, 0.01]
base_table_pose = PoseStamped()
base_table_pose.header.frame_id = self.reference_frame
base_table_pose.pose.position.x = 0
base_table_pose.pose.position.y = 0.0
base_table_pose.pose.position.z = height_shuipin_collision #高度
base_table_pose.pose.orientation.w = 1.0
self.scene.add_box(base_table_id, base_table_pose, base_table_size)
rospy.sleep(1)
##############添加后侧障碍#############
backward_wall_id = 'backward_wall'
self.scene.remove_world_object(backward_wall_id)
backward_wall_size = [0.01, 3, 3]
backward_wall_pose = PoseStamped()
backward_wall_pose.header.frame_id = self.reference_frame
backward_wall_pose.pose.position.x = -0.3
backward_wall_pose.pose.position.y = 0.0
backward_wall_pose.pose.position.z = 0.4
backward_wall_pose.pose.orientation.w = 1.0
self.scene.add_box(backward_wall_id, backward_wall_pose,
backward_wall_size)
rospy.sleep(1)
##############添加中间障碍#############
middle_collision_id = 'middle_collision'
self.scene.remove_world_object(middle_collision_id)
middle_collision_size = [0.22, 0.02, 0.27]
middle_collision_pose = PoseStamped()
middle_collision_pose.header.frame_id = self.reference_frame
middle_collision_pose.pose.position.x = 0.405
middle_collision_pose.pose.position.y = 0.0
middle_collision_pose.pose.position.z = height_shuipin_collision + middle_collision_size[
2] / 2.0
middle_collision_pose.pose.orientation.w = 1.0
self.scene.add_box(middle_collision_id, middle_collision_pose,
middle_collision_size)
rospy.sleep(1)
##############添加附着障碍#############
tool1_id = 'tool1'
self.scene.remove_attached_object(self.end_effector_link, tool1_id)
tool1_size = [0.052, 0.13, 0.003]
tool1_pose = PoseStamped()
tool1_pose.header.frame_id = self.end_effector_link
tool1_pose.pose.position.x = 0.0
tool1_pose.pose.position.y = 0.015
tool1_pose.pose.position.z = 0.0
tool1_pose.pose.orientation.w = 1.0
self.scene.attach_box(self.end_effector_link, tool1_id, tool1_pose,
tool1_size)
rospy.sleep(1)
##############添加附着障碍#############
tool2_id = 'tool2'
self.scene.remove_attached_object(self.end_effector_link, tool2_id)
tool2_size = [0.015, 0.015, 0.18]
tool2_pose = PoseStamped()
tool2_pose.header.frame_id = self.end_effector_link
tool2_pose.pose.position.x = 0.0
tool2_pose.pose.position.y = 0.07
tool2_pose.pose.position.z = 0.04
tool2_pose.pose.orientation.w = 1.0
self.scene.attach_box(self.end_effector_link, tool2_id, tool2_pose,
tool2_size)
rospy.sleep(1)
##############添加附着障碍#############
tool3_id = 'tool3'
self.scene.remove_attached_object(self.end_effector_link, tool3_id)
tool3_size = [0.09, 0.03, 0.026]
tool3_pose = PoseStamped()
tool3_pose.header.frame_id = self.end_effector_link
tool3_pose.pose.position.x = 0.0
tool3_pose.pose.position.y = -0.035
tool3_pose.pose.position.z = 0.013
tool3_pose.pose.orientation.w = 1.0
self.scene.attach_box(self.end_effector_link, tool3_id, tool3_pose,
tool3_size)
rospy.sleep(1)
#initialize arm position to home
self.arm.set_named_target('home')
self.arm.go()
rospy.sleep(1)
try:
#a = rospy.ServiceProxy('/set_robot_io', setRobotIo)
resp = io_serv(3, True, False)
except rospy.ServiceException, e:
print "Service call failed: %s" % e
def __init__(self):
# Initialize the move_group API
moveit_commander.roscpp_initialize(sys.argv)
rospy.init_node('moveit_obstacles_demo')
# Construct the initial scene object
scene = PlanningSceneInterface()
# Create a scene publisher to push changes to the scene
self.scene_pub = rospy.Publisher('planning_scene',
PlanningScene,
queue_size=5)
# Create a dictionary to hold object colors
self.colors = dict()
# Pause for the scene to get ready
rospy.sleep(1)
# Initialize the move group for the right arm
arm = MoveGroupCommander(GROUP_NAME_ARM)
# Get the name of the end-effector link
end_effector_link = arm.get_end_effector_link()
# Allow some leeway in position (meters) and orientation (radians)
arm.set_goal_position_tolerance(0.01)
arm.set_goal_orientation_tolerance(0.05)
# Allow replanning to increase the odds of a solution
arm.allow_replanning(True)
# Set the right arm reference frame accordingly
arm.set_pose_reference_frame(REFERENCE_FRAME)
# Allow 5 seconds per planning attempt
arm.set_planning_time(5)
# Give each of the scene objects a unique name
table_id = 'table'
box1_id = 'box1'
box2_id = 'box2'
# Remove leftover objects from a previous run
scene.remove_world_object(table_id)
scene.remove_world_object(box1_id)
scene.remove_world_object(box2_id)
# Give the scene a chance to catch up
rospy.sleep(1)
# Start the arm in the "resting" pose stored in the SRDF file
arm.set_named_target('l_arm_init')
arm.go()
rospy.sleep(2)
# Set the height of the table off the ground
table_ground = 0.65
# Set the length, width and height of the table and boxes
table_size = [0.2, 0.7, 0.01]
box1_size = [0.1, 0.05, 0.05]
box2_size = [0.05, 0.05, 0.15]
# Add a table top and two boxes to the scene
table_pose = PoseStamped()
table_pose.header.frame_id = REFERENCE_FRAME
table_pose.pose.position.x = 0.35
table_pose.pose.position.y = 0.0
table_pose.pose.position.z = table_ground + table_size[2] / 2.0
table_pose.pose.orientation.w = 1.0
scene.add_box(table_id, table_pose, table_size)
box1_pose = PoseStamped()
box1_pose.header.frame_id = REFERENCE_FRAME
box1_pose.pose.position.x = 0.3
box1_pose.pose.position.y = 0
box1_pose.pose.position.z = table_ground + table_size[
2] + box1_size[2] / 2.0
box1_pose.pose.orientation.w = 1.0
scene.add_box(box1_id, box1_pose, box1_size)
box2_pose = PoseStamped()
box2_pose.header.frame_id = REFERENCE_FRAME
box2_pose.pose.position.x = 0.3
box2_pose.pose.position.y = 0.25
box2_pose.pose.position.z = table_ground + table_size[
2] + box2_size[2] / 2.0
box2_pose.pose.orientation.w = 1.0
scene.add_box(box2_id, box2_pose, box2_size)
# Make the table red and the boxes orange
self.setColor(table_id, 0.8, 0, 0, 1.0)
self.setColor(box1_id, 0.8, 0.4, 0, 1.0)
self.setColor(box2_id, 0.8, 0.4, 0, 1.0)
# Send the colors to the planning scene
self.sendColors()
# Set the target pose in between the boxes and above the table
target_pose = PoseStamped()
target_pose.header.frame_id = REFERENCE_FRAME
target_pose.pose.position.x = 0.22
target_pose.pose.position.y = 0.14
target_pose.pose.position.z = table_pose.pose.position.z + table_size[
2] + 0.05
q = quaternion_from_euler(0, 0, -1.57079633)
target_pose.pose.orientation.x = q[0]
target_pose.pose.orientation.y = q[1]
target_pose.pose.orientation.z = q[2]
target_pose.pose.orientation.w = q[3]
# Set the target pose for the arm
arm.set_pose_target(target_pose, end_effector_link)
# Move the arm to the target pose (if possible)
arm.go()
# Pause for a moment...
rospy.sleep(2)
# Return the arm to the "resting" pose stored in the SRDF file
arm.set_named_target('l_arm_init')
arm.go()
# Exit MoveIt cleanly
moveit_commander.roscpp_shutdown()
# Exit the script
moveit_commander.os._exit(0)
def __init__(self):
# Initialize the move_group API
moveit_commander.roscpp_initialize(sys.argv)
rospy.init_node('moveit_demo')
# Use the planning scene object to add or remove objects
scene = PlanningSceneInterface()
# Create a scene publisher to push changes to the scene
self.scene_pub = rospy.Publisher('planning_scene',
PlanningScene,
queue_size=5)
# Create a publisher for displaying gripper poses
self.gripper_pose_pub = rospy.Publisher('gripper_pose',
PoseStamped,
queue_size=5)
# Create a dictionary to hold object colors
self.colors = dict()
# Initialize the move group for the right arm
right_arm = MoveGroupCommander(GROUP_NAME_ARM)
right_arm.set_planner_id("RRTConnectkConfigDefault")
# Initialize the move group for the right gripper
right_gripper = MoveGroupCommander(GROUP_NAME_GRIPPER)
# Get the name of the end-effector link
end_effector_link = right_arm.get_end_effector_link()
print(end_effector_link)
# Allow some leeway in position (meters) and orientation (radians)
right_arm.set_goal_position_tolerance(0.01)
right_arm.set_goal_orientation_tolerance(0.01)
# Allow replanning to increase the odds of a solution
right_arm.allow_replanning(True)
# Set the right arm reference frame
right_arm.set_pose_reference_frame(REFERENCE_FRAME)
# Allow 5 seconds per planning attempt
right_arm.set_planning_time(5)
# Set a limit on the number of pick attempts before bailing
max_pick_attempts = 2
# Set a limit on the number of place attempts
max_place_attempts = 5
# Give the scene a chance to catch up
rospy.sleep(2)
print(right_arm.get_current_pose())
# clean the scene
scene.remove_world_object("table")
scene.remove_world_object("part")
# Remove any attached objects from a previous session
scene.remove_attached_object(GRIPPER_FRAME, "part")
# Give the scene a chance to catch up
rospy.sleep(1)
# Start the arm in the "grasp" pose stored in the SRDF file
right_arm.set_named_target('default')
right_arm.go()
# Open the gripper to the neutral position
right_gripper.set_joint_value_target(GRIPPER_OPEN)
right_gripper.go()
rospy.sleep(1)
place_pose = PoseStamped()
place_pose.header.frame_id = REFERENCE_FRAME
place_pose.pose.position.x = 0.11
place_pose.pose.position.y = 0.22
place_pose.pose.position.z = 0.05
scene.add_box("part", place_pose, (0.015, 0.05, 0.015))
# Specify a pose to place the target after being picked up
target_pose = PoseStamped()
target_pose.header.frame_id = REFERENCE_FRAME
# start the gripper in a neutral pose part way to the target
target_pose.pose.position.x = 0.130465574219
target_pose.pose.position.y = 0.171727879517
target_pose.pose.position.z = 0.091324779826
target_pose.pose.orientation.x = -0.929337637511
target_pose.pose.orientation.y = 0.312980256889
target_pose.pose.orientation.z = -0.0625227051047
target_pose.pose.orientation.w = 0.185649739157
# Initialize the grasp pose to the target pose
grasp_pose = target_pose
print("going to start pose")
right_arm.set_pose_target(target_pose)
right_arm.go()
rospy.sleep(2)
# Shift the grasp pose by half the width of the target to center it
#grasp_pose.pose.position.y -= target_size[1] / 2.0
#grasp_pose.pose.position.x = 0.12792118579
#grasp_pose.pose.position.y = -0.285290879999
#grasp_pose.pose.position.z = 0.120301181892
# Generate a list of grasps
grasps = self.make_grasps(grasp_pose, 'part')
# Publish the grasp poses so they can be viewed in RViz
print "Publishing grasps"
for grasp in grasps:
self.gripper_pose_pub.publish(grasp.grasp_pose)
rospy.sleep(0.2)
# Track success/failure and number of attempts for pick operation
result = None
n_attempts = 0
# Repeat until we succeed or run out of attempts
while result != MoveItErrorCodes.SUCCESS and n_attempts < max_pick_attempts:
n_attempts += 1
rospy.loginfo("Pick attempt: " + str(n_attempts))
result = right_arm.pick('part', grasps)
rospy.sleep(0.2)
# If the pick was successful, attempt the place operation
if result == MoveItErrorCodes.SUCCESS:
result = None
n_attempts = 0
# Generate valid place poses
places = self.make_places(place_pose)
# Repeat until we succeed or run out of attempts
while result != MoveItErrorCodes.SUCCESS and n_attempts < max_place_attempts:
n_attempts += 1
rospy.loginfo("Place attempt: " + str(n_attempts))
for place in places:
result = right_arm.place('part', place)
if result == MoveItErrorCodes.SUCCESS:
break
rospy.sleep(0.2)
if result != MoveItErrorCodes.SUCCESS:
rospy.loginfo("Place operation failed after " +
str(n_attempts) + " attempts.")
else:
# Return the arm to the "resting" pose stored in the SRDF file
right_arm.set_named_target('right_arm_rest')
right_arm.go()
# Open the gripper to the open position
right_gripper.set_joint_value_target(GRIPPER_OPEN)
right_gripper.go()
else:
rospy.loginfo("Pick operation failed after " + str(n_attempts) +
" attempts.")
rospy.sleep(1)
# Shut down MoveIt cleanly
moveit_commander.roscpp_shutdown()
# Exit the script
moveit_commander.os._exit(0)
def __init__(self, group_name):
super(MoveGroupPythonIntefaceTutorial, self).__init__()
## BEGIN_SUB_TUTORIAL setup
##
## First initialize `moveit_commander`_ and a `rospy`_ node:
moveit_commander.roscpp_initialize(sys.argv)
rospy.init_node('move_group_python_interface_tutorial', anonymous=True)
## Instantiate a `RobotCommander`_ object. This object is the outer-level interface to
## the robot:
robot = moveit_commander.RobotCommander()
## Instantiate a `PlanningSceneInterface`_ object. This object is an interface
## to the world surrounding the robot:
scene = moveit_commander.PlanningSceneInterface()
## Instantiate a `MoveGroupCommander`_ object. This object is an interface
## to one group of joints. In this case the group is the joints in the Panda
## arm so we set ``group_name = panda_arm``. If you are using a different robot,
## you should change this value to the name of your robot arm planning group.
## This interface can be used to plan and execute motions on the Panda:
# group_name = "right_arm"
group = moveit_commander.MoveGroupCommander(group_name)
## We create a `DisplayTrajectory`_ publisher which is used later to publish
## trajectories for RViz to visualize:
display_trajectory_publisher = rospy.Publisher(
'/move_group/display_planned_path',
moveit_msgs.msg.DisplayTrajectory,
queue_size=20)
gripper_pub = rospy.Publisher(
'/yumi/gripper_effort_controller_l/command', Float64)
## END_SUB_TUTORIAL
## BEGIN_SUB_TUTORIAL basic_info
##
## Getting Basic Information
## ^^^^^^^^^^^^^^^^^^^^^^^^^
# We can get the name of the reference frame for this robot:
planning_frame = group.get_planning_frame()
print "============ Reference frame: %s" % planning_frame
# We can also print the name of the end-effector link for this group:
eef_link = group.get_end_effector_link()
print "============ End effector: %s" % eef_link
# We can get a list of all the groups in the robot:
group_names = robot.get_group_names()
print "============ Robot Groups:", robot.get_group_names()
# Sometimes for debugging it is useful to print the entire state of the
# robot:
print "============ Printing robot state"
print robot.get_current_state()
print ""
## END_SUB_TUTORIAL
# Misc variables
self.box_name = ''
self.robot = robot
self.scene = scene
self.group = group
self.display_trajectory_publisher = display_trajectory_publisher
self.planning_frame = planning_frame
self.eef_link = eef_link
self.group_names = group_names
self.gripper_pub = gripper_pub
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
print "start pose", start_pose
orientation_list = [
start_pose.orientation.x, start_pose.orientation.y,
start_pose.orientation.z, start_pose.orientation.w
]
(roll, pitch, yaw) = euler_from_quaternion(orientation_list)
print roll, pitch, yaw
quat = quaternion_from_euler(roll, pitch, yaw)
print quat
# 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.1
#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 __init__(self, temp):
## First initialize `moveit_commander`_ and a `rospy`_ node:
moveit_commander.roscpp_initialize(sys.argv)
rospy.init_node('move_group_python', anonymous=True)
## Instantiate a `RobotCommander`_ object. Provides information such as the robot's
## kinematic model and the robot's current joint states
robot = moveit_commander.RobotCommander()
## Instantiate a `PlanningSceneInterface`_ object. This provides a remote interface
## for getting, setting, and updating the robot's internal understanding of the
## surrounding world:
scene = moveit_commander.PlanningSceneInterface()
## Instantiate a `MoveGroupCommander`_ object. This object is an interface
## to a planning group (group of joints). In this tutorial the group is the primary
## arm joints in the Panda robot, so we set the group's name to "panda_arm".
## If you are using a different robot, change this value to the name of your robot
## arm planning group.
## This interface can be used to plan and execute motions:
group_name = temp
move_group = moveit_commander.MoveGroupCommander(group_name)
## Create a `DisplayTrajectory`_ ROS publisher which is used to display
## trajectories in Rviz:
display_trajectory_publisher = rospy.Publisher(
'/move_group/display_planned_path',
moveit_msgs.msg.DisplayTrajectory,
queue_size=20)
## Getting Basic Information
# We can get the name of the reference frame for this robot:
planning_frame = move_group.get_planning_frame()
#print "============ Planning frame: %s" % planning_frame
# We can also print the name of the end-effector link for this group:
eef_link = move_group.get_end_effector_link()
#print "============ End effector link: %s" % eef_link
# We can get a list of all the groups in the robot:
group_names = robot.get_group_names()
print "============ Available Planning Groups:", robot.get_group_names(
)
# Sometimes for debugging it is useful to print the entire state of the
# robot:
# print "============ Printing robot state"
# print robot.get_current_state()
# print ""
#
# print('--------------------------------------------')
# ''''Get the current pose of the end-effector of the group.'''
# print(move_group.get_current_pose().pose)
# Misc variables
self.robot = robot
self.scene = scene
self.move_group = move_group
self.display_trajectory_publisher = display_trajectory_publisher
self.planning_frame = planning_frame
self.eef_link = eef_link
self.group_names = group_names
self.name_arm = group_name
self.tag_name = temp #ok
def handle_req(req):
## First initialize `moveit_commander`_ and a `rospy`_ node:
moveit_commander.roscpp_initialize(sys.argv)
## Instantiate a `RobotCommander`_ object. Provides information such as the robot's
## kinematic model and the robot's current joint states
robot = moveit_commander.RobotCommander()
## Instantiate a `PlanningSceneInterface`_ object. This provides a remote interface
## for getting, setting, and updating the robot's internal understanding of the
## surrounding world:
scene = moveit_commander.PlanningSceneInterface()
## Instantiate a `MoveGroupCommander`_ object. This object is an interface
## to a planning group (group of joints).
## This interface can be used to plan and execute motions:
group_name = req.group
## print "============ Robot Groups:"
print robot.get_group_names()
## print "============ Printing robot state"
print robot.get_current_state()
move_group = moveit_commander.MoveGroupCommander(group_name.data)
print "Pose is: "
print move_group.get_current_pose().pose
# The group_thumb has only two valid joints, hence the if-else statement
# if the group_thumb is selected, accept only two joint values
# else 3 joint values
if(group_name.data == "group_thumb"):
## Create a `DisplayTrajectory`_ ROS publisher which is used to display
## trajectories in Rviz:
display_trajectory_publisher = rospy.Publisher('/move_group/display_planned_path',
moveit_msgs.msg.DisplayTrajectory,
queue_size=20)
# We can get the joint values from the group and adjust some of the values:
joint_goal = move_group.get_current_joint_values()
# print Joint values
print move_group.get_current_joint_values()[0]
print move_group.get_current_joint_values()[1]
joint_goal[0] = req.joint_goal1.data
joint_goal[1] = req.joint_goal2.data
# The go command can be called with joint values, poses, or without any
# parameters if you have already set the pose or joint target for the group
move_group.go(joint_goal, wait=True)
# Calling ``stop()`` ensures that there is no residual movement
move_group.stop()
# For testing:
current_joints = move_group.get_current_joint_values()
# print Joint values
print move_group.get_current_joint_values()[0]
print move_group.get_current_joint_values()[1]
else:
## Instantiate a `MoveGroupCommander`_ object. This object is an interface
## to a planning group (group of joints).
## This interface can be used to plan and execute motions:
move_group = moveit_commander.MoveGroupCommander(group_name.data)
## Create a `DisplayTrajectory`_ ROS publisher which is used to display
## trajectories in Rviz:
display_trajectory_publisher = rospy.Publisher('/move_group/display_planned_path',
moveit_msgs.msg.DisplayTrajectory,
queue_size=20)
# We can get the joint values from the group and adjust some of the values:
joint_goal = move_group.get_current_joint_values()
# The go command can be called with joint values, poses, or without any
# parameters if you have already set the pose or joint target for the group
# print Joint values
print move_group.get_current_joint_values()[0]
print move_group.get_current_joint_values()[1]
print move_group.get_current_joint_values()[2]
joint_goal[0] = req.joint_goal1.data
joint_goal[1] = req.joint_goal2.data
joint_goal[2] = req.joint_goal3.data
# The go command can be called with joint values, poses, or without any
# parameters if you have already set the pose or joint target for the group
move_group.go(joint_goal, wait=True)
# Calling ``stop()`` ensures that there is no residual movement
move_group.stop()
# For testing:
current_joints = move_group.get_current_joint_values()
# print Joint values
print move_group.get_current_joint_values()[0]
print move_group.get_current_joint_values()[1]
print move_group.get_current_joint_values()[2]
rep = udm_serviceResponse()
rep.res = True
rep.message = "This action was planned at %s" %rospy.get_time()
return rep
def __init__(self):
super(PickAndDropProgram, self).__init__()
# First initialize `moveit_commander`_ and a `rospy`_ node:
moveit_commander.roscpp_initialize(sys.argv)
rospy.init_node('pick_and_drop_program', anonymous=True)
filepath = rospy.get_param('~goal_joint_states')
goal_joint_states = joint_state_filereader.read(filepath)
self.goal_names = [
"home", "near_box", "near_drop", "drop", "pick"
]
for name in self.goal_names:
assert name in goal_joint_states, \
"Joint state name '{}' is not found".format(name)
# Declare suctionpad topics
self.pub_to = rospy.Publisher('toArduino', String, queue_size=100)
self.pub_from = rospy.Publisher('fromArduino', String, queue_size=100)
# Instantiate a `RobotCommander`_ object. This object is the outer-level interface to
# the robot:
robot = moveit_commander.RobotCommander()
# Instantiate a `PlanningSceneInterface`_ object. This object is an interface
# to the world surrounding the robot:
scene = moveit_commander.PlanningSceneInterface()
# Instantiate a `MoveGroupCommander`_ object. This object is an interface
# to one group of joints. In this case the group is the joints in the UR5
# arm so we set ``group_name = manipulator``. If you are using a different robot,
# you should change this value to the name of your robot arm planning group.
group_name = "manipulator" # See .srdf file to get available group names
group = moveit_commander.MoveGroupCommander(group_name)
# We create a `DisplayTrajectory`_ publisher which is used later to publish
# trajectories for RViz to visualize:
# display_trajectory_publisher = rospy.Publisher('/move_group/display_planned_path',
# moveit_msgs.msg.DisplayTrajectory,
# queue_size=20)
# We can get the name of the reference frame for this robot:
planning_frame = group.get_planning_frame()
print("============ Reference frame: %s" % planning_frame)
# We can also print(the name of the end-effector link for this group:
eef_link = group.get_end_effector_link()
print("============ End effector: %s" % eef_link)
# We can get a list of all the groups in the robot:
group_names = robot.get_group_names()
print("============ Robot Groups:", robot.get_group_names())
# Sometimes for debugging it is useful to print the entire state of the
# robot:
print("============ Printing robot state")
print(robot.get_current_state())
print("")
# Misc variables
self.robot = robot
self.scene = scene
self.group = group
self.group_names = group_names
self.goal_joint_states = dict(goal_joint_states)
def __init__(self):
# Initialize move_group API
moveit_commander.roscpp_initialize(sys.argv)
# Initialize ROS node
rospy.init_node('moveit_obstacles_demo')
# Initialize scene
scene = PlanningSceneInterface()
# Create a publisher for scene change information
self.scene_pub = rospy.Publisher('planning_scene',
PlanningScene,
queue_size=5)
# Create a dictionary to store object color
self.colors = dict()
# Wait for scene to be ready
rospy.sleep(1)
# Initialize arm_group which needs to be controlled by move_group
arm = MoveGroupCommander('arm')
# Get end link name
end_effector_link = arm.get_end_effector_link()
# Set tolerance for position (m) and orientation (rad)
arm.set_goal_position_tolerance(0.01)
arm.set_goal_orientation_tolerance(0.05)
# Allow replanning if failed
arm.allow_replanning(True)
# Set reference frame for target position
reference_frame = 'base_link'
arm.set_pose_reference_frame(reference_frame)
# Set planning time limitation
arm.set_planning_time(5)
# Set scene object name
front_board_id = 'front_board'
back_board_id = 'back_board'
left_board_id = 'left_board'
right_board_id = "right_board"
bottle_id = 'bottle'
# Remove previous object
scene.remove_world_object(front_board_id)
scene.remove_world_object(back_board_id)
scene.remove_world_object(left_board_id)
scene.remove_world_object(right_board_id)
scene.remove_world_object(bottle_id)
rospy.sleep(1)
# Move arm to initial position
arm.set_named_target('home')
arm.go()
rospy.sleep(2)
# Initialize gripper_group which needs to be controlled by move_group
gripper = moveit_commander.MoveGroupCommander('gripper')
# Set object dimension
front_board_size = [0.2, 0.25, 0.01]
back_board_size = [0.2, 0.25, 0.01]
left_board_size = [0.25, 0.14, 0.01]
right_board_size = [0.25, 0.14, 0.01]
bottle_height, bottle_radius = 0.06, 0.02
# Add object to scene
front_board_pose = PoseStamped()
front_board_pose.header.frame_id = reference_frame
front_board_pose.pose.position.x = 0
front_board_pose.pose.position.y = 0.1
front_board_pose.pose.position.z = 0.125
front_board_pose.pose.orientation.w = 0.707
front_board_pose.pose.orientation.x = 0.707
scene.add_box(front_board_id, front_board_pose, front_board_size)
back_board_pose = PoseStamped()
back_board_pose.header.frame_id = reference_frame
back_board_pose.pose.position.x = 0
back_board_pose.pose.position.y = 0.24
back_board_pose.pose.position.z = 0.125
back_board_pose.pose.orientation.w = 0.707
back_board_pose.pose.orientation.x = 0.707
scene.add_box(back_board_id, back_board_pose, back_board_size)
left_board_pose = PoseStamped()
left_board_pose.header.frame_id = reference_frame
left_board_pose.pose.position.x = 0.1
left_board_pose.pose.position.y = 0.17
left_board_pose.pose.position.z = 0.125
left_board_pose.pose.orientation.w = 0.707
left_board_pose.pose.orientation.y = 0.707
scene.add_box(left_board_id, left_board_pose, left_board_size)
right_board_pose = PoseStamped()
right_board_pose.header.frame_id = reference_frame
right_board_pose.pose.position.x = -0.1
right_board_pose.pose.position.y = 0.17
right_board_pose.pose.position.z = 0.125
right_board_pose.pose.orientation.w = 0.707
right_board_pose.pose.orientation.y = 0.707
scene.add_box(right_board_id, right_board_pose, right_board_size)
bottle_pose = PoseStamped()
bottle_pose.header.frame_id = reference_frame
bottle_pose.pose.position.x = 0.1
bottle_pose.pose.position.y = -0.2
bottle_pose.pose.position.z = 0.02
bottle_pose.pose.orientation.w = 0.707
bottle_pose.pose.orientation.y = 0.707
scene.add_cylinder(bottle_id, bottle_pose, bottle_height,
bottle_radius)
# Set object color
self.setColor(front_board_id, 1.0, 1.0, 1.0, 0.6)
self.setColor(back_board_id, 1.0, 1.0, 1.0, 0.6)
self.setColor(left_board_id, 1.0, 1.0, 1.0, 0.6)
self.setColor(right_board_id, 1.0, 1.0, 1.0, 0.6)
self.setColor(bottle_id, 0.6, 0.1, 0, 1.0)
# Publish color
self.sendColors()
rospy.sleep(3)
# Set target position for gripper and move gripper
gripper_position = (np.array([36, -36, 36, 36]) * np.pi /
180.0).tolist()
gripper.set_joint_value_target(gripper_position)
gripper.go()
rospy.sleep(5)
# Set target position for arm
joint_position = (np.array([28, -10, 50, -88, 58]) * np.pi /
180.0).tolist()
arm.set_joint_value_target(joint_position)
arm.go()
# Set target position for arm
joint_position = (np.array([28, -44, 30, -91, 58]) * np.pi /
180.0).tolist()
arm.set_joint_value_target(joint_position)
arm.go()
# Set target position for gripper and move gripper
gripper_position = (np.array([20, -20, 20, 20]) * np.pi /
180.0).tolist()
gripper.set_joint_value_target(gripper_position)
gripper.go()
rospy.sleep(10)
# Move arm to initial position
arm.set_named_target('home')
arm.go()
# Set target position for gripper and move gripper
gripper_position = (np.array([-33, 33, -33, -33]) * np.pi /
180.0).tolist()
gripper.set_joint_value_target(gripper_position)
gripper.go()
# Close and exit MoveIt
moveit_commander.roscpp_shutdown()
def __init__(self):
# Initialize the move_group API
moveit_commander.roscpp_initialize(sys.argv)
rospy.init_node('moveit_ik')
# Initialize the move group for the right arm
arm = moveit_commander.MoveGroupCommander('arm')
# Get the name of the end-effector link
end_effector_link = arm.get_end_effector_link()
print end_effector_link
# Set the reference frame for pose targets
reference_frame = '/base_link'
# Set the arm reference frame accordingly
arm.set_pose_reference_frame(reference_frame)
# Allow replanning to increase the odds of a solution
arm.allow_replanning(True)
# Allow some leeway in position (meters) and orientation (radians)
arm.set_goal_position_tolerance(0.002)
arm.set_goal_orientation_tolerance(3.14)
# Start the arm in the "resting" pose stored in the SRDF file
#arm.set_named_target('resting')
#arm.go()
#rospy.sleep(2)
# Test forward kinematics
joint_positions = [0.0, 0.0, 0.0, 0.0, 0.0]
arm.set_joint_value_target(joint_positions)
traj = arm.plan()
arm.execute(traj)
# LeeChan
tpose = arm.get_current_pose()
print type(tpose)
print tpose
# Set the target pose. This particular pose has the gripper oriented horizontally
# 0.85 meters above the ground, 0.10 meters to the right and 0.20 meters ahead of
# the center of the robot base.
target_pose = PoseStamped()
target_pose.header.frame_id = reference_frame
target_pose.header.stamp = rospy.Time.now()
target_pose.pose.position.x = 0.25515
target_pose.pose.position.y = -0.255147
target_pose.pose.position.z = 0.510048
#target_pose.pose.orientation.x = 0.034
#target_pose.pose.orientation.y = 0.673
#target_pose.pose.orientation.z = 0.0
target_pose.pose.orientation.w = 1
print type(target_pose)
print target_pose
# Set the start state to the current state
arm.set_start_state_to_current_state()
# Set the goal pose of the end effector to the stored pose
arm.set_pose_target(target_pose, end_effector_link)
# Plan the trajectory to the goal
traj = arm.plan()
# Execute the planned trajectory
arm.execute(traj)
# Pause for a second
rospy.sleep(1)
###############################################
target_pose = PoseStamped()
target_pose.header.frame_id = reference_frame
target_pose.header.stamp = rospy.Time.now()
target_pose.pose.position.x = 0.25305
target_pose.pose.position.y = -0.1963
target_pose.pose.position.z = 0.6633
#target_pose.pose.orientation.x = 0.034
#target_pose.pose.orientation.y = 0.673
#target_pose.pose.orientation.z = 0.0
target_pose.pose.orientation.w = 1
print type(target_pose)
print target_pose
arm.set_start_state_to_current_state()
arm.set_pose_target(target_pose, end_effector_link)
traj = arm.plan()
arm.execute(traj)
rospy.sleep(1)
#################################################
# Shut down MoveIt cleanly
moveit_commander.roscpp_shutdown()
# Exit MoveIt
moveit_commander.os._exit(0)
