import roslib
roslib.load_manifest('moveit_commander')
import rospy
from geometry_msgs.msg import PoseStamped
from sensor_msgs.msg import JointState
from moveit_commander.torso import Torso
# initialize
rospy.init_node('test_torso_control', anonymous=True)
torso = Torso("torso_lift_joint/command")
# 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()
# 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()
#reached_goal = arm_mover.go(joints)
#if reached_goal:
# print 'Success.'
#else:
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
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)
