def main():
init_node()
# Preparing Left Arm
rospy.loginfo("Preparing Left Arm...")
larmg = MoveGroupCommander("left_arm")
larmg.set_pose_target([0.325, 0.182, 0.067, 0.0, -math.pi / 2, 0.0])
larmg.go()
# Right Arm
group = MoveGroupCommander("right_arm")
# Frame ID Definitoins
planning_frame_id = group.get_planning_frame()
tgt_frame_id = '/LARM_JOINT5_Link'
# Get a target pose
pose_target = get_current_target_pose(tgt_frame_id, planning_frame_id)
# Move to a point above target
if pose_target:
pose_target.pose.position.z += 0.4
rospy.loginfo("Set Target To: \n{}".format(pose_target))
group.set_pose_target(pose_target)
ret = group.go()
rospy.loginfo("Executed ... {}".format(ret))
else:
rospy.logwarn("Pose Error: {}".format(pose_target))
def main():
init_node()
# Preparing Head
rospy.loginfo("Preparing Head...")
headg = MoveGroupCommander("head")
headg.set_joint_value_target([0.0, 60.0 / 180.0 * math.pi])
headg.go()
# Preparing Both Arms
rospy.loginfo("Preparing Left Arm...")
barmg = MoveGroupCommander("botharms")
barmg.set_pose_target([0.325, 0.482, 0.167, 0.0, -math.pi / 2, 0.0],
'LARM_JOINT5_Link')
barmg.set_pose_target([0.325, -0.482, 0.167, 0.0, -math.pi / 2, 0.0],
'RARM_JOINT5_Link')
barmg.go()
rospy.sleep(2.0)
# Right Arm
group = MoveGroupCommander("right_arm")
# Frame ID Definitoins
planning_frame_id = group.get_planning_frame()
tgt_frame_id = '/ar_marker_4'
# Get a target pose
pose_target = get_current_target_pose(tgt_frame_id, planning_frame_id, 5.0)
# Move to a point above target
if pose_target:
# Rotate Pose for Right Hand
quat = []
quat.append(pose_target.pose.orientation.x)
quat.append(pose_target.pose.orientation.y)
quat.append(pose_target.pose.orientation.z)
quat.append(pose_target.pose.orientation.w)
quat = quaternion_multiply(
quat, quaternion_about_axis(math.pi / 2, (1, 0, 0)))
quat = quaternion_multiply(
quat, quaternion_about_axis(math.pi / 2, (0, 0, 1)))
pose_target.pose.orientation.x = quat[0]
pose_target.pose.orientation.y = quat[1]
pose_target.pose.orientation.z = quat[2]
pose_target.pose.orientation.w = quat[3]
pose_target.pose.position.z += 0.4
rospy.loginfo("Set Target To: \n{}".format(pose_target))
group.set_pose_target(pose_target)
ret = group.go()
rospy.loginfo("Executed ... {}".format(ret))
else:
rospy.logwarn("Pose Error: {}".format(pose_target))
#!/usr/bin/env python
import sys, copy
import rospy
from moveit_commander import MoveGroupCommander
from geometry_msgs.msg import Pose, PoseStamped
from tork_moveit_tutorial import init_node
if __name__ == '__main__':
init_node()
# Preparing Left Arm
rospy.loginfo("Preparing Left Arm...")
larmg = MoveGroupCommander("left_arm")
larm_init_pose = Pose()
larm_init_pose.position.x = 0.325
larm_init_pose.position.y = 0.182
larm_init_pose.position.z = 0.067
larm_init_pose.orientation.x = 0.0
larm_init_pose.orientation.y = -0.707
larm_init_pose.orientation.z = 0.0
larm_init_pose.orientation.w = 0.707
larmg.set_pose_target(larm_init_pose)
larmg.go()
# Right Arm
group = MoveGroupCommander("right_arm")