def setArmJoints(self, joint_states, joint_vel):
arm_joint_names = ["shoulder_pan_joint", "shoulder_lift_joint", "upperarm_roll_joint",
"elbow_flex_joint", "forearm_roll_joint", "wrist_flex_joint", "wrist_roll_joint"]
arm_joint_positions = joint_states
arm_joint_velocities = joint_vel
trajectory = JointTrajectory()
trajectory.joint_names = arm_joint_names
trajectory.points.append(JointTrajectoryPoint())
trajectory.points[0].positions = arm_joint_positions
trajectory.points[0].velocities = arm_joint_velocities
trajectory.points[0].accelerations = [
0.0 for _ in arm_joint_velocities]
trajectory.points[0].time_from_start = rospy.Duration(0.0001)
arm_goal = FollowJointTrajectoryGoal()
arm_goal.trajectory = trajectory
arm_goal.goal_time_tolerance = rospy.Duration(25.0)
rospy.loginfo("Setting Arm positions...")
self.arm_client.send_goal(arm_goal)
# self.arm_client.wait_for_result(rospy.Duration(25.0))
rospy.loginfo("...done")
def set_angle(self, yaw_angle, goal_sec):
goal = FollowJointTrajectoryGoal()
goal.trajectory.joint_names = ["waist_yaw_joint"]
# 現在の角度から開始（遷移時間は現在時刻）
yawpoint = JointTrajectoryPoint()
yawpoint.positions.append(self.present_angle)
yawpoint.time_from_start = rospy.Duration(nsecs=1)
goal.trajectory.points.append(yawpoint)
# 途中に角度と時刻をappendすると細かい速度制御が可能
# 参考=> http://wiki.ros.org/joint_trajectory_controller/UnderstandingTrajectoryReplacement
# ゴール角度を設定（指定されたゴール時間で到達）
yawpoint = JointTrajectoryPoint()
yawpoint.positions.append(yaw_angle)
yawpoint.time_from_start = rospy.Duration(goal_sec)
goal.trajectory.points.append(yawpoint)
self.present_angle = yaw_angle
# 軌道計画を実行
self.__client.send_goal(goal)
self.__client.wait_for_result(rospy.Duration(goal_sec))
return self.__client.get_result()
def send_arm_goal(self, arm_goal):
arm_joint_names = [
'joint_lift', 'joint_waist', 'joint_big_arm', 'joint_forearm',
'joint_wrist_pitching', 'joint_wrist_rotation'
]
rospy.loginfo("Waiting for follow_joint_trajectory server")
self.arm_client.wait_for_server()
rospy.loginfo("Connected to follow_joint_trajectory server")
rospy.sleep(1)
arm_trajectory = JointTrajectory()
arm_trajectory.joint_names = arm_joint_names
arm_trajectory.points.append(JointTrajectoryPoint())
arm_trajectory.points[0].positions = arm_goal
arm_trajectory.points[0].time_from_start = rospy.Duration(10)
rospy.loginfo("Preparing for moving the arm to goal position!")
rospy.sleep(1)
arm_goal_pos = FollowJointTrajectoryGoal()
arm_goal_pos.trajectory = arm_trajectory
arm_goal_pos.goal_time_tolerance = rospy.Duration(0)
self.arm_client.send_goal(arm_goal_pos)
rospy.loginfo("Send goal to the trajectory server successfully!")
self.arm_client.wait_for_result()
def move_head(self, pan, tilt):
# Which joints define the head?
head_joints = ['head_pan_joint', 'head_tilt_mod_joint']
# Create a single-point head trajectory with the head_goal as the end-point
head_trajectory = JointTrajectory()
head_trajectory.joint_names = head_joints
head_trajectory.points.append(JointTrajectoryPoint())
head_trajectory.points[0].positions = pan, tilt
head_trajectory.points[0].velocities = [0.0 for i in head_joints]
head_trajectory.points[0].accelerations = [0.0 for i in head_joints]
head_trajectory.points[0].time_from_start = rospy.Duration(3.0)
# Send the trajectory to the head action server
rospy.loginfo('Moving the head to goal position...')
head_goal = FollowJointTrajectoryGoal()
head_goal.trajectory = head_trajectory
head_goal.goal_time_tolerance = rospy.Duration(0.0)
# Send the goal
self.head_client.send_goal(head_goal)
# Wait for up to 5 seconds for the motion to complete
self.head_client.wait_for_result(rospy.Duration(2.0))
def clear(self, limb):
self._goal = FollowJointTrajectoryGoal()
self._goal.goal_time_tolerance = self._goal_time_tolerance
self._goal.trajectory.joint_names = self._joint_names
def __init__(self):
# The current status of the joints.
self.JointState = JointTrajectoryControllerState()
# The servo power's status of the robot.
self.ServoPowerState = Bool()
# The fault power's status of the robot.
self.PowerFaultState = Bool()
# The reference coordinate in the calculations of the elfin_basic_api node
self.RefCoordinate = String()
# The end coordinate in the calculations of the elfin_basic_api node
self.EndCoordinate = String()
#The value of the dynamic parameters of elfin_basic_api, e.g. velocity scaling.
self.DynamicArgs = Config()
# get the reference coordinate name of elfin_basic_api from the response of this service.
self.call_ref_coordinate = rospy.ServiceProxy(
'elfin_basic_api/get_reference_link', SetBool)
self.call_ref_coordinate_req = SetBoolRequest()
# get the end coordinate name of elfin_basic_api from the response of this service.
self.call_end_coordinate = rospy.ServiceProxy(
'elfin_basic_api/get_end_link', SetBool)
self.call_end_coordinate_req = SetBoolRequest()
# for publishing joint goals to elfin_basic_api
self.JointsPub = rospy.Publisher('elfin_basic_api/joint_goal',
JointState,
queue_size=1)
self.JointsGoal = JointState()
# for publishing cart goals to elfin_basic_api
self.CartGoalPub = rospy.Publisher('elfin_basic_api/cart_goal',
PoseStamped,
queue_size=1)
self.CartPos = PoseStamped()
# for pub cart path
self.CartPathPub = rospy.Publisher('elfin_basic_api/cart_path_goal',
PoseArray,
queue_size=1)
self.CartPath = PoseArray()
self.CartPath.header.stamp = rospy.get_rostime()
self.CartPath.header.frame_id = 'elfin_base_link'
# for pub one specific joint action to elfin_teleop_joint_cmd_no_limit
self.JointCmdPub = rospy.Publisher('elfin_teleop_joint_cmd_no_limit',
Int64,
queue_size=1)
self.JointCmd = Int64()
# action client, send goal to move_group
self.action_client = SimpleActionClient(
'elfin_module_controller/follow_joint_trajectory',
FollowJointTrajectoryAction)
self.action_goal = FollowJointTrajectoryGoal()
#self.goal_list = JointTrajectoryPoint()
self.goal_list = []
self.joints_ = []
self.ps_ = []
self.listener = tf.TransformListener()
self.robot = moveit_commander.RobotCommander()
self.scene = moveit_commander.PlanningSceneInterface()
self.group = moveit_commander.MoveGroupCommander('elfin_arm')
self.ref_link_name = self.group.get_planning_frame()
self.end_link_name = self.group.get_end_effector_link()
self.ref_link_lock = threading.Lock()
self.end_link_lock = threading.Lock()
self.call_teleop_stop = rospy.ServiceProxy(
'elfin_basic_api/stop_teleop', SetBool)
self.call_teleop_stop_req = SetBoolRequest()
self.call_teleop_joint = rospy.ServiceProxy(
'elfin_basic_api/joint_teleop', SetInt16)
self.call_teleop_joint_req = SetInt16Request()
self.call_teleop_cart = rospy.ServiceProxy(
'elfin_basic_api/cart_teleop', SetInt16)
self.call_teleop_cart_req = SetInt16Request()
self.call_move_homing = rospy.ServiceProxy(
'elfin_basic_api/home_teleop', SetBool)
self.call_move_homing_req = SetBoolRequest()
self.call_reset = rospy.ServiceProxy(
self.elfin_driver_ns + 'clear_fault', SetBool)
self.call_reset_req = SetBoolRequest()
self.call_reset_req.data = True
self.call_power_on = rospy.ServiceProxy(
self.elfin_driver_ns + 'enable_robot', SetBool)
self.call_power_on_req = SetBoolRequest()
self.call_power_on_req.data = True
self.call_power_off = rospy.ServiceProxy(
self.elfin_driver_ns + 'disable_robot', SetBool)
self.call_power_off_req = SetBoolRequest()
self.call_power_off_req.data = True
self.call_velocity_setting = rospy.ServiceProxy(
'elfin_basic_api/set_velocity_scale', SetFloat64)
self.call_velocity_req = SetFloat64Request()
self._velocity_scale = 0.78
self.set_velocity_scale(self._velocity_scale)
pass
def __init__(self):
filename = '/home/kaiser/WS_ROS/catkin_ws/src/urdf_serp/urdf/urdf_exportado4.urdf'
datos_articulaciones = open('datos_articulaciones.pkl', 'wb')
robot = urdf_parser_py.urdf.URDF.from_xml_file(filename)
(ok, tree) = kdl_parser_py.urdf.treeFromFile(filename)
cadena_der_up_down = tree.getChain("base_link", "pie_der_link")
cadena_der_down_up = tree.getChain("pie_der_link", "base_link")
cadena_izq_up_down = tree.getChain("base_link", "pie_izq_link")
cadena_izq_down_up = tree.getChain("pie_izq_link", "base_link")
print cadena_der_up_down.getNrOfSegments()
fksolver_der_up_down = PyKDL.ChainFkSolverPos_recursive(cadena_der_up_down)
fksolver_der_down_up = PyKDL.ChainFkSolverPos_recursive(cadena_der_down_up)
fksolver_izq_up_down = PyKDL.ChainFkSolverPos_recursive(cadena_izq_up_down)
fksolver_izq_down_up = PyKDL.ChainFkSolverPos_recursive(cadena_izq_down_up)
vik_der_up_down = PyKDL.ChainIkSolverVel_pinv(cadena_der_up_down)
ik_der_up_down = PyKDL.ChainIkSolverPos_NR(cadena_der_up_down, fksolver_der_up_down, vik_der_up_down)
vik_der_down_up = PyKDL.ChainIkSolverVel_pinv(cadena_der_down_up)
ik_der_down_up = PyKDL.ChainIkSolverPos_NR(cadena_der_down_up, fksolver_der_down_up, vik_der_down_up)
vik_izq_up_down = PyKDL.ChainIkSolverVel_pinv(cadena_izq_up_down)
ik_izq_up_down = PyKDL.ChainIkSolverPos_NR(cadena_izq_up_down, fksolver_izq_up_down, vik_izq_up_down)
vik_izq_down_up = PyKDL.ChainIkSolverVel_pinv(cadena_izq_down_up)
ik_izq_down_up = PyKDL.ChainIkSolverPos_NR(cadena_izq_down_up, fksolver_izq_down_up, vik_izq_down_up)
nj_izq = cadena_der_up_down.getNrOfJoints()
posicionInicial_der_up_down = PyKDL.JntArray(nj_izq)
posicionInicial_der_up_down[0] = 0.3
posicionInicial_der_up_down[1] = -0.3
posicionInicial_der_up_down[2] = 0
posicionInicial_der_up_down[3] = 0.6
posicionInicial_der_up_down[4] = -0.3
posicionInicial_der_up_down[5] = -0.3
nj_izq = cadena_izq_up_down.getNrOfJoints()
posicionInicial_izq_up_down = PyKDL.JntArray(nj_izq)
posicionInicial_izq_up_down[0] = 0.3
posicionInicial_izq_up_down[1] = -0.3
posicionInicial_izq_up_down[2] = 0.0
posicionInicial_izq_up_down[3] = 0.6
posicionInicial_izq_up_down[4] = -0.3
posicionInicial_izq_up_down[5] = -0.3
nj_izq = cadena_der_down_up.getNrOfJoints()
posicionInicial_der_down_up = PyKDL.JntArray(nj_izq)
posicionInicial_der_down_up[5] = 0.3
posicionInicial_der_down_up[4] = -0.3
posicionInicial_der_down_up[3] = 0.0
posicionInicial_der_down_up[2] = 0.6
posicionInicial_der_down_up[1] = -0.3
posicionInicial_der_down_up[0] = -0.3
nj_izq = cadena_izq_down_up.getNrOfJoints()
posicionInicial_izq_down_up = PyKDL.JntArray(nj_izq)
posicionInicial_izq_down_up[5] = 0.3
posicionInicial_izq_down_up[4] = -0.3
posicionInicial_izq_down_up[3] = 0.0
posicionInicial_izq_down_up[2] = 0.6
posicionInicial_izq_down_up[1] = -0.3
posicionInicial_izq_down_up[0] = -0.3
print "Forward kinematics"
finalFrame_izq_up_down = PyKDL.Frame()
finalFrame_izq_down_up = PyKDL.Frame()
finalFrame_der_up_down = PyKDL.Frame()
finalFrame_der_down_up = PyKDL.Frame()
print "Rotational Matrix of the final Frame: "
print finalFrame_izq_up_down.M
print "End-effector position: ", finalFrame_izq_up_down.p
rospy.init_node('trajectory_demo')
# Set to True to move back to the starting configurations
reset = rospy.get_param('~reset', False)
# Set to False to wait for arm to finish before moving head
sync = rospy.get_param('~sync', True)
# Which joints define the arm?
piernas_joints = ['cilinder_blue1_box1_der_joint', 'cilinder_blue_box1_der_joint', 'cilinder_blue_box2_der_joint',
'cilinder_blue_box4_der_joint', 'cilinder_blue1_box6_der_joint', 'cilinder_blue_box6_der_joint',
'cilinder_blue1_box1_izq_joint', 'cilinder_blue_box1_izq_joint', 'cilinder_blue_box2_izq_joint',
'cilinder_blue_box4_izq_joint', 'cilinder_blue1_box6_izq_joint', 'cilinder_blue_box6_izq_joint' ]
piernas_goal = [0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0]
#11111111111111111111111111111111111111111111
print "Inverse Kinematics"
fksolver_izq_up_down.JntToCart(posicionInicial_izq_up_down, finalFrame_izq_up_down)
fksolver_izq_down_up.JntToCart(posicionInicial_izq_down_up, finalFrame_izq_down_up)
q_init_izq_up_down = posicionInicial_izq_up_down # initial angles
desiredFrame = finalFrame_izq_up_down
desiredFrame.p[0] = finalFrame_izq_up_down.p[0]
desiredFrame.p[1] = finalFrame_izq_up_down.p[1]
desiredFrame.p[2] = finalFrame_izq_up_down.p[2]
print "Desired Position: ", desiredFrame.p
q_out_izq_up_down = PyKDL.JntArray(cadena_izq_up_down.getNrOfJoints())
ik_izq_up_down.CartToJnt(q_init_izq_up_down, desiredFrame, q_out_izq_up_down)
print "Output angles in rads: ", q_out_izq_up_down
piernas_goal[6] = q_out_izq_up_down[0]
piernas_goal[7] = q_out_izq_up_down[1]
piernas_goal[8] = q_out_izq_up_down[2]
piernas_goal[9] = q_out_izq_up_down[3]
piernas_goal[10] = q_out_izq_up_down[4]
piernas_goal[11] = q_out_izq_up_down[5]
print "Inverse Kinematics"
fksolver_der_up_down.JntToCart(posicionInicial_der_up_down, finalFrame_der_up_down)
fksolver_der_down_up.JntToCart(posicionInicial_der_down_up, finalFrame_der_down_up)
q_init_der_up_down = posicionInicial_der_up_down # initial angles
# desiredFrame = PyKDL.Frame(PyKDL.Vector(0.5, 0.4, 1))
desiredFrame = finalFrame_der_up_down
desiredFrame.p[0] = finalFrame_der_up_down.p[0]
desiredFrame.p[1] = finalFrame_der_up_down.p[1]
desiredFrame.p[2] = finalFrame_der_up_down.p[2]+0.02 #0.02
print "Desired Position: ", desiredFrame.p
q_out_der_up_down = PyKDL.JntArray(cadena_der_up_down.getNrOfJoints())
ik_der_up_down.CartToJnt(q_init_der_up_down, desiredFrame, q_out_der_up_down)
print "Output angles in rads: ", q_out_der_up_down
piernas_goal[0] = q_out_der_up_down[0]
piernas_goal[1] = q_out_der_up_down[1]
piernas_goal[2] = q_out_der_up_down[2]
piernas_goal[3] = q_out_der_up_down[3]
piernas_goal[4] = q_out_der_up_down[4]
piernas_goal[5] = q_out_der_up_down[5]
#121212122121212121212121212121212121212121212
piernas_goal12 = [0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0]
print "Inverse Kinematics"
desiredFrame = PyKDL.Frame()
fksolver_izq_up_down.JntToCart(q_out_izq_up_down, desiredFrame)
fksolver_izq_down_up.JntToCart(posicionInicial_izq_down_up, finalFrame_izq_down_up)
q_init_izq_up_down = posicionInicial_izq_up_down # initial angles
desiredFrame.p[0] = desiredFrame.p[0]
desiredFrame.p[1] = desiredFrame.p[1]
desiredFrame.p[2] = desiredFrame.p[2]
print "Desired Position: ", desiredFrame.p
q_out_izq_up_down = PyKDL.JntArray(cadena_izq_up_down.getNrOfJoints())
ik_izq_up_down.CartToJnt(q_init_izq_up_down, desiredFrame, q_out_izq_up_down)
print "Output angles in rads: ", q_out_izq_up_down
piernas_goal12[6] = q_out_izq_up_down[0]
piernas_goal12[7] = q_out_izq_up_down[1]
piernas_goal12[8] = q_out_izq_up_down[2]
piernas_goal12[9] = q_out_izq_up_down[3]
piernas_goal12[10] = q_out_izq_up_down[4]
piernas_goal12[11] = q_out_izq_up_down[5]
print "Inverse Kinematics"
desiredFrame0 = PyKDL.Frame()
fksolver_der_up_down.JntToCart(q_out_der_up_down, desiredFrame0)
fksolver_der_down_up.JntToCart(posicionInicial_der_down_up, finalFrame_der_down_up)
q_init_der_up_down = posicionInicial_der_up_down # initial angles
# desiredFrame = PyKDL.Frame(PyKDL.Vector(0.5, 0.4, 1))
desiredFrame0.p[0] = desiredFrame0.p[0]
desiredFrame0.p[1] = desiredFrame0.p[1] -0.07
desiredFrame0.p[2] = desiredFrame0.p[2]
print "Desired Position: ", desiredFrame0.p
q_out_der_up_down = PyKDL.JntArray(cadena_der_up_down.getNrOfJoints())
ik_der_up_down.CartToJnt(q_init_der_up_down, desiredFrame0, q_out_der_up_down)
print "Output angles in rads: ", q_out_der_up_down
piernas_goal12[0] = q_out_der_up_down[0]
piernas_goal12[1] = q_out_der_up_down[1]
piernas_goal12[2] = q_out_der_up_down[2]
piernas_goal12[3] = q_out_der_up_down[3]
piernas_goal12[4] = q_out_der_up_down[4]
piernas_goal12[5] = q_out_der_up_down[5]
# 222222222222222222222222222222222222222222
piernas_goal2 = [0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0]
print "Inverse Kinematics"
desiredFrame2 = PyKDL.Frame()
fksolver_izq_down_up.JntToCart(posicionInicial_izq_down_up, desiredFrame2)
q_init_izq_down_up = posicionInicial_izq_down_up # initial angles
# desiredFrame = PyKDL.Frame(PyKDL.Vector(0.5, 0.4, 1))
desiredFrame2.p[0] = desiredFrame2.p[0] -0.06 #0.05
desiredFrame2.p[1] = desiredFrame2.p[1] -0.06#0.06
desiredFrame2.p[2] = desiredFrame2.p[2] -0.01 #0.02
print "Desired Position2222aaa: ", desiredFrame2.p
#print desiredFrame2.M
q_out_izq_down_up = PyKDL.JntArray(cadena_izq_up_down.getNrOfJoints())
ik_izq_down_up.CartToJnt(q_init_izq_down_up, desiredFrame2, q_out_izq_down_up)
print "Output angles in rads2222: ", q_out_izq_down_up
piernas_goal2[6] = q_out_izq_down_up[5]#+0.1
piernas_goal2[7] = q_out_izq_down_up[4]#-0.05
piernas_goal2[8] = q_out_izq_down_up[3]
piernas_goal2[9] = q_out_izq_down_up[2]
piernas_goal2[10] = q_out_izq_down_up[1]
piernas_goal2[11] = q_out_izq_down_up[0]
#q_out_izq_down_up[4] -=0.1
print "Inverse Kinematics"
q_init_der_down_up = PyKDL.JntArray(6)
q_init_der_down_up[0] = q_out_der_up_down[5] # PROBLEMAS CON LA ASIGNACION
q_init_der_down_up[1] = q_out_der_up_down[4]
q_init_der_down_up[2] = q_out_der_up_down[3]
q_init_der_down_up[3] = q_out_der_up_down[2]
q_init_der_down_up[4] = q_out_der_up_down[1]
q_init_der_down_up[5] = q_out_der_up_down[0]+0.08
fksolver_der_down_up.JntToCart(q_init_der_down_up,finalFrame_der_down_up)
# desiredFrame = PyKDL.Frame(PyKDL.Vector(0.5, 0.4, 1))
desiredFrame3 = finalFrame_der_down_up
desiredFrame3.p[0] = finalFrame_der_down_up.p[0]- 0.05
desiredFrame3.p[1] = finalFrame_der_down_up.p[1]- 0.04
desiredFrame3.p[2] = finalFrame_der_down_up.p[2]-0.02
print "Desired Position2222der: ", desiredFrame3.p
q_out_der_down_up = PyKDL.JntArray(cadena_der_down_up.getNrOfJoints())
ik_der_down_up.CartToJnt(q_init_der_down_up, desiredFrame3, q_out_der_down_up)
print "Output angles in rads22222der: ", q_out_der_down_up
print "VALOR", q_out_der_up_down[5]
piernas_goal2[0] = -0.3
piernas_goal2[1] = -0.3
piernas_goal2[2] = 0
piernas_goal2[3] = 0.6
piernas_goal2[4] = 0.3
piernas_goal2[5] = -0.3
# 333333333333333333333333333333333333333333333333
piernas_goal3 = [0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0]
print "Inverse Kinematics"
desiredFrame4 = PyKDL.Frame()
fksolver_izq_down_up.JntToCart(q_out_izq_down_up,desiredFrame4)
# desiredFrame = PyKDL.Frame(PyKDL.Vector(0.5, 0.4, 1))
desiredFrame4.p[0] = desiredFrame4.p[0]
desiredFrame4.p[1] = desiredFrame4.p[1] - 0.02
desiredFrame4.p[2] = desiredFrame4.p[2]
ik_izq_down_up.CartToJnt(q_out_izq_down_up, desiredFrame4, q_out_izq_down_up)
q_init_izq_up_down[0] = q_out_izq_down_up[5]
q_init_izq_up_down[1] = q_out_izq_down_up[4]
q_init_izq_up_down[2] = q_out_izq_down_up[3]
q_init_izq_up_down[3] = q_out_izq_down_up[2]
q_init_izq_up_down[4] = q_out_izq_down_up[1]
q_init_izq_up_down[5] = q_out_izq_down_up[0]
desiredFrame5 = PyKDL.Frame()
fksolver_izq_up_down.JntToCart(q_init_izq_up_down,desiredFrame5)
desiredFrame5.p[0] = desiredFrame5.p[0]
desiredFrame5.p[1] = desiredFrame5.p[1] - 0.1
desiredFrame5.p[2] = desiredFrame5.p[2]+0.01
print "Desired Position: ", desiredFrame5.p
q_out_izq_up_down2 = PyKDL.JntArray(cadena_izq_up_down.getNrOfJoints())
ik_izq_up_down.CartToJnt(q_init_izq_up_down, desiredFrame5, q_out_izq_up_down2)
print "Output angles in rads: ", q_out_izq_up_down2
piernas_goal3[6] = q_out_izq_up_down2[0]
piernas_goal3[7] = q_out_izq_up_down2[1]
piernas_goal3[8] = q_out_izq_up_down2[2]
piernas_goal3[9] = q_out_izq_up_down2[3]
piernas_goal3[10] = q_out_izq_up_down2[4]#+0.15
piernas_goal3[11] = q_out_izq_up_down2[5]-0.08
print "Inverse Kinematics"
piernas_goal3[0] = -0.3
piernas_goal3[1] = -0.3
piernas_goal3[2] = 0
piernas_goal3[3] = 0.6
piernas_goal3[4] = 0.3
piernas_goal3[5] = -0.3
# 121212122121212121212121212121212121212121212
piernas_goal121 = [0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0]
print "Inverse Kinematics"
desiredFrame6 = PyKDL.Frame()
fksolver_izq_up_down.JntToCart(q_out_izq_up_down2, desiredFrame6)
fksolver_izq_down_up.JntToCart(posicionInicial_izq_down_up, finalFrame_izq_down_up)
q_init_izq_up_down = q_out_izq_up_down2 # initial angles #CUIDADO
desiredFrame6.p[0] = desiredFrame6.p[0]
desiredFrame6.p[1] = desiredFrame6.p[1]-0.05
desiredFrame6.p[2] = desiredFrame6.p[2]#+0.01
print "Desired Position: ", desiredFrame6.p
q_out_izq_up_down = PyKDL.JntArray(cadena_izq_up_down.getNrOfJoints())
ik_izq_up_down.CartToJnt(q_init_izq_up_down, desiredFrame6, q_out_izq_up_down)
print "Output angles in rads_izq_121: ", q_out_izq_up_down
piernas_goal121[6] = q_out_izq_up_down[0]
piernas_goal121[7] = q_out_izq_up_down[1]
piernas_goal121[8] = q_out_izq_up_down[2]
piernas_goal121[9] = q_out_izq_up_down[3]
piernas_goal121[10] = q_out_izq_up_down[4]
piernas_goal121[11] = q_out_izq_up_down[5]
print "Inverse Kinematics"
desiredFrame06 = PyKDL.Frame()
fksolver_der_up_down.JntToCart(q_out_der_up_down, desiredFrame06)
fksolver_der_down_up.JntToCart(posicionInicial_der_down_up, finalFrame_der_down_up)
q_init_der_up_down = q_out_der_up_down # initial angles
# desiredFrame = PyKDL.Frame(PyKDL.Vector(0.5, 0.4, 1))
desiredFrame06.p[0] = desiredFrame06.p[0]
desiredFrame06.p[1] = desiredFrame06.p[1]
desiredFrame06.p[2] = desiredFrame06.p[2]
print "Desired Position: ", desiredFrame06.p
q_out_der_up_down = PyKDL.JntArray(cadena_der_up_down.getNrOfJoints())
ik_der_up_down.CartToJnt(q_init_der_up_down, desiredFrame06, q_out_der_up_down)
print "Output angles in rads: ", q_out_der_up_down
q_out_der_up_down21 = PyKDL.JntArray(6)
q_out_der_up_down21 = [-.3, -.3, 0, .6, .3, -.3 ]
piernas_goal121[0] = q_out_der_up_down21[0]
piernas_goal121[1] = q_out_der_up_down21[1]
piernas_goal121[2] = q_out_der_up_down21[2]
piernas_goal121[3] = q_out_der_up_down21[3]
piernas_goal121[4] = q_out_der_up_down21[4]
piernas_goal121[5] = q_out_der_up_down21[5]
# 55555555555555555555555555555555555555555
piernas_goal25 = [0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0]
print "Inverse Kinematics"
desiredFrame7= PyKDL.Frame()
q_init_izq_down_up3 = PyKDL.JntArray(6)
q_init_izq_down_up3[0] = q_out_izq_up_down[5] * 1
q_init_izq_down_up3[1] = q_out_izq_up_down[4] * 1
q_init_izq_down_up3[2] = q_out_izq_up_down[3] * 1
q_init_izq_down_up3[3] = q_out_izq_up_down[2] * 1
q_init_izq_down_up3[4] = q_out_izq_up_down[1] * 1
q_init_izq_down_up3[5] = q_out_izq_up_down[0] * 1
fksolver_izq_down_up.JntToCart(q_init_izq_down_up3, desiredFrame7)
# desiredFrame = PyKDL.Frame(PyKDL.Vector(0.5, 0.4, 1))
desiredFrame7.p[0] = desiredFrame7.p[0] + 0.05#0.03 # PROBAR A PONERLO MAYOR
desiredFrame7.p[1] = desiredFrame7.p[1] - 0.06#0.04
desiredFrame7.p[2] = desiredFrame7.p[2] + 0.005
print "Desired Position2222: ", desiredFrame7.p
q_out_izq_down_up5 = PyKDL.JntArray(cadena_izq_up_down.getNrOfJoints())
ik_izq_down_up.CartToJnt(q_init_izq_down_up3, desiredFrame7, q_out_izq_down_up5)
print "Output angles in rads2222AAAAA: ", q_out_izq_down_up5
piernas_goal25[6] = q_out_izq_down_up5[5]
piernas_goal25[7] = q_out_izq_down_up5[4]
piernas_goal25[8] = q_out_izq_down_up5[3]
piernas_goal25[9] = q_out_izq_down_up5[2]
piernas_goal25[10] = q_out_izq_down_up5[1]-0.05
piernas_goal25[11] = q_out_izq_down_up5[0]+0.05
print "Inverse Kinematics"
q_init_der_down_up31 = PyKDL.JntArray(6)
q_init_der_down_up31[0] = q_out_der_up_down21[5] *1 # PROBLEMAS CON LA ASIGNACION
q_init_der_down_up31[1] = q_out_der_up_down21[4] *1
q_init_der_down_up31[2] = q_out_der_up_down21[3] *1
q_init_der_down_up31[3] = q_out_der_up_down21[2] *1
q_init_der_down_up31[4] = q_out_der_up_down21[1] *1
q_init_der_down_up31[5] = q_out_der_up_down21[0] *1
desiredFrame7 = PyKDL.Frame()
fksolver_der_down_up.JntToCart(q_init_der_down_up31, desiredFrame7)
# desiredFrame = PyKDL.Frame(PyKDL.Vector(0.5, 0.4, 1))
desiredFrame7.p[0] = desiredFrame7.p[0] + 0.05
desiredFrame7.p[1] = desiredFrame7.p[1] - 0.06
desiredFrame7.p[2] = desiredFrame7.p[2] - 0.02
print "Desired Position2222der: ", desiredFrame7.p
q_out_der_down_up71 = PyKDL.JntArray(cadena_der_down_up.getNrOfJoints())
ik_der_down_up.CartToJnt(q_init_der_down_up31, desiredFrame7, q_out_der_down_up71)
print "Output angles in rads22222der: ", q_out_der_down_up71
piernas_goal25[0] = q_out_der_down_up71[5]
piernas_goal25[1] = q_out_der_down_up71[4]
piernas_goal25[2] = q_out_der_down_up71[3]
piernas_goal25[3] = q_out_der_down_up71[2]
piernas_goal25[4] = q_out_der_down_up71[1]
piernas_goal25[5] = q_out_der_down_up71[0]
# 333333333333333333333333333333333333333333333333
piernas_goal341 = [0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0]
print "Inverse Kinematics"
desiredFrame441 = PyKDL.Frame()
fksolver_der_down_up.JntToCart(q_out_der_down_up71, desiredFrame441)
# desiredFrame = PyKDL.Frame(PyKDL.Vector(0.5, 0.4, 1))
desiredFrame441.p[0] = desiredFrame441.p[0]
desiredFrame441.p[1] = desiredFrame441.p[1] - 0.02
desiredFrame441.p[2] = desiredFrame441.p[2] - 0.015
ik_der_down_up.CartToJnt(q_out_der_down_up71, desiredFrame441, q_out_der_down_up71)
q_init_der_up_down[0] = q_out_der_down_up71[5]
q_init_der_up_down[1] = q_out_der_down_up71[4]
q_init_der_up_down[2] = q_out_der_down_up71[3]
q_init_der_up_down[3] = q_out_der_down_up71[2]
q_init_der_up_down[4] = q_out_der_down_up71[1]
q_init_der_up_down[5] = q_out_der_down_up71[0]
desiredFrame541 = PyKDL.Frame()
fksolver_der_up_down.JntToCart(q_init_der_up_down, desiredFrame541)
desiredFrame541.p[0] = desiredFrame541.p[0] - 0.03
desiredFrame541.p[1] = desiredFrame541.p[1] - 0.1
desiredFrame541.p[2] = desiredFrame541.p[2] - 0.01 #nada
print "Desired Position: ", desiredFrame541.p
q_out_der_up_down241 = PyKDL.JntArray(cadena_izq_up_down.getNrOfJoints())
ik_der_up_down.CartToJnt(q_init_der_up_down, desiredFrame541, q_out_der_up_down241)# con desiredFrame5 va
print "Output angles in radsaaaaa: ", q_out_der_up_down241
print "Inverse Kinematics"
piernas_goal341[6] = 0.3
piernas_goal341[7] = -0.3
piernas_goal341[8] = 0
piernas_goal341[9] = 0.6
piernas_goal341[10] = -0.3
piernas_goal341[11] = -0.3
piernas_goal341[0] = q_out_der_up_down241[0]#+0.1
piernas_goal341[1] = q_out_der_up_down241[1]
piernas_goal341[2] = q_out_der_up_down241[2]
piernas_goal341[3] = q_out_der_up_down241[3]
piernas_goal341[4] = q_out_der_up_down241[4]#-0.1
piernas_goal341[5] = q_out_der_up_down241[5]#-0.01
pickle.dump(piernas_goal, datos_articulaciones, -1)
pickle.dump(piernas_goal12, datos_articulaciones, -1)
pickle.dump(piernas_goal2, datos_articulaciones, -1)
pickle.dump(piernas_goal3, datos_articulaciones, -1)
pickle.dump(piernas_goal121, datos_articulaciones, -1)
pickle.dump(piernas_goal25, datos_articulaciones, -1)
pickle.dump(piernas_goal341, datos_articulaciones, -1)
datos_articulaciones.close()
# Connect to the right arm trajectory action server
rospy.loginfo('Waiting for right arm trajectory controller...')
arm_client = actionlib.SimpleActionClient('/piernas/piernas_controller/follow_joint_trajectory', FollowJointTrajectoryAction)
#/piernas/piernas_controller/follow_joint_trajectory
arm_client.wait_for_server()
rospy.loginfo('...connected.')
# Create a single-point arm trajectory with the piernas_goal as the end-point
arm_trajectory = JointTrajectory()
arm_trajectory.joint_names = piernas_joints
arm_trajectory.points.append(JointTrajectoryPoint())
arm_trajectory.points[0].positions = piernas_goal
arm_trajectory.points[0].velocities = [0.0 for i in piernas_joints]
arm_trajectory.points[0].accelerations = [0.0 for i in piernas_joints]
arm_trajectory.points[0].time_from_start = rospy.Duration(2.0)
arm_trajectory.points.append(JointTrajectoryPoint())
arm_trajectory.points[1].positions = piernas_goal12
arm_trajectory.points[1].velocities = [0.0 for i in piernas_joints]
arm_trajectory.points[1].accelerations = [0.0 for i in piernas_joints]
arm_trajectory.points[1].time_from_start = rospy.Duration(4.0)
arm_trajectory.points.append(JointTrajectoryPoint())
arm_trajectory.points[2].positions = piernas_goal2
arm_trajectory.points[2].velocities = [0.0 for i in piernas_joints]
arm_trajectory.points[2].accelerations = [0.0 for i in piernas_joints]
arm_trajectory.points[2].time_from_start = rospy.Duration(6.0)
arm_trajectory.points.append(JointTrajectoryPoint())
arm_trajectory.points[3].positions = piernas_goal3
arm_trajectory.points[3].velocities = [0.0 for i in piernas_joints]
arm_trajectory.points[3].accelerations = [0.0 for i in piernas_joints]
arm_trajectory.points[3].time_from_start = rospy.Duration(8.0)
arm_trajectory.points.append(JointTrajectoryPoint())
arm_trajectory.points[4].positions = piernas_goal121
arm_trajectory.points[4].velocities = [0.0 for i in piernas_joints]
arm_trajectory.points[4].accelerations = [0.0 for i in piernas_joints]
arm_trajectory.points[4].time_from_start = rospy.Duration(10.0)
arm_trajectory.points.append(JointTrajectoryPoint())
arm_trajectory.points[5].positions = piernas_goal25
arm_trajectory.points[5].velocities = [0.0 for i in piernas_joints]
arm_trajectory.points[5].accelerations = [0.0 for i in piernas_joints]
arm_trajectory.points[5].time_from_start = rospy.Duration(12.0)
arm_trajectory.points.append(JointTrajectoryPoint())
arm_trajectory.points[6].positions = piernas_goal341
arm_trajectory.points[6].velocities = [0.0 for i in piernas_joints]
arm_trajectory.points[6].accelerations = [0.0 for i in piernas_joints]
arm_trajectory.points[6].time_from_start = rospy.Duration(14.0)
'''arm_trajectory.points.append(JointTrajectoryPoint())
arm_trajectory.points[7].positions = piernas_goal12
arm_trajectory.points[7].velocities = [0.0 for i in piernas_joints]
arm_trajectory.points[7].accelerations = [0.0 for i in piernas_joints]
arm_trajectory.points[7].time_from_start = rospy.Duration(17.5)
arm_trajectory.points.append(JointTrajectoryPoint())
arm_trajectory.points[8].positions = piernas_goal2
arm_trajectory.points[8].velocities = [0.0 for i in piernas_joints]
arm_trajectory.points[8].accelerations = [0.0 for i in piernas_joints]
arm_trajectory.points[8].time_from_start = rospy.Duration(19.0)
arm_trajectory.points.append(JointTrajectoryPoint())
arm_trajectory.points[9].positions = piernas_goal3
arm_trajectory.points[9].velocities = [0.0 for i in piernas_joints]
arm_trajectory.points[9].accelerations = [0.0 for i in piernas_joints]
arm_trajectory.points[9].time_from_start = rospy.Duration(21.0)
arm_trajectory.points.append(JointTrajectoryPoint())
arm_trajectory.points[10].positions = piernas_goal121
arm_trajectory.points[10].velocities = [0.0 for i in piernas_joints]
arm_trajectory.points[10].accelerations = [0.0 for i in piernas_joints]
arm_trajectory.points[10].time_from_start = rospy.Duration(23.0)
arm_trajectory.points.append(JointTrajectoryPoint())
arm_trajectory.points[11].positions = piernas_goal25
arm_trajectory.points[11].velocities = [0.0 for i in piernas_joints]
arm_trajectory.points[11].accelerations = [0.0 for i in piernas_joints]
arm_trajectory.points[11].time_from_start = rospy.Duration(25.0)
arm_trajectory.points.append(JointTrajectoryPoint())
arm_trajectory.points[12].positions = piernas_goal341
arm_trajectory.points[12].velocities = [0.0 for i in piernas_joints]
arm_trajectory.points[12].accelerations = [0.0 for i in piernas_joints]
arm_trajectory.points[12].time_from_start = rospy.Duration(28.0)'''
# Send the trajectory to the arm action server
rospy.loginfo('Moving the arm to goal position...')
# Create an empty trajectory goal
piernas_goal = FollowJointTrajectoryGoal()
# Set the trajectory component to the goal trajectory created above
piernas_goal.trajectory = arm_trajectory
# Specify zero tolerance for the execution time
piernas_goal.goal_time_tolerance = rospy.Duration(0.01)
# Send the goal to the action server
arm_client.send_goal(piernas_goal)
if not sync:
# Wait for up to 5 seconds for the motion to complete
arm_client.wait_for_result(rospy.Duration(5.0))
arm_client.wait_for_result()
print arm_client.get_result()
rospy.loginfo('...done')
def clear(self, limb):
self._goal = FollowJointTrajectoryGoal()
self._goal.trajectory.joint_names = [limb + '_' + joint for joint in \
self.jointnames]
def __init__(self):
filename = '/home/kaiser/WS_ROS/catkin_ws/src/urdf_serp/urdf/mi_cuadrupedo_exp.urdf'
angulo_avance = +0.4 #rad
altura_pata = +0.06 #cm
avance_x = 0.03
# angulo_avance = 0 # +0.40 #rad
# altura_pata = 0 # -0.04 #cm
avance_x = 0.11
robot = urdf_parser_py.urdf.URDF.from_xml_file(filename)
(ok, tree) = kdl_parser_py.urdf.treeFromFile(filename)
cadena_RR = tree.getChain("base_link", "tarsus_RR")
cadena_RF = tree.getChain("base_link", "tarsus_RF")
cadena_LR = tree.getChain("base_link", "tarsus_LR")
cadena_LF = tree.getChain("base_link", "tarsus_LF")
print cadena_RR.getNrOfSegments()
fksolver_RR = PyKDL.ChainFkSolverPos_recursive(cadena_RR)
fksolver_RF = PyKDL.ChainFkSolverPos_recursive(cadena_RF)
fksolver_LR = PyKDL.ChainFkSolverPos_recursive(cadena_LR)
fksolver_LF = PyKDL.ChainFkSolverPos_recursive(cadena_LF)
vik_RR = PyKDL.ChainIkSolverVel_pinv(cadena_RR)
ik_RR = PyKDL.ChainIkSolverPos_NR(cadena_RR, fksolver_RR, vik_RR)
vik_RF = PyKDL.ChainIkSolverVel_pinv(cadena_RF)
ik_RF = PyKDL.ChainIkSolverPos_NR(cadena_RF, fksolver_RF, vik_RF)
vik_LR = PyKDL.ChainIkSolverVel_pinv(cadena_LR)
ik_LR = PyKDL.ChainIkSolverPos_NR(cadena_LR, fksolver_LR, vik_LR)
vik_LF = PyKDL.ChainIkSolverVel_pinv(cadena_LF)
ik_LF = PyKDL.ChainIkSolverPos_NR(cadena_LF, fksolver_LF, vik_LF)
nj_izq = cadena_RR.getNrOfJoints()
posicionInicial_R = PyKDL.JntArray(nj_izq)
posicionInicial_R[0] = 0
posicionInicial_R[1] = 0
posicionInicial_R[2] = 0
posicionInicial_R[3] = 0
nj_izq = cadena_LR.getNrOfJoints()
posicionInicial_L = PyKDL.JntArray(nj_izq)
posicionInicial_L[0] = 0
posicionInicial_L[1] = 0
posicionInicial_L[2] = 0
posicionInicial_L[3] = 0
print "Forward kinematics"
finalFrame_R = PyKDL.Frame()
finalFrame_L = PyKDL.Frame()
rospy.init_node('trajectory_demo')
# Set to True to move back to the starting configurations
reset = rospy.get_param('~reset', False)
# Set to False to wait for arm to finish before moving head
sync = rospy.get_param('~sync', True)
# Which joints define the arm?
piernas_joints_RR = [
'coxa_joint_RR', 'femur_joint_RR', 'tibia_joint_RR',
'tarsus_joint_RR'
]
piernas_joints_RF = [
'coxa_joint_RF', 'femur_joint_RF', 'tibia_joint_RF',
'tarsus_joint_RF'
]
piernas_joints_LR = [
'coxa_joint_LR', 'femur_joint_LR', 'tibia_joint_LR',
'tarsus_joint_LR'
]
piernas_joints_LF = [
'coxa_joint_LF', 'femur_joint_LF', 'tibia_joint_LF',
'tarsus_joint_LF'
]
rr_goal0 = [0.0, 0.0, 0.0, 0.0]
rf_goal0 = [0.0, 0.0, 0.0, 0.0]
rr_goal1 = [0.0, 0.0, 0.0, 0.0]
rf_goal1 = [0.0, 0.0, 0.0, 0.0]
lr_goal0 = [0.0, 0.0, 0.0, 0.0]
lf_goal0 = [0.0, 0.0, 0.0, 0.0]
lr_goal1 = [0.0, 0.0, 0.0, 0.0]
lf_goal1 = [0.0, 0.0, 0.0, 0.0]
#11111111111111111111111111111111111111111111
print "Inverse Kinematics"
fksolver_RR.JntToCart(posicionInicial_R, finalFrame_R)
q_init_RR = posicionInicial_R # initial angles
desiredFrameRR = finalFrame_R
desiredFrameRR.p[0] = finalFrame_R.p[0] + avance_x
desiredFrameRR.p[1] = finalFrame_R.p[1]
desiredFrameRR.p[2] = finalFrame_R.p[2] + altura_pata
print "Desired Position: ", desiredFrameRR.p
q_out_RR = PyKDL.JntArray(cadena_RR.getNrOfJoints())
ik_RR.CartToJnt(q_init_RR, desiredFrameRR, q_out_RR)
print "Output angles in rads: ", q_out_RR
rr_goal0[0] = q_out_RR[0]
rr_goal0[1] = q_out_RR[1] #+angulo_avance
rr_goal0[2] = q_out_RR[2]
rr_goal0[3] = q_out_RR[3]
print "Inverse Kinematics"
fksolver_LF.JntToCart(posicionInicial_L, finalFrame_L)
q_init_LF = posicionInicial_L # initial angles
desiredFrameLF = finalFrame_L
desiredFrameLF.p[0] = finalFrame_L.p[0] + avance_x
desiredFrameLF.p[1] = finalFrame_L.p[1]
desiredFrameLF.p[2] = finalFrame_L.p[2] + altura_pata
print "Desired Position: ", desiredFrameLF.p
q_out_LF = PyKDL.JntArray(cadena_LF.getNrOfJoints())
ik_LF.CartToJnt(q_init_LF, desiredFrameLF, q_out_LF)
print "Output angles in rads: ", q_out_LF
lf_goal0[0] = q_out_LF[0]
lf_goal0[1] = q_out_LF[1] #- angulo_avance
lf_goal0[2] = q_out_LF[2]
lf_goal0[3] = q_out_LF[3]
# 22222222222222222222222222222222222222222222
RR_actual = PyKDL.JntArray(nj_izq)
RR_actual[0] = rr_goal0[0]
RR_actual[1] = rr_goal0[1]
RR_actual[2] = rr_goal0[2]
RR_actual[3] = rr_goal0[3]
print "Inverse Kinematics"
fksolver_RR.JntToCart(RR_actual, finalFrame_R)
q_init_RR = RR_actual # initial angles
desiredFrameRR = finalFrame_R
desiredFrameRR.p[0] = finalFrame_R.p[0] - avance_x
desiredFrameRR.p[1] = finalFrame_R.p[1]
desiredFrameRR.p[2] = finalFrame_R.p[2] - altura_pata
print "Desired Position: ", desiredFrameRR.p
q_out_RR = PyKDL.JntArray(cadena_RR.getNrOfJoints())
ik_RR.CartToJnt(q_init_RR, desiredFrameRR, q_out_RR)
print "Output angles in rads: ", q_out_RR
rr_goal1[0] = q_out_RR[0]
rr_goal1[1] = q_out_RR[1]
rr_goal1[2] = q_out_RR[2]
rr_goal1[3] = q_out_RR[3]
LF_actual = PyKDL.JntArray(nj_izq)
LF_actual[0] = lf_goal0[0]
LF_actual[1] = lf_goal0[1]
LF_actual[2] = lf_goal0[2]
LF_actual[3] = lf_goal0[3]
print "Inverse Kinematics"
fksolver_LF.JntToCart(LF_actual, finalFrame_L)
q_init_LF = LF_actual # initial angles
desiredFrameLF = finalFrame_L
desiredFrameLF.p[0] = finalFrame_L.p[0] - avance_x
desiredFrameLF.p[1] = finalFrame_L.p[1]
desiredFrameLF.p[2] = finalFrame_L.p[2] - altura_pata
print "Desired Position: ", desiredFrameLF.p
q_out_LF = PyKDL.JntArray(cadena_LF.getNrOfJoints())
ik_LF.CartToJnt(q_init_LF, desiredFrameLF, q_out_LF)
print "Output angles in rads: ", q_out_LF
lf_goal1[0] = q_out_LF[0]
lf_goal1[1] = q_out_LF[1] # - angulo_avance
lf_goal1[2] = q_out_LF[2]
lf_goal1[3] = q_out_LF[3]
# 11111111111111111111111111111111111111111111
print "Inverse Kinematics"
fksolver_LR.JntToCart(posicionInicial_L, finalFrame_L)
q_init_LR = posicionInicial_L # initial angles
desiredFrameLR = finalFrame_L
desiredFrameLR.p[0] = finalFrame_L.p[0] #-avance_x
desiredFrameLR.p[1] = finalFrame_L.p[1]
desiredFrameLR.p[2] = finalFrame_L.p[2] # + altura_pata
print "Desired Position: ", desiredFrameLR.p
q_out_LR = PyKDL.JntArray(cadena_LR.getNrOfJoints())
ik_LR.CartToJnt(q_init_LR, desiredFrameLR, q_out_LR)
print "Output angles in rads: ", q_out_LR
lr_goal0[0] = q_out_LR[0]
lr_goal0[1] = q_out_LR[1] + angulo_avance
lr_goal0[2] = q_out_LR[2]
lr_goal0[3] = q_out_LR[3]
print "Inverse Kinematics"
fksolver_RF.JntToCart(posicionInicial_R, finalFrame_R)
q_init_RF = posicionInicial_R # initial angles
desiredFrameRF = finalFrame_R
desiredFrameRF.p[0] = finalFrame_R.p[0] # -avance_x
desiredFrameRF.p[1] = finalFrame_R.p[1]
desiredFrameRF.p[2] = finalFrame_R.p[2] #+ altura_pata
print "Desired Position: ", desiredFrameRF.p
q_out_RF = PyKDL.JntArray(cadena_RF.getNrOfJoints())
ik_RF.CartToJnt(q_init_RF, desiredFrameRF, q_out_RF)
print "Output angles in rads: ", q_out_RF
rf_goal0[0] = q_out_RF[0]
rf_goal0[1] = q_out_RF[1] - angulo_avance
rf_goal0[2] = q_out_RF[2]
rf_goal0[3] = q_out_RF[3]
# 2222222222222222222222222222222222222222222222
LR_actual = PyKDL.JntArray(nj_izq)
LR_actual[0] = lr_goal0[0]
LR_actual[1] = lr_goal0[1]
LR_actual[2] = lr_goal0[2]
LR_actual[3] = lr_goal0[3]
print "Inverse Kinematics"
fksolver_LR.JntToCart(LR_actual, finalFrame_L)
q_init_LR = LR_actual # initial angles
print "Inverse Kinematics"
desiredFrameLR = finalFrame_L
desiredFrameLR.p[0] = finalFrame_L.p[0] #+ avance_x
desiredFrameLR.p[1] = finalFrame_L.p[1]
desiredFrameLR.p[2] = finalFrame_L.p[2] # - altura_pata
print "Desired Position: ", desiredFrameLR.p
q_out_LR = PyKDL.JntArray(cadena_LR.getNrOfJoints())
ik_LR.CartToJnt(q_init_LR, desiredFrameLR, q_out_LR)
print "Output angles in rads: ", q_out_LR
lr_goal1[0] = q_out_LR[0] #- angulo_avance
lr_goal1[1] = q_out_LR[1]
lr_goal1[2] = q_out_LR[2]
lr_goal1[3] = q_out_LR[3] + angulo_avance
RF_actual = PyKDL.JntArray(nj_izq)
RF_actual[0] = rf_goal0[0]
RF_actual[1] = rf_goal0[1]
RF_actual[2] = rf_goal0[2]
RF_actual[3] = rf_goal0[3]
print "Inverse Kinematics"
fksolver_RF.JntToCart(RF_actual, finalFrame_R)
q_init_RF = RF_actual # initial angles
desiredFrameRF = finalFrame_R
desiredFrameRF.p[0] = finalFrame_R.p[0] # + avance_x
desiredFrameRF.p[1] = finalFrame_R.p[1]
desiredFrameRF.p[2] = finalFrame_R.p[2] #- altura_pata
print "Desired Position: ", desiredFrameRF.p
q_out_RF = PyKDL.JntArray(cadena_RF.getNrOfJoints())
ik_RF.CartToJnt(q_init_RF, desiredFrameRF, q_out_RF)
print "Output angles in rads: ", q_out_RF
rf_goal1[0] = q_out_RF[0]
rf_goal1[1] = q_out_RF[1]
rf_goal1[2] = q_out_RF[2]
rf_goal1[3] = q_out_RF[3] + angulo_avance
# Connect to the right arm trajectory action server
rospy.loginfo('Waiting for right arm trajectory controller...')
rr_client = actionlib.SimpleActionClient(
'/cuadrupedo/pata_rr/follow_joint_trajectory',
FollowJointTrajectoryAction)
rr_client.wait_for_server()
rf_client = actionlib.SimpleActionClient(
'/cuadrupedo/pata_rf/follow_joint_trajectory',
FollowJointTrajectoryAction)
rf_client.wait_for_server()
lr_client = actionlib.SimpleActionClient(
'/cuadrupedo/pata_lr/follow_joint_trajectory',
FollowJointTrajectoryAction)
lr_client.wait_for_server()
lf_client = actionlib.SimpleActionClient(
'/cuadrupedo/pata_lf/follow_joint_trajectory',
FollowJointTrajectoryAction)
lf_client.wait_for_server()
rospy.loginfo('...connected.')
# Create a single-point arm trajectory with the piernas_goal as the end-point
rr_trajectory1 = JointTrajectory()
rr_trajectory1.joint_names = piernas_joints_RR
rr_trajectory1.points.append(JointTrajectoryPoint())
rr_trajectory1.points[0].positions = rr_goal0
rr_trajectory1.points[0].velocities = [0.0 for i in piernas_joints_RR]
rr_trajectory1.points[0].accelerations = [
0.0 for i in piernas_joints_RR
]
rr_trajectory1.points[0].time_from_start = rospy.Duration(0.2)
rr_trajectory1.points.append(JointTrajectoryPoint())
rr_trajectory1.points[1].positions = rr_goal1
rr_trajectory1.points[1].velocities = [0.0 for i in piernas_joints_RR]
rr_trajectory1.points[1].accelerations = [
0.0 for i in piernas_joints_RR
]
rr_trajectory1.points[1].time_from_start = rospy.Duration(0.4)
rr_trajectory1.points.append(JointTrajectoryPoint())
rr_trajectory1.points[2].positions = rf_goal0
rr_trajectory1.points[2].velocities = [0.0 for i in piernas_joints_RR]
rr_trajectory1.points[2].accelerations = [
0.0 for i in piernas_joints_RR
]
rr_trajectory1.points[2].time_from_start = rospy.Duration(0.6)
rr_trajectory1.points.append(JointTrajectoryPoint())
rr_trajectory1.points[3].positions = rf_goal1
rr_trajectory1.points[3].velocities = [0.0 for i in piernas_joints_RR]
rr_trajectory1.points[3].accelerations = [
0.0 for i in piernas_joints_RR
]
rr_trajectory1.points[3].time_from_start = rospy.Duration(0.8)
#'''
rf_trajectory1 = JointTrajectory()
rf_trajectory1.joint_names = piernas_joints_RF
rf_trajectory1.points.append(JointTrajectoryPoint())
rf_trajectory1.points[0].positions = rf_goal0 # [0,0,0,0]
rf_trajectory1.points[0].velocities = [0.0 for i in piernas_joints_RF]
rf_trajectory1.points[0].accelerations = [
0.0 for i in piernas_joints_RF
]
rf_trajectory1.points[0].time_from_start = rospy.Duration(0.2)
rf_trajectory1.points.append(JointTrajectoryPoint())
rf_trajectory1.points[1].positions = rf_goal1 # [0,0,0,0]
rf_trajectory1.points[1].velocities = [0.0 for i in piernas_joints_RF]
rf_trajectory1.points[1].accelerations = [
0.0 for i in piernas_joints_RF
]
rf_trajectory1.points[1].time_from_start = rospy.Duration(0.4)
rf_trajectory1.points.append(JointTrajectoryPoint())
rf_trajectory1.points[2].positions = rr_goal0 # [0,0,0,0]
rf_trajectory1.points[2].velocities = [0.0 for i in piernas_joints_RF]
rf_trajectory1.points[2].accelerations = [
0.0 for i in piernas_joints_RF
]
rf_trajectory1.points[2].time_from_start = rospy.Duration(0.6)
rf_trajectory1.points.append(JointTrajectoryPoint())
rf_trajectory1.points[3].positions = rr_goal1 # [0,0,0,0]
rf_trajectory1.points[3].velocities = [0.0 for i in piernas_joints_RF]
rf_trajectory1.points[3].accelerations = [
0.0 for i in piernas_joints_RF
]
rf_trajectory1.points[3].time_from_start = rospy.Duration(0.8)
# '''
lr_trajectory1 = JointTrajectory()
lr_trajectory1.joint_names = piernas_joints_LR
lr_trajectory1.points.append(JointTrajectoryPoint())
lr_trajectory1.points[0].positions = lr_goal0 #lr_goal0
lr_trajectory1.points[0].velocities = [0.0 for i in piernas_joints_RR]
lr_trajectory1.points[0].accelerations = [
0.0 for i in piernas_joints_RR
]
lr_trajectory1.points[0].time_from_start = rospy.Duration(0.2)
lr_trajectory1.points.append(JointTrajectoryPoint())
lr_trajectory1.points[1].positions = lr_goal1
lr_trajectory1.points[1].velocities = [0.0 for i in piernas_joints_RR]
lr_trajectory1.points[1].accelerations = [
0.0 for i in piernas_joints_RR
]
lr_trajectory1.points[1].time_from_start = rospy.Duration(0.4)
lr_trajectory1.points.append(JointTrajectoryPoint())
lr_trajectory1.points[2].positions = lf_goal0
lr_trajectory1.points[2].velocities = [0.0 for i in piernas_joints_RR]
lr_trajectory1.points[2].accelerations = [
0.0 for i in piernas_joints_RR
]
lr_trajectory1.points[2].time_from_start = rospy.Duration(0.6)
lr_trajectory1.points.append(JointTrajectoryPoint())
lr_trajectory1.points[3].positions = lf_goal1 # lr_goal0
lr_trajectory1.points[3].velocities = [0.0 for i in piernas_joints_RR]
lr_trajectory1.points[3].accelerations = [
0.0 for i in piernas_joints_RR
]
lr_trajectory1.points[3].time_from_start = rospy.Duration(0.8)
# '''
lf_trajectory1 = JointTrajectory()
lf_trajectory1.joint_names = piernas_joints_LF
lf_trajectory1.points.append(JointTrajectoryPoint())
lf_trajectory1.points[0].positions = lf_goal0 #lf_goal0 # [0,0,0,0]
lf_trajectory1.points[0].velocities = [0.0 for i in piernas_joints_RF]
lf_trajectory1.points[0].accelerations = [
0.0 for i in piernas_joints_RF
]
lf_trajectory1.points[0].time_from_start = rospy.Duration(0.2)
lf_trajectory1.points.append(JointTrajectoryPoint())
lf_trajectory1.points[1].positions = lf_goal1 # [0,0,0,0]
lf_trajectory1.points[1].velocities = [0.0 for i in piernas_joints_RF]
lf_trajectory1.points[1].accelerations = [
0.0 for i in piernas_joints_RF
]
lf_trajectory1.points[1].time_from_start = rospy.Duration(0.4)
lf_trajectory1.points.append(JointTrajectoryPoint())
lf_trajectory1.points[2].positions = lr_goal0 # [0,0,0,0]
lf_trajectory1.points[2].velocities = [0.0 for i in piernas_joints_RF]
lf_trajectory1.points[2].accelerations = [
0.0 for i in piernas_joints_RF
]
lf_trajectory1.points[2].time_from_start = rospy.Duration(0.6)
lf_trajectory1.points.append(JointTrajectoryPoint())
lf_trajectory1.points[3].positions = lr_goal1 # lf_goal0 # [0,0,0,0]
lf_trajectory1.points[3].velocities = [0.0 for i in piernas_joints_RF]
lf_trajectory1.points[3].accelerations = [
0.0 for i in piernas_joints_RF
]
lf_trajectory1.points[3].time_from_start = rospy.Duration(0.8)
# '''
# Send the trajectory to the arm action server
rospy.loginfo('Moving the arm to goal position...')
lf_reposo_trajectory1 = JointTrajectory()
lf_reposo_trajectory1.joint_names = piernas_joints_LF
lf_reposo_trajectory1.points.append(JointTrajectoryPoint())
lf_reposo_trajectory1.points[0].positions = [
0.0 for i in piernas_joints_RF
]
lf_reposo_trajectory1.points[0].velocities = [
0.0 for i in piernas_joints_RF
]
lf_reposo_trajectory1.points[0].accelerations = [
0.0 for i in piernas_joints_RF
]
lf_reposo_trajectory1.points[0].time_from_start = rospy.Duration(0.2)
lr_reposo_trajectory1 = JointTrajectory()
lr_reposo_trajectory1.joint_names = piernas_joints_LR
lr_reposo_trajectory1.points.append(JointTrajectoryPoint())
lr_reposo_trajectory1.points[0].positions = [
0.0 for i in piernas_joints_RF
]
lr_reposo_trajectory1.points[0].velocities = [
0.0 for i in piernas_joints_RF
]
lr_reposo_trajectory1.points[0].accelerations = [
0.0 for i in piernas_joints_RF
]
lr_reposo_trajectory1.points[0].time_from_start = rospy.Duration(0.2)
rr_reposo_trajectory1 = JointTrajectory()
rr_reposo_trajectory1.joint_names = piernas_joints_RR
rr_reposo_trajectory1.points.append(JointTrajectoryPoint())
rr_reposo_trajectory1.points[0].positions = [
0.0 for i in piernas_joints_RF
]
rr_reposo_trajectory1.points[0].velocities = [
0.0 for i in piernas_joints_RF
]
rr_reposo_trajectory1.points[0].accelerations = [
0.0 for i in piernas_joints_RF
]
rr_reposo_trajectory1.points[0].time_from_start = rospy.Duration(0.2)
rf_reposo_trajectory1 = JointTrajectory()
rf_reposo_trajectory1.joint_names = piernas_joints_RF
rf_reposo_trajectory1.points.append(JointTrajectoryPoint())
rf_reposo_trajectory1.points[0].positions = [
0.0 for i in piernas_joints_RF
]
rf_reposo_trajectory1.points[0].velocities = [
0.0 for i in piernas_joints_RF
]
rf_reposo_trajectory1.points[0].accelerations = [
0.0 for i in piernas_joints_RF
]
rf_reposo_trajectory1.points[0].time_from_start = rospy.Duration(0.2)
# Create an empty trajectory goal
rr_goal = FollowJointTrajectoryGoal()
lm_goal = FollowJointTrajectoryGoal()
rf_goal = FollowJointTrajectoryGoal()
lr_goal = FollowJointTrajectoryGoal()
rm_goal = FollowJointTrajectoryGoal()
lf_goal = FollowJointTrajectoryGoal()
# Set the trajectory component to the goal trajectory created above
rr_goal.trajectory = rr_trajectory1
rf_goal.trajectory = rf_trajectory1
lr_goal.trajectory = lr_trajectory1
lf_goal.trajectory = lf_trajectory1
# Specify zero tolerance for the execution time
rr_goal.goal_time_tolerance = rospy.Duration(0.0)
lm_goal.goal_time_tolerance = rospy.Duration(0.0)
rf_goal.goal_time_tolerance = rospy.Duration(0.0)
lr_goal.goal_time_tolerance = rospy.Duration(0.0)
rm_goal.goal_time_tolerance = rospy.Duration(0.0)
lf_goal.goal_time_tolerance = rospy.Duration(0.0)
# Send the goal to the action server
rr_client.send_goal(rr_goal)
rf_client.send_goal(rf_goal)
#rr_client.wait_for_result(rospy.Duration(5.0))
print 'Resultado recibido'
lr_client.send_goal(lr_goal)
lf_client.send_goal(lf_goal)
rr_client.wait_for_result()
rr_client.send_goal(rr_goal)
rf_client.send_goal(rf_goal)
# rr_client.wait_for_result(rospy.Duration(5.0))
print 'Resultado recibido'
lr_client.send_goal(lr_goal)
lf_client.send_goal(lf_goal)
rr_client.wait_for_result()
rr_client.send_goal(rr_goal)
rf_client.send_goal(rf_goal)
# rr_client.wait_for_result(rospy.Duration(5.0))
print 'Resultado recibido'
lr_client.send_goal(lr_goal)
lf_client.send_goal(lf_goal)
rr_client.wait_for_result()
rr_client.send_goal(rr_goal)
rf_client.send_goal(rf_goal)
# rr_client.wait_for_result(rospy.Duration(5.0))
print 'Resultado recibido'
lr_client.send_goal(lr_goal)
lf_client.send_goal(lf_goal)
rr_client.wait_for_result()
rr_client.send_goal(rr_goal)
rf_client.send_goal(rf_goal)
# rr_client.wait_for_result(rospy.Duration(5.0))
print 'Resultado recibido'
lr_client.send_goal(lr_goal)
lf_client.send_goal(lf_goal)
rr_client.wait_for_result()
rr_client.send_goal(rr_goal)
rf_client.send_goal(rf_goal)
# rr_client.wait_for_result(rospy.Duration(5.0))
print 'Resultado recibido'
lr_client.send_goal(lr_goal)
lf_client.send_goal(lf_goal)
rr_client.wait_for_result()
rr_client.send_goal(rr_goal)
rf_client.send_goal(rf_goal)
# rr_client.wait_for_result(rospy.Duration(5.0))
print 'Resultado recibido'
lr_client.send_goal(lr_goal)
lf_client.send_goal(lf_goal)
rr_client.wait_for_result()
rr_client.send_goal(rr_goal)
rf_client.send_goal(rf_goal)
# rr_client.wait_for_result(rospy.Duration(5.0))
print 'Resultado recibido'
lr_client.send_goal(lr_goal)
lf_client.send_goal(lf_goal)
rr_client.wait_for_result()
rr_client.send_goal(rr_goal)
rf_client.send_goal(rf_goal)
# rr_client.wait_for_result(rospy.Duration(5.0))
print 'Resultado recibido'
lr_client.send_goal(lr_goal)
lf_client.send_goal(lf_goal)
rr_client.wait_for_result()
rr_client.send_goal(rr_goal)
rf_client.send_goal(rf_goal)
# rr_client.wait_for_result(rospy.Duration(5.0))
print 'Resultado recibido'
lr_client.send_goal(lr_goal)
lf_client.send_goal(lf_goal)
#lr_client.wait_for_result(rospy.Duration(5.0))
#rr_client.send_goal(rr_goal)
#lm_client.send_goal(lm_goal)
#rf_client.send_goal(rf_goal)'''
if not sync:
# Wait for up to 5 seconds for the motion to complete
rr_client.wait_for_result(rospy.Duration(5.0))
rr_client.wait_for_result()
print rr_client.get_result()
rr_goal.trajectory = rr_reposo_trajectory1
rf_goal.trajectory = rf_reposo_trajectory1
lr_goal.trajectory = lr_reposo_trajectory1
lf_goal.trajectory = lf_reposo_trajectory1
rr_client.send_goal(rr_goal)
rf_client.send_goal(rf_goal)
# rr_client.wait_for_result(rospy.Duration(5.0))
print 'Resultado recibido'
lr_client.send_goal(lr_goal)
lf_client.send_goal(lf_goal)
rr_client.wait_for_result()
rospy.loginfo('...done')
def __init__(self):
#create our action server clients
self._left_client = actionlib.SimpleActionClient(
'robot/limb/left/follow_joint_trajectory',
FollowJointTrajectoryAction,
)
self._right_client = actionlib.SimpleActionClient(
'robot/limb/right/follow_joint_trajectory',
FollowJointTrajectoryAction,
)
#verify joint trajectory action servers are available
l_server_up = self._left_client.wait_for_server(rospy.Duration(10.0))
r_server_up = self._right_client.wait_for_server(rospy.Duration(10.0))
if not l_server_up or not r_server_up:
msg = ("Action server not available."
" Verify action server availability.")
rospy.logerr(msg)
rospy.signal_shutdown(msg)
sys.exit(1)
#create our goal request
self._l_goal = FollowJointTrajectoryGoal()
self._r_goal = FollowJointTrajectoryGoal()
#limb interface - current angles needed for start move
self._l_arm = baxter_interface.Limb('left')
self._r_arm = baxter_interface.Limb('right')
#gripper interface - for gripper command playback
self._l_gripper = baxter_interface.Gripper('left', CHECK_VERSION)
self._r_gripper = baxter_interface.Gripper('right', CHECK_VERSION)
#flag to signify the arm trajectories have begun executing
self._arm_trajectory_started = False
#reentrant lock to prevent same-thread lockout
self._lock = threading.RLock()
# Verify Grippers Have No Errors and are Calibrated
if self._l_gripper.error():
self._l_gripper.reset()
if self._r_gripper.error():
self._r_gripper.reset()
if (not self._l_gripper.calibrated()
and self._l_gripper.type() != 'custom'):
self._l_gripper.calibrate()
if (not self._r_gripper.calibrated()
and self._r_gripper.type() != 'custom'):
self._r_gripper.calibrate()
#gripper goal trajectories
self._l_grip = FollowJointTrajectoryGoal()
self._r_grip = FollowJointTrajectoryGoal()
# Timing offset to prevent gripper playback before trajectory has started
self._slow_move_offset = 0.0
self._trajectory_start_offset = rospy.Duration(0.0)
self._trajectory_actual_offset = rospy.Duration(0.0)
#param namespace
self._param_ns = '/rsdk_joint_trajectory_action_server/'
#gripper control rate
self._gripper_rate = 20.0 # Hz
def __init__(self):
rospy.init_node('trajectory_demo')
# Set to True to move back to the starting configurations
reset = rospy.get_param('~reset', False)
# Set to False to wait for arm to finish before moving head
sync = rospy.get_param('~sync', True)
# Which joints define the arm?
arm_joints = [
'shoulder_pan_joint', 'shoulder_lift_joint', 'upperarm_roll_joint',
'elbow_flex_joint', 'forearm_roll_joint', 'wrist_flex_joint',
'wrist_roll_joint'
]
# Which joints define the head?
head_joints = ['head_pan_joint', 'head_tilt_joint']
# Which joint defines the torso?
torso_joints = ['torso_lift_joint']
if reset:
# Set the arm back to the tucked position
arm_goal = [-1.3901, 1.3439, -2.8327, -1.8119, 0.0, -1.6571, 0.0]
# Re-center the head
head_goal = [0.0, 0.0]
# Bring the toros back down
torso_goal = [0.0]
else:
# Set a goal configuration for the arm
arm_goal = [-1.0, 0, 0, -1.0, 0, 0, 0]
# Set a goal configuration for the head
head_goal = [-0.85, -0.25]
# Move the torso up
torso_goal = [0.35]
# Connect to the arm trajectory action server
rospy.loginfo('Waiting for arm trajectory controller...')
arm_client = actionlib.SimpleActionClient(
'arm_controller/follow_joint_trajectory',
FollowJointTrajectoryAction)
arm_client.wait_for_server()
rospy.loginfo('...connected.')
# Connect to the head trajectory action server
rospy.loginfo('Waiting for head trajectory controller...')
head_client = actionlib.SimpleActionClient(
'head_controller/follow_joint_trajectory',
FollowJointTrajectoryAction)
head_client.wait_for_server()
rospy.loginfo('...connected.')
# Connect to the torso trajectory action server
rospy.loginfo('Waiting for headtorso trajectory controller...')
torso_client = actionlib.SimpleActionClient(
'torso_controller/follow_joint_trajectory',
FollowJointTrajectoryAction)
torso_client.wait_for_server()
rospy.loginfo('...connected.')
# Create a single-point arm trajectory with the arm_goal as the end-point
arm_trajectory = JointTrajectory()
arm_trajectory.joint_names = arm_joints
arm_trajectory.points.append(JointTrajectoryPoint())
arm_trajectory.points[0].positions = arm_goal
arm_trajectory.points[0].velocities = [0.0 for i in arm_joints]
arm_trajectory.points[0].accelerations = [0.0 for i in arm_joints]
arm_trajectory.points[0].time_from_start = rospy.Duration(5.0)
# Send the trajectory to the arm action server
rospy.loginfo('Moving the arm to goal position...')
# Create an empty trajectory goal
arm_goal = FollowJointTrajectoryGoal()
# Set the trajectory component to the goal trajectory created above
arm_goal.trajectory = arm_trajectory
# Specify zero tolerance for the execution time
arm_goal.goal_time_tolerance = rospy.Duration(0.0)
# Send the goal to the action server
arm_client.send_goal(arm_goal)
if not sync:
# Wait for up to 5 seconds for the motion to complete
arm_client.wait_for_result(rospy.Duration(5.0))
# Create a single-point head trajectory with the head_goal as the end-point
head_trajectory = JointTrajectory()
head_trajectory.joint_names = head_joints
head_trajectory.points.append(JointTrajectoryPoint())
head_trajectory.points[0].positions = head_goal
head_trajectory.points[0].velocities = [0.0 for i in head_joints]
head_trajectory.points[0].accelerations = [0.0 for i in head_joints]
head_trajectory.points[0].time_from_start = rospy.Duration(5.0)
# Send the trajectory to the head action server
rospy.loginfo('Moving the head to goal position...')
head_goal = FollowJointTrajectoryGoal()
head_goal.trajectory = head_trajectory
head_goal.goal_time_tolerance = rospy.Duration(0.0)
# Send the goal
head_client.send_goal(head_goal)
if not sync:
# Wait for up to 5 seconds for the motion to complete
head_client.wait_for_result(rospy.Duration(5.0))
# Create a single-point torso trajectory with the torso_goal as the end-point
torso_trajectory = JointTrajectory()
torso_trajectory.joint_names = torso_joints
torso_trajectory.points.append(JointTrajectoryPoint())
torso_trajectory.points[0].positions = torso_goal
torso_trajectory.points[0].velocities = [0.0 for i in torso_joints]
torso_trajectory.points[0].accelerations = [0.0 for i in torso_joints]
torso_trajectory.points[0].time_from_start = rospy.Duration(5.0)
# Send the trajectory to the head action server
rospy.loginfo('Moving the head to goal position...')
torso_goal = FollowJointTrajectoryGoal()
torso_goal.trajectory = torso_trajectory
torso_goal.goal_time_tolerance = rospy.Duration(0.0)
# Send the goal
torso_client.send_goal(torso_goal)
# Wait for up to 8 seconds for the motion to complete
torso_client.wait_for_result(rospy.Duration(8.0))
rospy.loginfo('...done')
def execute_plan_traj(self, plannedTraj):
traj_goal = FollowJointTrajectoryGoal()
traj_goal.trajectory = plannedTraj.joint_trajectory
self.smooth_joint_trajectory_client.send_goal(traj_goal)
self.smooth_joint_trajectory_client.wait_for_result()
self.smooth_joint_trajectory_client.get_result()
def sendWaypointTrajectory(self,
waypoints,
durations=0.,
vels=0.,
accs=0.,
effs=0.):
if not self.ang_cmd_wait(waypoints[0]):
print 'Cannot go to the first point in the trajectory'
return None
# else:
# print 'Went to first'
if not self.traj_connection:
print 'Action server connection was not established'
return None
joint_traj = JointTrajectory()
joint_traj.joint_names = self.arm_joint_names
if not durations == 0:
if not len(durations) == waypoints:
raise Exception(
'The number of duration points is not equal to the number of provided waypoints'
)
if not vels == 0:
if not len(vels) == waypoints:
raise Exception(
'The number velocity points is not equal to the number of provided waypoints'
)
if not accs == 0:
if not len(accs) == waypoints:
raise Exception(
'The number acceleration points is not equal to the number of provided waypoints'
)
if not effs == 0:
if not len(effs) == waypoints:
raise Exception(
'The number effort points is not equal to the number of provided waypoints'
)
if not effs == 0:
if not (vels == 0 and accs == 0):
raise Exception(
'Cannot specify efforts with velocities and accelerations at the same time'
)
if (not accs == 0) and vels == 0:
raise Exception('Cannot specify accelerations without velocities')
total_time_from_start = 0.5
for t in range(0, len(waypoints)):
point = JointTrajectoryPoint()
waypoint = waypoints[t]
if not len(waypoint) == len(joint_traj.joint_names):
raise Exception(
'The number of provided joint positions is not equal to the number of available joints for index: '
+ str(t))
point.positions = waypoint
if not vels == 0.:
velocity = vels[t]
if not len(velocity) == len(joint_traj.joint_names):
raise Exception(
'The number of provided joint velocities is not equal to the number of available joints for index: '
+ str(t))
point.velocities = velocity
if not accs == 0.:
acceleration = accs[t]
if not len(acceleration) == len(joint_traj.joint_names):
raise Exception(
'The number of provided joint accelerations is not equal to the number of available joints for index: '
+ str(t))
point.accelerations = accelerations
if not effs == 0.:
effort = effs[t]
if not len(effort) == len(joint_traj.joint_names):
raise Exception(
'The number of provided joint efforts is not equal to the number of available joints for index: '
+ str(t))
point.effort = effort
if not durations == 0.:
point.duration = duration
# Deal with increasing time for each trajectory point
point.time_from_start = rospy.Duration(total_time_from_start)
total_time_from_start = total_time_from_start + 1.0
# Set the points
joint_traj.points.append(point)
traj_goal = FollowJointTrajectoryGoal()
traj_goal.trajectory = joint_traj
self.smooth_joint_trajectory_client.send_goal(traj_goal)
self.smooth_joint_trajectory_client.wait_for_result()
return self.smooth_joint_trajectory_client.get_result()
def trajectory_test(self):
"""Test robot movement"""
#### Make sure the controller is up and running ####
goal = SetModeGoal()
goal.target_robot_mode = RobotMode.RUNNING
goal.play_program = False # we use headless mode during tests
self.set_mode_client.send_goal(goal)
self.set_mode_client.wait_for_result()
rospy.sleep(2) # TODO properly wait until the robot is running
send_program_srv = rospy.ServiceProxy(
"/ur_hardware_interface/resend_robot_program", Trigger)
send_program_srv.call()
rospy.sleep(2) # TODO properly wait until the controller is running
goal = FollowJointTrajectoryGoal()
goal.trajectory.joint_names = [
"elbow_joint", "shoulder_lift_joint", "shoulder_pan_joint",
"wrist_1_joint", "wrist_2_joint", "wrist_3_joint"
]
position_list = [[0.0 for i in range(6)]]
position_list.append([-0.5 for i in range(6)])
position_list.append([-1.0 for i in range(6)])
duration_list = [6.0, 9.0, 12.0]
for i, position in enumerate(position_list):
point = JointTrajectoryPoint()
point.positions = position
point.time_from_start = rospy.Duration(duration_list[i])
goal.trajectory.points.append(point)
rospy.loginfo("Sending simple goal")
self.trajectory_client.send_goal(goal)
self.trajectory_client.wait_for_result()
self.assertEqual(self.trajectory_client.get_result().error_code,
FollowJointTrajectoryResult.SUCCESSFUL)
rospy.loginfo("Received result SUCCESSFUL")
"""Test trajectory server. This is more of a validation test that the testing suite does the
right thing."""
goal = FollowJointTrajectoryGoal()
goal.trajectory.joint_names = [
"elbow_joint", "shoulder_lift_joint", "shoulder_pan_joint",
"wrist_1_joint", "wrist_2_joint", "wrist_3_joint"
]
position_list = [[0.0 for i in range(6)]]
position_list.append([-0.5 for i in range(6)])
# Create illegal goal by making the second point come earlier than the first
duration_list = [6.0, 3.0]
for i, position in enumerate(position_list):
point = JointTrajectoryPoint()
point.positions = position
point.time_from_start = rospy.Duration(duration_list[i])
goal.trajectory.points.append(point)
rospy.loginfo("Sending illegal goal")
self.trajectory_client.send_goal(goal)
self.trajectory_client.wait_for_result()
# As timings are illegal, we expect the result to be INVALID_GOAL
self.assertEqual(self.trajectory_client.get_result().error_code,
FollowJointTrajectoryResult.INVALID_GOAL)
rospy.loginfo("Received result INVALID_GOAL")
"""Test robot movement"""
goal = FollowJointTrajectoryGoal()
goal.trajectory.joint_names = [
"elbow_joint", "shoulder_lift_joint", "shoulder_pan_joint",
"wrist_1_joint", "wrist_2_joint", "wrist_3_joint"
]
position_list = [[0.0 for i in range(6)]]
position_list.append([-1.0 for i in range(6)])
duration_list = [6.0, 6.5]
for i, position in enumerate(position_list):
point = JointTrajectoryPoint()
point.positions = position
point.time_from_start = rospy.Duration(duration_list[i])
goal.trajectory.points.append(point)
rospy.loginfo("Sending scaled goal without time restrictions")
self.trajectory_client.send_goal(goal)
self.trajectory_client.wait_for_result()
self.assertEqual(self.trajectory_client.get_result().error_code,
FollowJointTrajectoryResult.SUCCESSFUL)
rospy.loginfo("Received result SUCCESSFUL")
# Now do the same again, but with a goal time constraint
rospy.loginfo("Sending scaled goal with time restrictions")
goal.goal_time_tolerance = rospy.Duration(0.01)
self.trajectory_client.send_goal(goal)
self.trajectory_client.wait_for_result()
self.assertEqual(self.trajectory_client.get_result().error_code,
FollowJointTrajectoryResult.GOAL_TOLERANCE_VIOLATED)
rospy.loginfo("Received result GOAL_TOLERANCE_VIOLATED")
def __init__(self, debug=False, goal_type='API', state_type='OL'):
self.debug = debug
self.goal_type = goal_type #default API
self.state_type = state_type #default OL = open loop
if self.goal_type == 'trajectory':
super(RolloutExecuter, self).__init__()
#create our action server clients
self._left_client = actionlib.SimpleActionClient(
'robot/limb/left/follow_joint_trajectory',
FollowJointTrajectoryAction,
)
self._right_client = actionlib.SimpleActionClient(
'robot/limb/right/follow_joint_trajectory',
FollowJointTrajectoryAction,
)
#param namespace
self._param_ns = '/rsdk_joint_trajectory_action_server/'
#create our goal request
self._l_goal = FollowJointTrajectoryGoal()
self._r_goal = FollowJointTrajectoryGoal()
#gripper goal trajectories
self._l_grip = FollowJointTrajectoryGoal()
self._r_grip = FollowJointTrajectoryGoal()
#limb interface - current angles needed for start move
self._l_arm = baxter_interface.Limb('left')
self._r_arm = baxter_interface.Limb('right')
#flag to signify the arm trajectories have begun executing
self._arm_trajectory_started = False
#reentrant lock to prevent same-thread lockout
self._lock = threading.RLock()
#vicon subscription and world frame pose dictionary for rewards
self.vicon_sub = rospy.Subscriber('/vicon/marker_new/pose',
geometry_msgs.PoseStamped,
self.vicon_callback)
self.end_effector_pos = None
self.end_effector_desired = dict()
#robot subscription for state information (dictionary of lists indexed by timestep)
self.robot_state_sub = rospy.Subscriber('robot/joint_states',
sensor_msgs.JointState,
self.robot_state_callback)
self.robot_state_current = dict()
self.robot_joint_names = None
self.state_keys = [
'theta_commanded', 'theta_desired', 'theta_measured', 'velocity',
'torque'
]
if self.state_type == 'CL':
self.state_keys.append('world_pose')
#world frame error in current end effector position and desired from parsed file
self.world_error = []
#publish timing topics
self.timing_pub_state = rospy.Publisher('/cycle_bool',
std_msgs.Bool,
queue_size=1)
self.timing_msg_state = std_msgs.Bool()
self.timing_pub_time = rospy.Publisher('/cycle_time',
std_msgs.Time,
queue_size=1)
self.timing_msg_time = std_msgs.Time()
#maintain dictionaries of variables to dump at the end or query
self.vicon_dump = []
self.state_dump = dict()
self.action_dump = []
self.reward_dump = []
self.average_error = [0, 0, 0]
#stop flag
self.stop = False
def clear(self, limb):
self._goal = FollowJointTrajectoryGoal()
self._goal.goal_time_tolerance = self._goal_time_tolerance
self._goal.trajectory.joint_names = [limb + '_' + joint for joint in \
['s0', 's1', 'e0', 'e1', 'w0', 'w1', 'w2']]
def computeIKsPose(self, poselist, times, arm="arm", downsample_freq=None):
"""Giving a poselist (list of Pose) and times for every point compute it's iks and add it's times
if a value is given to downsample_freq then we will only compute IKs and so for the downsampled
rate of poses"""
rospy.loginfo("Computing " + str(len(poselist)) + " IKs")
fjt_goal = FollowJointTrajectoryGoal()
if arm == 'arm_torso':
fjt_goal.trajectory.joint_names = self.arm_torso
elif arm == 'arm_torso':
fjt_goal.trajectory.joint_names = self.arm_torso
elif arm == 'arm':
fjt_goal.trajectory.joint_names = self.arm
elif arm == 'arm':
fjt_goal.trajectory.joint_names = self.arm
# TODO: add other options
ik_answer = None
last_succesfull_ik_answer = None
if downsample_freq != None:
num_poses = 0
num_downsampled_poses = 0
for pose, time in zip(poselist, times):
if downsample_freq != None:
num_poses += 1
if num_poses % downsample_freq == 0:
num_downsampled_poses += 1
else: # if we are downsampling dont do IK calls if it's not one of the samples we want
continue
if last_succesfull_ik_answer != None:
ik_answer = self.getIkPose(
pose,
"arm",
previous_state=last_succesfull_ik_answer.solution)
else:
ik_answer = self.getIkPose(pose)
if DEBUG_MODE:
rospy.loginfo("Got error_code: " +
str(ik_answer.error_code.val) +
" which means: " +
moveit_error_dict[ik_answer.error_code.val])
if moveit_error_dict[ik_answer.error_code.val] == 'SUCCESS':
# We should check if the solution is very far away from joint config
# if so.. try again... being a generic solution i dont know how to manage this
last_succesfull_ik_answer = ik_answer
if DEBUG_MODE:
arrow = self.createArrowMarker(pose, ColorRGBA(0, 1, 0, 1))
self.ok_markers.markers.append(arrow)
jtp = JointTrajectoryPoint()
#ik_answer = GetConstraintAwarePositionIKResponse()
# sort positions and add only the ones of the joints we are interested in
positions = self.sortOutJointList(
fjt_goal.trajectory.joint_names,
ik_answer.solution.joint_state)
jtp.positions = positions
jtp.time_from_start = rospy.Duration(time)
fjt_goal.trajectory.points.append(jtp)
if DEBUG_MODE:
self.pub_ok_markers.publish(self.ok_markers)
else:
if DEBUG_MODE:
arrow = self.createArrowMarker(pose, ColorRGBA(1, 0, 0, 1))
self.fail_markers.markers.append(arrow)
self.pub_fail_markers.publish(self.fail_markers)
# Loop for a while to check if we get a solution on further checks?
if downsample_freq != None:
rospy.loginfo("From " + str(num_poses) +
" points we downsampled to " +
str(num_downsampled_poses) +
" and fjt_goal.trajectory.points has " +
str(fjt_goal.trajectory.points) + " points")
return fjt_goal
def run(self, joint_pose, duration=2.0):
"""
The run function for this step
Args:
duration (float) : The amount of time within which to execute the
trajectory
joint_pose (str, list, tuple, dict) :
The arm poses to move to. If the type is:
* str. Then if the string starts with
* `joint_poses`, get a ``assistance_msgs/ArmJointPose`` \
from :const:`ARM_JOINT_POSES_SERVICE_NAME` and move the \
joints to the desired pose
* list, tuple. Then if the list is of
* `floats`, move the joints to the desired pose indicated \
with the float values
.. seealso::
:meth:`task_executor.abstract_step.AbstractStep.run`
"""
# Parse out the pose
parsed_pose = self._parse_pose(joint_pose)
if parsed_pose is None:
rospy.logerr("Action {}: FAIL. Unknown Format: {}".format(self.name, joint_pose))
raise KeyError(self.name, "Unknown Format", joint_pose)
rospy.loginfo("Action {}: Moving arm to pose {}".format(self.name, parsed_pose))
# Create and send the goal
goal = FollowJointTrajectoryGoal()
trajectory = JointTrajectory()
trajectory.joint_names = ArmFollowJointTrajAction.JOINT_NAMES
trajectory.points.append(JointTrajectoryPoint())
trajectory.points[0].positions = parsed_pose
trajectory.points[0].velocities = [0.0 for _ in parsed_pose]
trajectory.points[0].accelerations = [0.0 for _ in parsed_pose]
trajectory.points[0].time_from_start = rospy.Duration(duration)
goal.trajectory = trajectory
self._arm_follow_joint_traj_client.send_goal(goal)
# Yield an empty dict while we're executing
while self._arm_follow_joint_traj_client.get_state() in AbstractStep.RUNNING_GOAL_STATES:
yield self.set_running()
# Wait for a result and yield based on how we exited
status = self._arm_follow_joint_traj_client.get_state()
self._arm_follow_joint_traj_client.wait_for_result()
result = self._arm_follow_joint_traj_client.get_result()
if status == GoalStatus.SUCCEEDED:
yield self.set_succeeded()
elif status == GoalStatus.PREEMPTED:
yield self.set_preempted(
action=self.name,
status=status,
result=result,
)
else:
yield self.set_aborted(
action=self.name,
status=status,
result=result,
)
def push_object(self, goal):
if self.action_server.is_preempt_requested():
rospy.loginfo('%s: preempted' % ACTION_NAME)
self.action_server.set_preempted()
collision_object_name = goal.collision_object_name
collision_support_surface_name = goal.collision_support_surface_name
current_state = rospy.wait_for_message('l_arm_controller/state',
FollowJointTrajectoryFeedback)
start_angles = current_state.actual.positions
full_state = rospy.wait_for_message('/joint_states', JointState)
req = GetPositionFKRequest()
req.header.frame_id = 'base_link'
req.fk_link_names = [GRIPPER_LINK_FRAME]
req.robot_state.joint_state = full_state
if not self.set_planning_scene_diff_client():
rospy.logerr('%s: failed to set planning scene diff' % ACTION_NAME)
self.action_server.set_aborted()
return
pose = self.get_fk_srv(req).pose_stamped[0].pose
frame_id = 'base_link'
approachpos = [pose.position.x, pose.position.y, pose.position.z]
approachquat = [
pose.orientation.x, pose.orientation.y, pose.orientation.z,
pose.orientation.w
]
push_distance = 0.40
grasppos = [
pose.position.x, pose.position.y - push_distance, pose.position.z
]
graspquat = [0.00000, 0.00000, 0.70711, -0.70711]
# attach object to gripper, they will move together
obj = AttachedCollisionObject()
obj.object.header.stamp = rospy.Time.now()
obj.object.header.frame_id = GRIPPER_LINK_FRAME
obj.object.operation.operation = CollisionObjectOperation.ATTACH_AND_REMOVE_AS_OBJECT
obj.object.id = collision_object_name
obj.link_name = GRIPPER_LINK_FRAME
obj.touch_links = GRIPPER_LINKS
self.attached_object_pub.publish(obj)
# disable collisions between attached object and table
collision_operation1 = CollisionOperation()
collision_operation1.object1 = CollisionOperation.COLLISION_SET_ATTACHED_OBJECTS
collision_operation1.object2 = collision_support_surface_name
collision_operation1.operation = CollisionOperation.DISABLE
collision_operation2 = CollisionOperation()
collision_operation2.object1 = collision_support_surface_name
collision_operation2.object2 = GRIPPER_GROUP_NAME
collision_operation2.operation = CollisionOperation.DISABLE
collision_operation2.penetration_distance = 0.02
ordered_collision_operations = OrderedCollisionOperations()
ordered_collision_operations.collision_operations = [
collision_operation1, collision_operation2
]
res = self.check_cartesian_path_lists(
approachpos,
approachquat,
grasppos,
graspquat,
start_angles,
frame=frame_id,
ordered_collision_operations=ordered_collision_operations)
error_code_dict = {
ArmNavigationErrorCodes.SUCCESS: 0,
ArmNavigationErrorCodes.COLLISION_CONSTRAINTS_VIOLATED: 1,
ArmNavigationErrorCodes.PATH_CONSTRAINTS_VIOLATED: 2,
ArmNavigationErrorCodes.JOINT_LIMITS_VIOLATED: 3,
ArmNavigationErrorCodes.PLANNING_FAILED: 4
}
trajectory_len = len(res.trajectory.joint_trajectory.points)
trajectory = [
res.trajectory.joint_trajectory.points[i].positions
for i in range(trajectory_len)
]
vels = [
res.trajectory.joint_trajectory.points[i].velocities
for i in range(trajectory_len)
]
times = [
res.trajectory.joint_trajectory.points[i].time_from_start
for i in range(trajectory_len)
]
error_codes = [
error_code_dict[error_code.val]
for error_code in res.trajectory_error_codes
]
# if even one code is not 0, abort
if sum(error_codes) != 0:
for ind in range(len(trajectory)):
rospy.loginfo("error code " + str(error_codes[ind]) +
" pos : " + self.pplist(trajectory[ind]))
rospy.loginfo("")
for ind in range(len(trajectory)):
rospy.loginfo("time: " + "%5.3f " % times[ind].to_sec() +
"vels: " + self.pplist(vels[ind]))
rospy.logerr('%s: attempted push failed' % ACTION_NAME)
self.action_server.set_aborted()
return
req = FilterJointTrajectoryRequest()
req.trajectory = res.trajectory.joint_trajectory
req.trajectory.points = req.trajectory.points[
1:] # skip zero velocity point
req.allowed_time = rospy.Duration(2.0)
filt_res = self.trajectory_filter_srv(req)
goal = FollowJointTrajectoryGoal()
goal.trajectory = filt_res.trajectory
goal.trajectory.header.stamp = rospy.Time.now() + rospy.Duration(0.1)
self.start_audio_recording_srv(InfomaxAction.PUSH, goal.category_id)
rospy.sleep(0.5)
self.trajectory_controller.send_goal(goal)
self.trajectory_controller.wait_for_result()
state = self.trajectory_controller.get_state()
if state == GoalStatus.SUCCEEDED:
rospy.sleep(0.5)
sound_msg = self.stop_audio_recording_srv(True)
self.action_server.set_succeeded(
PushObjectResult(sound_msg.recorded_sound))
return
rospy.logerr('%s: attempted push failed' % ACTION_NAME)
self.stop_audio_recording_srv(False)
self.action_server.set_aborted()
def __init__(self):
rospy.init_node('trajectory_demo')
# Set to True to move back to the starting configurations
reset = rospy.get_param('~reset', False)
# Set to False to wait for arm to finish before moving head
sync = rospy.get_param('~sync', True)
# Which joints define the arm?
arm_joints = ['right_e0',
'right_e1',
'right_s0',
'right_s1',
'right_w0',
'right_w1',
'right_w2']
# Which joints define the head?
#head_joints = ['head_pan_joint', 'head_tilt_joint']
if reset:
# Set the arm back to the resting position
arm_goal = [0, 0, 0, 0, 0, 0]
# Re-center the head
head_goal = [0, 0]
else:
# Set a goal configuration for the arm
arm_goal = [-0.3, -2.0, -1.0, 0.8, 1.0, -0.7]
# Set a goal configuration for the head
head_goal = [-1.3, -0.1]
# Connect to the right arm trajectory action server
rospy.loginfo('Waiting for right arm trajectory controller...')
arm_client = actionlib.SimpleActionClient('right_arm_controller/follow_joint_trajectory', FollowJointTrajectoryAction)
arm_client.wait_for_server()
rospy.loginfo('...connected.')
# Connect to the head trajectory action server
rospy.loginfo('Waiting for head trajectory controller...')
head_client = actionlib.SimpleActionClient('head_controller/follow_joint_trajectory', FollowJointTrajectoryAction)
head_client.wait_for_server()
rospy.loginfo('...connected.')
# Create a single-point arm trajectory with the arm_goal as the end-point
arm_trajectory = JointTrajectory()
arm_trajectory.joint_names = arm_joints
arm_trajectory.points.append(JointTrajectoryPoint())
arm_trajectory.points[0].positions = arm_goal
arm_trajectory.points[0].velocities = [0.0 for i in arm_joints]
arm_trajectory.points[0].accelerations = [0.0 for i in arm_joints]
arm_trajectory.points[0].time_from_start = rospy.Duration(3.0)
# Send the trajectory to the arm action server
rospy.loginfo('Moving the arm to goal position...')
# Create an empty trajectory goal
arm_goal = FollowJointTrajectoryGoal()
# Set the trajectory component to the goal trajectory created above
arm_goal.trajectory = arm_trajectory
# Specify zero tolerance for the execution time
arm_goal.goal_time_tolerance = rospy.Duration(0.0)
# Send the goal to the action server
arm_client.send_goal(arm_goal)
if not sync:
# Wait for up to 5 seconds for the motion to complete
arm_client.wait_for_result(rospy.Duration(5.0))
# Create a single-point head trajectory with the head_goal as the end-point
head_trajectory = JointTrajectory()
head_trajectory.joint_names = head_joints
head_trajectory.points.append(JointTrajectoryPoint())
head_trajectory.points[0].positions = head_goal
head_trajectory.points[0].velocities = [0.0 for i in head_joints]
head_trajectory.points[0].accelerations = [0.0 for i in head_joints]
head_trajectory.points[0].time_from_start = rospy.Duration(3.0)
# Send the trajectory to the head action server
rospy.loginfo('Moving the head to goal position...')
head_goal = FollowJointTrajectoryGoal()
head_goal.trajectory = head_trajectory
head_goal.goal_time_tolerance = rospy.Duration(0.0)
# Send the goal
head_client.send_goal(head_goal)
# Wait for up to 5 seconds for the motion to complete
head_client.wait_for_result(rospy.Duration(5.0))
rospy.loginfo('...done')
def marker_inf_cb(self, data):
print "in marker callback"
q = Quaternion()
q = data.pose.orientation
marker_orientation = np.array([q.x, q.y, q.z, q.w])
euler_angles = tr.euler_from_quaternion(marker_orientation, 'rxyz')
vec3 = Vector3()
vec3.x = data.pose.position.x
vec3.y = data.pose.position.y
vec3.z = data.pose.position.z
marker_pose = np.array([
vec3.x, vec3.y, vec3.z, euler_angles[0], euler_angles[1],
euler_angles[2]
])
for i in range(6):
if np.isnan(marker_pose[i]):
return
cam_object_T = self.set_cam_object_T(marker_pose)
cam_object_R = tr.identity_matrix()
cam_object_R[0:3, 0:3] = cam_object_T[0:3, 0:3]
cam_object_q = tr.quaternion_from_matrix(cam_object_R)
self.br.sendTransform(
(cam_object_T[0, 3], cam_object_T[1, 3], cam_object_T[2, 3]),
cam_object_q, rospy.Time.now(), "object_in_cam_frame",
"camera_rgb_optical_frame")
# publishing the object marker to rviz for object_in_cam
det_marker = Marker()
det_marker.header.frame_id = "camera_rgb_optical_frame"
det_marker.header.stamp = rospy.Time.now()
det_marker.type = Marker.SPHERE
det_marker.pose.position.x = cam_object_T[0, 3]
det_marker.pose.position.y = cam_object_T[1, 3]
det_marker.pose.position.z = cam_object_T[2, 3]
det_marker.pose.orientation.x = 0
det_marker.pose.orientation.y = 0
det_marker.pose.orientation.z = 0
det_marker.pose.orientation.w = 1
det_marker.scale.x = 0.05
det_marker.scale.y = 0.05
det_marker.scale.z = 0.05
det_marker.color.a = 1.0
det_marker.color.r = 1.0
det_marker.color.g = 0.0
det_marker.color.b = 0.0
self.marker_object_in_cam_pub.publish(det_marker)
base_object_T = np.dot(np.dot(self.base_end_T, self.end_cam_T),
cam_object_T)
# print base_object_T
final_err = self.error_computation(base_object_T, self.base_target_T)
# target thresholds:
trans_th = [0.05, 0.05, 0.05]
rot_th = [15 * np.pi / 180, 15 * np.pi / 180, 15 * np.pi / 180]
print "timer: ", self.timer
if self.timer > 5:
print "goal reached ............................"
self.gripper_final_action()
if np.abs(final_err[0]) < trans_th[0] and np.abs(
final_err[1]) < trans_th[1] and np.abs(
final_err[2]) < trans_th[2]:
if np.abs(final_err[3]) < rot_th[0] and np.abs(
final_err[4]) < rot_th[1] and np.abs(
final_err[5]) < rot_th[2]:
self.timer += 1
return
else:
self.timer = 0
print "error :", final_err
else:
self.timer = 0
print "error :", final_err
# use of error_gains:
error_gains = np.reshape(np.array([1, 1, 1, 1, 1, 1]), (6, 1))
final_err2 = np.multiply(final_err, error_gains)
# error value type 2
end_object_T = np.dot(self.end_cam_T, cam_object_T)
end_target_T = np.dot(self.end_grip_T, self.grip_target_T)
object_target_T = np.dot(end_object_T, np.linalg.inv(end_target_T))
# print "error style 2: ", object_target_T
# publishing the object marker to rviz
det_marker = Marker()
det_marker.header.frame_id = "iiwa_base_link"
det_marker.header.stamp = rospy.Time.now()
det_marker.type = Marker.SPHERE
det_marker.pose.position.x = base_object_T[0, 3]
det_marker.pose.position.y = base_object_T[1, 3]
det_marker.pose.position.z = base_object_T[2, 3]
det_marker.pose.orientation.x = 0
det_marker.pose.orientation.y = 0
det_marker.pose.orientation.z = 0
det_marker.pose.orientation.w = 1
det_marker.scale.x = 0.05
det_marker.scale.y = 0.05
det_marker.scale.z = 0.05
det_marker.color.a = 1.0
det_marker.color.r = 0.0
det_marker.color.g = 0.0
det_marker.color.b = 1.0
self.marker_object_in_base_pub.publish(det_marker)
# print "final error is : ", final_err
# print "base_target_T: ", base_object_T
# computation of jacobian of the manipulator
J = self.kdl_kin.jacobian(self.end_eff_current_pose)
if self.used_method == "DLSM":
# use of damped_least_squares as matrix inverse
J_inverse = np.dot(
J.T,
np.linalg.inv(
np.dot(J, J.T) + ((self.landa**2) * np.identity(6))))
elif self.used_method == "JPM":
# use of matrix pseudo_inverse as matrix inverse
J_inverse = np.linalg.pinv(J)
elif self.used_method == "JTM":
# use of matrix transpose as matrix inverse
J_inverse = np.transpose(J)
final_vel = np.dot(J_inverse, final_err2)
# the dereived final speed command for the manipulator
speed_cmd = list(np.array(final_vel).reshape(-1, ))
# avoid huge joint velocities:
joint_vel_th = [0.7, 0.7, 0.7, 0.7, 0.7, 0.7, 0.7]
if speed_cmd[0] > joint_vel_th[0]:
print "huge velocity for the first joint!"
return
elif speed_cmd[1] > joint_vel_th[1]:
print "huge velocity for the second joint!"
return
elif speed_cmd[2] > joint_vel_th[2]:
print "huge velocity for the third joint!"
return
elif speed_cmd[3] > joint_vel_th[3]:
print "huge velocity for the fourth joint!"
return
elif speed_cmd[4] > joint_vel_th[4]:
print "huge velocity for the fifth joint!"
return
elif speed_cmd[5] > joint_vel_th[5]:
print "huge velocity for the sixth joint!"
return
elif speed_cmd[6] > joint_vel_th[6]:
print "huge velocity for the seventh joint!"
return
# block to handle speed publisher using pose publisher
intergration_time_step = 0.5
pose_change = [speed * intergration_time_step for speed in speed_cmd]
final_pose = [
a + b for a, b in zip(self.end_eff_current_pose, pose_change)
]
# actionlib commands
self.trajectory.points[0].positions = final_pose
self.trajectory.points[0].velocities = [0.0] * len(self.joint_names)
self.trajectory.points[0].accelerations = [0.0] * len(self.joint_names)
self.trajectory.points[0].time_from_start = rospy.Duration(1.0)
# print "pose_change", pose_change
# print "The manipulator is moving..."
goal = FollowJointTrajectoryGoal()
goal.trajectory = self.trajectory
goal.goal_time_tolerance = rospy.Duration(0.0)
self.manipulator_client.send_goal(goal)
action_result = self.manipulator_client.wait_for_result()
if action_result is True:
# publishing the path of the object in "red" color
final_object_position = base_object_T[0:3, 3]
final_object_R = tr.identity_matrix()
final_object_R[0:3, 0:3] = base_object_T[0:3, 0:3]
final_object_quaternion = tr.quaternion_from_matrix(final_object_R)
self.object_path.markers.append(Marker())
self.object_path.markers[-1].header.frame_id = "iiwa_base_link"
self.object_path.markers[-1].header.stamp = rospy.Time.now()
self.object_path.markers[-1].id = len(self.object_path.markers)
self.object_path.markers[-1].type = Marker.SPHERE
self.object_path.markers[
-1].pose.position.x = final_object_position[0]
self.object_path.markers[
-1].pose.position.y = final_object_position[1]
self.object_path.markers[
-1].pose.position.z = final_object_position[2]
self.object_path.markers[
-1].pose.orientation.x = final_object_quaternion[0]
self.object_path.markers[
-1].pose.orientation.y = final_object_quaternion[1]
self.object_path.markers[
-1].pose.orientation.z = final_object_quaternion[2]
self.object_path.markers[
-1].pose.orientation.w = final_object_quaternion[3]
self.object_path.markers[-1].scale.x = 0.01
self.object_path.markers[-1].scale.y = 0.01
self.object_path.markers[-1].scale.z = 0.01
self.object_path.markers[-1].color.a = 1.0
self.object_path.markers[-1].color.r = 1.0
self.object_path.markers[-1].color.g = 0.0
self.object_path.markers[-1].color.b = 0.0
self.marker_obj_path_pub.publish(self.object_path)
# publishing path of the end effector in "green" color
final_ee_position = self.base_end_T[0:3, 3]
final_ee_R = tr.identity_matrix()
final_ee_R[0:3, 0:3] = self.base_end_T[0:3, 0:3]
final_ee_quaternion = tr.quaternion_from_matrix(final_ee_R)
self.ee_path.markers.append(Marker())
self.ee_path.markers[-1].header.frame_id = "iiwa_base_link"
self.ee_path.markers[-1].header.stamp = rospy.Time.now()
self.ee_path.markers[-1].id = len(self.ee_path.markers)
self.ee_path.markers[-1].type = Marker.SPHERE
self.ee_path.markers[-1].pose.position.x = final_ee_position[0]
self.ee_path.markers[-1].pose.position.y = final_ee_position[1]
self.ee_path.markers[-1].pose.position.z = final_ee_position[2]
self.ee_path.markers[-1].pose.orientation.x = final_ee_quaternion[
0]
self.ee_path.markers[-1].pose.orientation.y = final_ee_quaternion[
1]
self.ee_path.markers[-1].pose.orientation.z = final_ee_quaternion[
2]
self.ee_path.markers[-1].pose.orientation.w = final_ee_quaternion[
3]
self.ee_path.markers[-1].scale.x = 0.01
self.ee_path.markers[-1].scale.y = 0.01
self.ee_path.markers[-1].scale.z = 0.01
self.ee_path.markers[-1].color.a = 1.0
self.ee_path.markers[-1].color.r = 0.0
self.ee_path.markers[-1].color.g = 1.0
self.ee_path.markers[-1].color.b = 0.0
self.marker_eff_path_pub.publish(self.ee_path)
def __init__(self, sweep_joint_dict, debug=False, goal_type='API', state_type='OL'):
self.debug = debug
self.goal_type = goal_type #default API
self.state_type = state_type #default OL = open loop
if self.goal_type == 'trajectory':
super(RolloutExecuter, self).__init__()
#create our action server clients
self._left_client = actionlib.SimpleActionClient(
'robot/limb/left/follow_joint_trajectory',
FollowJointTrajectoryAction,
)
self._right_client = actionlib.SimpleActionClient(
'robot/limb/right/follow_joint_trajectory',
FollowJointTrajectoryAction,
)
#param namespace
self._param_ns = '/rsdk_joint_trajectory_action_server/'
#create our goal request
self._l_goal = FollowJointTrajectoryGoal()
self._r_goal = FollowJointTrajectoryGoal()
#gripper goal trajectories
self._l_grip = FollowJointTrajectoryGoal()
self._r_grip = FollowJointTrajectoryGoal()
#limb interface - current angles needed for start move
self._l_arm = baxter_interface.Limb('left')
self._r_arm = baxter_interface.Limb('right')
#flag to signify the arm trajectories have begun executing
self._arm_trajectory_started = False
#reentrant lock to prevent same-thread lockout
self._lock = threading.RLock()
#vicon subscription and world frame pose dictionary for rewards
self.vicon_sub = rospy.Subscriber('/vicon/marker1/pose', geometry_msgs.PoseStamped, self.vicon_callback)
self.end_effector_pos = None
self.end_effector_desired = dict()
#robot subscription for state information (dictionary of lists indexed by timestep)
self.robot_state_sub = rospy.Subscriber('/robot/limb/right/gravity_compensation_torques', baxter_core_msgs.SEAJointState, self.robot_state_callback)
self.robot_state_current = dict()
# self.robot_joint_names = None
self.robot_joint_names = ['right_s0', 'right_s1', 'right_e0', 'right_e1', 'right_w0', 'right_w1', 'right_w2']
self.state_keys = ['theta_commanded', 'theta_measured', 'velocity', 'torque']
if self.state_type == 'CL':
self.state_keys.append('world_pose')
#world frame error in current end effector position and desired from parsed file
self.world_error = []
#publish timing topics
self.timing_pub_state = rospy.Publisher('/cycle_bool', std_msgs.Bool, queue_size = 1)
self.timing_msg_state = std_msgs.Bool()
self.timing_pub_time = rospy.Publisher('/cycle_time', std_msgs.Time, queue_size = 1)
self.timing_msg_time = std_msgs.Time()
#maintain dictionaries of variables to dump at the end or query
self.vicon_dump = []
self.state_dump = dict()
self.action_dump = []
self.reward_dump = []
self.average_error = [0, 0, 0]
#programmed trajectories
self.current = 0
self.trajectory_matrix = None
self.sweep_joint_dict = sweep_joint_dict
self.iterations = self.sweep_joint_dict['iterations']
self.stop = False
self.joint_scale = np.array([1/2.5, 1/2.5, 1.5/2.5, 1.5/2.5, 3/2.5, 2/2.5])
self.joint_shift = np.array([-np.pi/6, -np.pi/6, -np.pi/4, np.pi/4, 0, 0])
self.joint_min = [-np.pi/3, -np.pi/3, -np.pi/2, 0, -np.pi/2, -np.pi/3, -np.pi]
self.joint_max = [0, 0, 0, np.pi/2, np.pi/2, np.pi/3, np.pi]
self.random_walk = OrnsteinUhlenbeckProcess(
dimension=6, num_steps=self.iterations,
seed_vec=self.sweep_joint_dict['fixed'][1:-1])
self.random_trajectory = self.random_walk.make_new_walk(length=self.iterations, scale_vec=self.joint_scale, shift_vec=self.joint_shift)
if self.debug:
print("random walk trajectory: \n")
print("shape: ")
print(self.random_trajectory.shape)
print("------- \n")
print("max: ")
print(np.max(self.random_trajectory, 0))
print("\n")
print("min: ")
print(np.min(self.random_trajectory, 0))
print("\n")
print("------- \n")
self.random_trajectory = np.hstack((self.random_trajectory, np.ones((self.iterations+1, 1))*2.7))
import roslib
roslib.load_manifest('ur_tutorial')
import rospy
from trajectory_msgs.msg import JointTrajectory, JointTrajectoryPoint
from control_msgs.msg import FollowJointTrajectoryGoal, FollowJointTrajectoryAction
from actionlib import SimpleActionClient
rospy.init_node("simple_traj")
client = SimpleActionClient("/arm_controller/follow_joint_trajectory",
FollowJointTrajectoryAction)
print "waiting to connect..."
client.wait_for_server()
print "connected! "
g = FollowJointTrajectoryGoal()
g.trajectory = JointTrajectory()
g.trajectory.joint_names = [
'shoulder_pan_joint', 'shoulder_lift_joint', 'elbow_joint',
'wrist_1_joint', 'wrist_2_joint', 'wrist_3_joint'
]
p1 = JointTrajectoryPoint()
p1.positions = [1.5, -0.2, -1.57, 0, 0, 0]
p1.velocities = [0.0, 0.0, 0.0, 0.0, 0.0, 0.0]
p1.time_from_start = rospy.Duration(5.0)
g.trajectory.points.append(p1)
p2 = JointTrajectoryPoint()
p2.positions = [1.5, 0, -1.57, 0, 0, 0]
p2.velocities = [0.0, 0.0, 0.0, 0.0, 0.0, 0.0]
def send_trajectory(self,
traj,
stamp,
acceleration=0.5,
velocity=0.5,
cartesian=False,
linear=False):
rate = rospy.Rate(30)
t = rospy.Time(0)
# Make sure that the trajectory 0 is the current joint position
traj.points[0].positions = self.q0
if not self.valid_verify(stamp):
#rospy.logerr('verify failed:'+ str(stamp) + ", " + str(self.cur_stamp))
return 'FAILURE - preempted'
if self.simulation:
rospy.logwarn("Simulation mode is active")
if not linear:
for pt in traj.points[:-1]:
if not cartesian:
self.send_q(pt.positions, acceleration, velocity)
else:
self.send_cart(pt.positions, acceleration,
velocity) ##
self.set_goal(pt.positions)
start_t = rospy.Time.now()
rospy.sleep(
rospy.Duration(pt.time_from_start.to_sec() -
t.to_sec()))
t = pt.time_from_start
if not cartesian:
self.send_q(traj.points[-1].positions, acceleration, velocity)
else:
self.send_cart(traj.points[-1].positions, acceleration,
velocity) ##
self.set_goal(traj.points[-1].positions)
start_t = rospy.Time.now()
# wait until robot is at goal
#while self.moving:
while not self.at_goal and self.valid_verify(stamp):
if (rospy.Time.now() - start_t).to_sec() > 3:
return 'FAILURE - timeout'
rate.sleep()
if self.at_goal:
return 'SUCCESS - moved to pose'
else:
return 'FAILURE - did not reach destination'
else:
self.set_goal(traj.points[-1].positions)
goal = FollowJointTrajectoryGoal(trajectory=traj)
for i, pt in enumerate(traj.points):
print("Pt =", i, pt.positions)
if self.valid_verify(stamp):
self.client.send_goal(goal)
# max time before returning = 30 s
done = False
#while self.valid_verify(stamp) and not done:
rospy.loginfo("Waiting for UR...")
self.client.wait_for_result() #rospy.Duration.from_sec(1.0))
rospy.loginfo("Done waiting for UR.")
else:
return "FAILURE - preempted before trajectory sent"
# Check to make sure we weren't preempted.
if not self.valid_verify(stamp):
#rospy.logerr('verify failed:'+ str(stamp) + ", " + str(self.cur_stamp))
res = None
else:
#rospy.logerr('verify succeeded:'+ str(stamp) + ", " + str(self.cur_stamp))
res = self.client.get_result()
if res is not None and res.error_code >= 0:
return "SUCCESS"
elif res is None:
return "FAILURE - UR actionlib call aborted"
else:
return "FAILURE - %s" % res.error_code
def _send_joint_trajectory(self,
joints_references,
max_joint_vel=0.7,
timeout=rospy.Duration(5)):
"""
Low level method that sends a array of joint references to the arm.
If timeout is defined, it will wait for timeout*len(joints_reference) seconds for the
completion of the actionlib goal. It will return True as soon as possible when the goal
succeeded. On timeout, it will return False.
:param joints_references:[str] list of joint configurations,
which should be a list of the length equal to the number of joints to be moved
:param max_joint_vel:(int,float,[int], [float]) speed the robot can have when getting to the desired
configuration. A single value can be given, which will be used for all joints, or a list of values can be given
in which the order has to agree with the joints according to the joints_references.
:param timeout:(secs) timeout for each joint configuration in rospy.Duration(seconds); timeout of 0.0 is not
allowed
:return: True or False
"""
if not joints_references:
return False
if len(joints_references[0]) == len(self.joint_names) + len(
self.torso_joint_names):
joint_names = self.torso_joint_names + self.joint_names
else:
joint_names = self.joint_names
if isinstance(max_joint_vel, (float, int)):
max_joint_vel = [max_joint_vel] * len(joint_names)
if isinstance(max_joint_vel, list):
if isinstance(max_joint_vel[0], (int, float)):
if len(max_joint_vel) is not len(joint_names):
rospy.logerr(
"The length of 'max_joint_vel' is {} and the length of 'joint_names' is {}. \n"
"Please give the velocities for the following joints (in the correct order!): {}"
.format(len(max_joint_vel), len(joint_names),
joint_names))
ps = []
time_from_start = 0.0
start_joint_state = self.get_joint_states()
prev_joint_ref = [start_joint_state[jn] for jn in joint_names]
for joints_reference in joints_references:
max_diff = [
abs(prev - new)
for prev, new in zip(prev_joint_ref, joints_reference)
]
if len(joints_reference) != len(joint_names):
rospy.logwarn(
'Please use the correct {} number of joint references (current = {})'
.format(len(joint_names), len(joints_references)))
time_from_start += max(x / y
for x, y in zip(max_diff, max_joint_vel))
ps.append(
JointTrajectoryPoint(
positions=joints_reference,
time_from_start=rospy.Duration.from_sec(time_from_start)))
joint_trajectory = JointTrajectory(joint_names=joint_names, points=ps)
goal = FollowJointTrajectoryGoal(trajectory=joint_trajectory,
goal_time_tolerance=timeout)
rospy.logdebug("Send {0} arm to jointcoords \n{1}".format(
self.side, ps))
import time
time.sleep(
0.001
) # This is necessary: the rtt_actionlib in the hardware seems
# to only have a queue size of 1 and runs at 1000 hz. This
# means that if two goals are send approximately at the same
# time (e.g. an arm goal and a torso goal), one of the two
# goals probably won't make it. This sleep makes sure the
# goals will always arrive in different update hooks in the
# hardware TrajectoryActionLib server.
self._ac_joint_traj.send_goal(goal)
if timeout != rospy.Duration(0):
done = self._ac_joint_traj.wait_for_result(timeout *
len(joints_references))
if not done:
rospy.logwarn("Cannot reach joint goal {0}".format(goal))
return done
else:
return False
def clear(self):
self._goal = FollowJointTrajectoryGoal()
self._goal.goal_time_tolerance = self._goal_time_tolerance
self._goal.trajectory.joint_names = ['linear_joint']
rospy.loginfo("...connected.")
rospy.loginfo("Waiting for gripper_controller...")
gripper_client = actionlib.SimpleActionClient("gripper_controller/gripper_action", GripperCommandAction)
gripper_client.wait_for_server()
rospy.loginfo("...connected.")
trajectory = JointTrajectory()
trajectory.joint_names = head_joint_names
trajectory.points.append(JointTrajectoryPoint())
trajectory.points[0].positions = head_joint_positions
trajectory.points[0].velocities = [0.0] * len(head_joint_positions)
trajectory.points[0].accelerations = [0.0] * len(head_joint_positions)
trajectory.points[0].time_from_start = rospy.Duration(5.0)
head_goal = FollowJointTrajectoryGoal()
head_goal.trajectory = trajectory
head_goal.goal_time_tolerance = rospy.Duration(0.0)
trajectory = JointTrajectory()
trajectory.joint_names = arm_joint_names
trajectory.points.append(JointTrajectoryPoint())
trajectory.points[0].positions = [0.0] * len(arm_joint_positions)
trajectory.points[0].velocities = [0.0] * len(arm_joint_positions)
trajectory.points[0].accelerations = [0.0] * len(arm_joint_positions)
trajectory.points[0].time_from_start = rospy.Duration(1.0)
trajectory.points.append(JointTrajectoryPoint())
trajectory.points[1].positions = arm_intermediate_positions
trajectory.points[1].velocities = [0.0] * len(arm_joint_positions)
trajectory.points[1].accelerations = [0.0] * len(arm_joint_positions)
trajectory.points[1].time_from_start = rospy.Duration(4.0)
def follow_trajectory(self, trajectory):
follow_goal = FollowJointTrajectoryGoal()
follow_goal.trajectory = trajectory
self.client.send_goal(follow_goal)
self.client.wait_for_result()
def __init__(self, timeout=5.0):
"""
`TrajectoryController` constructor.
Parameters
----------
timeout: float
Time in seconds to wait for the `joint_trajectory_action` server
"""
# Initialize variables
self.mutex = threading.Lock()
self.num_active_joints = 0
self.active_joint_names = []
self.active_indices = []
# Wait for the node to connect to roscore
while rospy.get_time() <= 0:
time.sleep(0.1)
# Start the trajectory action client
action_server = 'joint_trajectory_action'
self.client = actionlib.SimpleActionClient(
action_server, FollowJointTrajectoryAction)
rospy.logdebug('Waiting for [{0}] action server'.format(action_server))
server_up = self.client.wait_for_server(
timeout=rospy.Duration(timeout))
if not server_up:
rospy.logerr(
'Timed out waiting for Joint Trajectory Action Server ' +
'to connect. Start the action server before.')
msg = 'JointTrajectoryController timeout: {0}'.format(
action_server)
raise rospy.ROSException(msg)
rospy.logdebug('Successfully connected to [{0}]'.format(action_server))
# Subscribe to the joint_states topic
param = '/controller_joint_names'
self.joint_names = rospy.get_param(param, [])
if len(self.joint_names) == 0:
msg = 'Failed to find parameter: {}'.format(param)
rospy.logerr(msg)
raise rospy.ROSException(msg)
self.num_joints = len(self.joint_names)
# Initialize the joint positions to nan (not a number)
self.joint_positions = np.full(self.num_joints, np.nan, dtype=float)
rospy.Subscriber('joint_states',
JointState,
self.cb_joint_states,
queue_size=1)
rospy.logdebug('Waiting for topic: joint_states')
success = False
t0 = rospy.get_time()
while not success:
success = np.isnan(self.joint_positions).any()
if rospy.is_shutdown() or (rospy.get_time() - t0) > timeout:
break
rospy.sleep(0.01)
if not success:
msg = 'Timed out waiting for topic: joint_states'
rospy.logerr(msg)
raise rospy.ROSException(msg)
# Create the goal instance
self.goal = FollowJointTrajectoryGoal()
self.goal.trajectory.joint_names = list(self.joint_names)
# Give it some time for the connections to settle
rospy.sleep(0.5)
rospy.loginfo('TrajectoryController successfully initialized')
def __init__(self, parent, id):
the_size = (700, 570)
wx.Frame.__init__(self,
parent,
id,
'Elfin Control Panel',
pos=(250, 100))
self.panel = wx.Panel(self)
font = self.panel.GetFont()
font.SetPixelSize((12, 24))
self.panel.SetFont(font)
self.listener = tf.TransformListener()
self.robot = moveit_commander.RobotCommander()
self.scene = moveit_commander.PlanningSceneInterface()
self.group = moveit_commander.MoveGroupCommander('elfin_arm')
self.controller_ns = 'elfin_arm_controller/'
self.elfin_driver_ns = 'elfin/'
self.elfin_basic_api_ns = 'elfin_basic_api/'
self.joint_names = rospy.get_param(self.controller_ns + 'joints', [])
self.ref_link_name = self.group.get_planning_frame()
self.end_link_name = self.group.get_end_effector_link()
self.ref_link_lock = threading.Lock()
self.end_link_lock = threading.Lock()
self.js_display = [0] * 6 # joint_states
self.jm_button = [0] * 6 # joints_minus
self.jp_button = [0] * 6 # joints_plus
self.js_label = [0] * 6 # joint_states
self.ps_display = [0] * 6 # pcs_states
self.pm_button = [0] * 6 # pcs_minus
self.pp_button = [0] * 6 # pcs_plus
self.ps_label = [0] * 6 # pcs_states
self.display_init()
self.key = []
btn_height = 390
btn_lengths = []
self.power_on_btn = wx.Button(self.panel,
label=' Servo On ',
name='Servo On',
pos=(20, btn_height))
btn_lengths.append(self.power_on_btn.GetSize()[0])
btn_total_length = btn_lengths[0]
self.power_off_btn = wx.Button(self.panel,
label=' Servo Off ',
name='Servo Off')
btn_lengths.append(self.power_off_btn.GetSize()[0])
btn_total_length += btn_lengths[1]
self.reset_btn = wx.Button(self.panel,
label=' Clear Fault ',
name='Clear Fault')
btn_lengths.append(self.reset_btn.GetSize()[0])
btn_total_length += btn_lengths[2]
self.home_btn = wx.Button(self.panel, label='Home', name='home_btn')
btn_lengths.append(self.home_btn.GetSize()[0])
btn_total_length += btn_lengths[3]
self.stop_btn = wx.Button(self.panel, label='Stop', name='Stop')
btn_lengths.append(self.stop_btn.GetSize()[0])
btn_total_length += btn_lengths[4]
btn_interstice = (550 - btn_total_length) / 4
btn_pos_tmp = btn_lengths[0] + btn_interstice + 20
self.power_off_btn.SetPosition((btn_pos_tmp, btn_height))
btn_pos_tmp += btn_lengths[1] + btn_interstice
self.reset_btn.SetPosition((btn_pos_tmp, btn_height))
btn_pos_tmp += btn_lengths[2] + btn_interstice
self.home_btn.SetPosition((btn_pos_tmp, btn_height))
btn_pos_tmp += btn_lengths[3] + btn_interstice
self.stop_btn.SetPosition((btn_pos_tmp, btn_height))
self.servo_state_label = wx.StaticText(self.panel,
label='Servo state:',
pos=(590, btn_height - 10))
self.servo_state_show = wx.TextCtrl(self.panel,
style=(wx.TE_CENTER
| wx.TE_READONLY),
value='',
pos=(600, btn_height + 10))
self.servo_state = bool()
self.servo_state_lock = threading.Lock()
self.fault_state_label = wx.StaticText(self.panel,
label='Fault state:',
pos=(590, btn_height + 60))
self.fault_state_show = wx.TextCtrl(self.panel,
style=(wx.TE_CENTER
| wx.TE_READONLY),
value='',
pos=(600, btn_height + 80))
self.fault_state = bool()
self.fault_state_lock = threading.Lock()
self.reply_show_label = wx.StaticText(self.panel,
label='Result:',
pos=(20, btn_height + 120))
self.reply_show = wx.TextCtrl(self.panel,
style=(wx.TE_CENTER | wx.TE_READONLY),
value='',
size=(670, 30),
pos=(20, btn_height + 140))
link_textctrl_length = (btn_pos_tmp - 40) / 2
self.ref_links_show_label = wx.StaticText(self.panel,
label='Ref. link:',
pos=(20, btn_height + 60))
self.ref_link_show = wx.TextCtrl(self.panel,
style=(wx.TE_READONLY),
value=self.ref_link_name,
size=(link_textctrl_length, 30),
pos=(20, btn_height + 80))
self.end_link_show_label = wx.StaticText(self.panel,
label='End link:',
pos=(link_textctrl_length +
30, btn_height + 60))
self.end_link_show = wx.TextCtrl(self.panel,
style=(wx.TE_READONLY),
value=self.end_link_name,
size=(link_textctrl_length, 30),
pos=(link_textctrl_length + 30,
btn_height + 80))
self.set_links_btn = wx.Button(self.panel,
label='Set links',
name='Set links')
self.set_links_btn.SetPosition((btn_pos_tmp, btn_height + 75))
# the variables about velocity scaling
velocity_scaling_init = rospy.get_param(self.elfin_basic_api_ns +
'velocity_scaling',
default=0.4)
default_velocity_scaling = str(round(velocity_scaling_init, 2))
self.velocity_setting_label = wx.StaticText(self.panel,
label='Velocity Scaling',
pos=(20, btn_height - 70))
self.velocity_setting = wx.Slider(self.panel,
value=int(velocity_scaling_init *
100),
minValue=1,
maxValue=100,
style=wx.SL_HORIZONTAL,
size=(500, 30),
pos=(45, btn_height - 50))
self.velocity_setting_txt_lower = wx.StaticText(self.panel,
label='1%',
pos=(20,
btn_height - 45))
self.velocity_setting_txt_upper = wx.StaticText(self.panel,
label='100%',
pos=(550,
btn_height - 45))
self.velocity_setting_show = wx.TextCtrl(
self.panel,
style=(wx.TE_CENTER | wx.TE_READONLY),
value=default_velocity_scaling,
pos=(600, btn_height - 55))
self.velocity_setting.Bind(wx.EVT_SLIDER, self.velocity_setting_cb)
self.teleop_api_dynamic_reconfig_client = dynamic_reconfigure.client.Client(
self.elfin_basic_api_ns,
config_callback=self.basic_api_reconfigure_cb)
self.dlg = wx.Dialog(self.panel, title='messag')
self.dlg.Bind(wx.EVT_CLOSE, self.closewindow)
self.dlg_panel = wx.Panel(self.dlg)
self.dlg_label = wx.StaticText(self.dlg_panel,
label='hello',
pos=(15, 15))
self.set_links_dlg = wx.Dialog(self.panel,
title='Set links',
size=(400, 100))
self.set_links_dlg_panel = wx.Panel(self.set_links_dlg)
self.sld_ref_link_show = wx.TextCtrl(self.set_links_dlg_panel,
style=wx.TE_PROCESS_ENTER,
value='',
pos=(20, 20),
size=(link_textctrl_length, 30))
self.sld_end_link_show = wx.TextCtrl(self.set_links_dlg_panel,
style=wx.TE_PROCESS_ENTER,
value='',
pos=(20, 70),
size=(link_textctrl_length, 30))
self.sld_set_ref_link_btn = wx.Button(self.set_links_dlg_panel,
label='Update ref. link',
name='Update ref. link')
self.sld_set_ref_link_btn.SetPosition((link_textctrl_length + 30, 15))
self.sld_set_end_link_btn = wx.Button(self.set_links_dlg_panel,
label='Update end link',
name='Update end link')
self.sld_set_end_link_btn.SetPosition((link_textctrl_length + 30, 65))
self.set_links_dlg.SetSize(
(link_textctrl_length + self.sld_set_ref_link_btn.GetSize()[0] +
50, 120))
self.call_teleop_joint = rospy.ServiceProxy(
self.elfin_basic_api_ns + 'joint_teleop', SetInt16)
self.call_teleop_joint_req = SetInt16Request()
self.call_teleop_cart = rospy.ServiceProxy(
self.elfin_basic_api_ns + 'cart_teleop', SetInt16)
self.call_teleop_cart_req = SetInt16Request()
self.call_teleop_stop = rospy.ServiceProxy(
self.elfin_basic_api_ns + 'stop_teleop', SetBool)
self.call_teleop_stop_req = SetBoolRequest()
self.call_stop = rospy.ServiceProxy(
self.elfin_basic_api_ns + 'stop_teleop', SetBool)
self.call_stop_req = SetBoolRequest()
self.call_stop_req.data = True
self.stop_btn.Bind(
wx.EVT_BUTTON,
lambda evt, cl=self.call_stop, rq=self.call_stop_req: self.
call_set_bool_common(evt, cl, rq))
self.call_reset = rospy.ServiceProxy(
self.elfin_driver_ns + 'clear_fault', SetBool)
self.call_reset_req = SetBoolRequest()
self.call_reset_req.data = True
self.reset_btn.Bind(
wx.EVT_BUTTON,
lambda evt, cl=self.call_reset, rq=self.call_reset_req: self.
call_set_bool_common(evt, cl, rq))
self.call_power_on = rospy.ServiceProxy(
self.elfin_basic_api_ns + 'enable_robot', SetBool)
self.call_power_on_req = SetBoolRequest()
self.call_power_on_req.data = True
self.power_on_btn.Bind(
wx.EVT_BUTTON,
lambda evt, cl=self.call_power_on, rq=self.call_power_on_req: self.
call_set_bool_common(evt, cl, rq))
self.call_power_off = rospy.ServiceProxy(
self.elfin_basic_api_ns + 'disable_robot', SetBool)
self.call_power_off_req = SetBoolRequest()
self.call_power_off_req.data = True
self.power_off_btn.Bind(
wx.EVT_BUTTON,
lambda evt, cl=self.call_power_off, rq=self.call_power_off_req:
self.call_set_bool_common(evt, cl, rq))
self.call_move_homing = rospy.ServiceProxy(
self.elfin_basic_api_ns + 'home_teleop', SetBool)
self.call_move_homing_req = SetBoolRequest()
self.call_move_homing_req.data = True
self.home_btn.Bind(
wx.EVT_LEFT_DOWN,
lambda evt, cl=self.call_move_homing, rq=self.call_move_homing_req:
self.call_set_bool_common(evt, cl, rq))
self.home_btn.Bind(
wx.EVT_LEFT_UP,
lambda evt, mark=100: self.release_button(evt, mark))
self.call_set_ref_link = rospy.ServiceProxy(
self.elfin_basic_api_ns + 'set_reference_link', SetString)
self.call_set_end_link = rospy.ServiceProxy(
self.elfin_basic_api_ns + 'set_end_link', SetString)
self.set_links_btn.Bind(wx.EVT_BUTTON, self.show_set_links_dialog)
self.sld_set_ref_link_btn.Bind(wx.EVT_BUTTON, self.update_ref_link)
self.sld_set_end_link_btn.Bind(wx.EVT_BUTTON, self.update_end_link)
self.sld_ref_link_show.Bind(wx.EVT_TEXT_ENTER, self.update_ref_link)
self.sld_end_link_show.Bind(wx.EVT_TEXT_ENTER, self.update_end_link)
self.action_client = SimpleActionClient(
self.controller_ns + 'follow_joint_trajectory',
FollowJointTrajectoryAction)
self.action_goal = FollowJointTrajectoryGoal()
self.action_goal.trajectory.joint_names = self.joint_names
self.SetMinSize(the_size)
self.SetMaxSize(the_size)
