def move_ik(self, xyz):
move_group = self.arm_group
fixed_end_effector = OrientationConstraint()
fixed_end_effector.link_name = "end_effector_link"
fixed_end_effector.header.frame_id = "link1"
fixed_end_effector.orientation.w = 1.0
fixed_end_effector.absolute_x_axis_tolerance = 0.1
fixed_end_effector.absolute_y_axis_tolerance = 0.1
fixed_end_effector.absolute_z_axis_tolerance = 3.14
fixed_end_effector.weight = 1.0
constraint = Constraints()
constraint.name = "tilt constraint"
constraint.orientation_constraints = [fixed_end_effector]
move_group.set_path_constraints(constraint)
move_group.set_position_target(xyz)
try:
move_group.go(wait=True)
except MoveItCommanderException:
print "sorry cant move here!"
# move_group.stop()
self.moving = False
def init_upright_path_constraints(): cur_pose = manipulator.get_current_pose().pose upright_constraints = Constraints() upright_constraints.name = "upright" orientation_constraint = OrientationConstraint() # orientation_constraint.header = pose.header orientation_constraint.header.frame_id = "world" orientation_constraint.link_name = manipulator.get_end_effector_link() print "end link: ", manipulator.get_end_effector_link() orientation_constraint.orientation = cur_pose.orientation #IIWA orientation_constraint.absolute_x_axis_tolerance = 0.1 orientation_constraint.absolute_y_axis_tolerance = 0.1 orientation_constraint.absolute_z_axis_tolerance = 3.14 #UR5 # orientation_constraint.absolute_x_axis_tolerance = 3.14 # orientation_constraint.absolute_y_axis_tolerance = 0.1 # orientation_constraint.absolute_z_axis_tolerance = 0.1 orientation_constraint.weight = 1.0 upright_constraints.orientation_constraints.append(orientation_constraint) return upright_constraints
def set_pose_constraints(self, tol_joint1, tol_joint2, tol_joint3):
angle_constraints = Constraints()
joint_constraint = JointConstraint()
angle_constraints.name = "angle"
joint_constraint.position = self.joint1_con
joint_constraint.tolerance_above = tol_joint1
joint_constraint.tolerance_below = tol_joint1
joint_constraint.weight = 0.1
joint_constraint.joint_name = "arm_shoulder_pan_joint"
angle_constraints.joint_constraints.append(joint_constraint)
joint_constraint2 = JointConstraint()
joint_constraint2.position = self.joint2_con
joint_constraint2.tolerance_above = tol_joint2
joint_constraint2.tolerance_below = tol_joint2
joint_constraint2.weight = 0.2
joint_constraint2.joint_name = "arm_wrist_2_joint"
angle_constraints.joint_constraints.append(joint_constraint2)
joint_constraint3 = JointConstraint()
joint_constraint3.position = self.joint3_con
joint_constraint3.tolerance_above = tol_joint3
joint_constraint3.tolerance_below = tol_joint3
joint_constraint3.weight = 0.3
joint_constraint3.joint_name = "arm_wrist_1_joint"
angle_constraints.joint_constraints.append(joint_constraint3)
group.set_path_constraints(angle_constraints)
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 set_pose_quat_target(self, pose):
############################
rospy.logwarn("set_pose_quat_target")
# Create a contraints list and give it a name
constraints = Constraints()
constraints.name = "Keep gripper horizontal"
start_pose = self.arm.get_current_pose(self.end_effector_link)
rospy.logwarn(self.end_effector_link)
rospy.logwarn("start_pose:")
rospy.logwarn(start_pose)
rospy.logwarn("pose:")
rospy.logwarn(pose)
# Create an orientation constraint for the right gripper
orientation_constraint = OrientationConstraint()
orientation_constraint.header = start_pose.header
orientation_constraint.link_name = self.end_effector_link
orientation_constraint.orientation.x = start_pose.pose.orientation.x
orientation_constraint.orientation.y = start_pose.pose.orientation.y
orientation_constraint.orientation.z = start_pose.pose.orientation.z
orientation_constraint.orientation.w = start_pose.pose.orientation.w
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)
rospy.logwarn("constraints:")
rospy.logwarn(constraints)
# Set the path constraints on the right_arm
self.arm.set_path_constraints(constraints)
############################
self.arm.set_pose_target(pose, self.end_effector_link)
def init_orient_constraints(x_tol, y_tol, z_tol):
if x_tol < 0.1:
x_tol = 0.1
if y_tol < 0.1:
y_tol = 0.1
if z_tol < 0.1:
z_tol = 0.1
print x_tol, y_tol, z_tol
cur_pose = manipulator.get_current_pose().pose
upright_constraints = Constraints()
upright_constraints.name = "upright"
orientation_constraint = OrientationConstraint()
# orientation_constraint.header = pose.header
orientation_constraint.header.frame_id = "world"
orientation_constraint.link_name = manipulator.get_end_effector_link()
print "end link: ", manipulator.get_end_effector_link()
orientation_constraint.orientation = cur_pose.orientation
#IIWA
orientation_constraint.absolute_x_axis_tolerance = x_tol
orientation_constraint.absolute_y_axis_tolerance = y_tol
orientation_constraint.absolute_z_axis_tolerance = z_tol
orientation_constraint.weight = 1.0
upright_constraints.orientation_constraints.append(orientation_constraint)
return upright_constraints
def set_upright_constraints(self,pose):
upright_constraints = Constraints()
orientation_constraint = OrientationConstraint()
upright_constraints.name = "upright"
orientation_constraint.header = pose.header
orientation_constraint.link_name = group.get_end_effector_link()
orientation_constraint.orientation = pose.pose.orientation
orientation_constraint.absolute_x_axis_tolerance = 3.14
orientation_constraint.absolute_y_axis_tolerance = 0.1
orientation_constraint.absolute_z_axis_tolerance = 0.1
#orientation_constraint.absolute_z_axis_tolerance = 3.14 #ignore this axis
orientation_constraint.weight = 1
upright_constraints.orientation_constraints.append(orientation_constraint)
group.set_path_constraints(upright_constraints)
def add_robot_constraints():
constraint = Constraints()
constraint.name = "camera constraint"
roll_constraint = OrientationConstraint()
# 'base_link' is equal to the world link
roll_constraint.header.frame_id = 'world'
# The link that must be oriented upwards
roll_constraint.link_name = "camera"
roll_constraint.orientation = Quaternion(*tf.transformations.quaternion_from_euler(0,np.pi/3,0))
# Allow rotation of 45 degrees around the x and y axis
roll_constraint.absolute_x_axis_tolerance = np.pi #Allow max rotation of x degrees
roll_constraint.absolute_y_axis_tolerance = np.pi
roll_constraint.absolute_z_axis_tolerance = np.pi/2
# The roll constraint is the only constraint
roll_constraint.weight = 1
#constraint.orientation_constraints = [roll_constraint]
joint_1_constraint = JointConstraint()
joint_1_constraint.joint_name = "joint_1"
joint_1_constraint.position = 0
joint_1_constraint.tolerance_above = np.pi/2
joint_1_constraint.tolerance_below = np.pi/2
joint_1_constraint.weight = 1
joint_4_constraint = JointConstraint()
joint_4_constraint.joint_name = "joint_4"
joint_4_constraint.position = 0
joint_4_constraint.tolerance_above = np.pi/2
joint_4_constraint.tolerance_below = np.pi/2
joint_4_constraint.weight = 1
joint_5_constraint = JointConstraint()
joint_5_constraint.joint_name = "joint_5"
joint_5_constraint.position = np.pi/2
joint_5_constraint.tolerance_above = np.pi/2
joint_5_constraint.tolerance_below = np.pi/2
joint_5_constraint.weight = 1
joint_6_constraint = JointConstraint()
joint_6_constraint.joint_name = "joint_6"
joint_6_constraint.position = np.pi-0.79
joint_6_constraint.tolerance_above = np.pi
joint_6_constraint.tolerance_below = np.pi
joint_6_constraint.weight = 1
constraint.joint_constraints = [joint_1_constraint, joint_6_constraint]
return constraint
def addConstrains():
pose = group.get_current_pose()
constraint = Constraints()
constraint.name = "downRight"
orientation_constraint = OrientationConstraint()
orientation_constraint.header = pose.header
orientation_constraint.link_name = group.get_end_effector_link()
orientation_constraint.orientation = pose.pose.orientation
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.absolute_z_axis_tolerance = 3.14 #ignore this axis
orientation_constraint.weight = 1
constraint.orientation_constraints.append(orientation_constraint)
group.set_path_constraints(constraint)
def __init__(self):
moveit_commander.roscpp_initialize(sys.argv)
rospy.init_node('moveit_test')
arm = MoveGroupCommander('arm')
end_effector_link = arm.get_end_effector_link()
arm.allow_replanning(True)
arm.set_planning_time(5)
target_pose = PoseStamped()
target_pose.header.frame_id = REFERENCE_FRAME
target_init_orientation = Quaternion()
target_init_orientation = quaternion_from_euler(0.0, 1.57, 0.0)
self.setPose(target_pose, [0.5, 0.2, 0.5],list(target_init_orientation))
current_pose = arm.get_current_pose(end_effector_link)
self.setPose(current_pose, [current_pose.pose.position.x, current_pose.pose.position.y, current_pose.pose.position.z], list(target_init_orientation))
arm.set_pose_target(current_pose)
arm.go()
rospy.sleep(2)
constraints = Constraints()
orientation_constraint = OrientationConstraint()
constraints.name = 'gripper constraint'
orientation_constraint.header = target_pose.header
orientation_constraint.link_name = end_effector_link
orientation_constraint.orientation.x = target_init_orientation[0]
orientation_constraint.orientation.y = target_init_orientation[1]
orientation_constraint.orientation.z = target_init_orientation[2]
orientation_constraint.orientation.w = target_init_orientation[3]
orientation_constraint.absolute_x_axis_tolerance = 0.03
orientation_constraint.absolute_y_axis_tolerance = 0.03
orientation_constraint.absolute_z_axis_tolerance = 3.14
orientation_constraint.weight = 1.0
constraints.orientation_constraints.append(orientation_constraint)
arm.set_path_constraints(constraints)
arm.set_pose_target(target_pose, end_effector_link)
arm.go()
rospy.sleep(1)
moveit_commander.roscpp_shutdown()
moveit_commander.os._exit(0)
def 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 get_constraint(self,
euler_orientation=[0, math.pi / 2, 0],
tol=[3, 3, .5]):
#method takes euler-angle inputs, this converts it to a quaternion
q_orientation = tf.transformations.quaternion_from_euler(
euler_orientation[0], euler_orientation[1], euler_orientation[2])
orientation_msg = Quaternion(q_orientation[0], q_orientation[1],
q_orientation[2], q_orientation[3])
#defines a constraint that sets the end-effector so a cup in it's hand will be upright, or straight upside-down
constraint = Constraints()
constraint.name = 'upright_wrist'
upright_orientation = OrientationConstraint()
upright_orientation.link_name = self.group.get_end_effector_link()
upright_orientation.orientation = orientation_msg
upright_orientation.absolute_x_axis_tolerance = tol[0]
upright_orientation.absolute_y_axis_tolerance = tol[1]
upright_orientation.absolute_z_axis_tolerance = tol[2]
upright_orientation.weight = 1.0
upright_orientation.header = self.start_pose.header
constraint.orientation_constraints.append(upright_orientation)
return (constraint)
def test_constraints(self):
constraint = Constraints()
constraint.name = "tilt constraint"
ocm = OrientationConstraint()
# 'base_link' is equal to the world link
ocm.header.frame_id = "world"
# The link that must be oriented upwards
ocm.link_name = "tcp"
# ocm.orientation = Quaternion(0.5, 0.5, 0.5, 0.5)
ocm.orientation.x = 0.683951325248
ocm.orientation.y = -0.209429903965
ocm.orientation.z = -0.202265206016
ocm.orientation.w = 0.668908429049
# Allow rotation of 45 degrees around the x and y axis
ocm.absolute_x_axis_tolerance = 0.5 #Allow max rotation of 45 degrees
ocm.absolute_y_axis_tolerance = 1.5 #Allow max rotation of 360 degrees
ocm.absolute_z_axis_tolerance = 0.5 #Allow max rotation of 45 degrees
# The tilt constraint is the only constraint
ocm.weight = 1
constraint.orientation_constraints = [ocm]
move_group = self.robot_group
pose_goal = geometry_msgs.msg.Pose()
pose_goal.orientation.x = 0.683951325248
pose_goal.orientation.y = -0.209429903965
pose_goal.orientation.z = -0.202265206016
pose_goal.orientation.w = 0.668908429049
pose_goal.position.x = -0.461741097024
pose_goal.position.y = -0.433851093412
pose_goal.position.z = 1.26149858128
move_group.set_pose_target(pose_goal)
move_group.set_path_constraints(constraint)
move_group.plan()
def plan_between_states(group_name,
joint_state_A,
joint_state_B,
robot_state=None):
"""
Plan trajectory between two joint states.
"""
# Create robot state
robot_state = joint_to_robot_state(joint_state_A, robot_state)
# Prepare motion plan request
req = GetMotionPlanRequest()
req.motion_plan_request.start_state = robot_state
req.motion_plan_request.group_name = group_name
req.motion_plan_request.planner_id = "RRTConnectkConfigDefault"
constraints = Constraints()
constraints.name = "goal"
for name, pos in zip(joint_state_B.name, joint_state_B.position):
joint_constraints = JointConstraint()
joint_constraints.joint_name = name
joint_constraints.position = pos
joint_constraints.tolerance_above = 0.001
joint_constraints.tolerance_below = 0.001
joint_constraints.weight = 100
constraints.joint_constraints.append(joint_constraints)
req.motion_plan_request.goal_constraints.append(constraints)
# Call planner
planner_srv = rospy.ServiceProxy('/plan_kinematic_path', GetMotionPlan)
try:
res = planner_srv(req)
except rospy.service.ServiceException, e:
rospy.logerr("Service 'plan_kinematic_path' call failed: %s", str(e))
return None
def move_group_client():
client = actionlib.SimpleActionClient('move_group', MoveGroupAction)
client.wait_for_server()
goal = MoveGroupGoal()
goal.request.group_name = "right_arm"
goal_constraints = Constraints()
goal_constraints.name = 'goal_test'
#goal_constraints.joint_constraints.extend(createJointConstraints())
#goal_constraints.joint_constraints.extend(createJointConstraintsZero())
goal_constraints.position_constraints.extend(createPositionConstraint())
goal_constraints.orientation_constraints.extend(
createOrientationConstraint())
goal.request.goal_constraints.append(goal_constraints)
client.send_goal(goal)
client.wait_for_result()
return client.get_result()
def move_group_client():
client = actionlib.SimpleActionClient('move_group', MoveGroupAction)
client.wait_for_server()
goal = MoveGroupGoal()
goal.request.group_name = "right_arm"
goal_constraints = Constraints()
goal_constraints.name = 'goal_test'
#goal_constraints.joint_constraints.extend(createJointConstraints())
#goal_constraints.joint_constraints.extend(createJointConstraintsZero())
goal_constraints.position_constraints.extend(createPositionConstraint())
goal_constraints.orientation_constraints.extend(createOrientationConstraint())
goal.request.goal_constraints.append(goal_constraints)
client.send_goal(goal)
client.wait_for_result()
return client.get_result()
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 kinect_planner():
global fresh_data
global joint_target, pose_target, goal
global joints_req, pose_req
moveit_commander.roscpp_initialize(sys.argv)
rospy.init_node('kinect_trajectory_planner', anonymous=True)
rate = rospy.Rate(0.5)
#rospy.sleep(3)
# Instantiate a RobotCommander object.
robot = moveit_commander.RobotCommander()
# Instantiate a PlanningSceneInterface object (interface to the world surrounding the robot).
scene = moveit_commander.PlanningSceneInterface()
print "Remember to disable firewall if it's not working"
# Instantiate MoveGroupCommander objects for arms and Kinect2.
group = moveit_commander.MoveGroupCommander("Kinect2_Target")
group_left_arm = moveit_commander.MoveGroupCommander("left_arm")
group_right_arm = moveit_commander.MoveGroupCommander("right_arm")
# We create this DisplayTrajectory publisher to publish
# trajectories for RVIZ to visualize.
display_trajectory_publisher = rospy.Publisher('planned_path',
moveit_msgs.msg.DisplayTrajectory, queue_size=5)
# Set the planner for Moveit
group.set_planner_id("RRTConnectkConfigDefault")
group.set_planning_time(8)
group.set_pose_reference_frame('base_footprint')
# Setting tolerance
group.set_goal_tolerance(0.08)
group.set_num_planning_attempts(10)
# Suscribing to the desired pose topic
target = rospy.Subscriber("desired_pose", PoseStamped, callback)
target_joint = rospy.Subscriber("desired_joints", pose_w_joints, callback_joints)
# Locating the arms and the Kinect2 sensor
group_left_arm_values = group_left_arm.get_current_joint_values()
group_right_arm_values = group_right_arm.get_current_joint_values()
#group_kinect_values = group_kinect.get_current_joint_values()
neck_init_joints = group.get_current_joint_values()
neck_init_joints[0] = -1.346
neck_init_joints[1] = -1.116
neck_init_joints[2] = -2.121
neck_init_joints[3] = 0.830
neck_init_joints[4] = 1.490
neck_init_joints[5] = 0.050
neck_init_joints[6] = 0
neck_init_joints[7] = 0
neck_init_joints[8] = 0
neck_init_joints[9] = 0
# Creating a box to limit the arm position
box_pose = PoseStamped()
box_pose.pose.orientation.w = 1
box_pose.pose.position.x = 0.6
box_pose.pose.position.y = 0.03
box_pose.pose.position.z = 1.5
box_pose.header.frame_id = 'base_footprint'
scene.add_box('box1', box_pose, (0.4, 0.4, 0.1))
rospy.sleep(2)
# Defining position constraints for the trajectory of the kinect
neck_const = Constraints()
neck_const.name = 'neck_constraints'
target_const = JointConstraint()
target_const.joint_name = "neck_joint_end"
target_const.position = 0.95
target_const.tolerance_above = 0.45
target_const.tolerance_below = 0.05
target_const.weight = 0.9 # Importance of this constraint
neck_const.joint_constraints.append(target_const)
# Talking to the robot
#client = actionlib.SimpleActionClient('/Kinect2_Target_controller/follow_joint_trajectory', FollowJointTrajectoryAction)
client = actionlib.SimpleActionClient('/neck/follow_joint_trajectory', FollowJointTrajectoryAction)
print "====== Waiting for server..."
client.wait_for_server()
print "====== Connected to server"
while not rospy.is_shutdown():
print('.')
skip_iter = False
if fresh_data == True: #If a new pose is received, plan.
# Update arms position
group_left_arm_values = group_left_arm.get_current_joint_values()
group_right_arm_values = group_right_arm.get_current_joint_values()
# Set the target pose (or joint values) and generate a plan
if pose_req:
group.set_pose_target(pose_target)
if joints_req:
print('about to set joint target')
print(joint_target)
try:
if len(joint_target) == 6:
#Setting also values for the four virtual joints (one prismatic, and three rotational)
new_joint_target = joint_target + [0.1]*4
else:
new_joint_target = joint_target
print('setting: '),
print new_joint_target
group.set_joint_value_target(new_joint_target)
except:
print('cannot set joint target')
skip_iter = True
print "=========== Calculating trajectory... \n"
# Generate several plans and compare them to get the shorter one
plan_opt = dict()
differ = dict()
if not skip_iter:
try:
num = 1
rep = 0
# Generate several plans and compare them to get the shortest one
#for num in range(1,8):
while num < 7:
num += 1
plan_temp = group.plan()
move(plan_temp)
plan_opt[num] = goal.trajectory
diff=0
for point in goal.trajectory.points:
# calculate the distance between initial pose and planned one
for i in range(0,6):
diff = abs(neck_init_joints[i] - point.positions[i])+abs(diff)
differ[num] = diff
# If the current plan is good, take it
if diff < 110:
break
# If plan is too bad, don't consider it
if diff > 400:
num = num - 1
print "Plan is too long. Replanning."
rep = rep + 1
if rep > 4:
num = num +1
# If no plan was found...
if differ == {}:
print "---- Fail: No motion plan found. No execution attempted. Probably robot joints are out of range."
break
else:
# Select the shortest plan
min_plan = min(differ.itervalues())
select = [k for k, value in differ.iteritems() if value == min_plan]
goal.trajectory = plan_opt[select[0]]
#print " Plan difference:======= ", differ
#print " Selected plan:========= ", select[0]
# Remove the last 4 names and data from each point (dummy joints) before sending the goal
goal.trajectory.joint_names = goal.trajectory.joint_names[:6]
for point in goal.trajectory.points:
point.positions = point.positions[:6]
point.velocities = point.velocities[:6]
point.accelerations = point.accelerations[:6]
print "Sending goal"
client.send_goal(goal)
print "Waiting for result"
client.wait_for_result()
# Change the position of the virtual joint to avoid collision
neck_joints = group.get_current_joint_values()
print('neck joints:')
print neck_joints
#neck_joints[6] = 0.7
#group.set_joint_value_target(neck_joints)
#group.go(wait=True)
except (KeyboardInterrupt, SystemExit):
client.cancel_goal()
raise
rate.sleep()
fresh_data = False
rate.sleep()
def __init__(self):
# Initialize the move_group API
moveit_commander.roscpp_initialize(sys.argv)
# Initialize the ROS node
rospy.init_node('moveit_demo', anonymous=True)
robot = RobotCommander()
# Connect to the right_arm move group
right_arm = MoveGroupCommander(GROUP_NAME_ARM)
# Set the right arm reference frame
right_arm.set_pose_reference_frame(REFERENCE_FRAME)
# Allow some leeway in position(meters) and orientation (radians)
right_arm.set_goal_tolerance(0.00001)
# Get the name of the end-effector link
end_effector_link = right_arm.get_end_effector_link()
# Start in the "resting" configuration stored in the SRDF file
right_arm.set_named_target('right')
# Plan and execute a trajectory to the goal configuration
right_arm.go()
rospy.sleep(1)
# Set an initial target pose with the arm up and to the right
target_pose = PoseStamped()
target_pose.header.frame_id = REFERENCE_FRAME
target_pose.pose.position.x = 0.4
target_pose.pose.position.y = 0.3
target_pose.pose.position.z = 0.4
target_pose.pose.orientation.x = 0.670820393249937
target_pose.pose.orientation.y = 0.223606797749979
target_pose.pose.orientation.z = 0.223606797749979
target_pose.pose.orientation.w = 0.670820393249937
# Set the start state and target pose, then plan and execute
right_arm.set_pose_target(target_pose, end_effector_link)
right_arm.go()
rospy.sleep(2)
# Store the current pose
start_pose = right_arm.get_current_pose(end_effector_link)
# Create a contraints list and give it a name
constraints = Constraints()
constraints.name = "Keep gripper horizontal"
# Create an orientation constraint for the right gripper
orientation_constraint = OrientationConstraint()
orientation_constraint.header = start_pose.header
orientation_constraint.link_name = end_effector_link
orientation_constraint.absolute_x_axis_tolerance = 0.00001
orientation_constraint.absolute_y_axis_tolerance = 3.14
orientation_constraint.absolute_z_axis_tolerance = 0.00001
orientation_constraint.weight = 1.0
orientation_constraint.orientation = start_pose.pose.orientation
# Append the constraint to the list of contraints
constraints.orientation_constraints.append(orientation_constraint)
# Set the path constraints on the right_arm
right_arm.set_path_constraints(constraints)
# Set a target pose for the arm
target_pose = deepcopy(start_pose)
target_pose.header.frame_id = REFERENCE_FRAME
target_pose.pose.position.x = 0.4
target_pose.pose.position.y = -0.3
target_pose.pose.position.z = 0.4
target_pose.pose.orientation.x = 0.670820393249937
target_pose.pose.orientation.y = -0.223606797749979
target_pose.pose.orientation.z = -0.223606797749979
target_pose.pose.orientation.w = 0.670820393249937
#target_pose.pose.orientation.x = 0.707107
#target_pose.pose.orientation.y = 0
#target_pose.pose.orientation.z = 0
#target_pose.pose.orientation.w = 0.707107
# Set the start state and target pose, then plan and execute
right_arm.set_start_state_to_current_state()
right_arm.set_pose_target(target_pose, end_effector_link)
right_arm.go()
rospy.sleep(1)
# Clear all path constraints
right_arm.clear_path_constraints()
# Return to the "resting" configuration stored in the SRDF file
right_arm.set_named_target('up')
# Plan and execute a trajectory to the goal configuration
right_arm.go()
rospy.sleep(1)
# Shut down MoveIt cleanly
moveit_commander.roscpp_shutdown()
# Exit MoveIt
moveit_commander.os._exit(0)
def getOrientation(self, xyz):
pass
jc1 = JointConstraint()
jc1.joint_name = "j5"
jc1.position = 1.25752837976
jc1.tolerance_above = 0.0001
jc1.tolerance_below = 0.0001
jc1.weight = 1.0
jc0 = JointConstraint()
jc0.joint_name = "flange"
jc0.position = -4.43859335239
jc0.tolerance_above = 0.0001
jc0.tolerance_below = 0.0001
jc0.weight = 1.0
cons = Constraints()
cons.name = ""
cons.joint_constraints.append(jc2)
cons.joint_constraints.append(jc3)
cons.joint_constraints.append(jc4)
cons.joint_constraints.append(jc5)
cons.joint_constraints.append(jc1)
cons.joint_constraints.append(jc0)
msg.workspace_parameters = workspace_parameters
msg.start_state = start_state
msg.goal_constraints.append(cons)
msg.num_planning_attempts = 1
msg.allowed_planning_time = 5.0
msg.group_name = "manipulator"
def joint_position_callback(joints):
global plan_only
fixed_frame = rospy.get_param("/fixed_frame")
client = actionlib.SimpleActionClient('move_group', MoveGroupAction)
client.wait_for_server()
move_group_goal = MoveGroupGoal()
try:
msg = MotionPlanRequest()
workspace_parameters = WorkspaceParameters()
workspace_parameters.header.stamp = rospy.Time.now()
workspace_parameters.header.frame_id = fixed_frame
workspace_parameters.min_corner = Vector3(-1.0, -1.0, -1.0)
workspace_parameters.max_corner = Vector3(1.0, 1.0, 1.0)
start_state = RobotState()
# start_state.joint_state.header.stamp = rospy.Time.now()
start_state.joint_state.header.frame_id = fixed_frame
start_state.joint_state.name = []
start_state.joint_state.position = []
cons = Constraints()
cons.name = ""
i = 0
for dim in joints.start_joint.layout.dim:
start_state.joint_state.name.append(dim.label)
start_state.joint_state.position.append(joints.start_joint.data[i])
jc = JointConstraint()
jc.joint_name = dim.label
jc.position = joints.goal_joint.data[i]
jc.tolerance_above = 0.0001
jc.tolerance_below = 0.0001
jc.weight = 1.0
i = i + 1
cons.joint_constraints.append(jc)
msg.workspace_parameters = workspace_parameters
msg.start_state = start_state
msg.goal_constraints.append(cons)
msg.num_planning_attempts = 1
msg.allowed_planning_time = 5.0
msg.group_name = joints.group_name
move_group_goal.request = msg
if joints.plan_only:
plan_only = True
move_group_goal.planning_options.plan_only = True
else:
plan_only = False
client.send_goal(move_group_goal)
client.wait_for_result(rospy.Duration.from_sec(5.0))
except rospy.ROSInterruptException, e:
print "failed: %s"%e
rospy.loginfo("Add Objects to the Planning Scene...")
box_pose = PoseStamped()
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()))
def __init__(self):
# Initialize the move_group API
moveit_commander.roscpp_initialize(sys.argv)
# Initialize the ROS node
rospy.init_node('moveit_constraints_demo', anonymous=True)
robot = RobotCommander()
# Connect to the arm move group
arm = MoveGroupCommander(GROUP_NAME_ARM)
# Initialize the move group for the right gripper
gripper = MoveGroupCommander(GROUP_NAME_GRIPPER)
# Increase the planning time since constraint planning can take a while
arm.set_planning_time(5)
# Allow replanning to increase the odds of a solution
arm.allow_replanning(True)
# Set the right arm reference frame
arm.set_pose_reference_frame(REFERENCE_FRAME)
# Allow some leeway in position(meters) and orientation (radians)
arm.set_goal_position_tolerance(0.05)
arm.set_goal_orientation_tolerance(0.1)
# Get the name of the end-effector link
end_effector_link = arm.get_end_effector_link()
# Start in the "resting" configuration stored in the SRDF file
arm.set_named_target('l_arm_init')
# Plan and execute a trajectory to the goal configuration
arm.go()
rospy.sleep(1)
# Open the gripper
gripper.set_joint_value_target(GRIPPER_NEUTRAL)
gripper.go()
rospy.sleep(1)
# Set an initial target pose with the arm up and to the right
target_pose = PoseStamped()
target_pose.header.frame_id = REFERENCE_FRAME
target_pose.pose.position.x = 0.263803774718
target_pose.pose.position.y = 0.295405791959
target_pose.pose.position.z = 0.690438884208
q = quaternion_from_euler(0, 0, -1.57079633)
target_pose.pose.orientation.x = q[0]
target_pose.pose.orientation.y = q[1]
target_pose.pose.orientation.z = q[2]
target_pose.pose.orientation.w = q[3]
# Set the start state and target pose, then plan and execute
arm.set_start_state(robot.get_current_state())
arm.set_pose_target(target_pose, end_effector_link)
arm.go()
rospy.sleep(2)
# Close the gripper
gripper.set_joint_value_target(GRIPPER_CLOSED)
gripper.go()
rospy.sleep(1)
# Store the current pose
start_pose = arm.get_current_pose(end_effector_link)
# Create a contraints list and give it a name
constraints = Constraints()
constraints.name = "Keep gripper horizontal"
# Create an orientation constraint for the right gripper
orientation_constraint = OrientationConstraint()
orientation_constraint.header = start_pose.header
orientation_constraint.link_name = arm.get_end_effector_link()
orientation_constraint.orientation.w = 1.0
orientation_constraint.absolute_x_axis_tolerance = 0.1
orientation_constraint.absolute_y_axis_tolerance = 0.1
orientation_constraint.absolute_z_axis_tolerance = 0.1
orientation_constraint.weight = 1.0
# q = quaternion_from_euler(0, 0, -1.57079633)
# orientation_constraint.orientation.x = q[0]
# orientation_constraint.orientation.y = q[1]
# orientation_constraint.orientation.z = q[2]
# orientation_constraint.orientation.w = q[3]
# Append the constraint to the list of contraints
constraints.orientation_constraints.append(orientation_constraint)
# Set the path constraints on the arm
arm.set_path_constraints(constraints)
# Set a target pose for the arm
target_pose = PoseStamped()
target_pose.header.frame_id = REFERENCE_FRAME
target_pose.pose.position.x = 0.39000848183
target_pose.pose.position.y = 0.185900663329
target_pose.pose.position.z = 0.732752341378
target_pose.pose.orientation.w = 1
# Set the start state and target pose, then plan and execute
arm.set_start_state_to_current_state()
arm.set_pose_target(target_pose, end_effector_link)
arm.go()
rospy.sleep(1)
# Clear all path constraints
arm.clear_path_constraints()
# Open the gripper
gripper.set_joint_value_target(GRIPPER_NEUTRAL)
gripper.go()
rospy.sleep(1)
# Return to the "resting" configuration stored in the SRDF file
arm.set_named_target('l_arm_init')
# Plan and execute a trajectory to the goal configuration
arm.go()
rospy.sleep(1)
# Shut down MoveIt cleanly
moveit_commander.roscpp_shutdown()
# Exit MoveIt
moveit_commander.os._exit(0)
def __init__(self):
moveit_commander.roscpp_initialize(sys.argv)
rospy.init_node('moveit_demo')
scene = PlanningSceneInterface('world')
self.scene_pub = rospy.Publisher('planning_scene', PlanningScene)
# self.gripper_pose_pub = rospy.Publisher('gripper_pose', PoseStamped)
self.colors = dict()
max_pick_attempts = 10
max_place_attempts = 10
rospy.sleep(1)
arm = MoveGroupCommander('arm')
end_effector_link = arm.get_end_effector_link()
# arm.set_goal_position_tolerance(0.02)
# arm.set_goal_orientation_tolerance(0.03)
arm.allow_replanning(True)
arm.set_planning_time(5)
table_id = 'table'
side_id = 'side'
table2_id = 'table2'
box1_id = 'box1'
box2_id = 'box2'
target_id = 'target'
wall_id = 'wall'
ground_id = 'ground'
scene.remove_world_object(box1_id)
scene.remove_world_object(box2_id)
scene.remove_world_object(table_id)
scene.remove_world_object(table2_id)
scene.remove_world_object(target_id)
scene.remove_world_object(wall_id)
scene.remove_world_object(ground_id)
scene.remove_world_object(side_id)
rospy.sleep(1)
table_ground = 0.55
table_size = [0.2, 0.7, 0.01]
table2_size = [0.25, 0.6, 0.01]
box1_size = [0.02, 0.9, 0.6]
box2_size = [0.05, 0.05, 0.15]
target_size = [0.03, 0.06, 0.10]
wall_size = [0.01, 0.9, 0.6]
ground_size = [3, 3, 0.01]
side_size = [0.01, 0.7, table_ground]
table_pose = PoseStamped()
table_pose.header.frame_id = REFERENCE_FRAME
self.setPose(table_pose,
[0.7, 0.0, table_ground + table_size[2] / 2.0])
scene.add_box(table_id, table_pose, table_size)
side_pose = PoseStamped()
side_pose.header.frame_id = REFERENCE_FRAME
self.setPose(side_pose, [0.605, 0.0, side_size[2] / 2.0])
scene.add_box(side_id, side_pose, side_size)
table2_pose = PoseStamped()
table2_pose.header.frame_id = REFERENCE_FRAME
self.setPose(table2_pose,
[-0.325, 0.0, box1_size[2] + table2_size[2] / 2.0])
# scene.add_box(table2_id, table2_pose, table2_size)
box1_pose = PoseStamped()
box1_pose.header.frame_id = REFERENCE_FRAME
self.setPose(box1_pose, [-0.36, 0.0, box1_size[2] / 2.0])
# scene.add_box(box1_id, box1_pose, box1_size)
box2_pose = PoseStamped()
box2_pose.header.frame_id = REFERENCE_FRAME
self.setPose(
box2_pose,
[0.63, -0.12, table_ground + table_size[2] + box2_size[2] / 2.0])
# scene.add_box(box2_id, box2_pose, box2_size)
target_pose = PoseStamped()
target_pose.header.frame_id = REFERENCE_FRAME
self.setPose(
target_pose,
[0.62, 0.0, table_ground + table_size[2] + target_size[2] / 2.0])
scene.add_box(target_id, target_pose, target_size)
wall_pose = PoseStamped()
wall_pose.header.frame_id = REFERENCE_FRAME
self.setPose(
wall_pose,
[-0.405, 0.0, box1_size[2] + table2_size[2] + wall_size[2] / 2.0])
# scene.add_box(wall_id, wall_pose, wall_size)
ground_pose = PoseStamped()
ground_pose.header.frame_id = REFERENCE_FRAME
self.setPose(ground_pose, [0.0, 0.0, -ground_size[2] / 2.0])
# scene.add_box(ground_id, ground_pose, ground_size)
self.setColor(table_id, 0.8, 0.0, 0.0, 1.0)
# self.setColor(table2_id, 0.8, 0.0, 0.0)
# self.setColor(box1_id, 0.9, 0.9, 0.9)
# self.setColor(box2_id, 0.8, 0.4, 1.0)
self.setColor(target_id, 0.5, 0.4, 1.0)
# self.setColor(wall_id, 0.9, 0.9, 0.9)
self.setColor(ground_id, 0.3, 0.3, 0.3, 1.0)
self.setColor(side_id, 0.8, 0.0, 0.0, 1.0)
self.sendColors()
constraints = Constraints()
constraints.name = 'gripper constraint'
orientation_constraint = OrientationConstraint()
arm.set_support_surface_name(table_id)
test_pose = PoseStamped()
test_pose.header.frame_id = REFERENCE_FRAME
test_orientation = Quaternion()
test_orientation = quaternion_from_euler(0.0, 1.57, -3.14)
self.setPose(test_pose, [
-0.15, -0.22, box1_size[2] + table2_size[2] + target_size[2] / 2.0
], list(test_orientation))
place_pose = PoseStamped()
place_pose.header.frame_id = REFERENCE_FRAME
self.setPose(
place_pose,
[0.62, -0.22, table_ground + table_size[2] + target_size[2] / 2.0])
grasp_pose = target_pose
grasp_init_orientation = Quaternion()
grasp_init_orientation = quaternion_from_euler(0.0, 1.57, 0.0)
grasp_pose.pose.orientation.x = grasp_init_orientation[0]
grasp_pose.pose.orientation.y = grasp_init_orientation[1]
grasp_pose.pose.orientation.z = grasp_init_orientation[2]
grasp_pose.pose.orientation.w = grasp_init_orientation[3]
grasp_pose.pose.position.x -= 0.02
rospy.loginfo('quaterion: ' + str(grasp_pose.pose.orientation))
place_pose.pose.orientation.x = grasp_init_orientation[0]
place_pose.pose.orientation.y = grasp_init_orientation[1]
place_pose.pose.orientation.z = grasp_init_orientation[2]
place_pose.pose.orientation.w = grasp_init_orientation[3]
orientation_constraint.header = grasp_pose.header
orientation_constraint.link_name = end_effector_link
orientation_constraint.orientation.x = grasp_init_orientation[0]
orientation_constraint.orientation.y = grasp_init_orientation[1]
orientation_constraint.orientation.z = grasp_init_orientation[2]
orientation_constraint.orientation.w = grasp_init_orientation[3]
orientation_constraint.absolute_x_axis_tolerance = 0.03
orientation_constraint.absolute_y_axis_tolerance = 0.03
orientation_constraint.absolute_z_axis_tolerance = 3.14
orientation_constraint.weight = 1.0
constraints.orientation_constraints.append(orientation_constraint)
grasps = self.make_grasps(grasp_pose, [table_id],
[0.05, 0.07, [1.0, 0.0, 0.0]],
[0.22, 0.26, [-1.0, 0.0, 0.0]])
result = None
n_attempts = 0
while result != MoveItErrorCodes.SUCCESS and n_attempts < max_pick_attempts:
result = arm.pick(target_id, grasps)
n_attempts += 1
rospy.loginfo('Pick attempt:' + str(n_attempts))
rospy.sleep(0.2)
arm.set_path_constraints(constraints)
arm.set_pose_target(place_pose)
arm.go()
'''
if result == MoveItErrorCodes.SUCCESS:
result = None
n_attempts = 0
places = self.make_places(place_pose, [table_id], [0.8, 0.13, [1.0, 0.0, 0.0]], [0.1, 0.12, [-1.0, 0.0, 0.0]])
while result != MoveItErrorCodes.SUCCESS and n_attempts < max_place_attempts:
for place in places:
result = arm.place(target_id, place)
if result == MoveItErrorCodes.SUCCESS:
break
n_attempts += 1
rospy.loginfo('Place attempt:' + str(n_attempts))
rospy.sleep(0.2)
'''
'''
grasp_pose = place_pose
grasp_pose.pose.orientation.x = grasp_init_orientation[0]
grasp_pose.pose.orientation.y = grasp_init_orientation[1]
grasp_pose.pose.orientation.z = grasp_init_orientation[2]
grasp_pose.pose.orientation.w = grasp_init_orientation[3]
grasp_pose.pose.position.x -= 0.02
grasps = self.make_grasps(grasp_pose, [table_id], [0.05, 0.07, [1.0, 0.0, 0.0]], [0.15, 0.20, [-1.0, 0.0, 0.0]])
result = None
n_attempts = 0
while result != MoveItErrorCodes.SUCCESS and n_attempts < max_pick_attempts:
result = arm.pick(target_id, grasps)
n_attempts += 1
rospy.loginfo('Pick attempt:' + str(n_attempts))
rospy.sleep(0.2)
'''
'''
arm.set_support_surface_name(table2_id)
self.setPose(place_pose, [-0.26, -0.22, box1_size[2] + table2_size[2] + target_size[2]/2.0])
if result == MoveItErrorCodes.SUCCESS:
result = None
n_attempts = 0
places = self.make_places(place_pose, [table2_id], [0.03, 0.06, [-1.0, 0.0, 0.0]], [0.1, 0.15, [1.0, 0.0, 0.0]])
while result != MoveItErrorCodes.SUCCESS and n_attempts < max_place_attempts:
for place in places:
result = arm.place(target_id, place)
if result == MoveItErrorCodes.SUCCESS:
break
n_attempts += 1
rospy.loginfo('Place attempt:' + str(n_attempts))
rospy.sleep(0.2)
'''
rospy.sleep(2)
moveit_commander.roscpp_shutdown()
moveit_commander.os._exit(0)
# Notes on getting jacobian etc.
# Install pykdl_utils by checking out git into catkin_ws and catkin_make_isolated
if __name__ == '__main__':
rospy.init_node('touch_table', anonymous=True)
moveit_commander.roscpp_initialize(sys.argv)
robot = moveit_commander.RobotCommander()
scene = moveit_commander.PlanningSceneInterface()
group_name = "panda_arm"
move_group = moveit_commander.MoveGroupCommander(group_name)
path_constr = Constraints()
path_constr.name = "arm_constr"
joint_constraint = JointConstraint()
joint_constraint.position = 0.8
joint_constraint.tolerance_above = 3.14
joint_constraint.tolerance_below = 0.1
joint_constraint.weight = 1
joint_constraint.joint_name = "panda_joint2"
path_constr.joint_constraints.append(joint_constraint)
move_group.set_path_constraints(path_constr)
try:
touch_and_refine_table(robot, scene, move_group)
except KeyboardInterrupt:
print("Shutting down")
jc1 = JointConstraint()
jc1.joint_name = "j5"
jc1.position = 1.25752837976
jc1.tolerance_above = 0.0001
jc1.tolerance_below = 0.0001
jc1.weight = 1.0
jc0 = JointConstraint()
jc0.joint_name = "flange"
jc0.position = -4.43859335239
jc0.tolerance_above = 0.0001
jc0.tolerance_below = 0.0001
jc0.weight = 1.0
cons = Constraints()
cons.name = ""
cons.joint_constraints.append(jc2)
cons.joint_constraints.append(jc3)
cons.joint_constraints.append(jc4)
cons.joint_constraints.append(jc5)
cons.joint_constraints.append(jc1)
cons.joint_constraints.append(jc0)
msg.workspace_parameters = workspace_parameters
msg.start_state = start_state
msg.goal_constraints.append(cons)
msg.num_planning_attempts = 1
msg.allowed_planning_time = 5.0
msg.group_name = "manipulator"
def kinect_planner():
global fresh_data
global joint_target, pose_target, goal
global joints_req, pose_req
moveit_commander.roscpp_initialize(sys.argv)
rospy.init_node('kinect_trajectory_planner', anonymous=True)
rate = rospy.Rate(0.5)
#rospy.sleep(3)
# Instantiate a RobotCommander object.
robot = moveit_commander.RobotCommander()
# Instantiate a PlanningSceneInterface object (interface to the world surrounding the robot).
scene = moveit_commander.PlanningSceneInterface()
print "Remember to disable firewall if it's not working"
# Instantiate MoveGroupCommander objects for arms and Kinect2.
group = moveit_commander.MoveGroupCommander("Kinect2_Target")
group_left_arm = moveit_commander.MoveGroupCommander("left_arm")
group_right_arm = moveit_commander.MoveGroupCommander("right_arm")
# We create this DisplayTrajectory publisher to publish
# trajectories for RVIZ to visualize.
display_trajectory_publisher = rospy.Publisher('planned_path',
moveit_msgs.msg.DisplayTrajectory, queue_size=5)
# Set the planner for Moveit
group.set_planner_id("RRTConnectkConfigDefault")
group.set_planning_time(8)
group.set_pose_reference_frame('base_footprint')
# Setting tolerance
group.set_goal_tolerance(0.08)
group.set_num_planning_attempts(10)
# Suscribing to the desired pose topic
target = rospy.Subscriber("desired_pose", PoseStamped, callback)
target_joint = rospy.Subscriber("desired_joints", pose_w_joints, callback_joints)
# Locating the arms and the Kinect2 sensor
group_left_arm_values = group_left_arm.get_current_joint_values()
group_right_arm_values = group_right_arm.get_current_joint_values()
#group_kinect_values = group_kinect.get_current_joint_values()
neck_init_joints = group.get_current_joint_values()
neck_init_joints[0] = -1.346
neck_init_joints[1] = -1.116
neck_init_joints[2] = -2.121
neck_init_joints[3] = 0.830
neck_init_joints[4] = 1.490
neck_init_joints[5] = 0.050
neck_init_joints[6] = 0
neck_init_joints[7] = 0
neck_init_joints[8] = 0
neck_init_joints[9] = 0
# Creating a box to limit the arm position
box_pose = PoseStamped()
box_pose.pose.orientation.w = 1
box_pose.pose.position.x = 0.6
box_pose.pose.position.y = 0.03
box_pose.pose.position.z = 1.5
box_pose.header.frame_id = 'base_footprint'
scene.add_box('box1', box_pose, (0.4, 0.4, 0.1))
rospy.sleep(2)
# Defining position constraints for the trajectory of the kinect
neck_const = Constraints()
neck_const.name = 'neck_constraints'
target_const = JointConstraint()
target_const.joint_name = "neck_joint_end"
target_const.position = 0.95
target_const.tolerance_above = 0.45
target_const.tolerance_below = 0.05
target_const.weight = 0.9 # Importance of this constraint
neck_const.joint_constraints.append(target_const)
# Talking to the robot
client = actionlib.SimpleActionClient('/Kinect2_Target_controller/follow_joint_trajectory', FollowJointTrajectoryAction)
print "====== Waiting for server..."
client.wait_for_server()
print "====== Connected to server"
while not rospy.is_shutdown():
if fresh_data == True: #If a new pose is received, plan.
# Update arms position
group_left_arm_values = group_left_arm.get_current_joint_values()
group_right_arm_values = group_right_arm.get_current_joint_values()
# Set the target pose (or joint values) and generate a plan
if pose_req:
group.set_pose_target(pose_target)
if joints_req:
group.set_joint_value_target(joint_target)
print "=========== Calculating trajectory... \n"
# Generate several plans and compare them to get the shorter one
plan_opt = dict()
differ = dict()
try:
num = 1
rep = 0
# Generate several plans and compare them to get the shortest one
#for num in range(1,8):
while num < 7:
num += 1
plan_temp = group.plan()
move(plan_temp)
plan_opt[num] = goal.trajectory
diff=0
for point in goal.trajectory.points:
# calculate the distance between initial pose and planned one
for i in range(0,6):
diff = abs(neck_init_joints[i] - point.positions[i])+abs(diff)
differ[num] = diff
# If the current plan is good, take it
if diff < 110:
break
# If plan is too bad, don't consider it
if diff > 400:
num = num - 1
print "Plan is too long. Replanning."
rep = rep + 1
if rep > 4:
num = num +1
# If no plan was found...
if differ == {}:
print "---- Fail: No motion plan found. No execution attempted. Probably robot joints are out of range."
break
else:
# Select the shortest plan
min_plan = min(differ.itervalues())
select = [k for k, value in differ.iteritems() if value == min_plan]
goal.trajectory = plan_opt[select[0]]
#print " Plan difference:======= ", differ
#print " Selected plan:========= ", select[0]
# Remove the last 4 names and data from each point (dummy joints) before sending the goal
goal.trajectory.joint_names = goal.trajectory.joint_names[:6]
for point in goal.trajectory.points:
point.positions = point.positions[:6]
point.velocities = point.velocities[:6]
point.accelerations = point.accelerations[:6]
print "Sending goal"
client.send_goal(goal)
print "Waiting for result"
client.wait_for_result()
# Change the position of the virtual joint to avoid collision
neck_joints = group.get_current_joint_values()
#neck_joints[6] = 0.7
#group.set_joint_value_target(neck_joints)
#group.go(wait=True)
except (KeyboardInterrupt, SystemExit):
client.cancel_goal()
raise
rate.sleep()
fresh_data = False
rate.sleep()
def main():
roscpp_initialize(sys.argv)
rospy.init_node('grasp_ur5', anonymous=True)
robot = RobotCommander()
scene = PlanningSceneInterface()
arm_group = MoveGroupCommander("manipulator")
q = quaternion_from_euler(1.5701, 0.0, -1.5701)
pose_target = geometry_msgs.msg.Pose()
pose_target.orientation.x = q[0]
pose_target.orientation.y = q[1]
pose_target.orientation.z = q[2]
pose_target.orientation.w = q[3]
pose_target.position.z = 0.23 #0.23
pose_target.position.y = 0.11 #0.11
pose_target.position.x = -0.45 #-0.45
arm_group.set_pose_target(pose_target)
plan = arm_group.plan(pose_target)
while plan[0]!= True:
plan = arm_group.plan(pose_target)
if plan[0]:
traj = plan[1]
arm_group.execute(traj, wait = True)
arm_group.stop()
arm_group.clear_pose_targets()
# rospy.sleep(1)
#FAKE PLAN WITHOUT RESTRICTIONS #
# state = RobotState()
# arm_group.set_start_state(state)
pose_target.position.z = 0.77
arm_group.set_pose_target(pose_target)
plan_fake = arm_group.plan(pose_target)
while plan_fake[0] != True:
plan_fake = arm_group.plan(pose_target)
pose = arm_group.get_current_pose()
constraint = Constraints()
constraint.name = "restricao"
orientation_constraint = OrientationConstraint()
orientation_constraint.header = pose.header
orientation_constraint.link_name = arm_group.get_end_effector_link()
orientation_constraint.orientation = pose.pose.orientation
orientation_constraint.absolute_x_axis_tolerance = 3.14
orientation_constraint.absolute_y_axis_tolerance = 3.14
orientation_constraint.absolute_z_axis_tolerance = 3.14
orientation_constraint.weight = 1
constraint.orientation_constraints.append(orientation_constraint)
arm_group.set_path_constraints(constraint)
# state = RobotState()
# arm_group.set_start_state(state)
pose_target.position.z = 0.77 # 0.77
# pose_target.position.y = -0.11 # -0.11
# pose_target.position.x = 0.31 # 0.31
arm_group.set_pose_target(pose_target)
plan = arm_group.plan(pose_target)
while plan[0]!= True:
plan = arm_group.plan(pose_target)
if plan[0]:
traj = plan[1]
arm_group.execute(traj, wait = True)
arm_group.stop()
arm_group.clear_pose_targets()
rospy.init_node('move_group_python_interface_tutorial', anonymous=True)
robot = moveit_commander.RobotCommander()
scene = moveit_commander.PlanningSceneInterface()
group_name = "manipulator"
group = moveit_commander.MoveGroupCommander(group_name)
# We can get the joint values from the group and adjust some of the values:
pose_goal = geometry_msgs.msg.Pose()
#Rotation
group.set_max_velocity_scaling_factor(0.1)
#group.set_planner_id("ESTkConfigDefault")
upright_constraints = Constraints()
upright_constraints.name = "upright"
"""
joint_constraint=JointConstraint()
joint_constraint.joint_name='upperArm_joint'
joint_constraint.position=-1.57
joint_constraint.tolerance_above=1.57
joint_constraint.tolerance_below=1.57
joint_constraint.weight=.5
upright_constraints.joint_constraints.append(joint_constraint)
joint_constraint=JointConstraint()
joint_constraint.joint_name='foreArm_joint'
joint_constraint.position=1.57
joint_constraint.tolerance_above=1.57
joint_constraint.tolerance_below=1.57
def __init__(self):
# Initialize the move_group API
moveit_commander.roscpp_initialize(sys.argv)
# Initialize the ROS node
rospy.init_node('moveit_demo', anonymous=True)
robot = RobotCommander()
# Connect to the right_arm move group
right_arm = MoveGroupCommander(GROUP_NAME_ARM)
# Initialize the move group for the right gripper
right_gripper = MoveGroupCommander(GROUP_NAME_GRIPPER)
# Increase the planning time since constraint planning can take a while
right_arm.set_planning_time(15)
# Allow replanning to increase the odds of a solution
right_arm.allow_replanning(True)
# Set the right arm reference frame
right_arm.set_pose_reference_frame(REFERENCE_FRAME)
# Allow some leeway in position(meters) and orientation (radians)
right_arm.set_goal_position_tolerance(0.05)
right_arm.set_goal_orientation_tolerance(0.1)
# Get the name of the end-effector link
end_effector_link = right_arm.get_end_effector_link()
# Start in the "resting" configuration stored in the SRDF file
right_arm.set_named_target('right_arm_zero')
# Plan and execute a trajectory to the goal configuration
right_arm.go()
rospy.sleep(1)
# Open the gripper
right_gripper.set_joint_value_target(GRIPPER_NEUTRAL)
right_gripper.go()
rospy.sleep(1)
# Set an initial target pose with the arm up and to the right
target_pose = PoseStamped()
target_pose.header.frame_id = REFERENCE_FRAME
target_pose.pose.position.x = 0.237012590198
target_pose.pose.position.y = -0.0747191267505
target_pose.pose.position.z = 0.901578401949
target_pose.pose.orientation.w = 1.0
# Set the start state and target pose, then plan and execute
right_arm.set_start_state(robot.get_current_state())
right_arm.set_pose_target(target_pose, end_effector_link)
right_arm.go()
rospy.sleep(2)
# Close the gripper
right_gripper.set_joint_value_target(GRIPPER_CLOSED)
right_gripper.go()
rospy.sleep(1)
# Store the current pose
start_pose = right_arm.get_current_pose(end_effector_link)
# Create a contraints list and give it a name
constraints = Constraints()
constraints.name = "Keep gripper horizontal"
# Create an orientation constraint for the right gripper
orientation_constraint = OrientationConstraint()
orientation_constraint.header = start_pose.header
orientation_constraint.link_name = right_arm.get_end_effector_link()
orientation_constraint.orientation.w = 1.0
orientation_constraint.absolute_x_axis_tolerance = 0.1
orientation_constraint.absolute_y_axis_tolerance = 0.1
orientation_constraint.absolute_z_axis_tolerance = 3.14
orientation_constraint.weight = 1.0
# Append the constraint to the list of contraints
constraints.orientation_constraints.append(orientation_constraint)
# Set the path constraints on the right_arm
right_arm.set_path_constraints(constraints)
# Set a target pose for the arm
target_pose = PoseStamped()
target_pose.header.frame_id = REFERENCE_FRAME
target_pose.pose.position.x = 0.173187824708
target_pose.pose.position.y = -0.0159929871606
target_pose.pose.position.z = 0.692596608605
target_pose.pose.orientation.w = 1.0
# Set the start state and target pose, then plan and execute
right_arm.set_start_state_to_current_state()
right_arm.set_pose_target(target_pose, end_effector_link)
right_arm.go()
rospy.sleep(1)
# Clear all path constraints
right_arm.clear_path_constraints()
# Open the gripper
right_gripper.set_joint_value_target(GRIPPER_NEUTRAL)
right_gripper.go()
rospy.sleep(1)
# Return to the "resting" configuration stored in the SRDF file
right_arm.set_named_target('right_arm_zero')
# Plan and execute a trajectory to the goal configuration
right_arm.go()
rospy.sleep(1)
# Shut down MoveIt cleanly
moveit_commander.roscpp_shutdown()
# Exit MoveIt
moveit_commander.os._exit(0)
# Initialize moveit_commander and node
moveit_commander.roscpp_initialize(sys.argv)
rospy.init_node('d1_get_basic_info', anonymous=False)
# Get instance from moveit_commander
robot = moveit_commander.RobotCommander()
scene = moveit_commander.PlanningSceneInterface()
# Get group_commander from MoveGroupCommander
group_name = "arm"
move_group = moveit_commander.MoveGroupCommander(group_name)
# Move using target_pose with constraint
contraints = Constraints()
contraints.name = "constraints1"
jc1 = JointConstraint()
jc1.joint_name = "joint2"
jc1.position = 0.0
jc1.tolerance_above = 0.01
jc1.tolerance_below = 0.01
contraints.joint_constraints.append(jc1)
move_group.set_path_constraints(contraints)
# Move using target_pose
x = 0.185
y = 0
z = 0.365
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 joint_position_callback(joints):
global plan_only
fixed_frame = rospy.get_param("/fixed_frame")
client = actionlib.SimpleActionClient('move_group', MoveGroupAction)
client.wait_for_server()
move_group_goal = MoveGroupGoal()
try:
msg = MotionPlanRequest()
workspace_parameters = WorkspaceParameters()
workspace_parameters.header.stamp = rospy.Time.now()
workspace_parameters.header.frame_id = fixed_frame
workspace_parameters.min_corner = Vector3(-1.0, -1.0, -1.0)
workspace_parameters.max_corner = Vector3(1.0, 1.0, 1.0)
start_state = RobotState()
# start_state.joint_state.header.stamp = rospy.Time.now()
start_state.joint_state.header.frame_id = fixed_frame
start_state.joint_state.name = []
start_state.joint_state.position = []
cons = Constraints()
cons.name = ""
i = 0
for dim in joints.start_joint.layout.dim:
start_state.joint_state.name.append(dim.label)
start_state.joint_state.position.append(joints.start_joint.data[i])
jc = JointConstraint()
jc.joint_name = dim.label
jc.position = joints.goal_joint.data[i]
jc.tolerance_above = 0.0001
jc.tolerance_below = 0.0001
jc.weight = 1.0
i = i + 1
cons.joint_constraints.append(jc)
msg.workspace_parameters = workspace_parameters
msg.start_state = start_state
msg.goal_constraints.append(cons)
msg.num_planning_attempts = 1
msg.allowed_planning_time = 5.0
msg.group_name = joints.group_name
move_group_goal.request = msg
if joints.plan_only:
plan_only = True
move_group_goal.planning_options.plan_only = True
else:
plan_only = False
client.send_goal(move_group_goal)
client.wait_for_result(rospy.Duration.from_sec(5.0))
except rospy.ROSInterruptException, e:
print "failed: %s" % e
