-(table.x_min + table.x_max) / 2.0,
(new_pose.pose.position.z / 2.0)
], [
new_pose.pose.orientation.x, new_pose.pose.orientation.y,
new_pose.pose.orientation.z, new_pose.pose.orientation.w
], table.x_max - table.x_min, table.y_max - table.y_min,
new_pose.pose.position.z)
if __name__ == "__main__":
# initialize
rospy.init_node('test_arm_control', anonymous=True)
object_recognizer = ObjectRecognition()
arm_mover = MoveGroupCommander('arm')
arm_mover.go([0, 0, 0, 0, 0, 0, 0])
os.system('rosrun gripper_control lcg_set_pos.py -n low_cost -f 5 -p 0.13')
listener = tf.TransformListener()
planning_scene = PlanningSceneInterface()
object_listener = ObjectListener(planning_scene, listener)
rospy.Subscriber("/table_array", TableArray,
object_listener.table_callback)
while True:
arm_mover.go([0, 0, 0, 0, 0, 0, 0])
print "Looking for objects."
objects = object_recognizer.look_for_objects()
while not objects.recognized_objects.objects:
new_pose.pose.orientation.z,
new_pose.pose.orientation.w,
],
table.x_max - table.x_min,
table.y_max - table.y_min,
new_pose.pose.position.z,
)
if __name__ == "__main__":
# initialize
rospy.init_node("test_arm_control", anonymous=True)
object_recognizer = ObjectRecognition()
arm_mover = MoveGroupCommander("arm")
arm_mover.go([0, 0, 0, 0, 0, 0, 0])
os.system("rosrun gripper_control lcg_set_pos.py -n low_cost -f 5 -p 0.13")
listener = tf.TransformListener()
planning_scene = PlanningSceneInterface()
object_listener = ObjectListener(planning_scene, listener)
rospy.Subscriber("/table_array", TableArray, object_listener.table_callback)
while True:
arm_mover.go([0, 0, 0, 0, 0, 0, 0])
print "Looking for objects."
objects = object_recognizer.look_for_objects()
while not objects.recognized_objects.objects:
objects = object_recognizer.look_for_objects()
import roslib
roslib.load_manifest('moveit_commander')
import rospy
from geometry_msgs.msg import PoseStamped
from sensor_msgs.msg import JointState
from moveit_commander.commander import MoveGroupCommander
# initialize
rospy.init_node('test_arm_control', anonymous=True)
arm_mover = MoveGroupCommander('arm')
# desired joint positions
#msg = JointState()
#joint_position = [-0.817, 0.001, -1.253, -0.892, 60.854, -0.250, 3.338]
#joints = [-0.199792813192, -0.0301778257579, -0.119474613742, 0.113024891124, -0.0663316698686, -0.497162338433, 0.488334857099]
#msg.position = joints
# get a joint state message already configured for this arm
js = arm_mover.get_current_joint_values()
print "Current joints: %s" % (str(js))
arm_mover.set_pose_reference_frame('base_link')
pose = arm_mover.get_current_pose()
print "Current pose: %s" % (str(pose))
# set desired joint positions
#js.position = joints
#print 'Moving to %s' % (str(joints))
# send out the command
#reached_goal = arm_mover.move_to_goal(js)
#if not reached_goal:
# print arm_mover.get_exceptions()
import rospy
from geometry_msgs.msg import PoseStamped
from sensor_msgs.msg import JointState
from moveit_commander.commander import MoveGroupCommander
from moveit_commander.torso import Torso
from moveit_msgs.srv import ExecuteKnownTrajectory
from moveit_msgs.msg import RobotTrajectory
from trajectory_msgs.msg import JointTrajectoryPoint
# initialize
rospy.init_node('tuck_arm', anonymous=True)
torso = Torso("torso_lift_joint/command")
torso.move(0.2)
#joints = [-0.199792813192, -0.0301778257579, -0.119474613742, 0.113024891124, -0.0663316698686, -0.497162338433, 0.488334857099]
arm_mover = MoveGroupCommander('arm')
joints = [1.0337725352659688, -0.2635069358441139, 0.2732018645713051, -1.1433179087498708, -0.0010712502247107196, 1.0083989964733064, -0.3671447970689187]
reached_goal = arm_mover.go(joints)
if reached_goal:
print 'Success.'
else:
print 'Goal not reached.'
"""
traj = RobotTrajectory()
traj.joint_names = ['shoulder_pan_joint', 'shoulder_lift_joint', 'upper_arm_roll_joint', 'elbow_flex_joint', 'forearm_roll_joint', 'wrist_flex_joint', 'wrist_roll_joint']
point = JointTrajectoryPoint()
point.time_from_start = rospy.Duration(1)
point.positions = []
point.velocities = [0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0]
import roslib
roslib.load_manifest('moveit_commander')
import rospy
from geometry_msgs.msg import PoseStamped
from sensor_msgs.msg import JointState
from moveit_commander.commander import MoveGroupCommander
# initialize
rospy.init_node('test_arm_control', anonymous=True)
arm_mover = MoveGroupCommander('arm')
# desired joint positions
#msg = JointState()
#joint_position = [-0.817, 0.001, -1.253, -0.892, 60.854, -0.250, 3.338]
#joints = [-0.199792813192, -0.0301778257579, -0.119474613742, 0.113024891124, -0.0663316698686, -0.497162338433, 0.488334857099]
#msg.position = joints
# get a joint state message already configured for this arm
js = arm_mover.get_current_joint_values()
print "Current joints: %s" % (str(js))
arm_mover.set_pose_reference_frame('base_link')
pose = arm_mover.get_current_pose()
print "Current pose: %s" % (str(pose))
# set desired joint positions
#js.position = joints
#print 'Moving to %s' % (str(joints))
# send out the command
#reached_goal = arm_mover.move_to_goal(js)
#if not reached_goal:
# print arm_mover.get_exceptions()
