def set_path_constraints(self, value):
# In order to force planning in joint coordinates we add dummy joint
# constrains.
empty_joint_constraints = JointConstraint()
empty_joint_constraints.joint_name = "r1_joint_grip"
empty_joint_constraints.position = 0
empty_joint_constraints.tolerance_above = 1.5
empty_joint_constraints.tolerance_below = 1.5
empty_joint_constraints.weight = 1
value.joint_constraints.append(empty_joint_constraints)
MoveGroupCommander.set_path_constraints(self, value)
def move_to(self,
pos_x,
pos_y,
pos_z,
orien_x,
orien_y,
orien_z,
orien_w,
ik,
orien_const=None):
#Construct the request
request = GetPositionIKRequest()
request.ik_request.group_name = "right_arm"
request.ik_request.ik_link_name = "right_gripper"
request.ik_request.attempts = 20
request.ik_request.pose_stamped.header.frame_id = "base"
#Set the desired orientation for the end effector HERE
request.ik_request.pose_stamped.pose.position.x = pos_x
request.ik_request.pose_stamped.pose.position.y = pos_y
request.ik_request.pose_stamped.pose.position.z = pos_z
request.ik_request.pose_stamped.pose.orientation.x = orien_x
request.ik_request.pose_stamped.pose.orientation.y = orien_y
request.ik_request.pose_stamped.pose.orientation.z = orien_z
request.ik_request.pose_stamped.pose.orientation.w = orien_w
try:
#Send the request to the service
response = ik(request)
#Print the response HERE
print(response)
group = MoveGroupCommander("right_arm")
if orien_const is not None:
constraints = Constraints()
constraints.orientation_constraints = orien_const
group.set_path_constraints(constraints)
# Setting position and orientation target
group.set_pose_target(request.ik_request.pose_stamped)
# TRY THIS
# Setting just the position without specifying the orientation
# group.set_position_target([0.5, 0.5, 0.0])
# Plan IK and execute
group.go()
except rospy.ServiceException, e:
print("Service call failed: %s")
def __init__(self):
moveit_commander.roscpp_initialize(sys.argv)
rospy.init_node('moveit_test')
arm = MoveGroupCommander('arm')
end_effector_link = arm.get_end_effector_link()
arm.allow_replanning(True)
arm.set_planning_time(5)
target_pose = PoseStamped()
target_pose.header.frame_id = REFERENCE_FRAME
target_init_orientation = Quaternion()
target_init_orientation = quaternion_from_euler(0.0, 1.57, 0.0)
self.setPose(target_pose, [0.5, 0.2, 0.5],list(target_init_orientation))
current_pose = arm.get_current_pose(end_effector_link)
self.setPose(current_pose, [current_pose.pose.position.x, current_pose.pose.position.y, current_pose.pose.position.z], list(target_init_orientation))
arm.set_pose_target(current_pose)
arm.go()
rospy.sleep(2)
constraints = Constraints()
orientation_constraint = OrientationConstraint()
constraints.name = 'gripper constraint'
orientation_constraint.header = target_pose.header
orientation_constraint.link_name = end_effector_link
orientation_constraint.orientation.x = target_init_orientation[0]
orientation_constraint.orientation.y = target_init_orientation[1]
orientation_constraint.orientation.z = target_init_orientation[2]
orientation_constraint.orientation.w = target_init_orientation[3]
orientation_constraint.absolute_x_axis_tolerance = 0.03
orientation_constraint.absolute_y_axis_tolerance = 0.03
orientation_constraint.absolute_z_axis_tolerance = 3.14
orientation_constraint.weight = 1.0
constraints.orientation_constraints.append(orientation_constraint)
arm.set_path_constraints(constraints)
arm.set_pose_target(target_pose, end_effector_link)
arm.go()
rospy.sleep(1)
moveit_commander.roscpp_shutdown()
moveit_commander.os._exit(0)
def __init__(self):
# Initialize the move_group API
moveit_commander.roscpp_initialize(sys.argv)
# Initialize the ROS node
rospy.init_node('moveit_constraints_demo', anonymous=True)
robot = RobotCommander()
# Connect to the arm move group
arm = MoveGroupCommander(GROUP_NAME_ARM)
# Initialize the move group for the right gripper
gripper = MoveGroupCommander(GROUP_NAME_GRIPPER)
# Increase the planning time since constraint planning can take a while
arm.set_planning_time(5)
# Allow replanning to increase the odds of a solution
arm.allow_replanning(True)
# Set the right arm reference frame
arm.set_pose_reference_frame(REFERENCE_FRAME)
# Allow some leeway in position(meters) and orientation (radians)
arm.set_goal_position_tolerance(0.05)
arm.set_goal_orientation_tolerance(0.1)
# Get the name of the end-effector link
end_effector_link = arm.get_end_effector_link()
# Start in the "resting" configuration stored in the SRDF file
arm.set_named_target('l_arm_init')
# Plan and execute a trajectory to the goal configuration
arm.go()
rospy.sleep(1)
# Open the gripper
gripper.set_joint_value_target(GRIPPER_NEUTRAL)
gripper.go()
rospy.sleep(1)
# Set an initial target pose with the arm up and to the right
target_pose = PoseStamped()
target_pose.header.frame_id = REFERENCE_FRAME
target_pose.pose.position.x = 0.263803774718
target_pose.pose.position.y = 0.295405791959
target_pose.pose.position.z = 0.690438884208
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 start state and target pose, then plan and execute
arm.set_start_state(robot.get_current_state())
arm.set_pose_target(target_pose, end_effector_link)
arm.go()
rospy.sleep(2)
# Close the gripper
gripper.set_joint_value_target(GRIPPER_CLOSED)
gripper.go()
rospy.sleep(1)
# Store the current pose
start_pose = arm.get_current_pose(end_effector_link)
# Create a contraints list and give it a name
constraints = Constraints()
constraints.name = "Keep gripper horizontal"
# Create an orientation constraint for the right gripper
orientation_constraint = OrientationConstraint()
orientation_constraint.header = start_pose.header
orientation_constraint.link_name = arm.get_end_effector_link()
orientation_constraint.orientation.w = 1.0
orientation_constraint.absolute_x_axis_tolerance = 0.1
orientation_constraint.absolute_y_axis_tolerance = 0.1
orientation_constraint.absolute_z_axis_tolerance = 0.1
orientation_constraint.weight = 1.0
# q = quaternion_from_euler(0, 0, -1.57079633)
# orientation_constraint.orientation.x = q[0]
# orientation_constraint.orientation.y = q[1]
# orientation_constraint.orientation.z = q[2]
# orientation_constraint.orientation.w = q[3]
# Append the constraint to the list of contraints
constraints.orientation_constraints.append(orientation_constraint)
# Set the path constraints on the arm
arm.set_path_constraints(constraints)
# Set a target pose for the arm
target_pose = PoseStamped()
target_pose.header.frame_id = REFERENCE_FRAME
target_pose.pose.position.x = 0.39000848183
target_pose.pose.position.y = 0.185900663329
target_pose.pose.position.z = 0.732752341378
target_pose.pose.orientation.w = 1
# Set the start state and target pose, then plan and execute
arm.set_start_state_to_current_state()
arm.set_pose_target(target_pose, end_effector_link)
arm.go()
rospy.sleep(1)
# Clear all path constraints
arm.clear_path_constraints()
# Open the gripper
gripper.set_joint_value_target(GRIPPER_NEUTRAL)
gripper.go()
rospy.sleep(1)
# Return to the "resting" configuration stored in the SRDF file
arm.set_named_target('l_arm_init')
# Plan and execute a trajectory to the goal configuration
arm.go()
rospy.sleep(1)
# Shut down MoveIt cleanly
moveit_commander.roscpp_shutdown()
# Exit MoveIt
moveit_commander.os._exit(0)
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)
robot = RobotCommander()
# Connect to the right_arm move group
right_arm = MoveGroupCommander(GROUP_NAME_ARM)
# Initialize the move group for the right gripper
right_gripper = MoveGroupCommander(GROUP_NAME_GRIPPER)
# Increase the planning time since constraint planning can take a while
right_arm.set_planning_time(15)
# 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 some leeway in position(meters) and orientation (radians)
right_arm.set_goal_position_tolerance(0.05)
right_arm.set_goal_orientation_tolerance(0.1)
# Get the name of the end-effector link
end_effector_link = right_arm.get_end_effector_link()
# Start in the "resting" configuration stored in the SRDF file
right_arm.set_named_target('right_arm_zero')
# Plan and execute a trajectory to the goal configuration
right_arm.go()
rospy.sleep(1)
# Open the gripper
right_gripper.set_joint_value_target(GRIPPER_NEUTRAL)
right_gripper.go()
rospy.sleep(1)
# Set an initial target pose with the arm up and to the right
target_pose = PoseStamped()
target_pose.header.frame_id = REFERENCE_FRAME
target_pose.pose.position.x = 0.237012590198
target_pose.pose.position.y = -0.0747191267505
target_pose.pose.position.z = 0.901578401949
target_pose.pose.orientation.w = 1.0
# Set the start state and target pose, then plan and execute
right_arm.set_start_state(robot.get_current_state())
right_arm.set_pose_target(target_pose, end_effector_link)
right_arm.go()
rospy.sleep(2)
# Close the gripper
right_gripper.set_joint_value_target(GRIPPER_CLOSED)
right_gripper.go()
rospy.sleep(1)
# Store the current pose
start_pose = right_arm.get_current_pose(end_effector_link)
# Create a contraints list and give it a name
constraints = Constraints()
constraints.name = "Keep gripper horizontal"
# Create an orientation constraint for the right gripper
orientation_constraint = OrientationConstraint()
orientation_constraint.header = start_pose.header
orientation_constraint.link_name = right_arm.get_end_effector_link()
orientation_constraint.orientation.w = 1.0
orientation_constraint.absolute_x_axis_tolerance = 0.1
orientation_constraint.absolute_y_axis_tolerance = 0.1
orientation_constraint.absolute_z_axis_tolerance = 3.14
orientation_constraint.weight = 1.0
# Append the constraint to the list of contraints
constraints.orientation_constraints.append(orientation_constraint)
# Set the path constraints on the right_arm
right_arm.set_path_constraints(constraints)
# Set a target pose for the arm
target_pose = PoseStamped()
target_pose.header.frame_id = REFERENCE_FRAME
target_pose.pose.position.x = 0.173187824708
target_pose.pose.position.y = -0.0159929871606
target_pose.pose.position.z = 0.692596608605
target_pose.pose.orientation.w = 1.0
# Set the start state and target pose, then plan and execute
right_arm.set_start_state_to_current_state()
right_arm.set_pose_target(target_pose, end_effector_link)
right_arm.go()
rospy.sleep(1)
# Clear all path constraints
right_arm.clear_path_constraints()
# Open the gripper
right_gripper.set_joint_value_target(GRIPPER_NEUTRAL)
right_gripper.go()
rospy.sleep(1)
# Return to the "resting" configuration stored in the SRDF file
right_arm.set_named_target('right_arm_zero')
# Plan and execute a trajectory to the goal configuration
right_arm.go()
rospy.sleep(1)
# Shut down MoveIt cleanly
moveit_commander.roscpp_shutdown()
# Exit MoveIt
moveit_commander.os._exit(0)
def inverse_kinematics():
# Construct the request
request = GetPositionIKRequest()
request.ik_request.group_name = "right_arm"
request.ik_request.ik_link_name = "right_gripper"
request.ik_request.attempts = 50
request.ik_request.pose_stamped.header.frame_id = "base"
# Create joint constraints
#This is joint constraint will need to be set at the group
constraints = Constraints()
joint_constr = JointConstraint()
joint_constr.joint_name = "right_j0"
joint_constr.position = TORSO_POSITION
joint_constr.tolerance_above = TOLERANCE
joint_constr.tolerance_below = TOLERANCE
joint_constr.weight = 0.5
constraints.joint_constraints.append(joint_constr)
# Get the transformed AR Tag (x,y,z) coordinates
# Only care about the x coordinate of AR tag; tells use
# how far away wall is
# x,y, z tell us the origin of the AR Tag
x_coord = board_x # DONT CHANGE
y_coord = bounding_points[0]
z_coord = bounding_points[1]
y_width = bounding_points[2]
z_height = bounding_points[3]
#Creating Path Planning
waypoints = []
z_bais = 0
print(
"OMMMMMMMMMMMMMMMMMMMMMMMMMGGGGGGGGGGGGGGGGGGGGGGGGGGGGGGGGG!!!!!!!!!!!!!!"
)
print(bounding_points)
for i in range(int(float(z_height) / .03)):
target_pose = Pose()
target_pose.position.x = float(x_coord - PLANNING_BIAS)
if i % 2 == 0:
#Starting positions
target_pose.position.y = float(y_coord)
else:
target_pose.position.y = y_coord + float(y_width)
#Starting positions
target_pose.position.z = float(z_coord + z_bais)
target_pose.orientation.y = 1.0 / 2**(1 / 2.0)
target_pose.orientation.w = 1.0 / 2**(1 / 2.0)
waypoints.append(target_pose)
z_bais += .03
#Set the desired orientation for the end effector HERE
request.ik_request.pose_stamped.pose = target_pose
try:
#Send the request to the service
response = compute_ik(request)
group = MoveGroupCommander("right_arm")
group.set_max_velocity_scaling_factor(0.75)
#Set the desired orientation for the end effector HERE
request.ik_request.pose_stamped.pose = target_pose
#Creating a Robot Trajectory for the Path Planning
jump_thres = 0.0
eef_step = 0.1
path, fraction = group.compute_cartesian_path([target_pose],
eef_step, jump_thres)
print("Path fraction: {}".format(fraction))
#Setting position and orientation target
group.set_pose_target(request.ik_request.pose_stamped)
#Setting the Joint constraint
group.set_path_constraints(constraints)
if fraction < 0.5:
group.go()
else:
group.execute(path)
if i < int(float(z_height) / 0.03) and i > 0:
target2 = target_pose
target2.position.z += 0.03
path, fraction = group.compute_cartesian_path([target2],
eef_step,
jump_thres)
group.set_path_constraints(constraints)
group.execute(path)
except rospy.ServiceException, e:
print "Service call failed: %s" % e
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)
robot = RobotCommander()
# Connect to the right_arm move group
right_arm = MoveGroupCommander(GROUP_NAME_ARM)
# Initialize the move group for the right gripper
right_gripper = MoveGroupCommander(GROUP_NAME_GRIPPER)
# Increase the planning time since contraint planning can take a while
right_arm.set_planning_time(15)
# 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 some leeway in position(meters) and orientation (radians)
right_arm.set_goal_position_tolerance(0.05)
right_arm.set_goal_orientation_tolerance(0.1)
# Get the name of the end-effector link
end_effector_link = right_arm.get_end_effector_link()
# Start in the "resting" configuration stored in the SRDF file
right_arm.set_named_target('resting')
# Plan and execute a trajectory to the goal configuration
right_arm.go()
rospy.sleep(1)
# Open the gripper
right_gripper.set_joint_value_target(GRIPPER_NEUTRAL)
right_gripper.go()
rospy.sleep(1)
# Set an initial target pose with the arm up and to the right
target_pose = PoseStamped()
target_pose.header.frame_id = REFERENCE_FRAME
target_pose.pose.position.x = 0.237012590198
target_pose.pose.position.y = -0.0747191267505
target_pose.pose.position.z = 0.901578401949
target_pose.pose.orientation.w = 1.0
# Set the start state and target pose, then plan and execute
right_arm.set_start_state(robot.get_current_state())
right_arm.set_pose_target(target_pose, end_effector_link)
right_arm.go()
rospy.sleep(2)
# Close the gripper
right_gripper.set_joint_value_target(GRIPPER_CLOSED)
right_gripper.go()
rospy.sleep(1)
# Store the current pose
start_pose = right_arm.get_current_pose(end_effector_link)
# Create a contraints list and give it a name
constraints = Constraints()
constraints.name = "Keep gripper horizontal"
# Create an orientation constraint for the right gripper
orientation_constraint = OrientationConstraint()
orientation_constraint.header = start_pose.header
orientation_constraint.link_name = right_arm.get_end_effector_link()
orientation_constraint.orientation.w = 1.0
orientation_constraint.absolute_x_axis_tolerance = 0.1
orientation_constraint.absolute_y_axis_tolerance = 0.1
orientation_constraint.absolute_z_axis_tolerance = 3.14
orientation_constraint.weight = 1.0
# Append the constraint to the list of contraints
constraints.orientation_constraints.append(orientation_constraint)
# Set the path constraints on the right_arm
right_arm.set_path_constraints(constraints)
# Set a target pose for the arm
target_pose = PoseStamped()
target_pose.header.frame_id = REFERENCE_FRAME
target_pose.pose.position.x = 0.173187824708
target_pose.pose.position.y = -0.0159929871606
target_pose.pose.position.z = 0.692596608605
target_pose.pose.orientation.w = 1.0
# Set the start state and target pose, then plan and execute
right_arm.set_start_state_to_current_state()
right_arm.set_pose_target(target_pose, end_effector_link)
right_arm.go()
rospy.sleep(1)
# Clear all path constraints
right_arm.clear_path_constraints()
# Open the gripper
right_gripper.set_joint_value_target(GRIPPER_NEUTRAL)
right_gripper.go()
rospy.sleep(1)
# Return to the "resting" configuration stored in the SRDF file
right_arm.set_named_target('resting')
# Plan and execute a trajectory to the goal configuration
right_arm.go()
rospy.sleep(1)
# Shut down MoveIt cleanly
moveit_commander.roscpp_shutdown()
# Exit MoveIt
moveit_commander.os._exit(0)
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)
robot = RobotCommander()
# Connect to the right_arm move group
right_arm = MoveGroupCommander(GROUP_NAME_ARM)
# Set the right arm reference frame
right_arm.set_pose_reference_frame(REFERENCE_FRAME)
# Allow some leeway in position(meters) and orientation (radians)
right_arm.set_goal_tolerance(0.00001)
# Get the name of the end-effector link
end_effector_link = right_arm.get_end_effector_link()
# Start in the "resting" configuration stored in the SRDF file
right_arm.set_named_target('right')
# Plan and execute a trajectory to the goal configuration
right_arm.go()
rospy.sleep(1)
# Set an initial target pose with the arm up and to the right
target_pose = PoseStamped()
target_pose.header.frame_id = REFERENCE_FRAME
target_pose.pose.position.x = 0.4
target_pose.pose.position.y = 0.3
target_pose.pose.position.z = 0.4
target_pose.pose.orientation.x = 0.670820393249937
target_pose.pose.orientation.y = 0.223606797749979
target_pose.pose.orientation.z = 0.223606797749979
target_pose.pose.orientation.w = 0.670820393249937
# Set the start state and target pose, then plan and execute
right_arm.set_pose_target(target_pose, end_effector_link)
right_arm.go()
rospy.sleep(2)
# Store the current pose
start_pose = right_arm.get_current_pose(end_effector_link)
# Create a contraints list and give it a name
constraints = Constraints()
constraints.name = "Keep gripper horizontal"
# Create an orientation constraint for the right gripper
orientation_constraint = OrientationConstraint()
orientation_constraint.header = start_pose.header
orientation_constraint.link_name = end_effector_link
orientation_constraint.absolute_x_axis_tolerance = 0.00001
orientation_constraint.absolute_y_axis_tolerance = 3.14
orientation_constraint.absolute_z_axis_tolerance = 0.00001
orientation_constraint.weight = 1.0
orientation_constraint.orientation = start_pose.pose.orientation
# Append the constraint to the list of contraints
constraints.orientation_constraints.append(orientation_constraint)
# Set the path constraints on the right_arm
right_arm.set_path_constraints(constraints)
# Set a target pose for the arm
target_pose = deepcopy(start_pose)
target_pose.header.frame_id = REFERENCE_FRAME
target_pose.pose.position.x = 0.4
target_pose.pose.position.y = -0.3
target_pose.pose.position.z = 0.4
target_pose.pose.orientation.x = 0.670820393249937
target_pose.pose.orientation.y = -0.223606797749979
target_pose.pose.orientation.z = -0.223606797749979
target_pose.pose.orientation.w = 0.670820393249937
#target_pose.pose.orientation.x = 0.707107
#target_pose.pose.orientation.y = 0
#target_pose.pose.orientation.z = 0
#target_pose.pose.orientation.w = 0.707107
# Set the start state and target pose, then plan and execute
right_arm.set_start_state_to_current_state()
right_arm.set_pose_target(target_pose, end_effector_link)
right_arm.go()
rospy.sleep(1)
# Clear all path constraints
right_arm.clear_path_constraints()
# Return to the "resting" configuration stored in the SRDF file
right_arm.set_named_target('up')
# Plan and execute a trajectory to the goal configuration
right_arm.go()
rospy.sleep(1)
# Shut down MoveIt cleanly
moveit_commander.roscpp_shutdown()
# Exit MoveIt
moveit_commander.os._exit(0)
def getOrientation(self, xyz):
pass
class MoveCup():
def __init__(self):
#basic initiatioon
moveit_commander.roscpp_initialize(sys.argv)
rospy.init_node('moveit_tutorial')
self.robot = moveit_commander.RobotCommander()
################ Collision Object
self.scene = moveit_commander.PlanningSceneInterface()
table = CollisionObject()
primitive = SolidPrimitive()
primitive.type = primitive.BOX
box_pose = geometry_msgs.msg.PoseStamped()
box_pose.header.stamp = rospy.Time.now()
box_pose.header.frame_id = 'tablelol'
box_pose.pose.position.x = 1.25
box_pose.pose.position.y = 0.0
box_pose.pose.position.z = -0.6
table.primitives.append(primitive)
table.primitive_poses.append(box_pose)
table.operation = table.ADD
self.scene.add_box('table', box_pose, size=(.077, .073, .122))
#use joint_group parameter to change which arm it uses?
self.joint_group = rospy.get_param('~arm', default="left_arm")
self.group = MoveGroupCommander(self.joint_group)
#self.group.set_planner_id("BKPIECEkConfigDefault")
#this node will scale any tf pose requests to be at most max_reach from the base frame
self.max_reach = rospy.get_param('~max_reach', default=1.1)
#define a start pose that we can move to before stuff runs
self.start_pose = PoseStamped()
self.start_pose = self.get_start_pose()
#remove this when working for realz
self.display_publisher = rospy.Publisher(
'/move_group/display_planned_path',
moveit_msgs.msg.DisplayTrajectory,
queue_size=10)
self.rate = rospy.Rate(1)
self.ik_srv = rospy.ServiceProxy(
'ExternalTools/left/PositionKinematicsNode/IKService',
SolvePositionIK)
self.limb = baxter_interface.Limb('left')
self.rangesub = rospy.Subscriber('cup_center', geometry_msgs.msg.Point,
self.rangecb)
self.stop = False
self.planning = False
def rangecb(self, point):
if self.planning and point.z == 0:
rospy.loginfo('stop')
self.stop = True
self.move_start()
self.rangesub.unregister()
def callback(self, targetarray):
#callback that moves in a constrained path to anything published to /target_poses
##First, scale the position to be withing self.max_reach
#new_target = self.project_point(targetarray.data)
# rospy.loginfo(self.stop)
if not self.stop:
self.planning = True
new_target = self.project_point(targetarray)
target = PoseStamped()
target.header.stamp = rospy.Time.now()
target.header.frame_id = 'base'
target.pose.position = new_target
#change orientation to be upright
target.pose.orientation = self.start_pose.pose.orientation
#clear group info and set it again
self.group.clear_pose_targets()
# self.group.set_path_constraints(self.get_constraint())
self.group.set_planning_time(10)
# self.group.set_pose_target(target)
################### Try joint space planning
jt_state = JointState()
jt_state.header.stamp = rospy.Time.now()
angles = self.limb.joint_angles()
jt_state.name = list(angles.keys())
jt_state.position = list(angles.values())
jt_state.header.frame_id = 'base'
result = self.ik_srv([target], [jt_state], 0)
angles = {}
i = 0
for name in result.joints[0].name:
angles[name] = result.joints[0].position[i]
i = i + 1
self.group.set_joint_value_target(angles)
#plan and execute plan. If I find a way, I should add error checking her
#currently, if the plan fails, it just doesn't move and waits for another pose to be published
plan = self.group.plan()
self.group.execute(plan)
self.rate.sleep()
return
def scale_movegroup(self, vel=.9, acc=.9):
#slows down baxters arm so we stop getting all those velocity limit errors
self.group.set_max_velocity_scaling_factor(vel)
self.group.set_max_acceleration_scaling_factor(acc)
def unscale_movegroup(self):
self.group.set_max_velocity_scaling_factor(1)
self.group.set_max_acceleration_scaling_factor(1)
def start_baxter_interface(self):
#I copied this from an example but have no idea what it does
self._pub_rate = rospy.Publisher('robot/joint_state_publish_rate',
UInt16,
queue_size=10)
self._left_arm = baxter_interface.limb.Limb("left")
self._left_joint_names = self._left_arm.joint_names()
print(self._left_arm.endpoint_pose())
self._rs = baxter_interface.RobotEnable(CHECK_VERSION)
self._init_state = self._rs.state().enabled
print("Enabling robot... ")
self._rs.enable()
# set joint state publishing to 100Hz
self._pub_rate.publish(100)
return
def get_start_pose(self, point=[.9, 0.2, 0], rpy=[0, math.pi / 2, 0]):
#define a starting position for the move_start method
new_p = PoseStamped()
new_p.header.frame_id = self.robot.get_planning_frame()
new_p.pose.position.x = point[0]
new_p.pose.position.y = point[1]
new_p.pose.position.z = point[2]
p_orient = tf.transformations.quaternion_from_euler(
rpy[0], rpy[1], rpy[2])
p_orient = Quaternion(*p_orient)
new_p.pose.orientation = p_orient
return (new_p)
def project_point(self, multiarray):
#scales an array and returns a point (see: Pose.position) to be within self.max_reach
#convert points from sonar ring frame to shoulder frame
x = multiarray.data[2] + sr[0] - lls[0]
y = multiarray.data[0] + sr[1] - lls[1]
z = (-1 * multiarray.data[1]) + sr[2] - lls[2]
#scale point to a finite reach distance from the shoulder
obj_dist = math.sqrt(x**2 + y**2 + z**2)
scale_val = min(self.max_reach / obj_dist, .99)
point_scaled = Point()
#scale point and bring into the base frames
point_scaled.x = scale_val * x + lls[0]
point_scaled.y = scale_val * y + lls[1]
point_scaled.z = scale_val * z + lls[2]
return (point_scaled)
def move_random(self):
#moves baxter to a random position. used for testing
randstate = PoseStamped()
randstate = self.group.get_random_pose()
self.group.clear_pose_targets()
self.group.set_pose_target(randstate)
self.group.set_planning_time(8)
self.scale_movegroup()
plan = self.group.plan()
while len(
plan.joint_trajectory.points) == 1 and not rospy.is_shutdown():
print('plan no work')
plan = self.group.plan()
self.group.execute(plan)
self.rate.sleep()
return
def move_random_constrained(self):
#move baxter to a random position with constrained path planning. also for testing
self.scale_movegroup()
randstate = PoseStamped()
randstate = self.group.get_random_pose()
self.group.clear_pose_targets()
self.group.set_pose_target(randstate)
self.group.set_path_constraints(self.get_constraint())
self.group.set_planning_time(100)
self.scale_movegroup()
constrained_plan = self.group.plan()
self.group.execute(constrained_plan)
self.unscale_movegroup()
rospy.sleep(3)
return
def move_start(self):
#move baxter to the self.start_pose pose
self.group.clear_pose_targets()
self.group.set_pose_target(self.start_pose)
self.group.set_planning_time(10)
print('moving to start')
plan = self.group.plan()
self.group.execute(plan)
print('at start')
self.rate.sleep()
return
def get_constraint(self,
euler_orientation=[0, math.pi / 2, 0],
tol=[3, 3, .5]):
#method takes euler-angle inputs, this converts it to a quaternion
q_orientation = tf.transformations.quaternion_from_euler(
euler_orientation[0], euler_orientation[1], euler_orientation[2])
orientation_msg = Quaternion(q_orientation[0], q_orientation[1],
q_orientation[2], q_orientation[3])
#defines a constraint that sets the end-effector so a cup in it's hand will be upright, or straight upside-down
constraint = Constraints()
constraint.name = 'upright_wrist'
upright_orientation = OrientationConstraint()
upright_orientation.link_name = self.group.get_end_effector_link()
upright_orientation.orientation = orientation_msg
upright_orientation.absolute_x_axis_tolerance = tol[0]
upright_orientation.absolute_y_axis_tolerance = tol[1]
upright_orientation.absolute_z_axis_tolerance = tol[2]
upright_orientation.weight = 1.0
upright_orientation.header = self.start_pose.header
constraint.orientation_constraints.append(upright_orientation)
return (constraint)
# Set Path Constraint
constraints = Constraints()
constraints.name = "down"
orientation_constraint = OrientationConstraint()
orientation_constraint.header.frame_id = group.get_planning_frame()
orientation_constraint.link_name = group.get_end_effector_link()
orientation_constraint.orientation = pose_target_1.orientation
orientation_constraint.absolute_x_axis_tolerance = 3.1415
orientation_constraint.absolute_y_axis_tolerance = 0.05
orientation_constraint.absolute_z_axis_tolerance = 0.05
orientation_constraint.weight = 1.0
constraints.orientation_constraints.append(orientation_constraint)
group.set_path_constraints(constraints)
rospy.loginfo("Get Path Constraints:\n{}".format(
group.get_path_constraints()))
# Pose Target 2
rospy.loginfo("Start Pose Target 2")
pose_target_2 = Pose()
pose_target_2.position.x = 0.3
pose_target_2.position.y = -0.5
pose_target_2.position.z = 0.15
pose_target_2.orientation.x = 0.0
pose_target_2.orientation.y = -0.707
pose_target_2.orientation.z = 0.0
pose_target_2.orientation.w = 0.707
group.set_planner_id("RRTConnectkConfigDefault")
def main():
roscpp_initialize(sys.argv)
rospy.init_node('grasp_ur5', anonymous=True)
robot = RobotCommander()
scene = PlanningSceneInterface()
arm_group = MoveGroupCommander("manipulator")
q = quaternion_from_euler(1.5701, 0.0, -1.5701)
pose_target = geometry_msgs.msg.Pose()
pose_target.orientation.x = q[0]
pose_target.orientation.y = q[1]
pose_target.orientation.z = q[2]
pose_target.orientation.w = q[3]
pose_target.position.z = 0.23 #0.23
pose_target.position.y = 0.11 #0.11
pose_target.position.x = -0.45 #-0.45
arm_group.set_pose_target(pose_target)
plan = arm_group.plan(pose_target)
while plan[0]!= True:
plan = arm_group.plan(pose_target)
if plan[0]:
traj = plan[1]
arm_group.execute(traj, wait = True)
arm_group.stop()
arm_group.clear_pose_targets()
# rospy.sleep(1)
#FAKE PLAN WITHOUT RESTRICTIONS #
# state = RobotState()
# arm_group.set_start_state(state)
pose_target.position.z = 0.77
arm_group.set_pose_target(pose_target)
plan_fake = arm_group.plan(pose_target)
while plan_fake[0] != True:
plan_fake = arm_group.plan(pose_target)
pose = arm_group.get_current_pose()
constraint = Constraints()
constraint.name = "restricao"
orientation_constraint = OrientationConstraint()
orientation_constraint.header = pose.header
orientation_constraint.link_name = arm_group.get_end_effector_link()
orientation_constraint.orientation = pose.pose.orientation
orientation_constraint.absolute_x_axis_tolerance = 3.14
orientation_constraint.absolute_y_axis_tolerance = 3.14
orientation_constraint.absolute_z_axis_tolerance = 3.14
orientation_constraint.weight = 1
constraint.orientation_constraints.append(orientation_constraint)
arm_group.set_path_constraints(constraint)
# state = RobotState()
# arm_group.set_start_state(state)
pose_target.position.z = 0.77 # 0.77
# pose_target.position.y = -0.11 # -0.11
# pose_target.position.x = 0.31 # 0.31
arm_group.set_pose_target(pose_target)
plan = arm_group.plan(pose_target)
while plan[0]!= True:
plan = arm_group.plan(pose_target)
if plan[0]:
traj = plan[1]
arm_group.execute(traj, wait = True)
arm_group.stop()
arm_group.clear_pose_targets()
def inverse_kinematics():
# Construct the request
request = GetPositionIKRequest()
request.ik_request.group_name = "right_arm"
request.ik_request.ik_link_name = "right_gripper"
request.ik_request.attempts = 50
request.ik_request.pose_stamped.header.frame_id = "base"
# Create joint constraints
#This is joint constraint will need to be set at the group
constraints = Constraints()
joint_constr = JointConstraint()
joint_constr.joint_name = "right_j0"
joint_constr.position = TORSO_POSITION
joint_constr.tolerance_above = TOLERANCE
joint_constr.tolerance_below = TOLERANCE
joint_constr.weight = 0.5
constraints.joint_constraints.append(joint_constr)
# Get the transformed AR Tag (x,y,z) coordinates
# Only care about the x coordinate of AR tag; tells use
# how far away wall is
x_coord = board_x
y_coord = board_y
z_coord = board_z
y_bias = raw_input("Input y coordinate: ")
z_bias = raw_input("Input z coordinate: ")
y_coord += float(y_bias)
z_coord += float(z_bias)
#Creating Path Planning
waypoints = []
target_pose = Pose()
target_pose.position.x = float(x_coord - PLANNING_BIAS)
target_pose.position.y = float(y_coord)
target_pose.position.z = float(z_coord)
target_pose.orientation.y = 1.0/2**(1/2.0)
target_pose.orientation.w = 1.0/2**(1/2.0)
waypoints.append(target_pose)
#Set the desired orientation for the end effector HERE
request.ik_request.pose_stamped.pose = target_pose
try:
#Send the request to the service
response = compute_ik(request)
group = MoveGroupCommander("right_arm")
#Creating a Robot Trajectory for the Path Planning
jump_thres = 0.0
eef_step = 0.01
path,fraction = group.compute_cartesian_path(waypoints, eef_step, jump_thres)
print("Path fraction: {}".format(fraction))
#Setting position and orientation target
group.set_pose_target(request.ik_request.pose_stamped)
#Setting the Joint constraint
group.set_path_constraints(constraints)
if fraction < 0.5:
group.go()
else:
group.execute(path)
except rospy.ServiceException, e:
print "Service call failed: %s"%e
class PathPlanner:
"""
This path planner wraps the planning and actuation functionality provided by MoveIt.
All positions are arrays holding x, y, and z coordinates. Orientations are
arrays holding x, y, z, and w coordinates (in quaternion form).
Attributes:
frame_id (str): The frame all coordinates are relative to.
workspace (list): The bounds of the planning space (a box). Specified
as the minimum x, y, and z coordinates, then the maximum x, y, and
z coordinates.
group_name (str): The MoveIt group name (for example, 'right_arm').
time_limit (float): The maximum number of seconds MoveIt will plan for.
robot: The MoveIt robot commander.
scene: The planning scene.
group: The MoveIt MoveGroup.
scene_publisher: A publisher that updates the planning scene.
"""
PLANNING_SCENE_TOPIC = '/collision_object'
def __init__(self, frame_id, group_name, time_limit=10, workspace=None,
register_shutdown=True):
if workspace is None:
workspace = [-2, -2, -2, 2, 2, 2]
if rospy.get_param('verbose'):
rospy.loginfo('Move group: {}'.format(group_name))
self.frame_id, self.workspace = frame_id, workspace
self.time_limit, self.group_name = time_limit, group_name
self.robot = RobotCommander()
self.scene = PlanningSceneInterface()
self.scene_publisher = rospy.Publisher(self.PLANNING_SCENE_TOPIC,
CollisionObject, queue_size=10)
self.group = MoveGroupCommander(group_name)
if register_shutdown:
rospy.on_shutdown(self.shutdown)
self.group.set_planning_time(time_limit)
self.group.set_workspace(workspace)
rospy.sleep(0.5) # Sleep to ensure initialization has finished.
if rospy.get_param('verbose'):
rospy.loginfo('Initialized path planner.')
def shutdown(self):
""" Stop the path planner safely. """
self.group.stop()
del self.group
if rospy.get_param('verbose'):
rospy.logwarn('Terminated path planner.')
def move_to_pose(self, position, orientation=None):
"""
Move the end effector to the given pose naively.
Arguments:
position: The x, y, and z coordinates to move to.
orientation: The orientation to take (quaternion, optional).
Raises (rospy.ServiceException): Failed to execute movement.
"""
if orientation is None:
self.group.set_position_target(position)
else:
self.group.set_pose_target(create_pose(self.frame_id, position, orientation))
if rospy.get_param('verbose'):
log_pose('Moving to pose.', position, orientation)
self.group.go(wait=True)
self.group.stop()
self.group.clear_pose_targets()
def move_to_pose_with_planner(self, position, orientation=None, orientation_constraints=None):
"""
Move the end effector to the given pose, taking into account
constraints and planning scene obstacles.
Arguments:
position: The x, y, and z coordinates to move to.
orientation: The orientation to take (quaternion, optional).
orientation_constraints (list): A list of `OrientationConstraint`
objects created with `make_orientation_constraint`.
Returns (bool): Whether or not the movement executed successfully.
"""
if orientation_constraints is None:
orientation_constraints = []
target = create_pose(self.frame_id, position, orientation)
if rospy.get_param('verbose'):
log_pose('Moving to pose with planner.', position, orientation)
for _ in range(3):
try:
plan = self.plan_to_pose(target, orientation_constraints)
if not self.group.execute(plan, True):
raise ValueError('Execution failed.')
except ValueError as exc:
rospy.logwarn('Failed to perform movement: {}. Retrying.'.format(str(exc)))
else:
break
else:
raise ValueError('Failed to move to pose.')
def plan_to_pose(self, target, orientation_constraints):
"""
Plan a movement to a pose from the current state, given constraints.
Arguments:
target: The destination pose.
orientation_constraints (list): The constraints.
Returns (moveit_msgs.msg.RobotTrajectory): The path.
"""
self.group.set_pose_target(target)
self.group.set_start_state_to_current_state()
constraints = Constraints()
constraints.orientation_constraints = orientation_constraints
self.group.set_path_constraints(constraints)
return self.group.plan()
def add_box_obstacle(self, dimensions, name, com_position, com_orientation):
"""
Add a rectangular prism obstacle to the planning scene.
Arguments:
dimensions: An array containing the width, length, and height of
the box (in the box's body frame, corresponding to x, y, and z).
name: A unique name for identifying this obstacle.
com_position: The position of the center-of-mass (COM) of this box,
relative to the global frame `frame_id`.
com_orientation: The orientation of the COM.
"""
pose = create_pose(self.frame_id, com_position, com_orientation)
obj = CollisionObject()
obj.id, obj.operation, obj.header = name, CollisionObject.ADD, pose.header
box = SolidPrimitive()
box.type, box.dimensions = SolidPrimitive.BOX, dimensions
obj.primitives, obj.primitive_poses = [box], [pose.pose]
self.scene_publisher.publish(obj)
if rospy.get_param('verbose'):
rospy.loginfo('Added box object "{}" to planning scene: '
'(x={}, y={}, z={}).'.format(name, *dimensions))
def remove_obstacle(self, name):
""" Remove a named obstacle from the planning scene. """
obj = CollisionObject()
obj.id, obj.operation = name, CollisionObject.REMOVE
self.scene_publisher.publish(obj)
if rospy.get_param('verbose'):
rospy.loginfo('Removed object "{}" from planning scene.'.format(name))
def make_orientation_constraint(self, orientation, link_id, tolerance=0.1, weight=1):
"""
Make an orientation constraint in the context of the robot and world.
Arguments:
orientation: The orientation the link should have.
link_id: The name of the link frame.
Returns (OrientationConstraint): The constraint.
"""
constraint = OrientationConstraint()
constraint.header.frame_id = self.frame_id
constraint.link_name = link_id
const_orien = constraint.orientation
const_orien.x, const_orien.y, const_orien.z, const_orien.w = orientation
constraint.absolute_x_axis_tolerance = tolerance
constraint.absolute_y_axis_tolerance = tolerance
constraint.absolute_z_axis_tolerance = tolerance
constraint.weight = weight
return constraint
def __init__(self):
moveit_commander.roscpp_initialize(sys.argv)
rospy.init_node('moveit_demo')
scene = PlanningSceneInterface('world')
self.scene_pub = rospy.Publisher('planning_scene', PlanningScene)
# self.gripper_pose_pub = rospy.Publisher('gripper_pose', PoseStamped)
self.colors = dict()
max_pick_attempts = 10
max_place_attempts = 10
rospy.sleep(1)
arm = MoveGroupCommander('arm')
end_effector_link = arm.get_end_effector_link()
# arm.set_goal_position_tolerance(0.02)
# arm.set_goal_orientation_tolerance(0.03)
arm.allow_replanning(True)
arm.set_planning_time(5)
table_id = 'table'
side_id = 'side'
table2_id = 'table2'
box1_id = 'box1'
box2_id = 'box2'
target_id = 'target'
wall_id = 'wall'
ground_id = 'ground'
scene.remove_world_object(box1_id)
scene.remove_world_object(box2_id)
scene.remove_world_object(table_id)
scene.remove_world_object(table2_id)
scene.remove_world_object(target_id)
scene.remove_world_object(wall_id)
scene.remove_world_object(ground_id)
scene.remove_world_object(side_id)
rospy.sleep(1)
table_ground = 0.55
table_size = [0.2, 0.7, 0.01]
table2_size = [0.25, 0.6, 0.01]
box1_size = [0.02, 0.9, 0.6]
box2_size = [0.05, 0.05, 0.15]
target_size = [0.03, 0.06, 0.10]
wall_size = [0.01, 0.9, 0.6]
ground_size = [3, 3, 0.01]
side_size = [0.01, 0.7, table_ground]
table_pose = PoseStamped()
table_pose.header.frame_id = REFERENCE_FRAME
self.setPose(table_pose,
[0.7, 0.0, table_ground + table_size[2] / 2.0])
scene.add_box(table_id, table_pose, table_size)
side_pose = PoseStamped()
side_pose.header.frame_id = REFERENCE_FRAME
self.setPose(side_pose, [0.605, 0.0, side_size[2] / 2.0])
scene.add_box(side_id, side_pose, side_size)
table2_pose = PoseStamped()
table2_pose.header.frame_id = REFERENCE_FRAME
self.setPose(table2_pose,
[-0.325, 0.0, box1_size[2] + table2_size[2] / 2.0])
# scene.add_box(table2_id, table2_pose, table2_size)
box1_pose = PoseStamped()
box1_pose.header.frame_id = REFERENCE_FRAME
self.setPose(box1_pose, [-0.36, 0.0, box1_size[2] / 2.0])
# scene.add_box(box1_id, box1_pose, box1_size)
box2_pose = PoseStamped()
box2_pose.header.frame_id = REFERENCE_FRAME
self.setPose(
box2_pose,
[0.63, -0.12, table_ground + table_size[2] + box2_size[2] / 2.0])
# scene.add_box(box2_id, box2_pose, box2_size)
target_pose = PoseStamped()
target_pose.header.frame_id = REFERENCE_FRAME
self.setPose(
target_pose,
[0.62, 0.0, table_ground + table_size[2] + target_size[2] / 2.0])
scene.add_box(target_id, target_pose, target_size)
wall_pose = PoseStamped()
wall_pose.header.frame_id = REFERENCE_FRAME
self.setPose(
wall_pose,
[-0.405, 0.0, box1_size[2] + table2_size[2] + wall_size[2] / 2.0])
# scene.add_box(wall_id, wall_pose, wall_size)
ground_pose = PoseStamped()
ground_pose.header.frame_id = REFERENCE_FRAME
self.setPose(ground_pose, [0.0, 0.0, -ground_size[2] / 2.0])
# scene.add_box(ground_id, ground_pose, ground_size)
self.setColor(table_id, 0.8, 0.0, 0.0, 1.0)
# self.setColor(table2_id, 0.8, 0.0, 0.0)
# self.setColor(box1_id, 0.9, 0.9, 0.9)
# self.setColor(box2_id, 0.8, 0.4, 1.0)
self.setColor(target_id, 0.5, 0.4, 1.0)
# self.setColor(wall_id, 0.9, 0.9, 0.9)
self.setColor(ground_id, 0.3, 0.3, 0.3, 1.0)
self.setColor(side_id, 0.8, 0.0, 0.0, 1.0)
self.sendColors()
constraints = Constraints()
constraints.name = 'gripper constraint'
orientation_constraint = OrientationConstraint()
arm.set_support_surface_name(table_id)
test_pose = PoseStamped()
test_pose.header.frame_id = REFERENCE_FRAME
test_orientation = Quaternion()
test_orientation = quaternion_from_euler(0.0, 1.57, -3.14)
self.setPose(test_pose, [
-0.15, -0.22, box1_size[2] + table2_size[2] + target_size[2] / 2.0
], list(test_orientation))
place_pose = PoseStamped()
place_pose.header.frame_id = REFERENCE_FRAME
self.setPose(
place_pose,
[0.62, -0.22, table_ground + table_size[2] + target_size[2] / 2.0])
grasp_pose = target_pose
grasp_init_orientation = Quaternion()
grasp_init_orientation = quaternion_from_euler(0.0, 1.57, 0.0)
grasp_pose.pose.orientation.x = grasp_init_orientation[0]
grasp_pose.pose.orientation.y = grasp_init_orientation[1]
grasp_pose.pose.orientation.z = grasp_init_orientation[2]
grasp_pose.pose.orientation.w = grasp_init_orientation[3]
grasp_pose.pose.position.x -= 0.02
rospy.loginfo('quaterion: ' + str(grasp_pose.pose.orientation))
place_pose.pose.orientation.x = grasp_init_orientation[0]
place_pose.pose.orientation.y = grasp_init_orientation[1]
place_pose.pose.orientation.z = grasp_init_orientation[2]
place_pose.pose.orientation.w = grasp_init_orientation[3]
orientation_constraint.header = grasp_pose.header
orientation_constraint.link_name = end_effector_link
orientation_constraint.orientation.x = grasp_init_orientation[0]
orientation_constraint.orientation.y = grasp_init_orientation[1]
orientation_constraint.orientation.z = grasp_init_orientation[2]
orientation_constraint.orientation.w = grasp_init_orientation[3]
orientation_constraint.absolute_x_axis_tolerance = 0.03
orientation_constraint.absolute_y_axis_tolerance = 0.03
orientation_constraint.absolute_z_axis_tolerance = 3.14
orientation_constraint.weight = 1.0
constraints.orientation_constraints.append(orientation_constraint)
grasps = self.make_grasps(grasp_pose, [table_id],
[0.05, 0.07, [1.0, 0.0, 0.0]],
[0.22, 0.26, [-1.0, 0.0, 0.0]])
result = None
n_attempts = 0
while result != MoveItErrorCodes.SUCCESS and n_attempts < max_pick_attempts:
result = arm.pick(target_id, grasps)
n_attempts += 1
rospy.loginfo('Pick attempt:' + str(n_attempts))
rospy.sleep(0.2)
arm.set_path_constraints(constraints)
arm.set_pose_target(place_pose)
arm.go()
'''
if result == MoveItErrorCodes.SUCCESS:
result = None
n_attempts = 0
places = self.make_places(place_pose, [table_id], [0.8, 0.13, [1.0, 0.0, 0.0]], [0.1, 0.12, [-1.0, 0.0, 0.0]])
while result != MoveItErrorCodes.SUCCESS and n_attempts < max_place_attempts:
for place in places:
result = arm.place(target_id, place)
if result == MoveItErrorCodes.SUCCESS:
break
n_attempts += 1
rospy.loginfo('Place attempt:' + str(n_attempts))
rospy.sleep(0.2)
'''
'''
grasp_pose = place_pose
grasp_pose.pose.orientation.x = grasp_init_orientation[0]
grasp_pose.pose.orientation.y = grasp_init_orientation[1]
grasp_pose.pose.orientation.z = grasp_init_orientation[2]
grasp_pose.pose.orientation.w = grasp_init_orientation[3]
grasp_pose.pose.position.x -= 0.02
grasps = self.make_grasps(grasp_pose, [table_id], [0.05, 0.07, [1.0, 0.0, 0.0]], [0.15, 0.20, [-1.0, 0.0, 0.0]])
result = None
n_attempts = 0
while result != MoveItErrorCodes.SUCCESS and n_attempts < max_pick_attempts:
result = arm.pick(target_id, grasps)
n_attempts += 1
rospy.loginfo('Pick attempt:' + str(n_attempts))
rospy.sleep(0.2)
'''
'''
arm.set_support_surface_name(table2_id)
self.setPose(place_pose, [-0.26, -0.22, box1_size[2] + table2_size[2] + target_size[2]/2.0])
if result == MoveItErrorCodes.SUCCESS:
result = None
n_attempts = 0
places = self.make_places(place_pose, [table2_id], [0.03, 0.06, [-1.0, 0.0, 0.0]], [0.1, 0.15, [1.0, 0.0, 0.0]])
while result != MoveItErrorCodes.SUCCESS and n_attempts < max_place_attempts:
for place in places:
result = arm.place(target_id, place)
if result == MoveItErrorCodes.SUCCESS:
break
n_attempts += 1
rospy.loginfo('Place attempt:' + str(n_attempts))
rospy.sleep(0.2)
'''
rospy.sleep(2)
moveit_commander.roscpp_shutdown()
moveit_commander.os._exit(0)
def __init__(self):
# Initialize the move_group API
moveit_commander.roscpp_initialize(sys.argv)
# Initialize the ROS node
rospy.init_node('moveit_constraints_demo', anonymous=True)
robot = RobotCommander()
# Connect to the arm move group
arm = MoveGroupCommander(GROUP_NAME_ARM)
# Initialize the move group for the right gripper
gripper = MoveGroupCommander(GROUP_NAME_GRIPPER)
# Increase the planning time since constraint planning can take a while
arm.set_planning_time(5)
# Allow replanning to increase the odds of a solution
arm.allow_replanning(True)
# Set the right arm reference frame
arm.set_pose_reference_frame(REFERENCE_FRAME)
# Allow some leeway in position(meters) and orientation (radians)
arm.set_goal_position_tolerance(0.05)
arm.set_goal_orientation_tolerance(0.1)
# Get the name of the end-effector link
end_effector_link = arm.get_end_effector_link()
# Start in the "resting" configuration stored in the SRDF file
arm.set_named_target('l_arm_init')
# Plan and execute a trajectory to the goal configuration
arm.go()
rospy.sleep(1)
# Open the gripper
gripper.set_joint_value_target(GRIPPER_NEUTRAL)
gripper.go()
rospy.sleep(1)
# Set an initial target pose with the arm up and to the right
target_pose = PoseStamped()
target_pose.header.frame_id = REFERENCE_FRAME
target_pose.pose.position.x = 0.263803774718
target_pose.pose.position.y = 0.295405791959
target_pose.pose.position.z = 0.690438884208
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 start state and target pose, then plan and execute
arm.set_start_state(robot.get_current_state())
arm.set_pose_target(target_pose, end_effector_link)
arm.go()
rospy.sleep(2)
# Close the gripper
gripper.set_joint_value_target(GRIPPER_CLOSED)
gripper.go()
rospy.sleep(1)
# Store the current pose
start_pose = arm.get_current_pose(end_effector_link)
# Create a contraints list and give it a name
constraints = Constraints()
constraints.name = "Keep gripper horizontal"
# Create an orientation constraint for the right gripper
orientation_constraint = OrientationConstraint()
orientation_constraint.header = start_pose.header
orientation_constraint.link_name = arm.get_end_effector_link()
orientation_constraint.orientation.w = 1.0
orientation_constraint.absolute_x_axis_tolerance = 0.1
orientation_constraint.absolute_y_axis_tolerance = 0.1
orientation_constraint.absolute_z_axis_tolerance = 0.1
orientation_constraint.weight = 1.0
# q = quaternion_from_euler(0, 0, -1.57079633)
# orientation_constraint.orientation.x = q[0]
# orientation_constraint.orientation.y = q[1]
# orientation_constraint.orientation.z = q[2]
# orientation_constraint.orientation.w = q[3]
# Append the constraint to the list of contraints
constraints.orientation_constraints.append(orientation_constraint)
# Set the path constraints on the arm
arm.set_path_constraints(constraints)
# Set a target pose for the arm
target_pose = PoseStamped()
target_pose.header.frame_id = REFERENCE_FRAME
target_pose.pose.position.x = 0.39000848183
target_pose.pose.position.y = 0.185900663329
target_pose.pose.position.z = 0.732752341378
target_pose.pose.orientation.w = 1
# Set the start state and target pose, then plan and execute
arm.set_start_state_to_current_state()
arm.set_pose_target(target_pose, end_effector_link)
arm.go()
rospy.sleep(1)
# Clear all path constraints
arm.clear_path_constraints()
# Open the gripper
gripper.set_joint_value_target(GRIPPER_NEUTRAL)
gripper.go()
rospy.sleep(1)
# Return to the "resting" configuration stored in the SRDF file
arm.set_named_target('l_arm_init')
# Plan and execute a trajectory to the goal configuration
arm.go()
rospy.sleep(1)
# Shut down MoveIt cleanly
moveit_commander.roscpp_shutdown()
# Exit MoveIt
moveit_commander.os._exit(0)
class LinkProxy():
def __init__(self, arm):
self._arm = arm
self._group_name = None
self._group = None
self._planner = None
self.orientation_frozen = False
def _initialize_group(self):
# get list of planners available to this move group
available_planners = rospy.get_param(
"/move_group/%s/planner_configs" % self._group_name, None)
if available_planners is None:
ROS_ERR("The Move group `%s` does not have planners configured.",
self._group_name)
return False
if self._planner not in available_planners:
ROS_WARN(
"The planner `%s` is not available for the group `%s`, using `%s` instead.",
self._planner, self._group_name, available_planners[0])
self._planner = available_planners[0]
self._group = MoveGroupCommander(self._group_name)
# set planner
self._group.set_planner_id(self._planner)
def _is_group_valid(self):
if self._group is None:
if isinstance(self, TerminalLink) or isinstance(
self, IntermediateLink):
ROS_ERR("Link object was created without a group")
return False
else:
ROS_ERR("Cannot instantiate abstract classes")
return False
return True
def plan_to_pose(self, pose, *args):
raise NotImplentedError()
def plan_cartesian_path(self, waypoints, *args):
raise NotImplentedError()
def execute_plan(self, plan):
if self._is_group_valid():
return self._arm._execute_plan(self._group, plan)
return False
def get_pose(self):
return self._group.get_current_pose().pose
def is_at(self, pose, epsilon):
cur_pose = self.get_pose()
cur_position = [
cur_pose.position.x, cur_pose.position.y, cur_pose.position.z
]
goal_position = [pose.position.x, pose.position.y, pose.position.z]
eucl_dist = np.linalg.norm(
np.array(cur_position) - np.array(goal_position))
return eucl_dist < epsilon
def get_planning_frame(self):
if self._is_group_valid():
return self._group.get_planning_frame()
return False
def freeze_orientation(self):
# get current pose
cur_pose = self._group.get_current_pose()
# create pose constraint
constraints = Constraints()
constraints.name = "orientation_constraints"
orientation_constraint = OrientationConstraint()
orientation_constraint.header = cur_pose.header
orientation_constraint.header.stamp = rospy.Time.now()
orientation_constraint.link_name = self._group.get_end_effector_link()
orientation_constraint.orientation = cur_pose.pose.orientation
orientation_constraint.absolute_x_axis_tolerance = 0.5
orientation_constraint.absolute_y_axis_tolerance = 0.5
orientation_constraint.absolute_z_axis_tolerance = 0.5
orientation_constraint.weight = 1.0
constraints.orientation_constraints.append(orientation_constraint)
self._group.set_path_constraints(constraints)
self.orientation_frozen = True
def clear_path_constraints(self):
self._group.set_path_constraints(None)
self.orientation_frozen = False
