def create_move_group_pose_goal(self, goal_pose=Pose(), group="manipulator", end_link_name=None, plan_only=True):
header = Header()
header.frame_id = 'torso_base_link'
header.stamp = rospy.Time.now()
moveit_goal = MoveGroupGoal()
goal_c = Constraints()
position_c = PositionConstraint()
position_c.header = header
if end_link_name != None:
position_c.link_name = end_link_name
position_c.constraint_region.primitives.append(SolidPrimitive(type=SolidPrimitive.SPHERE, dimensions=[0.01]))
position_c.constraint_region.primitive_poses.append(goal_pose)
position_c.weight = 1.0
goal_c.position_constraints.append(position_c)
orientation_c = OrientationConstraint()
orientation_c.header = header
if end_link_name != None:
orientation_c.link_name = end_link_name
orientation_c.orientation = goal_pose.orientation
orientation_c.absolute_x_axis_tolerance = 0.01
orientation_c.absolute_y_axis_tolerance = 0.01
orientation_c.absolute_z_axis_tolerance = 0.01
orientation_c.weight = 1.0
goal_c.orientation_constraints.append(orientation_c)
moveit_goal.request.goal_constraints.append(goal_c)
moveit_goal.request.num_planning_attempts = 5
moveit_goal.request.allowed_planning_time = 5.0
moveit_goal.planning_options.plan_only = plan_only
moveit_goal.planning_options.planning_scene_diff.is_diff = True # Necessary
moveit_goal.request.group_name = group
return moveit_goal
def append_pose_to_move_group_goal(goal_to_append=None, goal_pose=Pose(), link_name=None):
""" Appends a pose to the given move_group goal, returns it appended
Goals for now are for the same link TODO: let it be for different links"""
if goal_to_append == None:
rospy.logerr("append_pose_to_move_group_goal needs a goal!")
return
goal_to_append = MoveGroupGoal()
goal_c = Constraints()
position_c = PositionConstraint()
position_c.header = goal_to_append.request.goal_constraints[0].header
position_c.link_name = goal_to_append.request.goal_constraints[0].link_name
position_c.constraint_region.primitives.append(SolidPrimitive(type=SolidPrimitive.SPHERE, dimensions=[0.01]))
position_c.constraint_region.primitive_poses.append(goal_pose)
position_c.weight = 1.0
goal_c.position_constraints.append(position_c)
orientation_c = OrientationConstraint()
orientation_c.header = goal_to_append.request.goal_constraints[0].header
orientation_c.link_name = goal_to_append.request.goal_constraints[0].link_name
orientation_c.orientation = goal_pose.orientation
orientation_c.absolute_x_axis_tolerance = 0.01
orientation_c.absolute_y_axis_tolerance = 0.01
orientation_c.absolute_z_axis_tolerance = 0.01
orientation_c.weight = 1.0
goal_c.orientation_constraints.append(orientation_c)
goal_to_append.request.goal_constraints.append(goal_c)
return goal_to_append
def append_traj_to_move_group_goal(goal_to_append=None, goal_pose=Pose(), link_name=None):
""" Appends a trajectory_point to the given move_group goal, returns it appended"""
if goal_to_append == None:
rospy.logerr("append_trajectory_point_to_move_group_goal needs a goal!")
return
#goal_to_append = MoveGroupGoal()
#rospy.logwarn("goal_to_append is: \n" + str(goal_to_append))
traj_c = TrajectoryConstraints()
goal_c = Constraints()
goal_c.name = "traj_constraint"
# Position constraint
position_c = PositionConstraint()
position_c.header = goal_to_append.request.goal_constraints[0].position_constraints[0].header
position_c.link_name = goal_to_append.request.goal_constraints[0].position_constraints[0].link_name if link_name == None else link_name
position_c.constraint_region.primitives.append(SolidPrimitive(type=SolidPrimitive.SPHERE, dimensions=[0.01]))
position_c.constraint_region.primitive_poses.append(goal_pose)
position_c.weight = 2.0
goal_c.position_constraints.append(position_c)
# Orientation constraint
orientation_c = OrientationConstraint()
orientation_c.header = goal_to_append.request.goal_constraints[0].position_constraints[0].header
orientation_c.link_name = goal_to_append.request.goal_constraints[0].position_constraints[0].link_name if link_name == None else link_name
orientation_c.orientation = goal_pose.orientation
orientation_c.absolute_x_axis_tolerance = 0.01
orientation_c.absolute_y_axis_tolerance = 0.01
orientation_c.absolute_z_axis_tolerance = 0.01
orientation_c.weight = 1.0
goal_c.orientation_constraints.append(orientation_c)
traj_c.constraints.append(goal_c)
goal_to_append.request.trajectory_constraints = traj_c
return goal_to_append
def execute(self, userdata):
# Prepare the position
goal_pose = Pose()
goal_pose.position = userdata.manip_goal_pose
# Set the rotation of the tool link, all 0 means for the right hand palm looking left straight
# roll = rotation over X, pitch = rotation over Y, yaw = rotation over Z
quat = quaternion_from_euler(0.0, 0.0, 0.0) # roll, pitch, yaw
goal_pose.orientation = Quaternion(*quat.tolist())
header = Header()
header.frame_id = 'base_link'
header.stamp = rospy.Time.now()
userdata.move_it_goal = MoveGroupGoal()
goal_c = Constraints()
position_c = PositionConstraint()
position_c.header = header
if userdata.manip_end_link != None: # For some groups the end_link_name can be deduced, but better add it manually
position_c.link_name = userdata.manip_end_link
# how big is the area where the end effector can be
position_c.constraint_region.primitives.append(SolidPrimitive(type=SolidPrimitive.SPHERE, dimensions=[0.01]))
position_c.constraint_region.primitive_poses.append(goal_pose)
position_c.weight = 1.0
goal_c.position_constraints.append(position_c)
orientation_c = OrientationConstraint()
orientation_c.header = header
if userdata.manip_end_link != None:
orientation_c.link_name = userdata.manip_end_link
orientation_c.orientation = goal_pose.orientation
# Tolerances, MoveIt! by default uses 0.001 which may be too low sometimes
orientation_c.absolute_x_axis_tolerance = 0.01
orientation_c.absolute_y_axis_tolerance = 0.01
orientation_c.absolute_z_axis_tolerance = 0.01
orientation_c.weight = 1.0
goal_c.orientation_constraints.append(orientation_c)
userdata.move_it_goal.request.goal_constraints.append(goal_c)
userdata.move_it_goal.request.num_planning_attempts = 5 # The number of times this plan is to be computed. Shortest solution will be reported.
userdata.move_it_goal.request.allowed_planning_time = 5.0
userdata.move_it_goal.planning_options.plan_only = False # True: Plan-Only..... False : Plan + execute
userdata.move_it_goal.planning_options.planning_scene_diff.is_diff = True # Necessary
userdata.move_it_goal.request.group_name = userdata.manip_group
return 'succeeded'
def on_enter(self, userdata):
self._failed = False
request = GetCartesianPathRequest()
request.group_name = self._move_group
request.avoid_collisions = not self._ignore_collisions
request.max_step = 0.05
request.header = userdata.eef_pose.header
request.waypoints.append(userdata.eef_pose.pose)
now = rospy.Time.now()
try:
self._tf.waitForTransform('base_link', 'gripper_cam_link', now, rospy.Duration(1))
(p,q) = self._tf.lookupTransform('gripper_cam_link', 'base_link', now)
c = OrientationConstraint()
c.header.frame_id = 'base_link'
c.header.stamp = now
c.link_name = 'gripper_cam_link'
c.orientation.x = q[0]
c.orientation.y = q[1]
c.orientation.z = q[2]
c.orientation.w = q[3]
c.weight = 1
c.absolute_x_axis_tolerance = 0.1
c.absolute_y_axis_tolerance = 0.1
c.absolute_z_axis_tolerance = 0.1
#request.path_constraints.orientation_constraints.append(c)
self._result = self._srv.call(self._topic, request)
except Exception as e:
Logger.logwarn('Exception while calculating path:\n%s' % str(e))
self._failed = True
def move_to_coord(trans, rot, keep_oreint=False):
#coordinates are in baxter's torso!
global arm
goal = PoseStamped()
goal.header.frame_id = "base"
# x, y, and z position
goal.pose.position.x = trans[0]
goal.pose.position.y = trans[1]
goal.pose.position.z = trans[2]
# Orientation as a quaternion
goal.pose.orientation.x = rot[0]
goal.pose.orientation.y = rot[1]
goal.pose.orientation.z = rot[2]
goal.pose.orientation.w = rot[3]
# Set the goal state to the pose you just defined
arm.set_pose_target(goal)
# Set the start state for the arm
arm.set_start_state_to_current_state()
if keep_oreint:
# Create a path constraint for the arm
orien_const = OrientationConstraint()
orien_const.link_name = 'left'+"_gripper";
orien_const.header.frame_id = "base";
#constrain it to be the same as my goal state. Seems reasonable.
orien_const.orientation.x = rot[0];
orien_const.orientation.y = rot[1];
orien_const.orientation.z = rot[2];
orien_const.orientation.w = rot[3];
orien_const.absolute_x_axis_tolerance = 0.02;
orien_const.absolute_y_axis_tolerance = 0.02;
orien_const.absolute_z_axis_tolerance = 0.02;
orien_const.weight = 1.0;
consts = Constraints()
consts.orientation_constraints = [orien_const]
print(consts)
arm.set_path_constraints(consts)
# Plan a path
arm_plan = arm.plan()
# Execute the plan
#arm.execute(arm_plan)
arm.go(arm_plan,wait=True)
def create_move_group_pose_goal(goal_pose=Pose(), group="right_arm_torso", end_link_name=None, plan_only=True):
""" Creates a move_group goal based on pose.
@arg group string representing the move_group group to use
@arg end_link_name string representing the ending link to use
@arg goal_pose Pose() representing the goal pose
@arg plan_only bool to for only planning or planning and executing
@returns MoveGroupGoal with the data given on the arguments"""
header = Header()
header.frame_id = 'base_link'
header.stamp = rospy.Time.now()
# We are filling in the MoveGroupGoal a MotionPlanRequest and a PlanningOptions message
# http://docs.ros.org/hydro/api/moveit_msgs/html/msg/MotionPlanRequest.html
# http://docs.ros.org/hydro/api/moveit_msgs/html/msg/PlanningOptions.html
moveit_goal = MoveGroupGoal()
goal_c = Constraints()
position_c = PositionConstraint()
position_c.header = header
if end_link_name != None: # For some groups the end_link_name can be deduced, but better add it manually
position_c.link_name = end_link_name
position_c.constraint_region.primitives.append(SolidPrimitive(type=SolidPrimitive.SPHERE, dimensions=[0.01])) # how big is the area where the end effector can be
position_c.constraint_region.primitive_poses.append(goal_pose)
position_c.weight = 1.0
goal_c.position_constraints.append(position_c)
orientation_c = OrientationConstraint()
orientation_c.header = header
if end_link_name != None:
orientation_c.link_name = end_link_name
orientation_c.orientation = goal_pose.orientation
orientation_c.absolute_x_axis_tolerance = 0.01 # Tolerances, MoveIt! by default uses 0.001 which may be too low sometimes
orientation_c.absolute_y_axis_tolerance = 0.01
orientation_c.absolute_z_axis_tolerance = 0.01
orientation_c.weight = 1.0
goal_c.orientation_constraints.append(orientation_c)
moveit_goal.request.goal_constraints.append(goal_c)
moveit_goal.request.num_planning_attempts = 5 # The number of times this plan is to be computed. Shortest solution will be reported.
moveit_goal.request.allowed_planning_time = 5.0
moveit_goal.planning_options.plan_only = plan_only
moveit_goal.planning_options.planning_scene_diff.is_diff = True # Necessary
moveit_goal.request.group_name = group
return moveit_goal
def planMoveToPosHoldOrientation(pos=None, orientation=None, arm=None):
arm = arm or left_arm
if arm is left_arm:
pos = pos or left_arm_default_state[0]
orientation = orientation or left_arm_default_state[1]
if arm is right_arm:
pos = pos or right_arm_default_state[0]
orientation = orientation or right_arm_default_state[1]
#Second goal pose -----------------------------------------------------
goal_2 = PoseStamped()
goal_2.header.frame_id = "base"
#x, y, and z position
goal_2.pose.position.x = pos[0]
goal_2.pose.position.y = pos[1]
goal_2.pose.position.z = pos[2]
#Orientation as a quaternion
orientation = orientation/np.linalg.norm(orientation)
goal_2.pose.orientation.x = orientation[0]
goal_2.pose.orientation.y = orientation[1]
goal_2.pose.orientation.z = orientation[2]
goal_2.pose.orientation.w = orientation[3]
#Set the goal state to the pose you just defined
arm.set_pose_target(goal_2)
#Set the start state for the left arm
arm.set_start_state_to_current_state()
# #Create a path constraint for the arm
# #UNCOMMENT TO ENABLE ORIENTATION CONSTRAINTS
orien_const = OrientationConstraint()
orien_const.link_name = "left_gripper";
orien_const.header.frame_id = "base";
orien_const.orientation.x = orientation[0]
orien_const.orientation.y = orientation[1]
orien_const.orientation.z = orientation[2]
orien_const.orientation.w = orientation[3]
orien_const.absolute_x_axis_tolerance = 0.1;
orien_const.absolute_y_axis_tolerance = 0.1;
orien_const.absolute_z_axis_tolerance = 0.1;
orien_const.weight = 1.0;
consts = Constraints()
consts.orientation_constraints = [orien_const]
arm.set_path_constraints(consts)
#Plan a path
return (arm, arm.plan())
def createOrientationConstraint():
orientation_constraints = []
o = OrientationConstraint()
o.header.frame_id = 'base_link'
o.link_name = 'arm_right_tool_link'
o.orientation.x = 0.0
o.orientation.y = 0.0
o.orientation.z = 0.0
o.orientation.w = 1.0
o.weight = 1.0
o.absolute_x_axis_tolerance = 0.1
o.absolute_y_axis_tolerance = 0.1
o.absolute_z_axis_tolerance = 0.1
orientation_constraints.append(o)
return orientation_constraints
def create_move_group_pose_goal(
self, goal_pose=Pose(), group="left_arm", end_link_name="arm_left_tool_link", plan_only=True
):
""" Creates a move_group goal based on pose.
@arg group string representing the move_group group to use
@arg end_link_name string representing the ending link to use
@arg goal_pose Pose() representing the goal pose"""
# Specifying the header makes the planning fail...
header = Header()
header.frame_id = "base_link"
header.stamp = rospy.Time.now()
moveit_goal = MoveGroupGoal()
goal_c = Constraints()
position_c = PositionConstraint()
position_c.header = header
position_c.link_name = end_link_name
position_c.constraint_region.primitives.append(SolidPrimitive(type=SolidPrimitive.SPHERE, dimensions=[0.01]))
position_c.constraint_region.primitive_poses.append(goal_pose)
position_c.weight = 1.0
goal_c.position_constraints.append(position_c)
orientation_c = OrientationConstraint()
orientation_c.header = header
orientation_c.link_name = end_link_name
orientation_c.orientation = goal_pose.orientation
orientation_c.absolute_x_axis_tolerance = 0.01
orientation_c.absolute_y_axis_tolerance = 0.01
orientation_c.absolute_z_axis_tolerance = 0.01
orientation_c.weight = 1.0
goal_c.orientation_constraints.append(orientation_c)
moveit_goal.request.goal_constraints.append(goal_c)
moveit_goal.request.num_planning_attempts = 3
moveit_goal.request.allowed_planning_time = 1.0
moveit_goal.planning_options.plan_only = plan_only
moveit_goal.planning_options.planning_scene_diff.is_diff = True
moveit_goal.request.group_name = group
return moveit_goal
def planPath(positions, orientation=None, arm=None, holdOrientation=False, step=0.01, threshold=1000):
arm = arm or left_arm
if arm is left_arm:
positions = (left_arm_default_state[0],) if positions is None else positions
orientation = left_arm_default_state[1] if orientation is None else orientation
if arm is right_arm:
positions = (right_arm_default_state[0],) if positions is None else positions
orientation = right_arm_default_state[1] if orientation is None else orientation
# Compute path
waypoints = []
# start with the current pose
#waypoints.append(arm.get_current_pose().pose)
wpose = Pose()
orientation = orientation/np.linalg.norm(orientation)
wpose.orientation.x = orientation[0]
wpose.orientation.y = orientation[1]
wpose.orientation.z = orientation[2]
wpose.orientation.w = orientation[3]
for pos in positions:
wpose.position.x = pos[0]
wpose.position.y = pos[1]
wpose.position.z = pos[2]
waypoints.append(copy.deepcopy(wpose))
if holdOrientation:
# #Create a path constraint for the arm
# #UNCOMMENT TO ENABLE ORIENTATION CONSTRAINTS
orien_const = OrientationConstraint()
orien_const.link_name = "left_gripper";
orien_const.header.frame_id = "base";
orien_const.orientation.x = orientation[0]
orien_const.orientation.y = orientation[1]
orien_const.orientation.z = orientation[2]
orien_const.orientation.w = orientation[3]
orien_const.absolute_x_axis_tolerance = 0.1;
orien_const.absolute_y_axis_tolerance = 0.1;
orien_const.absolute_z_axis_tolerance = 0.1;
orien_const.weight = 1.0;
consts = Constraints()
consts.orientation_constraints = [orien_const]
arm.set_path_constraints(consts)
#Plan a path
(plan, fraction) = arm.compute_cartesian_path(waypoints, step, threshold)
return (arm, plan, fraction)
def move_to_coord(trans, rot, arm, which_arm='left', keep_oreint=False):
goal = PoseStamped()
goal.header.frame_id = "base"
# x, y, and z position
goal.pose.position.x = trans[0]
goal.pose.position.y = trans[1]
goal.pose.position.z = trans[2]
# Orientation as a quaternion
goal.pose.orientation.x = rot[0]
goal.pose.orientation.y = rot[1]
goal.pose.orientation.z = rot[2]
goal.pose.orientation.w = rot[3]
# Set the goal state to the pose you just defined
arm.set_pose_target(goal)
# Set the start state for the arm
arm.set_start_state_to_current_state()
if keep_oreint:
# Create a path constraint for the arm
orien_const = OrientationConstraint()
orien_const.link_name = which_arm+"_gripper";
orien_const.header.frame_id = "base";
orien_const.orientation.y = -1.0;
orien_const.absolute_x_axis_tolerance = 0.1;
orien_const.absolute_y_axis_tolerance = 0.1;
orien_const.absolute_z_axis_tolerance = 0.1;
orien_const.weight = 1.0;
consts = Constraints()
consts.orientation_constraints = [orien_const]
arm.set_path_constraints(consts)
# Plan a path
arm_plan = arm.plan()
# Execute the plan
arm.execute(arm_plan)
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 motionPlanToPose(self, pose, tolerance=0.01, wait=True, **kwargs):
"""
Move the arm to set of joint position goals
:param joints: joint names for which the target position
is specified.
:param positions: target joint positions
:param tolerance: allowed tolerance in the final joint positions.
:param wait: if enabled, makes the fuctions wait until the
target joint position is reached
:type joints: list of string element type
:type positions: list of float element type
:type tolerance: float
:type wait: bool
"""
# Check arguments
supported_args = (
"max_velocity_scaling_factor",
"max_acceleration_scaling_factor",
"planner_id",
"planning_scene_diff",
"planning_time",
"plan_only",
"start_state",
)
for arg in kwargs.keys():
if not arg in supported_args:
rospy.loginfo("motionPlanToPose: unsupported argument: %s", arg)
# Create goal
g = MotionPlanRequest()
# 1. fill in request workspace_parameters
# 2. fill in request start_state
try:
g.start_state = kwargs["start_state"]
except KeyError:
g.start_state.is_diff = True
# 3. fill in request goal_constraints
c1 = Constraints()
c1.position_constraints.append(PositionConstraint())
c1.position_constraints[0].header.frame_id = self._fixed_frame
c1.position_constraints[0].link_name = self._gripper_frame
# c1.position_constraints[0].target_point_offset
b = BoundingVolume()
s = SolidPrimitive()
s.dimensions = [tolerance]
s.type = s.SPHERE
b.primitives.append(s)
b.primitive_poses.append(pose)
c1.position_constraints[0].constraint_region = b
c1.position_constraints[0].weight = 1.0
c1.orientation_constraints.append(OrientationConstraint())
c1.orientation_constraints[0].header.frame_id = self._fixed_frame
c1.orientation_constraints[0].orientation = pose.orientation
c1.orientation_constraints[0].link_name = self._gripper_frame
c1.orientation_constraints[0].absolute_x_axis_tolerance = tolerance
c1.orientation_constraints[0].absolute_y_axis_tolerance = tolerance
c1.orientation_constraints[0].absolute_z_axis_tolerance = tolerance
c1.orientation_constraints[0].weight = 1.0
g.goal_constraints.append(c1)
# 4. fill in request path constraints
# 5. fill in request trajectory constraints
# 6. fill in request planner id
try:
g.planner_id = kwargs["planner_id"]
except KeyError:
if self.planner_id:
g.planner_id = self.planner_id
# 7. fill in request group name
g.group_name = self._group
# 8. fill in request number of planning attempts
try:
g.num_planning_attempts = kwargs["num_attempts"]
except KeyError:
g.num_planning_attempts = 1
# 9. fill in request allowed planning time
try:
g.allowed_planning_time = kwargs["planning_time"]
except KeyError:
g.allowed_planning_time = self.planning_time
# 10. Fill in velocity scaling factor
try:
g.max_velocity_scaling_factor = kwargs["max_velocity_scaling_factor"]
except KeyError:
pass # do not fill in at all
# 11. Fill in acceleration scaling factor
try:
g.max_velocity_scaling_factor = kwargs["max_acceleration_scaling_factor"]
except KeyError:
pass # do not fill in at all
result = self._mp_service(g)
traj = result.motion_plan_response.trajectory.joint_trajectory.points
if len(traj) < 1:
rospy.logwarn("No motion plan found.")
return None
return traj
def moveToPose(self, pose, tolerance=0.01, wait=True, **kwargs):
"""
Move the arm, based on a goal pose_stamped for the end effector.
:param pose: target pose to which we want to move
specified link to
:param tolerance: allowed tolerance in the final joint positions.
:param wait: if enabled, makes the fuctions wait until the
target joint position is reached
:type pose_stamped: ros message object of type PoseStamped
:type gripper_frame: string
:type tolerance: float
:type wait: bool
"""
# Check arguments
supported_args = (
"max_velocity_scaling_factor",
"planner_id",
"planning_time",
"plan_only",
"start_state",
)
for arg in kwargs.keys():
if not arg in supported_args:
rospy.loginfo("moveToPose: unsupported argument: %s", arg)
# Create goal
g = MoveGroupGoal()
# 1. fill in request workspace_parameters
# 2. fill in request start_state
try:
g.request.start_state = kwargs["start_state"]
except KeyError:
g.request.start_state.is_diff = True
# 3. fill in request goal_constraints
c1 = Constraints()
c1.position_constraints.append(PositionConstraint())
c1.position_constraints[0].header.frame_id = self._fixed_frame
c1.position_constraints[0].link_name = self._gripper_frame
# c1.position_constraints[0].target_point_offset
b = BoundingVolume()
s = SolidPrimitive()
s.dimensions = [tolerance]
s.type = s.SPHERE
b.primitives.append(s)
b.primitive_poses.append(pose)
c1.position_constraints[0].constraint_region = b
c1.position_constraints[0].weight = 1.0
c1.orientation_constraints.append(OrientationConstraint())
c1.orientation_constraints[0].header.frame_id = self._fixed_frame
c1.orientation_constraints[0].orientation = pose.orientation
c1.orientation_constraints[0].link_name = self._gripper_frame
c1.orientation_constraints[0].absolute_x_axis_tolerance = tolerance
c1.orientation_constraints[0].absolute_y_axis_tolerance = tolerance
c1.orientation_constraints[0].absolute_z_axis_tolerance = tolerance
c1.orientation_constraints[0].weight = 1.0
g.request.goal_constraints.append(c1)
# 4. fill in request path constraints
# 5. fill in request trajectory constraints
# 6. fill in request planner id
try:
g.request.planner_id = kwargs["planner_id"]
except KeyError:
if self.planner_id:
g.request.planner_id = self.planner_id
# 7. fill in request group name
g.request.group_name = self._group
# 8. fill in request number of planning attempts
try:
g.request.num_planning_attempts = kwargs["num_attempts"]
except KeyError:
g.request.num_planning_attempts = 1
# 9. fill in request allowed planning time
try:
g.request.allowed_planning_time = kwargs["planning_time"]
except KeyError:
g.request.allowed_planning_time = self.planning_time
# Fill in velocity scaling factor
try:
g.request.max_velocity_scaling_factor = kwargs[
"max_velocity_scaling_factor"
]
except KeyError:
pass # do not fill in at all
# 10. fill in planning options diff
g.planning_options.planning_scene_diff.is_diff = True
g.planning_options.planning_scene_diff.robot_state.is_diff = True
# 11. fill in planning options plan only
try:
g.planning_options.plan_only = kwargs["plan_only"]
except KeyError:
g.planning_options.plan_only = self.plan_only
# 12. fill in other planning options
g.planning_options.look_around = False
g.planning_options.replan = False
# 13. send goal
self._action.send_goal(g)
if wait:
self._action.wait_for_result()
result = self._action.get_result()
return processResult(result)
else:
rospy.loginfo("Failed while waiting for action result.")
return False
import traceback from moveit_msgs.msg import OrientationConstraint from geometry_msgs.msg import PoseStamped from path_planner import PathPlanner # from planning import path_planner # from path_planner import PathPlanner from baxter_interface import gripper as robot_gripper import baxter_dataflow from threading import Thread NEUTRAL_X_LEFT, NEUTRAL_Y_LEFT, NEUTRAL_Z_LEFT = 0.85, 0.3001, -0.04 NEUTRAL_X_RIGHT, NEUTRAL_Y_RIGHT, NEUTRAL_Z_RIGHT = 0.85, -0.2995, -0.044 BOARD_LEN_Y = 0.50 MOVE_RATE = 0.98 orien_const_left_vert = OrientationConstraint() orien_const_left_vert.link_name = "left_gripper"; orien_const_left_vert.header.frame_id = "base"; orien_const_left_vert.orientation.y = -1.0; orien_const_left_vert.absolute_x_axis_tolerance = 0.1; orien_const_left_vert.absolute_y_axis_tolerance = 0.1; orien_const_left_vert.absolute_z_axis_tolerance = 0.1; orien_const_left_vert.weight = 1.0; orien_const_right_vert = OrientationConstraint() orien_const_right_vert.link_name = "right_gripper"; orien_const_right_vert.header.frame_id = "base"; orien_const_right_vert.orientation.y = -1.0; orien_const_right_vert.absolute_x_axis_tolerance = 0.1; orien_const_right_vert.absolute_y_axis_tolerance = 0.1; orien_const_right_vert.absolute_z_axis_tolerance = 0.1;
def callback(message):
global last_x
global last_y
global last_z
global error
try:
if message.transforms[0].child_frame_id == 'ar_marker_23':
#Read the position of artag
(trans,rot) = listener.lookupTransform('/base', '/ar_marker_23', rospy.Time(0))
#Execute only when the difference of the current position and the last position exceed the threshold
if ((abs(trans[0]-last_x) < threshold) and (abs(trans[1]-last_y) < threshold) and (abs(trans[2]-last_z) < threshold)):
pass
else:
last_x = trans[0]
last_y = trans[1]
last_z = trans[2]
print trans
#Goal position
goal = PoseStamped()
goal.header.frame_id = "base"
#x, y, and z position
goal.pose.position.x = trans[0]
goal.pose.position.y = trans[1]
goal.pose.position.z = trans[2]-0.1
#Orientation as a quaternion: default orientation
goal.pose.orientation.x = 0.5
goal.pose.orientation.y = 0.5
goal.pose.orientation.z = 0.5
goal.pose.orientation.w = -0.5
#Set the goal state
right_arm.set_pose_target(goal)
#Set the start state
right_arm.set_start_state_to_current_state()
#Create a orientation constraint for the arm
orien_const = OrientationConstraint()
orien_const.link_name = "right_gripper";
orien_const.header.frame_id = "base";
orien_const.orientation.x = 0.5;
orien_const.orientation.y = 0.5;
orien_const.orientation.z = 0.5;
orien_const.orientation.w = -0.5;
orien_const.absolute_x_axis_tolerance = 0.1;
orien_const.absolute_y_axis_tolerance = 0.1;
orien_const.absolute_z_axis_tolerance = 0.1;
orien_const.weight = 1.0;
consts = Constraints()
consts.orientation_constraints = [orien_const]
right_arm.set_path_constraints(consts)
#Plan a path
right_plan = right_arm.plan()
#Execute the plan
right_arm.execute(right_plan)
else:
pass
except (tf.LookupException, tf.ConnectivityException, tf.ExtrapolationException):
print 'exception'
def both_arms_go_to_pose_goal(self):
# 设置动作对象变量,此处为both_arms
both_arms = self.both_arms
# 获取当前各轴转角
axis_angle = both_arms.get_current_joint_values()
# print axis_angle
# 获取当前末端执行器位置姿态
right_pose_goal = both_arms.get_current_pose(end_effector_link="gripper_r_finger_r")
left_pose_goal = both_arms.get_current_pose(end_effector_link="gripper_l_finger_r")
print right_pose_goal
# 限制末端夹具运动
right_joint_const = JointConstraint()
right_joint_const.joint_name = "gripper_r_joint_r"
if Rightfinger > -55 :
right_joint_const.position = 0.024
else:
right_joint_const.position = 0
left_joint_const = JointConstraint()
left_joint_const.joint_name = "gripper_l_joint_r"
if Leftfinger > -32 :
left_joint_const.position = 0.024
else:
left_joint_const.position = 0
# 添加末端姿态约束:
right_orientation_const = OrientationConstraint()
right_orientation_const.header = Header()
right_orientation_const.orientation = right_pose_goal.pose.orientation
right_orientation_const.link_name = "gripper_r_joint_r"
right_orientation_const.absolute_x_axis_tolerance = 0.6
right_orientation_const.absolute_y_axis_tolerance = 0.6
right_orientation_const.absolute_z_axis_tolerance = 0.6
right_orientation_const.weight = 1
left_orientation_const = OrientationConstraint()
left_orientation_const.header = Header()
left_orientation_const.orientation = left_pose_goal.pose.orientation
left_orientation_const.link_name = "gripper_l_joint_r"
left_orientation_const.absolute_x_axis_tolerance = 0.6
left_orientation_const.absolute_y_axis_tolerance = 0.6
left_orientation_const.absolute_z_axis_tolerance = 0.6
left_orientation_const.weight = 1
# 施加全约束
consts = Constraints()
consts.joint_constraints = [right_joint_const, left_joint_const]
# consts.orientation_constraints = [right_orientation_const, left_orientation_const]
both_arms.set_path_constraints(consts)
# # 设置动作对象目标位置姿态
# # 右臂
# right_pose_goal.pose.orientation.x = Right_Qux
# right_pose_goal.pose.orientation.y = Right_Quy
# right_pose_goal.pose.orientation.z = Right_Quz
# right_pose_goal.pose.orientation.w = Right_Quw
# right_pose_goal.pose.position.x = Neurondata[5]
# right_pose_goal.pose.position.y = Neurondata[3]
# right_pose_goal.pose.position.z = Neurondata[4]
# # 左臂
# left_pose_goal.pose.orientation.x = Left_Qux
# left_pose_goal.pose.orientation.y = Left_Quy
# left_pose_goal.pose.orientation.z = Left_Quz
# left_pose_goal.pose.orientation.w = Left_Quw
# left_pose_goal.pose.position.x = Neurondata[11]
# left_pose_goal.pose.position.y = Neurondata[9]
# left_pose_goal.pose.position.z = Neurondata[10]
# # 右臂
# right_pose_goal.pose.orientation.x = Right_Qux
# right_pose_goal.pose.orientation.y = Right_Quy
# right_pose_goal.pose.orientation.z = Right_Quz
# right_pose_goal.pose.orientation.w = Right_Quw
# right_pose_goal.pose.position.x = (1266/740)*(Neurondata[5]+0.28)-0.495
# right_pose_goal.pose.position.y = (1295/780)*(Neurondata[3]+0.56)-0.754
# right_pose_goal.pose.position.z = (1355/776)*(Neurondata[4]-0.054)-0.184
# # 左臂
# left_pose_goal.pose.orientation.x = Left_Qux
# left_pose_goal.pose.orientation.y = Left_Quy
# left_pose_goal.pose.orientation.z = Left_Quz
# left_pose_goal.pose.orientation.w = Left_Quw
# left_pose_goal.pose.position.x = (1266/850)*(Neurondata[11]+0.33)-0.495
# left_pose_goal.pose.position.y = (1295/720)*(Neurondata[9]+0.19)-0.541
# left_pose_goal.pose.position.z = (1355/745)*(Neurondata[10]-0.055)-0.184
# 右臂
right_pose_goal.pose.orientation.x = Right_Qux
right_pose_goal.pose.orientation.y = Right_Quy
right_pose_goal.pose.orientation.z = Right_Quz
right_pose_goal.pose.orientation.w = Right_Quw
right_pose_goal.pose.position.x = (Neurondata[5]-0.05)*1.48+0.053
right_pose_goal.pose.position.y = (Neurondata[3]+0.18)*1.48-0.12
right_pose_goal.pose.position.z = (Neurondata[4]-0.53)*1.48+0.47
# 左臂
left_pose_goal.pose.orientation.x = Left_Qux
left_pose_goal.pose.orientation.y = Left_Quy
left_pose_goal.pose.orientation.z = Left_Quz
left_pose_goal.pose.orientation.w = Left_Quw
left_pose_goal.pose.position.x = (Neurondata[11]-0.05)*1.48+0.053
left_pose_goal.pose.position.y = (Neurondata[9]-0.18)*1.48+0.12
left_pose_goal.pose.position.z = (Neurondata[10]-0.53)*1.48+0.47
# # 右臂
# right_pose_goal.pose.orientation.x = right_pose_goal.pose.orientation.x
# right_pose_goal.pose.orientation.y = right_pose_goal.pose.orientation.y
# right_pose_goal.pose.orientation.z = right_pose_goal.pose.orientation.z
# right_pose_goal.pose.orientation.w = right_pose_goal.pose.orientation.w
# right_pose_goal.pose.position.x = right_pose_goal.pose.position.x
# right_pose_goal.pose.position.y = right_pose_goal.pose.position.y
# right_pose_goal.pose.position.z = right_pose_goal.pose.position.z
# # 左臂
# left_pose_goal.pose.orientation.x = left_pose_goal.pose.orientation.x
# left_pose_goal.pose.orientation.y = left_pose_goal.pose.orientation.y
# left_pose_goal.pose.orientation.z = left_pose_goal.pose.orientation.z
# left_pose_goal.pose.orientation.w = left_pose_goal.pose.orientation.w
# left_pose_goal.pose.position.x = left_pose_goal.pose.position.x
# left_pose_goal.pose.position.y = left_pose_goal.pose.position.y
# left_pose_goal.pose.position.z = left_pose_goal.pose.position.z
# 设置动作组的两个目标点
both_arms.set_pose_target(right_pose_goal, end_effector_link="gripper_r_finger_r")
both_arms.set_pose_target(left_pose_goal, end_effector_link="gripper_l_finger_r")
# 规划和输出动作
traj = both_arms.plan()
both_arms.execute(traj, wait=False)
# # 清除路径约束
both_arms.clear_path_constraints()
# 确保输出停止
both_arms.stop()
def main():
#Initialize moveit_commander
moveit_commander.roscpp_initialize(sys.argv)
#Start a node
rospy.init_node('moveit_node')
#Set up the left gripper
left_gripper = baxter_gripper.Gripper('left')
#Calibrate the gripper (other commands won't work unless you do this first)
print('Calibrating...')
left_gripper.calibrate()
rospy.sleep(2.0)
#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(10)
right_arm.set_planner_id('RRTConnectkConfigDefault')
right_arm.set_planning_time(10)
#First goal pose ------------------------------------------------------
goal_1 = PoseStamped()
goal_1.header.frame_id = "base"
#x, y, and z position
goal_1.pose.position.x = 0.2
goal_1.pose.position.y = 0.6
goal_1.pose.position.z = 0.2
#Orientation as a quaternion
goal_1.pose.orientation.x = 0.0
goal_1.pose.orientation.y = -1.0
goal_1.pose.orientation.z = 0.0
goal_1.pose.orientation.w = 0.0
#Set the goal state to the pose you just defined
left_arm.set_pose_target(goal_1)
#Set the start state for the left arm
left_arm.set_start_state_to_current_state()
#Plan a path
left_plan = left_arm.plan()
#Execute the plan
raw_input('Press <Enter> to move the left arm to goal pose 1 (path constraints are never enforced during this motion): ')
left_arm.execute(left_plan)
#Second goal pose -----------------------------------------------------
rospy.sleep(2.0)
#Close the left gripper
#print('Closing...')
#left_gripper.close(block=True)
#rospy.sleep(0.5)
#Open the left gripper
#print('Opening...')
#left_gripper.open(block=True)
#rospy.sleep(1.0)
print('Done!')
goal_2 = PoseStamped()
goal_2.header.frame_id = "base"
#x, y, and z position
goal_2.pose.position.x = 0.6
goal_2.pose.position.y = 0.4
goal_2.pose.position.z = 0.0
#Orientation as a quaternion
goal_2.pose.orientation.x = 0.0
goal_2.pose.orientation.y = -1.0
goal_2.pose.orientation.z = 0.0
goal_2.pose.orientation.w = 0.0
#Set the goal state to the pose you just defined
left_arm.set_pose_target(goal_2)
#Set the start state for the left arm
left_arm.set_start_state_to_current_state()
# #Create a path constraint for the arm
# #UNCOMMENT TO ENABLE ORIENTATION CONSTRAINTS
orien_const = OrientationConstraint()
orien_const.link_name = "left_gripper";
orien_const.header.frame_id = "base";
orien_const.orientation.y = -1.0;
orien_const.absolute_x_axis_tolerance = 0.1;
orien_const.absolute_y_axis_tolerance = 0.1;
orien_const.absolute_z_axis_tolerance = 0.1;
orien_const.weight = 1.0;
consts = Constraints()
consts.orientation_constraints = [orien_const]
left_arm.set_path_constraints(consts)
#Plan a path
left_plan = left_arm.plan()
#Execute the plan
raw_input('Press <Enter> to move the left arm to goal pose 2: ')
left_arm.execute(left_plan)
#Third goal pose -----------------------------------------------------
rospy.sleep(2.0)
#Close the left gripper
#print('Closing...')
#left_gripper.close(block=True)
#rospy.sleep(0.5)
#Open the left gripper
#print('Opening...')
#left_gripper.open(block=True)
#rospy.sleep(1.0)
#print('Done!')
goal_3 = PoseStamped()
goal_3.header.frame_id = "base"
#x, y, and z position
goal_3.pose.position.x = 0.0
goal_3.pose.position.y = 0.7
goal_3.pose.position.z = 0.0
#Orientation as a quaternion
goal_3.pose.orientation.x = 0.0
goal_3.pose.orientation.y = -1.0
goal_3.pose.orientation.z = 0.0
goal_3.pose.orientation.w = 0.0
#Set the goal state to the pose you just defined
left_arm.set_pose_target(goal_3)
#Set the start state for the left arm
left_arm.set_start_state_to_current_state()
#Create a path constraint for the arm
# #UNCOMMENT TO ENABLE ORIENTATION CONSTRAINTS
orien_const = OrientationConstraint()
orien_const.link_name = "left_gripper";
orien_const.header.frame_id = "base";
orien_const.orientation.y = -1.0;
orien_const.absolute_x_axis_tolerance = 0.1;
orien_const.absolute_y_axis_tolerance = 0.1;
orien_const.absolute_z_axis_tolerance = 0.1;
orien_const.weight = 1.0;
consts = Constraints()
consts.orientation_constraints = [orien_const]
left_arm.set_path_constraints(consts)
#Plan a path
left_plan = left_arm.plan()
#Execute the plan
raw_input('Press <Enter> to move the left arm to goal pose 3: ')
left_arm.execute(left_plan)
#Fourth goal pose -----------------------------------------------------
rospy.sleep(2.0)
#Close the left gripper
#print('Closing...')
#left_gripper.close(block=True)
#rospy.sleep(0.5)
#Open the left gripper
#print('Opening...')
#left_gripper.open(block=True)
#rospy.sleep(1.0)
#print('Done!')
goal_4 = PoseStamped()
goal_4.header.frame_id = "base"
#x, y, and z position
goal_4.pose.position.x = 0.4
goal_4.pose.position.y = 0.7
goal_4.pose.position.z = 0.3
#Orientation as a quaternion
goal_4.pose.orientation.x = 0.0
goal_4.pose.orientation.y = -1.0
goal_4.pose.orientation.z = 0.0
goal_4.pose.orientation.w = 0.0
#Set the goal state to the pose you just defined
left_arm.set_pose_target(goal_4)
#Set the start state for the left arm
left_arm.set_start_state_to_current_state()
#Create a path constraint for the arm
# #UNCOMMENT TO ENABLE ORIENTATION CONSTRAINTS
#orien_const = OrientationConstraint()
#orien_const.link_name = "left_gripper";
#orien_const.header.frame_id = "base";
#orien_const.orientation.y = -1.0;
#orien_const.absolute_x_axis_tolerance = 0.1;
#orien_const.absolute_y_axis_tolerance = 0.1;
#orien_const.absolute_z_axis_tolerance = 0.1;
#orien_const.weight = 1.0;
#consts = Constraints()
#consts.orientation_constraints = [orien_const]
#left_arm.set_path_constraints(consts)
#Plan a path
left_plan = left_arm.plan()
#Execute the plan
raw_input('Press <Enter> to move the left arm to goal pose 4: ')
left_arm.execute(left_plan)
rospy.sleep(2.0)
#Close the left gripper
print('Closing...')
left_gripper.close(block=True)
rospy.sleep(0.5)
#Open the left gripper
print('Opening...')
left_gripper.open(block=True)
rospy.sleep(1.0)
print('Done!')
box_pose.header.frame_id = group.get_planning_frame()
box_pose.pose.position.x = 0.3
box_pose.pose.position.y = -0.3
box_pose.pose.position.z = -0.25
box_pose.pose.orientation.w = 1.0
scene.add_box('box_object', box_pose, (0.4, 0.1, 0.5))
rospy.sleep(2)
rospy.loginfo("Scene Objects : {}".format(scene.get_known_object_names()))
# 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
def main():
# Create table obstacle
rospy.init_node('arm_obstacle_demo')
planning_scene = PlanningSceneInterface('base_link')
planning_scene.removeCollisionObject('table')
table_size_x = 0.5
table_size_y = 1
table_size_z = 0.03
table_x = 0.8
table_y = 0
table_z = 0.6
planning_scene.addBox('table', table_size_x, table_size_y, table_size_z, table_x, table_y, table_z)
# Create divider obstacle
# planning_scene.removeCollisionObject('divider')
# size_x = 0.3
# size_y = 0.01
# size_z = 0.4
# x = table_x - (table_size_x / 2) + (size_x / 2)
# y = 0
# z = table_z + (table_size_z / 2) + (size_z / 2)
# planning_scene.addBox('divider', size_x, size_y, size_z, x, y, z)
pose1 = PoseStamped()
pose1.header.frame_id = 'base_link'
pose1.pose.position.x = 0.5
pose1.pose.position.y = -0.3
pose1.pose.position.z = 0.75
pose1.pose.orientation.w = 1
pose2 = PoseStamped()
pose2.header.frame_id = 'base_link'
pose2.pose.position.x = 0.5
pose2.pose.position.y = 0.3
pose2.pose.position.z = 0.75
pose2.pose.orientation.w = 1
arm = fetch_api.Arm()
def shutdown():
arm.cancel_all_goals()
rospy.on_shutdown(shutdown)
#Orientation constraint
oc = OrientationConstraint()
oc.header.frame_id = 'base_link'
oc.link_name = 'wrist_roll_link'
oc.orientation.w = 1
oc.absolute_x_axis_tolerance = 0.1
oc.absolute_y_axis_tolerance = 0.1
oc.absolute_z_axis_tolerance = 3.14
oc.weight = 1.0
# move to pose args
kwargs = {
'allowed_planning_time': 20,
'execution_timeout': 10,
'num_planning_attempts': 5,
'replan': True,
'orientation_constraint': oc
}
# Before moving to the first pose
planning_scene.removeAttachedObject('tray')
error = arm.move_to_pose(pose1, **kwargs)
if error is not None:
rospy.logerr('Pose 1 failed: {}'.format(error))
else:
rospy.loginfo('Pose 1 succeeded')
frame_attached_to = 'gripper_link'
frames_okay_to_collide_with = [
'gripper_link', 'l_gripper_finger_link', 'r_gripper_finger_link'
]
planning_scene.attachBox('tray', 0.3, 0.07, 0.01, 0.05, 0, 0,
frame_attached_to, frames_okay_to_collide_with)
planning_scene.setColor('tray', 1, 0, 1)
planning_scene.sendColors()
rospy.sleep(1)
error = arm.move_to_pose(pose2, **kwargs)
if error is not None:
rospy.logerr('Pose 2 failed: {}'.format(error))
else:
rospy.loginfo('Pose 2 succeeded')
planning_scene.removeCollisionObject('table')
planning_scene.removeCollisionObject('divider')
# At the end of your program
planning_scene.removeAttachedObject('tray')
# Was working without this but now its needed
planning_scene.clear()
def print_pointlist(self, point_list, future_print_status=False):
startime = datetime.datetime.now()
# Save original points list
full_point_list = copy.deepcopy(point_list)
full_point_array = np.delete(np.array(full_point_list), 2, axis=1)
self.target_list = full_point_list
if future_print_status: self.future_printing_status = True
# Constraints
pose = self.group.get_current_pose(self.group.get_end_effector_link())
constraints = Constraints()
# joint constraints
joint_constraint = JointConstraint()
joint_constraint.joint_name = 'arm_joint_1'
joint_constraint.position = 169 * pi / 180
joint_constraint.tolerance_above = 30 * pi / 180
joint_constraint.tolerance_below = 30 * pi / 180
joint_constraint.weight = 1.0
constraints.joint_constraints.append(joint_constraint)
joint_constraint = JointConstraint()
joint_constraint.joint_name = 'arm_joint_4'
joint_constraint.position = 150 * pi / 180
joint_constraint.tolerance_above = 30 * pi / 180
joint_constraint.tolerance_below = 30 * pi / 180
joint_constraint.weight = 1.0
constraints.joint_constraints.append(joint_constraint)
self.group.set_path_constraints(constraints)
# Orientation constrains
orientation_constraint = OrientationConstraint()
orientation_constraint.header = pose.header
orientation_constraint.link_name = self.group.get_end_effector_link()
orientation_constraint.orientation = pose.pose.orientation
orientation_constraint.absolute_x_axis_tolerance = 2 * pi
orientation_constraint.absolute_y_axis_tolerance = 2 * pi
orientation_constraint.absolute_z_axis_tolerance = 2 * pi
orientation_constraint.weight = 1.0
constraints.orientation_constraints.append(orientation_constraint)
# Record how many points has already finished
finsih_num = 0
print_num = 0
index_check = 0
while len(point_list) > 0:
print('New Plan, points left:', len(point_list), point_list)
# Move the robot point to first point and find the height
if len(point_list) > 1:
initial_plan = self.move_to_initial(point_list[1])
else:
initial_plan = self.move_to_initial(point_list[0])
# joint_goal = self.group.get_current_joint_values()
head = initial_plan.joint_trajectory.header
robot_state = self.robot.get_current_state()
# print(robot_state.joint_state)
robot_state.joint_state.position = tuple(initial_plan.joint_trajectory.points[-1].positions) + \
tuple(robot_state.joint_state.position[7:]) # the joints for the wheel
resp = self.request_fk(head, [self.group.get_end_effector_link()],
robot_state)
current_pose = self.group.get_current_pose().pose
current_pose.orientation = resp.pose_stamped[0].pose.orientation
(current_pose.position.x, current_pose.position.y,
current_pose.position.z) = point_list[0]
self.group.set_pose_target(current_pose)
self.group.go()
# Move the robot to the center of the striaght line to make sure Way point method can be executed
# Way points
waypoints = []
wpose = self.group.get_current_pose().pose
# Add the current pose to make sure the path is smooth
waypoints.append(copy.deepcopy(wpose))
success_num = 0
for point_num in range(len(point_list)):
(wpose.position.x, wpose.position.y,
wpose.position.z) = point_list[point_num]
waypoints.append(copy.deepcopy(wpose))
(plan, fraction) = self.group.compute_cartesian_path(
waypoints, # waypoints to follow
0.01, # eef_step
0.00,
path_constraints=constraints)
print 'Adding the first planing point, and fraction is', fraction
executing_state = 0
if fraction == 1:
success_num += 1
execute_plan = plan
if success_num == len(point_list):
self.group.execute(execute_plan, wait=False)
self.msg_print.data = True
executing_state = 1
success_num += 1
elif success_num == 0:
break
else:
# execute success plan
self.msg_print.data = True
self.group.execute(execute_plan, wait=False)
executing_state = 1
break
## 2nd loop
## always re-plan and check for obstacle while executing waypoints
executed_waypoint_index = 0 # initial value of nothing
success_point_list = point_list[:success_num]
print('when plan success, move_group_status:',
self.move_group_execution_status, 'success_plan_number:',
success_num)
if executing_state == 1:
success_planned_waypoint_array = np.delete(np.array(
point_list[:success_num]),
2,
axis=1)
# print 'success planned waypoint\n', success_planned_waypoint_array
print 'status', self.waypoint_execution_status
while self.waypoint_execution_status != 3:
if point_list == []: break
if self.waypoint_execution_status == 4:
# aborted state
print 'stop and abort waypoint execution'
self.msg_print.data = False
self.group.stop()
executing_state = 0
break
current_ee_pose = self.group.get_current_pose().pose
current_ee_position_array = np.array([
current_ee_pose.position.x, current_ee_pose.position.y
])
executed_waypoint_index = self.check_executed_waypoint_index(
success_planned_waypoint_array,
current_ee_position_array)
# print 'executed latest index', executed_waypoint_index
index_check = self.check_executed_waypoint_index(
full_point_array, current_ee_position_array)
self.further_printing_number = index_check
print 'index:', index_check, 'way_point index', executed_waypoint_index
if future_print_status == True:
# Check for enclosure
self.base_group.set_position_target(
[0, 0, 0.05],
self.base_group.get_end_effector_link())
result = self.base_group.plan()
self.base_group.clear_pose_targets()
if len(result.joint_trajectory.points) == 0:
print('Check enclosure failed')
else:
print('Check enclosure successful')
## Replan to check for dynamic obstacle
waypoints = []
# Add the current pose to make sure the path is smooth, get latest pose
current_ee_pose = self.group.get_current_pose().pose
waypoints.append(copy.deepcopy(current_ee_pose))
# discard the executed waypoints
new_point_list = point_list[
executed_waypoint_index:success_num]
for k in new_point_list:
(current_ee_pose.position.x,
current_ee_pose.position.y,
current_ee_pose.position.z) = k
waypoints.append(copy.deepcopy(current_ee_pose))
(plan2, fraction2) = self.group.compute_cartesian_path(
waypoints, # waypoints to follow
0.01, # eef_step
0.00,
path_constraints=constraints)
print 'Dynamic check fraction:', fraction2
if fraction2 < 1:
## new obstacle appear
# print 'executed latest index', executed_waypoint_index
# print 'fraction value', fraction,'\n'
print 'new obstacle appeared to be in the path'
self.group.stop()
self.msg_print.data = False
executing_state = 0
break
rospy.sleep(2)
print 'status:', self.waypoint_execution_status, 'executed_index:', executed_waypoint_index, 'success_num:', success_num
if self.waypoint_execution_status == 3:
# waypoint successfully printed
# self.print_list_visualize(point_list[:success_num])
self.rviz_visualise_marker(point_list[:success_num])
del (point_list[:success_num - 1])
elif self.waypoint_execution_status == 2 or 4:
# state 2 = preempted, state 4 = aborted.
# only printed partial waypoint
# self.print_list_visualize(point_list[:executed_waypoint_index+1])
if executed_waypoint_index > 0: # at index 0, it might have not print the point 0-1 edge successfully.
self.rviz_visualise_marker(
point_list[:executed_waypoint_index + 1])
del (point_list[:executed_waypoint_index]
) # delete up till whatever is executed
self.msg_print.data = False
if point_list == []: rospy.sleep(2)
self.group.stop()
# Delete the points what already execute
# if success_num > 0:
# Add obstacle after printing (need to revise)
self.group.set_path_constraints(None)
finshtime = datetime.datetime.now()
print(finshtime - startime).seconds
full_point_list_x = [base[0] for base in full_point_list]
full_point_list_y = [base[1] for base in full_point_list]
plt3 = plt.figure("Printing result", figsize=(6, 6))
plt.plot(self.re_position_x, self.re_position_y, 'b')
plt.plot(full_point_list_x, full_point_list_y, 'ro')
plt.xlim(min(full_point_list_x) - 0.1, max(full_point_list_x) + 0.1)
plt.ylim(min(full_point_list_y) - 0.1, max(full_point_list_y) + 0.1)
plt.legend(['Printing result', 'ground truth'],
fontsize=8,
bbox_to_anchor=(1.0, 1))
plt.xlabel("X axis (m)")
plt.ylabel("y axis (m)")
plt.title('Printing result')
plt.show()
plt3.savefig('result.png', dpi=plt3.dpi)
print('All finish, time:', (finshtime - startime).seconds)
def place(self, target_name, place_position, pre_place_distance,
post_place_retreat):
limit = {'dist': 0.01, 'r': 0.10, 'p': 0.10, 'y': 0.10}
constraints = Constraints()
oc = OrientationConstraint()
oc.header.frame_id = REFERENCE_FRAME
oc.link_name = 'j2s7s300_end_effector'
oc.absolute_x_axis_tolerance = limit['r'] # radian
oc.absolute_y_axis_tolerance = limit['p']
oc.absolute_z_axis_tolerance = limit['y']
oc.weight = 0.85
oc.orientation = place_position.pose.orientation
constraints.orientation_constraints.append(deepcopy(oc))
result = False
replan_times = 1
replan_state = True
while replan_state and replan_times <= 5:
(place_path, fraction) = self.get_path(place_position.pose,
0.01,
constraints=constraints)
if fraction >= 0.95:
print "Pre_place_approach..."
self.arm.execute(place_path)
result = self.check(place_position.pose, limit)
replan_state = False
replan_times += 1
if result:
result = False
print "Placing..."
self.gripper.set_named_target("Open")
self.gripper.go()
self.arm.detach_object(target_name)
rospy.sleep(1)
post_place_position = self.get_retreat_point(
place_position.pose, post_place_retreat)
replan_state = True
replan_times = 1
while replan_state and replan_times <= 5:
(retreat_path,
fraction) = self.get_path(post_place_position,
0.01,
constraints=constraints)
if fraction > 0.8:
self.arm.execute(retreat_path)
result = self.check(post_place_position, limit)
replan_state = False
replan_times += 1
if not result:
self.state = STATE.PLACE_RETREAT_ERR
else:
self.state = STATE.PRE_PLACE_ERR
if self._server.current_goal.get_goal_status(
).status == GoalStatus.PREEMPTING:
rospy.logwarn("Can't cancel task after place action start!")
if result:
self.state = STATE.PLACE_FINISH
self.arm.clear_path_constraints()
def __init__(self):
self.bridge = CvBridge()
joint_state_topic = ['joint_states:=/iiwa/joint_states']
moveit_commander.roscpp_initialize(joint_state_topic)
rospy.Subscriber("/iiwa/joint_states", JointState, self.State_callback)
# Instantiate a RobotCommander object. This object is
# an interface to the robot as a whole.
self.robot = moveit_commander.RobotCommander()
self.group = moveit_commander.MoveGroupCommander("manipulator")
# rospy.sleep(2)
# self.scene = moveit_commander.PlanningSceneInterface('/iiwa/move_group/monitored_planning_scene')
# box_pose = PoseStamped()
# box_pose.header.frame_id = "world"
# box_pose.pose.position.x = 1.0
# box_pose.pose.orientation.w = 1.0
# self.scene.add_box("test", box_pose, size=(0.1, 0.2, 0.3))
# while not rospy.is_shutdown():
# rospy.sleep(1.0)
# for k in self.scene.__dict__.keys():
# print(k, self.scene.__dict__[k])
# # print(self.scene)
# print(self.scene.get_known_object_names())
# print(self.scene.get_attached_objects())
# exit()
self.group.set_max_velocity_scaling_factor(0.05)
self.group.set_max_acceleration_scaling_factor(0.05)
current_pose = self.group.get_current_pose(end_effector_link='iiwa_link_ee').pose
self._joint_efforts = 0
self._joint_vel = 0
self._joint_name = 0
self._header = None
pose = PoseStamped()
self.upright_constraints = Constraints()
self.upright_constraints.name = "upright"
orientation_constraint = OrientationConstraint()
orientation_constraint.header.frame_id = self.group.get_planning_frame()
orientation_constraint.link_name = self.group.get_end_effector_link()
pose.pose.orientation.x = 1.0
pose.pose.orientation.y = 0.0
pose.pose.orientation.z = 0.0
pose.pose.orientation.w = 0.0
orientation_constraint.orientation = pose.pose.orientation
orientation_constraint.absolute_x_axis_tolerance = .7#3.0
orientation_constraint.absolute_y_axis_tolerance = .7#3.0
orientation_constraint.absolute_z_axis_tolerance = 3.1415
#orientation_constraint.absolute_z_axis_tolerance = 3.14 #ignore this axis
orientation_constraint.weight = 1
self.upright_constraints.orientation_constraints.append(orientation_constraint)
self.group.allow_replanning(True)
self.group.allow_looking(True)
workspace = [0.5,-0.3,0.15,0.7,0.2,0.25]
# self.group.set_workspace(workspace)
# self.group.set_path_constraints(self.upright_constraints)
self.traj_num = -1
self.im_num = 0
self.MAX_PATH_LENGTH = 15
def main():
"""
Main Script
"""
# Make sure that you've looked at and understand path_planner.py before starting
## TF CODE
tfBuffer = tf2_ros.Buffer()
tfListener = tf2_ros.TransformListener(tfBuffer)
## TF CODE
planner = PathPlanner("right_arm")
Kp = 0.1 * np.array([0.3, 2, 1, 1.5, 2, 2, 3]) # Stolen from 106B Students
Kd = 0.01 * np.array([2, 1, 2, 0.5, 0.5, 0.5, 0.5
]) # Stolen from 106B Students
Ki = 0.01 * np.array([1, 1, 1, 1, 1, 1, 1]) # Untuned
Kw = np.array([0.9, 0.9, 0.9, 0.9, 0.9, 0.9, 0.9]) # Untuned
limb = intera_interface.Limb("right")
control = Controller(Kp, Kd, Ki, Kw, limb)
##
## Add the obstacle to the planning scene here
##
X = 0.40
Y = 1.20
Z = 0.10
Xp = 0.5
Yp = 0.00
Zp = -0.15
Xpa = 0.00
Ypa = 0.00
Zpa = 0.00
Wpa = 1.00
pose = PoseStamped()
pose.header.stamp = rospy.Time.now()
pose.header.frame_id = "base"
pose.pose.position.x = Xp
pose.pose.position.y = Yp
pose.pose.position.z = Zp
pose.pose.orientation.x = Xpa
pose.pose.orientation.y = Ypa
pose.pose.orientation.z = Zpa
pose.pose.orientation.w = Wpa
#planner.add_box_obstacle([X,Y,Z], "wall", pose)
#Wall 2
X = 0.40
Y = 0.10
Z = 0.30
Xp = 0.5
Yp = -0.15
Zp = 0.05
Xpa = 0.00
Ypa = 0.00
Zpa = 0.00
Wpa = 1.00
pose = PoseStamped()
pose.header.stamp = rospy.Time.now()
pose.header.frame_id = "base"
pose.pose.position.x = Xp
pose.pose.position.y = Yp
pose.pose.position.z = Zp
pose.pose.orientation.x = Xpa
pose.pose.orientation.y = Ypa
pose.pose.orientation.z = Zpa
pose.pose.orientation.w = Wpa
#planner.add_box_obstacle([X,Y,Z], "wall2", pose)
# #Create a path constraint for the arm
# #UNCOMMENT FOR THE ORIENTATION CONSTRAINTS PART
orien_const = OrientationConstraint()
orien_const.link_name = "right_gripper"
orien_const.header.frame_id = "base"
#orien_const.orientation.y = -1.0;
#orien_const.absolute_x_axis_tolerance = 0.1;
orien_const.absolute_y_axis_tolerance = 0.05
orien_const.absolute_z_axis_tolerance = 0.05
orien_const.weight = 1.0
waypoints = list()
# Section to add waypoints
# step_size = size between x position changes
# final_length = final x position
#GOAL Points
# goal_1.pose.position.x = 0.585
# goal_1.pose.position.y = 0.156
# goal_1.pose.position.z = -0.138
# goal_1.pose.orientation.x = 0
# goal_1.pose.orientation.y = 0.707
# goal_1.pose.orientation.z = 0
# goal_1.pose.orientation.w = 0.707
#GOAL Points
def moveUp(planner, control, waypoints, value_of_step):
control._probing = 1
#STARTOF WORKING CODE
# Z MUST MOVE UP TWO BLOCK
# Z = 0.025
#control._velocity_scalar = 0.5
jenga = 0
while (jenga != 1):
step_size = 0.0005 #0.0025
FINAL_X = 0.785 #
offset = 0.14
tf_px = 0.585
tf_py = 0.156
tf_pz = -0.138
tf_rx = 0
tf_ry = 0.707
tf_rz = 0
tf_rw = 0.707
goal_x = 0.585
goal_y = 0.156
goal_z = -0.138
try:
trans = tfBuffer.lookup_transform('base', 'right_gripper_base',
rospy.Time())
tf_px = trans.transform.translation.x
tf_py = trans.transform.translation.y
tf_pz = trans.transform.translation.z
tf_rx = trans.transform.rotation.x
tf_ry = trans.transform.rotation.y
tf_rz = trans.transform.rotation.z
tf_rw = trans.transform.rotation.w
goal_x = tf_px
goal_y = tf_py
goal_z = tf_pz
except (tf2_ros.LookupException, tf2_ros.ConnectivityException,
tf2_ros.ExtrapolationException):
continue
offset = 0.14
FINAL_Z = goal_z + value_of_step # + offsett
z = tf_pz #+ offset
#print("TF'd zero")
update = 0
execute = 0
i = 0
STOP = 0
while ((z < FINAL_Z or update == 0) and (i < 3)):
if (update == 0):
try:
trans = tfBuffer.lookup_transform(
'base', 'right_gripper_base', rospy.Time())
tf_px = trans.transform.translation.x
tf_py = trans.transform.translation.y
tf_pz = trans.transform.translation.z
tf_rx = trans.transform.rotation.x
tf_ry = trans.transform.rotation.y
tf_rz = trans.transform.rotation.z
tf_rw = trans.transform.rotation.w
z = tf_pz # + offset
except (tf2_ros.LookupException,
tf2_ros.ConnectivityException,
tf2_ros.ExtrapolationException):
continue
update = 1
execute = 0
y = tf_py
x = tf_px + offset
if (y < goal_y - 0.05):
y = y + step_size
if (y > goal_y + 0.05):
y = y - step_size
#print("TF'd while")
goal_1 = PoseStamped()
goal_1.header.frame_id = "base"
goal_1.pose.position.x = x
goal_1.pose.position.y = y
goal_1.pose.position.z = z
#Orientation as a quaternion
goal_1.pose.orientation.x = 0 # -0.026
goal_1.pose.orientation.y = 0.707 # 0.723
goal_1.pose.orientation.z = 0 # -0.052
goal_1.pose.orientation.w = 0.707 # 0.689
waypoints.append(goal_1.pose)
z = z + step_size
if (z >= FINAL_Z):
execute = 1
#print("added waypoint")
if (execute == 1):
while not rospy.is_shutdown():
try:
plan = planner.plan_to_pose(waypoints, list())
if not control.execute_path(plan):
raise Exception("Execution failed")
except Exception as e:
print e
else:
break
#STOP = control._flag
waypoints = list()
update = 0
#print("EXEcute")
#print(i)
i = i + 1
#print("END OF LOOP")
jenga = 1
control._probing = 0
# END OF WORKING CODE
def moveDown(planner, control, waypoints):
control._probing = 1
#STARTOF WORKING CODE
# Z MUST MOVE UP TWO BLOCK
# Z = 0.025
value_of_step = 0.033
#control._velocity_scalar = 0.5
jenga = 0
while (jenga != 1):
step_size = 0.0005 #0.0025
FINAL_X = 0.785 #
offset = 0.14
tf_px = 0.585
tf_py = 0.156
tf_pz = -0.138
tf_rx = 0
tf_ry = 0.707
tf_rz = 0
tf_rw = 0.707
goal_x = 0.585
goal_y = 0.156
goal_z = -0.138
try:
trans = tfBuffer.lookup_transform('base', 'right_gripper_base',
rospy.Time())
tf_px = trans.transform.translation.x
tf_py = trans.transform.translation.y
tf_pz = trans.transform.translation.z
tf_rx = trans.transform.rotation.x
tf_ry = trans.transform.rotation.y
tf_rz = trans.transform.rotation.z
tf_rw = trans.transform.rotation.w
goal_x = tf_px
goal_y = tf_py
goal_z = tf_pz
except (tf2_ros.LookupException, tf2_ros.ConnectivityException,
tf2_ros.ExtrapolationException):
continue
offset = 0.14
FINAL_Z = goal_z - value_of_step # + offsett
z = tf_pz #+ offset
#print("TF'd zero")
update = 0
execute = 0
i = 0
STOP = 0
while ((z > FINAL_Z or update == 0) and (i < 3)):
if (update == 0):
try:
trans = tfBuffer.lookup_transform(
'base', 'right_gripper_base', rospy.Time())
tf_px = trans.transform.translation.x
tf_py = trans.transform.translation.y
tf_pz = trans.transform.translation.z
tf_rx = trans.transform.rotation.x
tf_ry = trans.transform.rotation.y
tf_rz = trans.transform.rotation.z
tf_rw = trans.transform.rotation.w
z = tf_pz # + offset
except (tf2_ros.LookupException,
tf2_ros.ConnectivityException,
tf2_ros.ExtrapolationException):
continue
update = 1
execute = 0
y = tf_py
x = tf_px + offset
if (y < goal_y - 0.05):
y = y + step_size
if (y > goal_y + 0.05):
y = y - step_size
#print("TF'd while")
goal_1 = PoseStamped()
goal_1.header.frame_id = "base"
goal_1.pose.position.x = x
goal_1.pose.position.y = y
goal_1.pose.position.z = z
#Orientation as a quaternion
goal_1.pose.orientation.x = 0 # -0.026
goal_1.pose.orientation.y = 0.707 # 0.723
goal_1.pose.orientation.z = 0 # -0.052
goal_1.pose.orientation.w = 0.707 # 0.689
waypoints.append(goal_1.pose)
z = z - step_size
if (z <= FINAL_Z):
execute = 1
#print("added waypoint")
if (execute == 1):
while not rospy.is_shutdown():
try:
plan = planner.plan_to_pose(waypoints, list())
if not control.execute_path(plan):
raise Exception("Execution failed")
except Exception as e:
print e
else:
break
#STOP = control._flag
waypoints = list()
update = 0
#print("EXEcute")
#print(i)
i = i + 1
#print("END OF LOOP")
jenga = 1
control._probing = 0
# END OF WORKING CODE
def moveLeft(planner, control, waypoints, value_of_step):
control._probing = 1
#STARTOF WORKING CODE
# Z MUST MOVE UP TWO BLOCK
# Z = 0.025
#value_of_step = 0.014
#control._velocity_scalar = 0.5
jenga = 0
while (jenga != 1):
step_size = 0.0005 #0.0025
FINAL_X = 0.785 #
offset = 0.14
tf_px = 0.585
tf_py = 0.156
tf_pz = -0.138
tf_rx = 0
tf_ry = 0.707
tf_rz = 0
tf_rw = 0.707
goal_x = 0.585
goal_y = 0.156
goal_z = -0.138
try:
trans = tfBuffer.lookup_transform('base', 'right_gripper_base',
rospy.Time())
tf_px = trans.transform.translation.x
tf_py = trans.transform.translation.y
tf_pz = trans.transform.translation.z
tf_rx = trans.transform.rotation.x
tf_ry = trans.transform.rotation.y
tf_rz = trans.transform.rotation.z
tf_rw = trans.transform.rotation.w
goal_x = tf_px
goal_y = tf_py
goal_z = tf_pz
#print(goal_z)
except (tf2_ros.LookupException, tf2_ros.ConnectivityException,
tf2_ros.ExtrapolationException):
continue
offset = 0.14
FINAL_Y = goal_y + value_of_step # + offsett
y = tf_py #+ offset
update = 0
execute = 0
i = 0
STOP = 0
while ((y < FINAL_Y or update == 0) and (i < 3)):
if (update == 0):
try:
trans = tfBuffer.lookup_transform(
'base', 'right_gripper_base', rospy.Time())
tf_px = trans.transform.translation.x
tf_py = trans.transform.translation.y
tf_pz = trans.transform.translation.z
tf_rx = trans.transform.rotation.x
tf_ry = trans.transform.rotation.y
tf_rz = trans.transform.rotation.z
tf_rw = trans.transform.rotation.w
y = tf_py # + offset
except (tf2_ros.LookupException,
tf2_ros.ConnectivityException,
tf2_ros.ExtrapolationException):
continue
update = 1
execute = 0
x = tf_px + offset
z = tf_pz
#print("TF'd while")
goal_1 = PoseStamped()
goal_1.header.frame_id = "base"
goal_1.pose.position.x = x
goal_1.pose.position.y = y
goal_1.pose.position.z = z
#Orientation as a quaternion
goal_1.pose.orientation.x = 0 # -0.026
goal_1.pose.orientation.y = 0.707 # 0.723
goal_1.pose.orientation.z = 0 # -0.052
goal_1.pose.orientation.w = 0.707 # 0.689
waypoints.append(goal_1.pose)
y = y + step_size
if (y >= FINAL_Y):
execute = 1
#print("added waypoint")
if (execute == 1):
while not rospy.is_shutdown():
try:
plan = planner.plan_to_pose(waypoints, list())
if not control.execute_path(plan):
raise Exception("Execution failed")
except Exception as e:
print e
else:
break
#STOP = control._flag
waypoints = list()
update = 0
#print("EXEcute")
#print(i)
i = i + 1
#print("END OF LOOP")
jenga = 1
control._probing = 0
# END OF WORKING CODE
def moveRight(planner, control, waypoints):
control._probing = 1
#STARTOF WORKING CODE
# Z MUST MOVE UP TWO BLOCK
# Z = 0.025
value_of_step = 0.014
#control._velocity_scalar = 0.5
jenga = 0
while (jenga != 1):
step_size = 0.0005 #0.0025
FINAL_X = 0.785 #
offset = 0.14
tf_px = 0.585
tf_py = 0.156
tf_pz = -0.138
tf_rx = 0
tf_ry = 0.707
tf_rz = 0
tf_rw = 0.707
goal_x = 0.585
goal_y = 0.156
goal_z = -0.138
try:
trans = tfBuffer.lookup_transform('base', 'right_gripper_base',
rospy.Time())
tf_px = trans.transform.translation.x
tf_py = trans.transform.translation.y
tf_pz = trans.transform.translation.z
tf_rx = trans.transform.rotation.x
tf_ry = trans.transform.rotation.y
tf_rz = trans.transform.rotation.z
tf_rw = trans.transform.rotation.w
goal_x = tf_px
goal_y = tf_py
goal_z = tf_pz
except (tf2_ros.LookupException, tf2_ros.ConnectivityException,
tf2_ros.ExtrapolationException):
continue
offset = 0.14
FINAL_Y = goal_y - value_of_step
y = tf_py #+ offset
#print("TF'd zero")
update = 0
execute = 0
i = 0
STOP = 0
while ((y > FINAL_Y or update == 0) and (i < 3)):
if (update == 0):
try:
trans = tfBuffer.lookup_transform(
'base', 'right_gripper_base', rospy.Time())
tf_px = trans.transform.translation.x
tf_py = trans.transform.translation.y
tf_pz = trans.transform.translation.z
tf_rx = trans.transform.rotation.x
tf_ry = trans.transform.rotation.y
tf_rz = trans.transform.rotation.z
tf_rw = trans.transform.rotation.w
y = tf_py # + offset
except (tf2_ros.LookupException,
tf2_ros.ConnectivityException,
tf2_ros.ExtrapolationException):
continue
update = 1
execute = 0
z = tf_pz
x = tf_px + offset
#print("TF'd while")
goal_1 = PoseStamped()
goal_1.header.frame_id = "base"
goal_1.pose.position.x = x
goal_1.pose.position.y = y
goal_1.pose.position.z = z
#Orientation as a quaternion
goal_1.pose.orientation.x = 0 # -0.026
goal_1.pose.orientation.y = 0.707 # 0.723
goal_1.pose.orientation.z = 0 # -0.052
goal_1.pose.orientation.w = 0.707 # 0.689
waypoints.append(goal_1.pose)
y = y - step_size
if (y <= FINAL_Y):
execute = 1
#print("added waypoint")
if (execute == 1):
while not rospy.is_shutdown():
try:
plan = planner.plan_to_pose(waypoints, list())
if not control.execute_path(plan):
raise Exception("Execution failed")
except Exception as e:
print e
else:
break
#STOP = control._flag
print("LAST WAYPOINT")
print(waypoints[-1].position)
waypoints = list()
update = 0
#print("EXEcute")
#print(i)
i = i + 1
#print("END OF LOOP")
jenga = 1
control._probing = 0
# END OF WORKING CODE
def probe(planner, control, waypoints):
control._probing = 1
value_of_step = 0.004
step_size = 0.0005 #0.0025
goal_x = 0.585
goal_y = 0.156
goal_z = -0.138
jenga = 0
while (jenga != 1):
FINAL_X = 0.785 #
offset = 0.14
tf_px = 0.585
tf_py = 0.156
tf_pz = -0.138
tf_rx = 0
tf_ry = 0.707
tf_rz = 0
tf_rw = 0.707
goal_x = 0.585
goal_y = 0.156
goal_z = -0.138
try:
trans = tfBuffer.lookup_transform('base', 'right_gripper_base',
rospy.Time())
tf_px = trans.transform.translation.x
tf_py = trans.transform.translation.y
tf_pz = trans.transform.translation.z
tf_rx = trans.transform.rotation.x
tf_ry = trans.transform.rotation.y
tf_rz = trans.transform.rotation.z
tf_rw = trans.transform.rotation.w
goal_x = tf_px
goal_y = tf_py
goal_z = tf_pz
except (tf2_ros.LookupException, tf2_ros.ConnectivityException,
tf2_ros.ExtrapolationException):
continue
offset = 0.14
FINAL_X = goal_x + offset + value_of_step
x = tf_px + offset
update = 0
execute = 0
STOP = 0
while ((x < FINAL_X or update == 0)):
if (update == 0):
try:
trans = tfBuffer.lookup_transform(
'base', 'right_gripper_base', rospy.Time())
tf_px = trans.transform.translation.x
tf_py = trans.transform.translation.y
tf_pz = trans.transform.translation.z
tf_rx = trans.transform.rotation.x
tf_ry = trans.transform.rotation.y
tf_rz = trans.transform.rotation.z
tf_rw = trans.transform.rotation.w
x = tf_px + offset
except (tf2_ros.LookupException,
tf2_ros.ConnectivityException,
tf2_ros.ExtrapolationException):
continue
update = 1
execute = 0
y = tf_py
z = tf_pz
if (y < goal_y - 0.05):
y = y + step_size
if (y > goal_y + 0.05):
y = y - step_size
if (z < goal_z - 0.05):
z = z + step_size
if (z > goal_z + 0.05):
z = z - step_size
goal_1 = PoseStamped()
goal_1.header.frame_id = "base"
goal_1.pose.position.x = x
goal_1.pose.position.y = y
goal_1.pose.position.z = z
goal_1.pose.orientation.x = 0 # -0.026
goal_1.pose.orientation.y = 0.707 # 0.723
goal_1.pose.orientation.z = 0 # -0.052
goal_1.pose.orientation.w = 0.707 # 0..
#print("added a point")
waypoints.append(goal_1.pose)
x = x + step_size
if (x >= FINAL_X):
execute = 1
if (execute == 1):
while not rospy.is_shutdown():
try:
#print("Executing Probe")
plan = planner.plan_to_pose(waypoints, list())
if not control.execute_path(plan):
raise Exception("Execution failed")
except Exception as e:
print e
else:
break
jenga = 1
waypoints = waypoints[::-1]
goal_1 = PoseStamped()
goal_1.header.frame_id = "base"
goal_1.pose.position.x = goal_x + offset
goal_1.pose.position.y = goal_y
goal_1.pose.position.z = goal_z
goal_1.pose.orientation.x = 0 # -0.026
goal_1.pose.orientation.y = 0.707 # 0.723
goal_1.pose.orientation.z = 0 # -0.052
goal_1.pose.orientation.w = 0.707 # 0..
#waypoints.append(goal_1.pose)
while not rospy.is_shutdown():
try:
plan = planner.plan_to_pose(waypoints, list())
if not control.execute_path(plan):
raise Exception("Execution failed")
except Exception as e:
print e
else:
break
print("PROBE TEST")
print(min(list(map(float, control._sensor_data))))
control._poke_data.append(min(list(map(float, control._sensor_data))))
control._sensor_data = list()
control._scalar_data = list()
control._probing = 0
def zero_spot(planner, control, waypoints):
control._probing = 1
jenga = 0
while (jenga != 1):
step_size = 0.0005 #0.0025
FINAL_X = 0.585
FINAL_Y = 0.156
FINAL_Z = -0.138
offset = 0.14
tf_px = 0.585
tf_py = 0.156
tf_pz = -0.138
goal_x = 0.585
goal_y = 0.156
goal_z = -0.138
x_done = 0
y_done = 0
z_done = 0
try:
trans = tfBuffer.lookup_transform('base', 'right_gripper_base',
rospy.Time())
tf_px = trans.transform.translation.x
tf_py = trans.transform.translation.y
tf_pz = trans.transform.translation.z
except (tf2_ros.LookupException, tf2_ros.ConnectivityException,
tf2_ros.ExtrapolationException):
continue
offset = 0.14
x = tf_px + offset
y = tf_py
z = tf_pz
update = 0
execute = 0
i = 0
while (((x_done == 0 or y_done == 0 or z_done == 0) or update == 0)
and (i < 3)):
if (update == 0):
try:
trans = tfBuffer.lookup_transform(
'base', 'right_gripper_base', rospy.Time())
tf_px = trans.transform.translation.x
tf_py = trans.transform.translation.y
tf_pz = trans.transform.translation.z
tf_rx = trans.transform.rotation.x
tf_ry = trans.transform.rotation.y
tf_rz = trans.transform.rotation.z
tf_rw = trans.transform.rotation.w
x = tf_px + offset
y = tf_py
z = tf_pz
except (tf2_ros.LookupException,
tf2_ros.ConnectivityException,
tf2_ros.ExtrapolationException):
continue
update = 1
execute = 0
#print("TF'd while")
goal_1 = PoseStamped()
goal_1.header.frame_id = "base"
goal_1.pose.position.x = x
goal_1.pose.position.y = y
goal_1.pose.position.z = z
#Orientation as a quaternion
goal_1.pose.orientation.x = 0 # -0.026
goal_1.pose.orientation.y = 0.707 # 0.723
goal_1.pose.orientation.z = 0 # -0.052
goal_1.pose.orientation.w = 0.707 # 0.689
waypoints.append(goal_1.pose)
print(x)
if (x < goal_x + offset - 0.05):
x = x + step_size
print("add")
elif (x > goal_x + offset + 0.05):
x = x - step_size
print("sub")
else:
x_done = 1
print("xdone")
if (y < goal_y - 0.05):
y = y + step_size
elif (y > goal_y + 0.05):
y = y - step_size
else:
y_done = 1
print("ydone")
if (z < goal_z - 0.05):
z = z + step_size
elif (z > goal_z + 0.05):
z = z - step_size
else:
z_done = 1
print("zdone")
if (x_done == 1 and y_done == 1 and z_done == 1):
execute = 1
print("execute done")
if (execute == 1):
while not rospy.is_shutdown():
try:
plan = planner.plan_to_pose(waypoints, list())
print("EXECUTED ZERO")
if not control.execute_path(plan):
raise Exception("Execution failed")
except Exception as e:
print e
else:
break
#STOP = control._flag
waypoints = list()
update = 0
#print("EXEcute")
#print(i)
i = i + 1
#print("END OF LOOP")
jenga = 1
control._probing = 0
# END OF WORKING CODE
while not rospy.is_shutdown():
value_of_step_up = 0.023
value_of_step_left = 0.0125
story = 0
while (story < 1):
control._probing = 1
## ZERO THE ROBOT ##
goal_1 = PoseStamped()
goal_1.header.frame_id = "base"
goal_1.pose.position.x = 0.585
goal_1.pose.position.y = 0.156
goal_1.pose.position.z = -0.138
goal_1.pose.orientation.x = 0 # -0.026
goal_1.pose.orientation.y = 0.707 # 0.723
goal_1.pose.orientation.z = 0 # -0.052
goal_1.pose.orientation.w = 0.707 # 0.689
waypoints.append(goal_1.pose)
while not rospy.is_shutdown():
try:
plan = planner.plan_to_pose(waypoints, list())
if not control.execute_path(plan):
raise Exception("Execution failed")
except Exception as e:
print e
else:
break
waypoints = list()
## ZERO THE ROBOT ##
print("ROBOT ZEROED")
#ospy.sleep(2)
bump = 0
while (story - bump > 0):
if (bump > 2):
value_of_step_up = 0.033
else:
value_of_step_up = 0.025
moveUp(planner, control, waypoints, value_of_step_up)
waypoints = list()
rospy.sleep(2)
bump += 1
probe(planner, control, waypoints)
waypoints = list()
rospy.sleep(2)
moveLeft(planner, control, waypoints, value_of_step_left)
waypoints = list()
rospy.sleep(2)
probe(planner, control, waypoints)
waypoints = list()
rospy.sleep(2)
moveLeft(planner, control, waypoints, value_of_step_left)
waypoints = list()
rospy.sleep(2)
probe(planner, control, waypoints)
waypoints = list()
rospy.sleep(2)
story += 1
control._probing = 1
## ZERO THE ROBOT ##
goal_1 = PoseStamped()
goal_1.header.frame_id = "base"
goal_1.pose.position.x = 0.585
goal_1.pose.position.y = 0.156
goal_1.pose.position.z = -0.138
goal_1.pose.orientation.x = 0 # -0.026
goal_1.pose.orientation.y = 0.707 # 0.723
goal_1.pose.orientation.z = 0 # -0.052
goal_1.pose.orientation.w = 0.707 # 0.689
waypoints.append(goal_1.pose)
while not rospy.is_shutdown():
try:
plan = planner.plan_to_pose(waypoints, list())
if not control.execute_path(plan):
raise Exception("Execution failed")
except Exception as e:
print e
else:
break
waypoints = list()
## ZERO THE ROBOT ##
print("ROBOT ZEROED")
#rospy.sleep(2)
control._probing = 0
control._trigger = 0
control._flag = 0
control._zero = 0
#control._velocity_scalar = 0.5
step_size = 0.0005 #0.0025
FINAL_X = 0.785 #
offset = 0.14
tf_px = 0.585
tf_py = 0.156
tf_pz = -0.138
tf_rx = 0
tf_ry = 0.707
tf_rz = 0
tf_rw = 0.707
goal_x = 0.585
goal_y = 0.156
goal_z = -0.138
try:
trans = tfBuffer.lookup_transform('base', 'right_gripper_base',
rospy.Time())
tf_px = trans.transform.translation.x
tf_py = trans.transform.translation.y
tf_pz = trans.transform.translation.z
tf_rx = trans.transform.rotation.x
tf_ry = trans.transform.rotation.y
tf_rz = trans.transform.rotation.z
tf_rw = trans.transform.rotation.w
goal_x = tf_px
goal_y = tf_py
goal_z = tf_pz
except (tf2_ros.LookupException, tf2_ros.ConnectivityException,
tf2_ros.ExtrapolationException):
continue
offset = 0.14
FINAL_X = goal_x + offset + 0.2
x = tf_px + offset
print("TF'd zero")
update = 0
execute = 0
i = 0
STOP = 0
while ((x < FINAL_X or update == 0) and (i < 5)):
if (update == 0):
try:
trans = tfBuffer.lookup_transform('base',
'right_gripper_base',
rospy.Time())
tf_px = trans.transform.translation.x
tf_py = trans.transform.translation.y
tf_pz = trans.transform.translation.z
tf_rx = trans.transform.rotation.x
tf_ry = trans.transform.rotation.y
tf_rz = trans.transform.rotation.z
tf_rw = trans.transform.rotation.w
x = tf_px + offset
except (tf2_ros.LookupException, tf2_ros.ConnectivityException,
tf2_ros.ExtrapolationException):
continue
update = 1
execute = 0
y = tf_py
z = tf_pz
if (y < goal_y - 0.05):
y = y + step_size
if (y > goal_y + 0.05):
y = y - step_size
if (z < goal_z - 0.05):
z = z + step_size
if (z > goal_z + 0.05):
z = z - step_size
#print("TF'd while")
goal_1 = PoseStamped()
goal_1.header.frame_id = "base"
goal_1.pose.position.x = x
goal_1.pose.position.y = y
goal_1.pose.position.z = z
#Orientation as a quaternion
goal_1.pose.orientation.x = 0 # -0.026
goal_1.pose.orientation.y = 0.707 # 0.723
goal_1.pose.orientation.z = 0 # -0.052
goal_1.pose.orientation.w = 0.707 # 0.689
waypoints.append(goal_1.pose)
x = x + step_size
if (x >= FINAL_X):
execute = 1
#print("added waypoint")
if (execute == 1):
while not rospy.is_shutdown():
try:
control._probing = 0
plan = planner.plan_to_pose(waypoints, list())
print("TAKING BLOCK OUT EXECUTE")
if not control.execute_path(plan):
raise Exception("Execution failed")
except Exception as e:
print e
else:
break
#STOP = control._flag
waypoints = list()
update = 0
#print("EXEcute")
#print(i)
i = i + 1
print("END OF LOOP")
control._probing = 0
control._trigger = 0
control._flag = 0
control._zero = 1
## ZERO THE ROBOT ##
goal_1 = PoseStamped()
goal_1.header.frame_id = "base"
goal_1.pose.position.x = 0.585
goal_1.pose.position.y = 0.156
goal_1.pose.position.z = -0.138
goal_1.pose.orientation.x = 0 # -0.026
goal_1.pose.orientation.y = 0.707 # 0.723
goal_1.pose.orientation.z = 0 # -0.052
goal_1.pose.orientation.w = 0.707 # 0.689
waypoints.append(goal_1.pose)
while not rospy.is_shutdown():
try:
plan = planner.plan_to_pose(waypoints, list())
if not control.execute_path(plan):
raise Exception("Execution failed")
except Exception as e:
print e
else:
break
waypoints = list()
## ZERO THE ROBOT ##
print("ROBOT ZEROED")
#ospy.sleep(2)
control._probing = 0
control._trigger = 0
control._flag = 0
control._zero = 0
moveLeft(planner, control, waypoints, value_of_step_left)
waypoints = list()
rospy.sleep(2)
moveLeft(planner, control, waypoints, value_of_step_left)
waypoints = list()
rospy.sleep(2)
#control._velocity_scalar = 0.5
step_size = 0.0005 #0.0025
FINAL_X = 0.785 #
offset = 0.14
tf_px = 0.585
tf_py = 0.156
tf_pz = -0.138
tf_rx = 0
tf_ry = 0.707
tf_rz = 0
tf_rw = 0.707
goal_x = 0.585
goal_y = 0.156
goal_z = -0.138
try:
trans = tfBuffer.lookup_transform('base', 'right_gripper_base',
rospy.Time())
tf_px = trans.transform.translation.x
tf_py = trans.transform.translation.y
tf_pz = trans.transform.translation.z
tf_rx = trans.transform.rotation.x
tf_ry = trans.transform.rotation.y
tf_rz = trans.transform.rotation.z
tf_rw = trans.transform.rotation.w
goal_x = tf_px
goal_y = tf_py
goal_z = tf_pz
except (tf2_ros.LookupException, tf2_ros.ConnectivityException,
tf2_ros.ExtrapolationException):
continue
offset = 0.14
FINAL_X = goal_x + offset + 0.2
x = tf_px + offset
print("TF'd zero")
update = 0
execute = 0
i = 0
STOP = 0
while ((x < FINAL_X or update == 0) and (i < 5)):
if (update == 0):
try:
trans = tfBuffer.lookup_transform('base',
'right_gripper_base',
rospy.Time())
tf_px = trans.transform.translation.x
tf_py = trans.transform.translation.y
tf_pz = trans.transform.translation.z
tf_rx = trans.transform.rotation.x
tf_ry = trans.transform.rotation.y
tf_rz = trans.transform.rotation.z
tf_rw = trans.transform.rotation.w
x = tf_px + offset
except (tf2_ros.LookupException, tf2_ros.ConnectivityException,
tf2_ros.ExtrapolationException):
continue
update = 1
execute = 0
y = tf_py
z = tf_pz
if (y < goal_y - 0.05):
y = y + step_size
if (y > goal_y + 0.05):
y = y - step_size
if (z < goal_z - 0.05):
z = z + step_size
if (z > goal_z + 0.05):
z = z - step_size
#print("TF'd while")
goal_1 = PoseStamped()
goal_1.header.frame_id = "base"
goal_1.pose.position.x = x
goal_1.pose.position.y = y
goal_1.pose.position.z = z
#Orientation as a quaternion
goal_1.pose.orientation.x = 0 # -0.026
goal_1.pose.orientation.y = 0.707 # 0.723
goal_1.pose.orientation.z = 0 # -0.052
goal_1.pose.orientation.w = 0.707 # 0.689
waypoints.append(goal_1.pose)
x = x + step_size
if (x >= FINAL_X):
execute = 1
#print("added waypoint")
if (execute == 1):
while not rospy.is_shutdown():
try:
control._probing = 0
plan = planner.plan_to_pose(waypoints, list())
print("TAKING BLOCK OUT EXECUTE")
if not control.execute_path(plan):
raise Exception("Execution failed")
except Exception as e:
print e
else:
break
#STOP = control._flag
waypoints = list()
update = 0
#print("EXEcute")
#print(i)
i = i + 1
print("END OF LOOP")
control._probing = 0
control._trigger = 0
control._flag = 0
control._zero = 1
## ZERO THE ROBOT ##
goal_1 = PoseStamped()
goal_1.header.frame_id = "base"
goal_1.pose.position.x = 0.585
goal_1.pose.position.y = 0.156
goal_1.pose.position.z = -0.138
goal_1.pose.orientation.x = 0 # -0.026
goal_1.pose.orientation.y = 0.707 # 0.723
goal_1.pose.orientation.z = 0 # -0.052
goal_1.pose.orientation.w = 0.707 # 0.689
waypoints.append(goal_1.pose)
while not rospy.is_shutdown():
try:
plan = planner.plan_to_pose(waypoints, list())
if not control.execute_path(plan):
raise Exception("Execution failed")
except Exception as e:
print e
else:
break
waypoints = list()
## ZERO THE ROBOT ##
print("ROBOT ZEROED")
#ospy.sleep(2)
control._probing = 0
control._trigger = 0
control._flag = 0
control._zero = 0
#waypoints = list()
#zero_spot(planner, control, waypoints)
#waypoints = list()
#rospy.sleep(2)
#waypoints = list()
#rospy.sleep(2)
#zero_spot(planner, control, waypoints)
#waypoints = list()
#rospy.sleep(2)
# #probe(planner, control, waypoints)
# waypoints = list()
# moveUp(planner, control, waypoints)
# waypoints = list()
# #probe(planner, control, waypoints)
# waypoints = list()
# moveLeft(planner, control, waypoints)
# waypoints = list()
# #probe(planner, control, waypoints)
# waypoints = list()
# moveLeft(planner, control, waypoints)
# waypoints = list()
# #probe(planner, control, waypoints)
# waypoints = list()
#plt.plot(control._sensor_data)
s = list(map(float, control._sensor_data))
scales = list(map(float, control._scalar_data))
goal_line = list(map(float, control._goal_list))
#s = list(map(float, control._debug))
#control._sensor_data = sorted(control._sensor_data)
#print(min(s))
#print(max(s))
plt.subplot(211)
plt.plot(s)
plt.plot(goal_line)
plt.axis([0, len(control._sensor_data), min(s), max(s)])
plt.ylabel('Sensor Data')
plt.subplot(212)
plt.plot(scales)
plt.show()
# PRINT GRID
control._sensor_data = list()
control._scalar_data = list()
control._goal_list = list()
waypoints = list()
control._velocity_scalar = 1
def right_arm_go_to_pose_goal(self):
# 设置动作对象变量,此处为arm
right_arm = self.right_arm
# 获取当前末端执行器位置姿态
pose_goal = right_arm.get_current_pose().pose
# 限制末端夹具运动
right_joint_const = JointConstraint()
right_joint_const.joint_name = "gripper_r_joint_r"
if Rightfinger > -55 :
right_joint_const.position = 0.024
else:
right_joint_const.position = 0
right_joint_const.weight = 1.0
# 限制1轴转动
right_joint1_const = JointConstraint()
right_joint1_const.joint_name = "yumi_joint_1_r"
right_joint1_const.position = 0
right_joint1_const.tolerance_above = 120
right_joint1_const.tolerance_below = 0
right_joint1_const.weight = 1.0
# 限制2轴转动
right_joint2_const = JointConstraint()
right_joint2_const.joint_name = "yumi_joint_2_r"
right_joint2_const.position = 0
right_joint2_const.tolerance_above = 0
right_joint2_const.tolerance_below = 150
right_joint2_const.weight = 1.0
# 限制3轴转动
right_joint3_const = JointConstraint()
right_joint3_const.joint_name = "yumi_joint_3_r"
right_joint3_const.position = 0
right_joint3_const.tolerance_above = 35
right_joint3_const.tolerance_below = 55
right_joint3_const.weight = 1.0
# 限制4轴转动
right_joint4_const = JointConstraint()
right_joint4_const.joint_name = "yumi_joint_4_r"
right_joint4_const.position = 0
right_joint4_const.tolerance_above = 60
right_joint4_const.tolerance_below = 75
right_joint4_const.weight = 1.0
# 限制5轴转动
right_joint5_const = JointConstraint()
right_joint5_const.joint_name = "yumi_joint_5_r"
right_joint5_const.position = 40
right_joint5_const.tolerance_above = 50
right_joint5_const.tolerance_below = 20
right_joint5_const.weight = 1.0
# 限制6轴转动
right_joint6_const = JointConstraint()
right_joint6_const.joint_name = "yumi_joint_6_r"
right_joint6_const.position = 0
right_joint6_const.tolerance_above = 10
right_joint6_const.tolerance_below = 35
right_joint6_const.weight = 1.0
# 限制7轴转动
right_joint7_const = JointConstraint()
right_joint7_const.joint_name = "yumi_joint_7_r"
right_joint7_const.position = -10
right_joint7_const.tolerance_above = 0
right_joint7_const.tolerance_below = 160
right_joint7_const.weight = 1.0
# 限制末端位移
right_position_const = PositionConstraint()
right_position_const.header = Header()
right_position_const.link_name = "gripper_r_joint_r"
right_position_const.target_point_offset.x = 0.5
right_position_const.target_point_offset.y = -0.5
right_position_const.target_point_offset.z = 1.0
right_position_const.weight = 1.0
# 添加末端姿态约束:
right_orientation_const = OrientationConstraint()
right_orientation_const.header = Header()
right_orientation_const.orientation = pose_goal.orientation
right_orientation_const.link_name = "gripper_r_finger_r"
right_orientation_const.absolute_x_axis_tolerance = 0.50
right_orientation_const.absolute_y_axis_tolerance = 0.25
right_orientation_const.absolute_z_axis_tolerance = 0.50
right_orientation_const.weight = 100
# 施加全约束
consts = Constraints()
consts.joint_constraints = [right_joint_const]
# consts.orientation_constraints = [right_orientation_const]
# consts.position_constraints = [right_position_const]
right_arm.set_path_constraints(consts)
# 设置动作对象目标位置姿态
pose_goal.orientation.x = Right_Qux
pose_goal.orientation.y = Right_Quy
pose_goal.orientation.z = Right_Quz
pose_goal.orientation.w = Right_Quw
pose_goal.position.x = (Neurondata[5]-0.05)*1.48+0.053
pose_goal.position.y = (Neurondata[3]+0.18)*1.48-0.12
pose_goal.position.z = (Neurondata[4]-0.53)*1.48+0.47
right_arm.set_pose_target(pose_goal)
print "End effector pose %s" % pose_goal
# # 设置动作对象目标位置姿态
# pose_goal.orientation.x = 0.1644
# pose_goal.orientation.y = 0.3111
# pose_goal.orientation.z = 0.9086
# pose_goal.orientation.w = 0.2247
# pose_goal.position.x = (Neurondata[5]-0.05)*1.48+0.053
# pose_goal.position.y = (Neurondata[3]+0.18)*1.48-0.12
# pose_goal.position.z = (Neurondata[4]-0.53)*1.48+0.47
# right_arm.set_pose_target(pose_goal)
# print "End effector pose %s" % pose_goal
# # 设置动作对象目标位置姿态
# pose_goal.orientation.x = pose_goal.orientation.x
# pose_goal.orientation.y = pose_goal.orientation.y
# pose_goal.orientation.z = pose_goal.orientation.z
# pose_goal.orientation.w = pose_goal.orientation.w
# pose_goal.position.x = pose_goal.position.x
# pose_goal.position.y = pose_goal.position.y - 0.01
# pose_goal.position.z = pose_goal.position.z
# right_arm.set_pose_target(pose_goal)
# print "End effector pose %s" % pose_goal
# 规划和输出动作
traj = right_arm.plan()
right_arm.execute(traj, wait=False)
# 动作完成后清除目标信息
right_arm.clear_pose_targets()
# 清除路径约束
right_arm.clear_path_constraints()
# 确保没有剩余未完成动作在执行
right_arm.stop()
def left_arm_go_to_pose_goal(self):
# 设置动作对象变量,此处为arm
left_arm = self.left_arm
# 获取当前末端执行器位置姿态
pose_goal = left_arm.get_current_pose().pose
# 限制末端夹具运动
left_joint_const = JointConstraint()
left_joint_const.joint_name = "gripper_l_joint_r"
if Leftfinger > -32 :
left_joint_const.position = 0.024
else:
left_joint_const.position = 0
left_joint_const.weight = 1.0
# 限制末端位移
left_position_const = PositionConstraint()
left_position_const.header = Header()
left_position_const.link_name = "gripper_l_joint_r"
left_position_const.target_point_offset.x = 0.5
left_position_const.target_point_offset.y = -0.5
left_position_const.target_point_offset.z = 1.0
left_position_const.weight = 1.0
# # 限制1轴转动
left_joint1_const = JointConstraint()
left_joint1_const.joint_name = "yumi_joint_1_l"
left_joint1_const.position = 0
left_joint1_const.tolerance_above = 1.76
left_joint1_const.tolerance_below = 0
left_position_const.weight = 1.0
# 限制2轴转动
left_joint2_const = JointConstraint()
left_joint2_const.joint_name = "yumi_joint_2_l"
left_joint2_const.position = 0
left_joint2_const.tolerance_above = 0
left_joint2_const.tolerance_below = 150
left_joint2_const.weight = 1.0
# 限制3轴转动
left_joint3_const = JointConstraint()
left_joint3_const.joint_name = "yumi_joint_3_l"
left_joint3_const.position = 0
left_joint3_const.tolerance_above = 35
left_joint3_const.tolerance_below = 55
left_joint3_const.weight = 1.0
# 限制4轴转动
left_joint4_const = JointConstraint()
left_joint4_const.joint_name = "yumi_joint_4_l"
left_joint4_const.position = 0
left_joint4_const.tolerance_above = 60
left_joint4_const.tolerance_below = 75
left_joint4_const.weight = 1.0
# 限制5轴转动
right_joint5_const = JointConstraint()
right_joint5_const.joint_name = "yumi_joint_5_l"
right_joint5_const.position = 40
right_joint5_const.tolerance_above = 50
right_joint5_const.tolerance_below = 20
right_joint5_const.weight = 1.0
# 限制6轴转动
left_joint6_const = JointConstraint()
left_joint6_const.joint_name = "yumi_joint_6_l"
left_joint6_const.position = 0
left_joint6_const.tolerance_above = 10
left_joint6_const.tolerance_below = 35
left_joint6_const.weight = 1.0
# 限制7轴转动
left_joint7_const = JointConstraint()
left_joint7_const.joint_name = "yumi_joint_7_l"
left_joint7_const.position = -10
left_joint7_const.tolerance_above = 0
left_joint7_const.tolerance_below = 160
left_joint7_const.weight = 1.0
# 限制末端位移
left_position_const = PositionConstraint()
left_position_const.header = Header()
left_position_const.link_name = "gripper_l_joint_r"
left_position_const.target_point_offset.x = 0.5
left_position_const.target_point_offset.y = 0.25
left_position_const.target_point_offset.z = 0.5
left_position_const.weight = 1.0
# 添加末端姿态约束:
left_orientation_const = OrientationConstraint()
left_orientation_const.header = Header()
left_orientation_const.orientation = pose_goal.orientation
left_orientation_const.link_name = "gripper_l_finger_r"
left_orientation_const.absolute_x_axis_tolerance = 0.5
left_orientation_const.absolute_y_axis_tolerance = 0.25
left_orientation_const.absolute_z_axis_tolerance = 0.5
left_orientation_const.weight = 1
# 施加全约束
consts = Constraints()
consts.joint_constraints = [left_joint_const]
# consts.orientation_constraints = [left_orientation_const]
# consts.position_constraints = [left_position_const]
left_arm.set_path_constraints(consts)
# 设置动作对象目标位置姿态
pose_goal.orientation.x = Left_Qux
pose_goal.orientation.y = Left_Quy
pose_goal.orientation.z = Left_Quz
pose_goal.orientation.w = Left_Quw
pose_goal.position.x = (Neurondata[11]-0.05)*1.48+0.053
pose_goal.position.y = (Neurondata[9]-0.18)*1.48+0.12
pose_goal.position.z = (Neurondata[10]-0.53)*1.48+0.47
left_arm.set_pose_target(pose_goal)
print "End effector pose %s" % pose_goal
# # 设置动作对象目标位置姿态
# pose_goal.orientation.x = pose_goal.orientation.x
# pose_goal.orientation.y = pose_goal.orientation.y
# pose_goal.orientation.z = pose_goal.orientation.z
# pose_goal.orientation.w = pose_goal.orientation.w
# pose_goal.position.x = pose_goal.position.x
# pose_goal.position.y = pose_goal.position.y - 0.01
# pose_goal.position.z = pose_goal.position.z
# left_arm.set_pose_target(pose_goal)
# print "End effector pose %s" % pose_goal
# 规划和输出动作
traj = left_arm.plan()
left_arm.execute(traj, wait=False)
# 动作完成后清除目标信息
left_arm.clear_pose_targets()
# 清除路径约束
left_arm.clear_path_constraints()
# 确保没有剩余未完成动作在执行
left_arm.stop()
def main():
global prev_msg
global sec_pre
plandraw = PathPlanner('right_arm')
# plandraw.gripper_close()
# rospy.sleep(10)
# plandraw.grip?per_op
plandraw.start_position()
## BOX
box_size = np.array([2.4, 2.4, 0.1])
box_pose = PoseStamped()
box_pose.header.stamp = rospy.Time.now()
box_pose.header.frame_id = "base"
box_pose.pose.position.x = 0
box_pose.pose.position.y = 0
box_pose.pose.position.z = -0.4
box_pose.pose.orientation.x = 0.00
box_pose.pose.orientation.y = 0.00
box_pose.pose.orientation.z = 0.00
box_pose.pose.orientation.w = 1.00
plandraw.add_box_obstacle(box_size, "box1", box_pose)
box_size2 = np.array([2.4, 2.4, 0.1])
box_pose2 = PoseStamped()
box_pose2.header.stamp = rospy.Time.now()
box_pose2.header.frame_id = "base"
box_pose2.pose.position.x = 0
box_pose2.pose.position.y = 0
box_pose2.pose.position.z = 1
box_pose2.pose.orientation.x = 0.00
box_pose2.pose.orientation.y = 0.00
box_pose2.pose.orientation.z = 0.00
box_pose2.pose.orientation.w = 1.00
plandraw.add_box_obstacle(box_size2, "box2", box_pose2)
orien_const = OrientationConstraint()
orien_const.link_name = "right_gripper_tip"
orien_const.header.frame_id = "base"
orien_const.orientation.y = -1.0
orien_const.absolute_x_axis_tolerance = 0.1
orien_const.absolute_y_axis_tolerance = 0.1
orien_const.absolute_z_axis_tolerance = 0.1
orien_const.weight = 1.0
def set_use_pen(pen_id, goal_1):
if pen_id == 0: # Blue The innar one
goal_1.pose.orientation.x = 0.0
goal_1.pose.orientation.y = -0.9848078
goal_1.pose.orientation.z = 0.0
goal_1.pose.orientation.w = -0.1736482
goal_1.pose.position.x -= 0.003
goal_1.pose.position.y -= 0.013
goal_1.pose.position.z -= 0.011
if pen_id == 1:
goal_1.pose.orientation.x = 0.0
goal_1.pose.orientation.y = -1.0
goal_1.pose.orientation.z = 0.0
goal_1.pose.orientation.w = 0.0
goal_1.pose.position.x -= 0.014
goal_1.pose.position.y -= 0.002
goal_1.pose.position.z -= 0.017
if pen_id == 2:
goal_1.pose.orientation.x = 0.0
goal_1.pose.orientation.y = -0.9848078
goal_1.pose.orientation.z = 0.0
goal_1.pose.orientation.w = 0.1736482
goal_1.pose.position.x -= 0.028
goal_1.pose.position.y -= 0.003
goal_1.pose.position.z -= 0.010
# if pen_id == 0:
# goal_1.pose.orientation.x = 0.0
# goal_1.pose.orientation.y = -0.925877
# goal_1.pose.orientation.z = 0.0
# goal_1.pose.orientation.w = -0.360095
# if pen_id == 1:
# goal_1.pose.orientation.x = 0.0
# goal_1.pose.orientation.y = -0.9994
# goal_1.pose.orientation.z = 0.0
# goal_1.pose.orientation.w = 0.005035
# if pen_id == 2:
# goal_1.pose.orientation.x = 0.0
# goal_1.pose.orientation.y = -0.974507
# goal_1.pose.orientation.z = 0.0
# goal_1.pose.orientation.w = 0.222739
waypoints = []
while not rospy.is_shutdown():
#raw_input("~~~~~~~~~~~~!!!!!!!!!!!!")
while not rospy.is_shutdown():
try:
while len(queue):
# print(len(queue))
cur = queue.popleft()
x, y, z = cur.position_x, cur.position_y, cur.position_z
x += 0.002 # ada different coordinate
z -= 0.103
z += 0.037
# z += 0.2
x -= 0.013
y += 0.010
z += 0.005
x += 0.12
if cur.status_type != "edge_grad":
# ti bi !!!!! luo bi !!!!
if cur.status_type == "starting":
print("start")
# waypoints = []
goal_1 = PoseStamped()
goal_1.header.frame_id = "base"
#x, y, and z position
goal_1.pose.position.x = x
goal_1.pose.position.y = y
goal_1.pose.position.z = z + 0.12
#Orientation as a quaternion
# [0.766, -0.623, 0.139, -0.082]
# [-0.077408, 0.99027, -0.024714, ]
set_use_pen(cur.pen_type, goal_1)
waypoints.append(copy.deepcopy(goal_1.pose))
goal_1.pose.position.z -= 0.12
waypoints.append(copy.deepcopy(goal_1.pose))
# plan = plandraw.plan_to_pose(goal_1, [orien_const], waypoints)
# if not plandraw.execute_plan(plan):
# raise Exception("Starting execution failed")
# else:
# queue.pop(0)
elif cur.status_type == "next_point":
# print("next")
goal_1 = PoseStamped()
goal_1.header.frame_id = "base"
#x, y, and z position
goal_1.pose.position.x = x
goal_1.pose.position.y = y
goal_1.pose.position.z = z
#Orientation as a quaternion
# goal_1.pose.orientation.x = 0.459962
# goal_1.pose.orientation.y = -0.7666033
# goal_1.pose.orientation.z = 0.0
# goal_1.pose.orientation.w = -0.4480562
set_use_pen(cur.pen_type, goal_1)
# waypoints = []
waypoints.append(copy.deepcopy(goal_1.pose))
# plan = plandraw.plan_to_pose(goal_1, [orien_const], waypoints)
# if not plandraw.execute_plan(plan):
# raise Exception("Execution failed, point is ", cur)
# else:
# queue.pop(0)
elif cur.status_type == "ending":
print("ppppppp ", cur)
# mmm = plandraw.get_cur_pos().pose
goal_1 = PoseStamped()
goal_1.header.frame_id = "base"
#x, y, and z position
goal_1.pose.position.x = x
goal_1.pose.position.y = y
goal_1.pose.position.z = z + 0.12
#Orientation as a quaternion
set_use_pen(1, goal_1)
# waypoints = []
waypoints.append(copy.deepcopy(goal_1.pose))
plan = plandraw.plan_to_pose(goal_1, [], waypoints)
waypoints = []
# queue.pop(0)
if not plandraw.execute_plan(plan):
raise Exception("Execution failed")
print("ti bi")
# rospy.sleep(5)
#raw_input("Press <Enter> to move next!!!")
except Exception as e:
print e
else:
#print("lllllllllllllllllllll")
break
def probe(self, nx, ny, dx, dy):
#Initialize arms
robot = moveit_commander.RobotCommander()
scene = moveit_commander.PlanningSceneInterface()
right_arm = moveit_commander.MoveGroupCommander('right_arm')
right_arm.set_planner_id('RRTConnectkConfigDefault')
right_arm.set_planning_time(15)
# Set constraints
rospy.sleep(2)
# Assuming you're facing the wall, looking at the robot
# And the robot's computer is to your left
# Then Wall 1 is the wall on "your" right
# Wall 2 is the wall to "your" left
# Wall 3 is the wall in the back
#raw_input("press any key to add wall 1")
p = PoseStamped()
p.header.frame_id = robot.get_planning_frame()
p.pose.position.x = 0.7
p.pose.position.y = 0.
p.pose.position.z = 0.
scene.add_box("wall1", p, (0.1, 5, 5))
#raw_input("press any key to add wall 2")
p = PoseStamped()
p.header.frame_id = robot.get_planning_frame()
p.pose.position.x = -1
p.pose.position.y = 0.
p.pose.position.z = 0.
scene.add_box("wall2", p, (0.1, 5, 5))
#raw_input("press any key to add wall 3")
p = PoseStamped()
p.header.frame_id = robot.get_planning_frame()
p.pose.position.x = 0
p.pose.position.y = 0.7
p.pose.position.z = 0.
scene.add_box("wall3", p, (5, 0.1, 5))
#raw_input("press any key to add patient")
p = PoseStamped()
p.header.frame_id = robot.get_planning_frame()
p.pose.position.x = self.table_x
p.pose.position.y = self.table_y
p.pose.position.z = self.table_z / 2.0
scene.add_box("patient", p, (0.3, 0.3, self.table_z / 2.0))
orien_const = OrientationConstraint()
orien_const.link_name = "right_gripper"
orien_const.header.frame_id = "base"
orien_const.orientation.y = -1.0
orien_const.absolute_x_axis_tolerance = 0.1
orien_const.absolute_y_axis_tolerance = 0.1
orien_const.absolute_z_axis_tolerance = 50
orien_const.weight = .5
consts = Constraints()
consts.orientation_constraints = [orien_const]
right_arm.set_path_constraints(consts)
ans = {} # start with empty dictionary
for i in range(-nx, nx + 1):
for j in range(-nx, nx + 1):
ans[(i, j)] = self.poke_at(right_arm, self.table_x + i * dx,
self.table_y + j * dy)
return ans
def moveToPose(self,
pose_stamped,
gripper_frame,
tolerance=0.01,
wait=True,
**kwargs):
# Check arguments
supported_args = ("max_velocity_scaling_factor", "planner_id",
"planning_time", "plan_only", "start_state")
for arg in kwargs.keys():
if not arg in supported_args:
rospy.loginfo("moveToPose: unsupported argument: %s", arg)
# Create goal
g = MoveGroupGoal()
pose_transformed = self._listener.transformPose(
self._fixed_frame, pose_stamped)
# 1. fill in request workspace_parameters
# 2. fill in request start_state
try:
g.request.start_state = kwargs["start_state"]
except KeyError:
g.request.start_state.is_diff = True
# 3. fill in request goal_constraints
c1 = Constraints()
c1.position_constraints.append(PositionConstraint())
c1.position_constraints[0].header.frame_id = self._fixed_frame
c1.position_constraints[0].link_name = gripper_frame
b = BoundingVolume()
s = SolidPrimitive()
s.dimensions = [tolerance * tolerance]
s.type = s.SPHERE
b.primitives.append(s)
b.primitive_poses.append(pose_transformed.pose)
c1.position_constraints[0].constraint_region = b
c1.position_constraints[0].weight = 1.0
c1.orientation_constraints.append(OrientationConstraint())
c1.orientation_constraints[0].header.frame_id = self._fixed_frame
c1.orientation_constraints[
0].orientation = pose_transformed.pose.orientation
c1.orientation_constraints[0].link_name = gripper_frame
c1.orientation_constraints[0].absolute_x_axis_tolerance = tolerance
c1.orientation_constraints[0].absolute_y_axis_tolerance = tolerance
c1.orientation_constraints[0].absolute_z_axis_tolerance = tolerance
c1.orientation_constraints[0].weight = 1.0
g.request.goal_constraints.append(c1)
# 4. fill in request path constraints
# 5. fill in request trajectory constraints
# 6. fill in request planner id
try:
g.request.planner_id = kwargs["planner_id"]
except KeyError:
if self.planner_id:
g.request.planner_id = self.planner_id
# 7. fill in request group name
g.request.group_name = self._group
# 8. fill in request number of planning attempts
try:
g.request.num_planning_attempts = kwargs["num_attempts"]
except KeyError:
g.request.num_planning_attempts = 1
# 9. fill in request allowed planning time
try:
g.request.allowed_planning_time = kwargs["planning_time"]
except KeyError:
g.request.allowed_planning_time = self.planning_time
# Fill in velocity scaling factor
try:
g.request.max_velocity_scaling_factor = kwargs[
"max_velocity_scaling_factor"]
except KeyError:
pass # do not fill in at all
# 10. fill in planning options diff
g.planning_options.planning_scene_diff.is_diff = True
g.planning_options.planning_scene_diff.robot_state.is_diff = True
# 11. fill in planning options plan only
try:
g.planning_options.plan_only = kwargs["plan_only"]
except KeyError:
g.planning_options.plan_only = self.plan_only
# 12. fill in other planning options
g.planning_options.look_around = False
g.planning_options.replan = False
# 13. send goal
self._action.send_goal(g)
if wait:
self._action.wait_for_result()
return self._action.get_result()
else:
return None
def main():
"""
Main Script
"""
#===================================================
# Code to add gripper
#rospy.init_node('gripper_test')
#Set up the right gripper
right_gripper = robot_gripper.Gripper('right_gripper')
#Calibrate the gripper (other commands won't work unless you do this first)
# print('Calibrating...')
# right_gripper.calibrate()
# rospy.sleep(2.0)
#Close the right gripper to hold publisher
# print('Closing...')
# right_gripper.close()
# rospy.sleep(1.0)
#===================================================
# Make sure that you've looked at and understand path_planner.py before starting
planner = PathPlanner("right_arm")
Kp = 0.1 * np.array([0.3, 2, 1, 1.5, 2, 2, 3]) # Stolen from 106B Students
Kd = 0.01 * np.array([2, 1, 2, 0.5, 0.5, 0.5, 0.5
]) # Stolen from 106B Students
Ki = 0.01 * np.array([1, 1, 1, 1, 1, 1, 1]) # Untuned
Kw = np.array([0.9, 0.9, 0.9, 0.9, 0.9, 0.9, 0.9]) # Untuned
limb = intera_interface.Limb("right")
control = Controller(Kp, Kd, Ki, Kw, limb)
##
## Add the obstacle to the planning scene here
##
#Tower
#TODO: make wrt to sawyer (currently wrt to ar tag)
X = 0.075
Y = 0.075
Z = 0.045
Xp = 0.0
Yp = -0.0425
Zp = 0.0225
Xpa = 0.00
Ypa = 0.00
Zpa = 0.00
Wpa = 1.00
# pose = PoseStamped()
# pose.header.stamp = rospy.Time.now()
# #TODO: is this the right frame name?
# pose.header.frame_id = "ar_marker_1"
# pose.pose.position.x = Xp
# pose.pose.position.y = Yp
# pose.pose.position.z = Zp
# pose.pose.orientation.x = Xpa
# pose.pose.orientation.y = Ypa
# pose.pose.orientation.z = Zpa
# pose.pose.orientation.w = Wpa
# planner.add_box_obstacle([X,Y,Z], "tower", pose)
#Table (currently wrt ar tag)
X = 1
Y = 1
Z = .005
Xp = 0
Yp = 0
Zp = 0
Xpa = 0.00
Ypa = 0.00
Zpa = 0.00
Wpa = 1.00
pose = PoseStamped()
pose.header.stamp = rospy.Time.now()
#TODO: Is this the correct frame name?
pose.header.frame_id = "ar_marker_1"
pose.pose.position.x = Xp
pose.pose.position.y = Yp
pose.pose.position.z = Zp
pose.pose.orientation.x = Xpa
pose.pose.orientation.y = Ypa
pose.pose.orientation.z = Zpa
pose.pose.orientation.w = Wpa
planner.add_box_obstacle([X, Y, Z], "table", pose)
# #Create a path constraint for the arm
# #UNCOMMENT FOR THE ORIENTATION CONSTRAINTS PART
orien_const = OrientationConstraint()
orien_const.link_name = "right_gripper_tip"
#changed from "base" frame_id
orien_const.header.frame_id = "ar_marker_1"
orien_const.orientation.w = 1
orien_const.absolute_x_axis_tolerance = 0.001
orien_const.absolute_y_axis_tolerance = 0.001
orien_const.absolute_z_axis_tolerance = 0.001
orien_const.weight = 20.0
rospy.sleep(1.0)
while not rospy.is_shutdown():
while not rospy.is_shutdown():
try:
#currently wrt ar tag
goal_1 = PoseStamped()
goal_1.header.frame_id = "ar_marker_1"
#x, y, and z position
goal_1.pose.position.x = 0.0
goal_1.pose.position.y = -0.0425
goal_1.pose.position.z = 0.0225
#Orientation as a quaternion
goal_1.pose.orientation.x = 0
goal_1.pose.orientation.y = np.pi / 2
goal_1.pose.orientation.z = 0
goal_1.pose.orientation.w = 0
plan = planner.plan_to_pose(goal_1, list())
# if not planner.execute_plan(plan):
if not control.execute_path(plan):
raise Exception("Execution failed")
except Exception as e:
print e
else:
break
def pick(self, target_name, grasp_position, pre_grasp_distance,
post_grasp_retreat):
pre_grasp_posture = JointTrajectory()
grasp_posture = JointTrajectory()
pre_grasp_posture = self.make_gripper_posture(GRIPPER_OPEN)
grasp_posture = self.make_gripper_posture(GRIPPER_CLOSED)
limit = {'dist': 0.01, 'r': 0.15, 'p': 0.15, 'y': 0.15}
constraints = Constraints()
oc = OrientationConstraint()
oc.header.frame_id = REFERENCE_FRAME
oc.link_name = 'j2s7s300_end_effector'
oc.absolute_x_axis_tolerance = limit['r'] # radian
oc.absolute_y_axis_tolerance = limit['p']
oc.absolute_z_axis_tolerance = limit['y']
oc.weight = 1.0
oc.orientation = grasp_position.pose.orientation
constraints.orientation_constraints.append(deepcopy(oc))
result = False
replan_times = 1
replan_state = True
while replan_state and replan_times <= 5:
(pre_grasp_path, fraction) = self.get_path(grasp_position.pose,
0.01,
constraints=constraints)
if self._server.current_goal.get_goal_status(
).status == GoalStatus.PREEMPTING:
self.state = STATE.STOP
return
if fraction >= 0.92:
print "Pre_grasp_approach..."
self.arm.execute(pre_grasp_path)
result = self.check(grasp_position.pose, limit)
replan_state = False
replan_times += 1
if result:
result = False
if self.arm.attach_object(
target_name, self.arm.get_end_effector_link(),
[
"j2s7s300_end_effector", "j2s7s300_link_finger_1",
"j2s7s300_link_finger_tip_1", "j2s7s300_link_finger_2",
"j2s7s300_link_finger_tip_2", "j2s7s300_link_finger_3",
"j2s7s300_link_finger_tip_3", "j2s7s300_joint_finger_1",
"j2s7s300_joint_finger_2", "j2s7s300_joint_finger_3"
]):
rospy.logwarn("Attach request sent successfully!")
else:
raise Exception("Can't send attach request!")
print "Grasping..."
self.gripper.set_named_target("Close")
if self._server.current_goal.get_goal_status(
).status == GoalStatus.PREEMPTING:
self.arm.detach_object(target_name)
self.back_to_init_pose()
self.state = STATE.STOP
return
self.gripper.go()
rospy.sleep(1)
post_grasp_position = self.get_retreat_point(
grasp_position.pose, post_grasp_retreat)
print "post_grasp_position: %s\n" % post_grasp_position
replan_times = 1
replan_state = True
while replan_state and replan_times <= 5:
(retreat_path,
fraction) = self.get_path(post_grasp_position,
0.005,
constraints=constraints)
if self._server.current_goal.get_goal_status(
).status == GoalStatus.PREEMPTING:
self.arm.detach_object(target_name)
self.back_to_init_pose(state=1)
self.state = STATE.STOP
return
if fraction > 0.8:
print "Retreating..."
self.arm.execute(retreat_path)
result = self.check(post_grasp_position, limit)
replan_state = False
replan_times += 1
if not result:
self.state = STATE.GRASP_RETREAT_ERR
else:
self.state = STATE.PRE_GRASP_ERR # pre_grasp planning failed
if result:
self.state = STATE.PICK_FINISH
self.arm.clear_path_constraints()
def main():
#Initialize moveit_commander
moveit_commander.roscpp_initialize(sys.argv)
#Start a node
rospy.init_node('moveit_node')
#Set up the right gripper
right_gripper = robot_gripper.Gripper('right')
#Calibrate the gripper (other commands won't work unless you do this first)
print('Calibrating...')
right_gripper.calibrate()
rospy.sleep(2.0)
#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(10)
right_arm.set_planner_id('RRTConnectkConfigDefault')
right_arm.set_planning_time(10)
#First goal pose ------------------------------------------------------
goal_1 = PoseStamped()
goal_1.header.frame_id = "base"
#x, y, and z position
goal_1.pose.position.x = 0.2
goal_1.pose.position.y = -0.5
goal_1.pose.position.z = 0.2
#Orientation as a quaternion
goal_1.pose.orientation.x = 0.0
goal_1.pose.orientation.y = -1.0
goal_1.pose.orientation.z = 0.0
goal_1.pose.orientation.w = 0.0
#Set the goal state to the pose you just defined
right_arm.set_pose_target(goal_1)
#Set the start state for the right arm
right_arm.set_start_state_to_current_state()
#Plan a path
right_plan = right_arm.plan()
#Execute the plan
raw_input(
'Press <Enter> to move the right arm to goal pose 1 (path constraints are never enforced during this motion): '
)
right_arm.execute(right_plan)
#Close the right gripper
print('Closing...')
right_gripper.close()
rospy.sleep(1.0)
#Open the right gripper
print('Opening...')
right_gripper.open()
rospy.sleep(1.0)
print('Done!')
#Second goal pose -----------------------------------------------------
rospy.sleep(2.0)
goal_2 = PoseStamped()
goal_2.header.frame_id = "base"
#x, y, and z position
goal_2.pose.position.x = 0.6
goal_2.pose.position.y = -0.3
goal_2.pose.position.z = 0.0
#Orientation as a quaternion
goal_2.pose.orientation.x = 0.0
goal_2.pose.orientation.y = -1.0
goal_2.pose.orientation.z = 0.0
goal_2.pose.orientation.w = 0.0
#Set the goal state to the pose you just defined
right_arm.set_pose_target(goal_2)
#Set the start state for the right arm
right_arm.set_start_state_to_current_state()
# #Create a path constraint for the arm
# #UNCOMMENT TO ENABLE ORIENTATION CONSTRAINTS
orien_const = OrientationConstraint()
orien_const.link_name = "right_gripper"
orien_const.header.frame_id = "base"
orien_const.orientation.y = -1.0
orien_const.absolute_x_axis_tolerance = 0.1
orien_const.absolute_y_axis_tolerance = 0.1
orien_const.absolute_z_axis_tolerance = 0.1
orien_const.weight = 1.0
consts = Constraints()
consts.orientation_constraints = [orien_const]
right_arm.set_path_constraints(consts)
#Plan a path
right_plan = right_arm.plan()
#Execute the plan
raw_input('Press <Enter> to move the right arm to goal pose 2: ')
right_arm.execute(right_plan)
#Close the right gripper
print('Closing...')
right_gripper.close()
rospy.sleep(1.0)
#Open the right gripper
print('Opening...')
right_gripper.open()
rospy.sleep(1.0)
print('Done!')
#Third goal pose -----------------------------------------------------
rospy.sleep(2.0)
goal_3 = PoseStamped()
goal_3.header.frame_id = "base"
#x, y, and z position
goal_3.pose.position.x = 0.6
goal_3.pose.position.y = -0.1
goal_3.pose.position.z = 0.1
#Orientation as a quaternion
goal_3.pose.orientation.x = 0.0
goal_3.pose.orientation.y = -1.0
goal_3.pose.orientation.z = 0.0
goal_3.pose.orientation.w = 0.0
#Set the goal state to the pose you just defined
right_arm.set_pose_target(goal_3)
#Set the start state for the right arm
right_arm.set_start_state_to_current_state()
# #Create a path constraint for the arm
# #UNCOMMENT TO ENABLE ORIENTATION CONSTRAINTS
orien_const = OrientationConstraint()
orien_const.link_name = "right_gripper"
orien_const.header.frame_id = "base"
orien_const.orientation.y = -1.0
orien_const.absolute_x_axis_tolerance = 0.1
orien_const.absolute_y_axis_tolerance = 0.1
orien_const.absolute_z_axis_tolerance = 0.1
orien_const.weight = 1.0
consts = Constraints()
consts.orientation_constraints = [orien_const]
right_arm.set_path_constraints(consts)
#Plan a path
right_plan = right_arm.plan()
#Execute the plan
raw_input('Press <Enter> to move the right arm to goal pose 3: ')
right_arm.execute(right_plan)
#Close the right gripper
print('Closing...')
right_gripper.close()
rospy.sleep(1.0)
#Open the right gripper
print('Opening...')
right_gripper.open()
rospy.sleep(1.0)
print('Done!')
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 main():
"""
Main Script
"""
#===================================================
# Code to add gripper
# rospy.init_node('gripper_test')
# # Set up the right gripper
# right_gripper = robot_gripper.Gripper('right_gripper')
# #Calibrate the gripper (other commands won't work unless you do this first)
# print('Calibrating...')
# right_gripper.calibrate()
# rospy.sleep(2.0)
# #Close the right gripper to hold publisher
# # MIGHT NOT NEED THIS
# print('Closing...')
# right_gripper.close()
# rospy.sleep(1.0)
#===================================================
## TF CODE
tfBuffer = tf2_ros.Buffer()
tfListener = tf2_ros.TransformListener(tfBuffer)
## TF CODE
planner = PathPlanner("right_arm")
Kp = 0.1 * np.array([0.3, 2, 1, 1.5, 2, 2, 3]) # Stolen from 106B Students
Kd = 0.01 * np.array([2, 1, 2, 0.5, 0.5, 0.5, 0.5]) # Stolen from 106B Students
Ki = 0.01 * np.array([1, 1, 1, 1, 1, 1, 1]) # Untuned
Kw = np.array([0.9, 0.9, 0.9, 0.9, 0.9, 0.9, 0.9]) # Untuned
limb = intera_interface.Limb("right")
control = Controller(Kp, Kd, Ki, Kw, limb)
##
## Add the obstacle to the planning scene here
##
#Tower
X = 0.075
Y = 0.075
Z = 0.0675
Xp = 0.0
Yp = -0.0425
Zp = 0.0225
Xpa = 0.00
Ypa = 0.00
Zpa = 0.00
Wpa = 1.00
pose = PoseStamped()
pose.header.stamp = rospy.Time.now()
pose.header.frame_id = "ar_marker_1"
pose.pose.position.x = Xp
pose.pose.position.y = Yp
pose.pose.position.z = Zp
pose.pose.orientation.x = Xpa
pose.pose.orientation.y = Ypa
pose.pose.orientation.z = Zpa
pose.pose.orientation.w = Wpa
planner.add_box_obstacle([X,Y,Z], "tower", pose)
#-------------------------------walls
pose = PoseStamped()
pose.header.stamp = rospy.Time.now()
pose.header.frame_id = "base"
pose.pose.position.x = -2
pose.pose.position.y = 0
pose.pose.position.z = 0
pose.pose.orientation.x = 0
pose.pose.orientation.y = 0
pose.pose.orientation.z = 0
pose.pose.orientation.w = 1
planner.add_box_obstacle([1,1,5], "left_side_wall", pose)
pose = PoseStamped()
pose.header.stamp = rospy.Time.now()
pose.header.frame_id = "base"
pose.pose.position.x = 0
pose.pose.position.y = -2
pose.pose.position.z = 0
pose.pose.orientation.x = 0
pose.pose.orientation.y = 0
pose.pose.orientation.z = 0
pose.pose.orientation.w = 1
planner.add_box_obstacle([1,1,5], "right_side_wall", pose)
pose = PoseStamped()
pose.header.stamp = rospy.Time.now()
pose.header.frame_id = "base"
pose.pose.position.x = 0
pose.pose.position.y = 2
pose.pose.position.z = 0
pose.pose.orientation.x = 0
pose.pose.orientation.y = 0
pose.pose.orientation.z = 0
pose.pose.orientation.w = 1
planner.add_box_obstacle([1,1,5], "back_wall", pose)
#-------------------------------walls
#Table
X = .2
Y = .2
Z = .01
Xp = 0
Yp = 0
Zp = 0
Xpa = 0.00
Ypa = 0.00
Zpa = 0.00
Wpa = 1.00
pose = PoseStamped()
pose.header.stamp = rospy.Time.now()
#TODO: Is this the correct frame name?
pose.header.frame_id = "ar_marker_1"
pose.pose.position.x = Xp
pose.pose.position.y = Yp
pose.pose.position.z = Zp
pose.pose.orientation.x = Xpa
pose.pose.orientation.y = Ypa
pose.pose.orientation.z = Zpa
pose.pose.orientation.w = Wpa
planner.add_box_obstacle([X,Y,Z], "table", pose)
orien_const = OrientationConstraint()
orien_const.link_name = "right_gripper";
orien_const.header.frame_id = "ar_marker_1";
#orien_const.orientation.y = np.pi/2
#orien_const.orientation.w = 1;
orien_const.absolute_x_axis_tolerance = 1;
orien_const.absolute_y_axis_tolerance = 1;
orien_const.absolute_z_axis_tolerance = 1;
orien_const.weight = .5;
#for stage 2
# orien_const1 = OrientationConstraint()
# orien_const1.link_name = "right_gripper";
# orien_const1.header.frame_id = "ar_marker_1";
# #orien_const.orientation.y = np.pi/2
# #orien_const.orientation.w = 1;
# orien_const1.absolute_x_axis_tolerance = 0.5;
# orien_const1.absolute_y_axis_tolerance = 0.5;
# orien_const1.absolute_z_axis_tolerance = 0.5;
# orien_const1.weight = 1.0;
#rospy.sleep(1.0)
# while not rospy.is_shutdown():
# while not rospy.is_shutdown():
current_x = 0
current_y = 0
current_z = 0
offset = 0.14
i = 0
while(i == 0):
try:
trans = tfBuffer.lookup_transform('ar_marker_1', 'right_gripper_tip', rospy.Time())
tf_px = trans.transform.translation.x
tf_py = trans.transform.translation.y
tf_pz = trans.transform.translation.z
tf_rx = trans.transform.rotation.x
tf_ry = trans.transform.rotation.y
tf_rz = trans.transform.rotation.z
tf_rw = trans.transform.rotation.w
current_x = tf_px + offset
current_y = tf_py
current_z = tf_pz
except (tf2_ros.LookupException, tf2_ros.ConnectivityException, tf2_ros.ExtrapolationException):
continue
i =1
x = current_x
y = current_y
z = current_z
try:
#right_gripper_tip
goal_1 = PoseStamped()
goal_1.header.frame_id = "ar_marker_1"
#x, y, and z position
goal_1.pose.position.x = -0.0175
goal_1.pose.position.y = -0.0018
goal_1.pose.position.z = 0.065 #0.0825
#Orientation as a quaternion
goal_1.pose.orientation.x = 0
goal_1.pose.orientation.y = 0.707
goal_1.pose.orientation.z = 0
goal_1.pose.orientation.w = 0.707
plan = planner.plan_to_pose(goal_1, list())#[orien_const])
if not planner.execute_plan(plan):
raise Exception("Execution failed0")
except Exception as e:
print e
def liftgripper(self, target_z=0.10):
# approx centers of onions at 0.82, width of onion is 0.038 m. table is at 0.78
# length of gripper is 0.163 m The gripper should not go lower than
# (height_z of table w.r.t base+gripper-height/2+tolerance) = 0.78-0.93+0.08+0.01=-0.24
# pnp._limb.endpoint_pose returns {'position': (x, y, z), 'orientation': (x, y, z, w)}
# moving from z=-.02 to z=-0.1
group = self.group
while self.target_location_x == -100:
rospy.sleep(0.05)
current_pose = group.get_current_pose().pose
current_orientation = group.get_current_pose().pose.orientation
curr_q = [
current_orientation.x, current_orientation.y,
current_orientation.z, current_orientation.w
]
position_constraint = PositionConstraint()
position_constraint.target_point_offset.x = 0.1
position_constraint.target_point_offset.y = 0.2
position_constraint.target_point_offset.z = 0.3
position_constraint.weight = 0.8
position_constraint.link_name = group.get_end_effector_link()
position_constraint.header.frame_id = group.get_planning_frame()
orientation_constraint = OrientationConstraint()
orientation_constraint.orientation = Quaternion(x=curr_q[0],
y=curr_q[1],
z=curr_q[2],
w=curr_q[3])
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 = 0.2
orientation_constraint.link_name = group.get_end_effector_link()
orientation_constraint.header.frame_id = group.get_planning_frame()
constraint = Constraints()
constraint.orientation_constraints.append(orientation_constraint)
constraint.position_constraints.append(position_constraint)
group.set_path_constraints(constraint)
allow_replanning = True
planning_time = 5
threshold = 0.02
time_limit = 120
start_time = time.time()
target_x = current_pose.position.x
target_y = current_pose.position.y
delta_z = (target_z - current_pose.position.z) / 4
print "Attempting to lift gripper"
i = 1
# if (delta_z < threshold):
# return True
# while (current_pose.position.z < target_z) and ((target_z - current_pose.position.z) > 3*threshold) and ((time.time()-start_time) < time_limit):
# status = self.go_to_pose_goal(curr_q[0], curr_q[1], curr_q[2], curr_q[3],
# target_x, target_y, current_pose.position.z + i*delta_z,
# allow_replanning, planning_time, thresh=threshold)
# rospy.sleep(0.1)
# current_pose = group.get_current_pose().pose
# i += 1
status = self.go_to_pose_goal(curr_q[0],
curr_q[1],
curr_q[2],
curr_q[3],
target_x,
target_y,
target_z,
allow_replanning,
planning_time,
thresh=threshold)
rospy.sleep(0.25)
print "Successfully lifted gripper to z: ", current_pose.position.z
group.clear_path_constraints()
# return ((target_z-current_pose.position.z) < 3*threshold or (current_pose.position.z > target_z))
return True
def main():
"""
Main Script
"""
# Setting up head camera
head_camera = CameraController("left_hand_camera")
head_camera.resolution = (1280, 800)
head_camera.open()
print("setting balance")
happy = False
while not happy:
e = head_camera.exposure
print("exposue value: " + str(e))
e_val = int(input("new exposure value"))
head_camera.exposure = e_val
satisfaction = str(raw_input("satified? y/n"))
happy = 'y' == satisfaction
planner = PathPlanner("right_arm")
listener = tf.TransformListener()
grip = Gripper('right')
grip.calibrate()
rospy.sleep(3)
grip.open()
# creating the table obstacle so that Baxter doesn't hit it
table_size = np.array([.5, 1, 10])
table_pose = PoseStamped()
table_pose.header.frame_id = "base"
thickness = 1
table_pose.pose.position.x = .9
table_pose.pose.position.y = 0.0
table_pose.pose.position.z = -.112 - thickness / 2
table_size = np.array([.5, 1, thickness])
planner.add_box_obstacle(table_size, "table", table_pose)
raw_input("gripper close")
grip.close()
# ###load cup positions found using cup_detection.py ran in virtual environment
# start_pos_xy = np.load(POURING_CUP_PATH)
# goal_pos_xy = np.load(RECEIVING_CUP_PATH)
# start_pos = np.append(start_pos_xy, OBJECT_HEIGHT - GRABBING_OFFSET)
# goal_pos = np.append(start_pos_xy, OBJECT_HEIGHT - GRABBING_OFFSET)
# #### END LOADING CUP POSITIONS
camera_subscriber = rospy.Subscriber("cameras/left_hand_camera/image", Image, get_img)
Kp = 0.1 * np.array([0.3, 2, 1, 1.5, 2, 2, 3]) # Stolen from 106B Students
Kd = 0.01 * np.array([2, 1, 2, 0.5, 0.5, 0.5, 0.5]) # Stolen from 106B Students
Ki = 0.01 * np.array([1, 1, 1, 1, 1, 1, 1]) # Untuned
Kw = np.array([0.9, 0.9, 0.9, 0.9, 0.9, 0.9, 0.9]) # Untuned
cam_pos= [0.188, -0.012, 0.750]
##
## Add the obstacle to the planning scene here
##
# Create a path constraint for the arm
orien_const = OrientationConstraint()
orien_const.link_name = "right_gripper";
orien_const.header.frame_id = "base";
orien_const.orientation.y = -1.0;
orien_const.absolute_x_axis_tolerance = 0.1;
orien_const.absolute_y_axis_tolerance = 0.1;
orien_const.absolute_z_axis_tolerance = 0.1;
orien_const.weight = 1.0;
horizontal = getQuaternion(np.array([0,1,0]), np.pi)
z_rot_pos = getQuaternion(np.array([0,0,1]), np.pi / 2)
orig = quatMult(z_rot_pos, horizontal)
orig = getQuaternion(np.array([0,1,0]), np.pi / 2)
#IN THE VIEW OF THE CAMERA
#CORNER1--------->CORNER2
# | |
# | |
# | |
#CORNER3 ------------|
width = 0.3
length = 0.6
CORNER1 = np.array([0.799, -0.524, -0.03])
CORNER2 = CORNER1 + np.array([-width, 0, 0])
CORNER3 = CORNER1 + np.array([0, length, 0])
#CREATE THE GRID
dir1 = CORNER2 - CORNER1
dir2 = CORNER3 - CORNER1
grid_vals = []
ret_vals = []
for i in range(0, 3):
for j in range(0, 4):
grid = CORNER1 + i * dir1 / 2 + j * dir2 / 3
grid_vals.append(grid)
ret_vals.append(np.array([grid[0], grid[1], OBJECT_HEIGHT]))
i = -1
while not rospy.is_shutdown():
for g in grid_vals:
i += 1
while not rospy.is_shutdown():
try:
goal_1 = PoseStamped()
goal_1.header.frame_id = "base"
#x, y, and z position
goal_1.pose.position.x = g[0]
goal_1.pose.position.y = g[1]
goal_1.pose.position.z = g[2]
y = - g[1] + cam_pos[1]
x = g[0] - cam_pos[0]
q = orig
#Orientation as a quaternion
goal_1.pose.orientation.x = q[1][0]
goal_1.pose.orientation.y = q[1][1]
goal_1.pose.orientation.z = q[1][2]
goal_1.pose.orientation.w = q[0]
plan = planner.plan_to_pose(goal_1, [])
if planner.execute_plan(plan):
# raise Exception("Execution failed")
rospy.sleep(1)
#grip.open()
raw_input("open")
rospy.sleep(1)
# plt.imshow(camera_image)
print("yay: " + str(i))
pos = listener.lookupTransform("/reference/right_gripper", "/reference/base", rospy.Time(0))[0]
print("move succesfully to " + str(pos))
fname = os.path.join(IMAGE_DIR, "calib_{}.jpg".format(i))
skimage.io.imsave(fname, camera_image)
print(e)
print("index: ", i)
else:
break
print(np.array(ret_vals))
# save the positions of the gripper so that the homography matrix can be calculated
np.save(POINTS_DIR, np.array(ret_vals))
print(np.load(POINTS_DIR))
break
def main():
rospy.init_node('arm_obstacle_demo')
wait_for_time()
argv = rospy.myargv()
planning_scene = PlanningSceneInterface('base_link')
planning_scene.clear()
planning_scene.removeAttachedObject('tray')
# Create table obstacle
planning_scene.removeCollisionObject('table')
table_size_x = 0.5
table_size_y = 1
table_size_z = 0.03
table_x = 0.8
table_y = 0
table_z = 0.6
planning_scene.addBox('table', table_size_x, table_size_y, table_size_z,
table_x, table_y, table_z)
# Create divider obstacle
planning_scene.removeCollisionObject('divider')
size_x = 0.5
size_y = 0.01
size_z = 0.05
x = table_x - (table_size_x / 2) + (size_x / 2)
y = 0
z = table_z + (table_size_z / 2) + (size_z / 2)
planning_scene.addBox('divider', size_x, size_y, size_z, x, y, z)
pose1 = PoseStamped()
pose1.header.frame_id = 'base_link'
pose1.pose.position.x = 0.5
pose1.pose.position.y = -0.3
pose1.pose.position.z = 0.75
pose1.pose.orientation.w = 1
pose2 = PoseStamped()
pose2.header.frame_id = 'base_link'
pose2.pose.position.x = 0.5
pose2.pose.position.y = 0.3
pose2.pose.position.z = 0.9
pose2.pose.orientation.z = 0.2588
pose2.pose.orientation.w = 0.9659
arm = fetch_api.Arm()
def shutdown():
arm.cancel_all_goals()
planning_scene.clear()
rospy.on_shutdown(shutdown)
kwargs = {
'allowed_planning_time': 5,
'execution_timeout': 30,
'group_name': 'arm',
'num_planning_attempts': 10,
'replan': True,
'replan_attempts': 5,
'tolerance': 0.001
}
error = arm.move_to_pose(pose1, **kwargs)
if error is not None:
arm.cancel_all_goals()
rospy.logerr('Pose 1 failed: {}'.format(error))
else:
rospy.loginfo('Pose 1 succeeded')
frame_attached_to = 'gripper_link'
frames_okay_to_collide_with = [
'gripper_link', 'l_gripper_finger_link', 'r_gripper_finger_link'
]
planning_scene.attachBox('tray', 0.3, 0.07, 0.01, 0.05, 0, 0,
frame_attached_to,
frames_okay_to_collide_with)
planning_scene.setColor('tray', 1, 0, 1)
planning_scene.sendColors()
rospy.sleep(2)
kwargs = {
'allowed_planning_time': 120,
'execution_timeout': 120,
'group_name': 'arm',
'num_planning_attempts': 1,
'replan': True,
'replan_attempts': 10,
'tolerance': 0.01
}
oc = OrientationConstraint()
oc.header.frame_id = 'base_link'
oc.link_name = 'wrist_roll_link'
oc.orientation.w = 1
oc.absolute_x_axis_tolerance = 0.1
oc.absolute_y_axis_tolerance = 0.1
oc.absolute_z_axis_tolerance = 3.14
oc.weight = 1.0
kwargs['orientation_constraint'] = oc
error = arm.move_to_pose(pose2, **kwargs)
if error is not None:
arm.cancel_all_goals()
rospy.logerr('Pose 2 failed: {}'.format(error))
else:
rospy.loginfo('Pose 2 succeeded')
def build(self, tf_timeout=rospy.Duration(5.0)):
"""Builds the goal message.
To set a pose or joint goal, call set_pose_goal or set_joint_goal
before calling build. To add a path orientation constraint, call
add_path_orientation_constraint first.
Args:
tf_timeout: rospy.Duration. How long to wait for a TF message. Only
used with pose goals.
Returns: moveit_msgs/MoveGroupGoal
"""
goal = MoveGroupGoal()
# Set start state
goal.request.start_state = copy.deepcopy(self.start_state)
# Set goal constraint
if self._pose_goal is not None:
self._tf_listener.waitForTransform(self._pose_goal.header.frame_id,
self.fixed_frame,
rospy.Time.now(), tf_timeout)
try:
pose_transformed = self._tf_listener.transformPose(
self.fixed_frame, self._pose_goal)
except (tf.LookupException, tf.ConnectivityException):
return None
c1 = Constraints()
c1.position_constraints.append(PositionConstraint())
c1.position_constraints[0].header.frame_id = self.fixed_frame
c1.position_constraints[0].link_name = self.gripper_frame
b = BoundingVolume()
s = SolidPrimitive()
s.dimensions = [self.tolerance * self.tolerance]
s.type = s.SPHERE
b.primitives.append(s)
b.primitive_poses.append(self._pose_goal.pose)
c1.position_constraints[0].constraint_region = b
c1.position_constraints[0].weight = 1.0
c1.orientation_constraints.append(OrientationConstraint())
c1.orientation_constraints[0].header.frame_id = self.fixed_frame
c1.orientation_constraints[
0].orientation = pose_transformed.pose.orientation
c1.orientation_constraints[0].link_name = self.gripper_frame
c1.orientation_constraints[
0].absolute_x_axis_tolerance = self.tolerance
c1.orientation_constraints[
0].absolute_y_axis_tolerance = self.tolerance
c1.orientation_constraints[
0].absolute_z_axis_tolerance = self.tolerance
c1.orientation_constraints[0].weight = 1.0
goal.request.goal_constraints.append(c1)
elif self._joint_names is not None:
c1 = Constraints()
for i in range(len(self._joint_names)):
c1.joint_constraints.append(JointConstraint())
c1.joint_constraints[i].joint_name = self._joint_names[i]
c1.joint_constraints[i].position = self._joint_positions[i]
c1.joint_constraints[i].tolerance_above = self.tolerance
c1.joint_constraints[i].tolerance_below = self.tolerance
c1.joint_constraints[i].weight = 1.0
goal.request.goal_constraints.append(c1)
# Set path constraints
goal.request.path_constraints.orientation_constraints = self._orientation_contraints
# Set trajectory constraints
# Set planner ID (name of motion planner to use)
goal.request.planner_id = self.planner_id
# Set group name
goal.request.group_name = self.group_name
# Set planning attempts
goal.request.num_planning_attempts = self.num_planning_attempts
# Set planning time
goal.request.allowed_planning_time = self.allowed_planning_time
# Set scaling factors
goal.request.max_acceleration_scaling_factor = self.max_acceleration_scaling_factor
goal.request.max_velocity_scaling_factor = self.max_velocity_scaling_factor
# Set planning scene diff
goal.planning_options.planning_scene_diff = copy.deepcopy(
self.planning_scene_diff)
# Set is plan only
goal.planning_options.plan_only = self.plan_only
# Set look around
goal.planning_options.look_around = self.look_around
# Set replanning options
goal.planning_options.replan = self.replan
goal.planning_options.replan_attempts = self.replan_attempts
goal.planning_options.replan_delay = self.replan_delay
return goal
def main():
"""
Main Script
"""
# Path Planner - right_arm for sawyer
planner = PathPlanner("right_arm")
##
## OBSTACLES
##
# poses = PoseStamped()
# poses.pose.position.x = .5
# poses.pose.position.y = 0
# poses.pose.position.z = 0
# poses.pose.orientation.x = 0
# poses.pose.orientation.y = 0
# poses.pose.orientation.z = 0
# poses.pose.orientation.w = 1
# poses.header.frame_id = "base"
# planner.add_box_obstacle(np.array([.4,1.2,.1]), "Howard", poses)
# ORIENTATION CONSTRAINT
orien_const = OrientationConstraint()
orien_const.link_name = "right_gripper"
orien_const.header.frame_id = "base"
orien_const.orientation.y = -1.0
orien_const.absolute_x_axis_tolerance = 0.1
orien_const.absolute_y_axis_tolerance = 0.1
orien_const.absolute_z_axis_tolerance = 0.1
orien_const.weight = 1.0
#FULL PATH
path = [[.8, .05, -.23, "1"], [.6, -.3, 0.0, "2"], [.6, -.1, .1, "3"]]
while not rospy.is_shutdown():
for pos in path:
while not rospy.is_shutdown():
try:
goal = PoseStamped()
goal.header.frame_id = "base"
goal.pose.position.x = pos[0]
goal.pose.position.y = pos[1]
goal.pose.position.z = pos[2]
goal.pose.orientation.x = 0.0
goal.pose.orientation.y = -1.0
goal.pose.orientation.z = 0.0
goal.pose.orientation.w = 0.0
plan = planner.plan_to_pose(goal, [orien_const])
raw_input(
"Press <Enter> to move the right arm to goal pose " +
pos[3] + ":")
if not planner.execute_plan(plan):
raise Exception("Execution failed")
except Exception as e:
print e
traceback.print_exc()
else:
break
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 print_pointlist(self, point_list):
# Get differential of way point list
differential = self.get_way_point_differential(point_list)
differential_num = [0]
differential_num = differential_num + differential
eef_rotation_change = [0, 0]
for i in range(2, len(differential_num)):
eef_rotation_change.append(differential_num[i] -
differential_num[i - 1])
pose = self.group.get_current_pose(self.group.get_end_effector_link())
constraints = Constraints()
#### joint constraints
joint_constraint = JointConstraint()
joint_constraint.joint_name = 'arm_joint_1'
joint_constraint.position = 169 * pi / 180
joint_constraint.tolerance_above = 30 * pi / 180
joint_constraint.tolerance_below = 30 * pi / 180
joint_constraint.weight = 1
constraints.joint_constraints.append(joint_constraint)
joint_constraint = JointConstraint()
joint_constraint.joint_name = 'arm_joint_4'
joint_constraint.position = 150 * pi / 180
joint_constraint.tolerance_above = 30 * pi / 180
joint_constraint.tolerance_below = 30 * pi / 180
joint_constraint.weight = 1
constraints.joint_constraints.append(joint_constraint)
self.group.set_path_constraints(constraints)
# Orientation constrains
# constraints = Constraints()
orientation_constraint = OrientationConstraint()
orientation_constraint.header = pose.header
orientation_constraint.link_name = self.group.get_end_effector_link()
orientation_constraint.orientation = pose.pose.orientation
orientation_constraint.absolute_x_axis_tolerance = 2 * pi
orientation_constraint.absolute_y_axis_tolerance = 2 * pi
orientation_constraint.absolute_z_axis_tolerance = 2 * pi
constraints.orientation_constraints.append(orientation_constraint)
while len(point_list) > 1:
# Move the robot point to first point and find the height
initial_plan = self.move_to_initial(point_list[1])
joint_goal = self.group.get_current_joint_values()
head = initial_plan.joint_trajectory.header
robot_state = self.robot.get_current_state()
# print(robot.get_current_state().joint_state.position)
robot_state.joint_state.position = tuple(initial_plan.joint_trajectory.points[-1].positions) + \
tuple(robot_state.joint_state.position[7:])
resp = self.request_fk(head, [self.group.get_end_effector_link()],
robot_state)
current_pose = self.group.get_current_pose().pose
current_pose.orientation = resp.pose_stamped[0].pose.orientation
(current_pose.position.x, current_pose.position.y,
current_pose.position.z) = point_list[0]
self.group.set_pose_target(current_pose)
self.group.go()
# Move the robot to the center of the striaght line to make sure Way point method can be executed
# Way points
waypoints = []
wpose = self.group.get_current_pose().pose
# Add the current pose to make sure the path is smooth
waypoints.append(copy.deepcopy(wpose))
success_num = 0
for point_num in range(len(point_list)):
(wpose.position.x, wpose.position.y,
wpose.position.z) = point_list[point_num]
(roll, pitch, yaw) = euler_from_quaternion([
wpose.orientation.x, wpose.orientation.y,
wpose.orientation.z, wpose.orientation.w
])
# [wpose.orientation.x, wpose.orientation.y, wpose.orientation.z, wpose.orientation.w] = \
# quaternion_from_euler(roll, pitch, yaw + eef_rotation_change[point_num])
# print(yaw + eef_rotation_change[point_num])
[wpose.orientation.x, wpose.orientation.y, wpose.orientation.z, wpose.orientation.w] = \
quaternion_from_euler(roll, pitch, yaw)
waypoints.append(copy.deepcopy(wpose))
(plan, fraction) = self.group.compute_cartesian_path(
waypoints, # waypoints to follow
0.01, # eef_step
0.00,
path_constraints=constraints)
if fraction == 1:
success_num += 1
execute_plan = plan
if success_num == len(point_list):
self.group.execute(execute_plan)
self.print_list_visualize(point_list)
elif success_num == 0:
break
else:
# execute success plan
self.msg_prit.data = True
self.group.execute(execute_plan)
self.print_list_visualize(point_list[:success_num])
break
self.msg_prit.data = False
# Delete the points what already execute
if success_num > 0:
del (point_list[0:success_num - 1])
del (eef_rotation_change[0:success_num - 1])
# Add obstacle after printing (need to revise)
self.group.set_path_constraints(None)
print 'all finish'
