def createHandGoal(side, j1, j2, j3):
"""Creates a FollowJointTrajectoryGoal with the values specified in j1, j2 and j3 for the joint positions
with the hand specified in side
@arg side string 'right' or 'left'
@arg j1 float value for joint 'hand_'+side+'_thumb_joint'
@arg j2 float value for joint 'hand_'+side+'_middle_joint'
@arg j3 float value for joint 'hand_'+side+'_index_joint'
@return FollowJointTrajectoryGoal with the specified goal"""
fjtg = FollowJointTrajectoryGoal()
fjtg.trajectory.joint_names.append('hand_'+side+'_thumb_joint')
fjtg.trajectory.joint_names.append('hand_'+side+'_middle_joint')
fjtg.trajectory.joint_names.append('hand_'+side+'_index_joint')
point = JointTrajectoryPoint()
point.positions.append(j1)
point.positions.append(j2)
point.positions.append(j3)
point.velocities.append(0.0)
point.velocities.append(0.0)
point.velocities.append(0.0)
point.time_from_start = rospy.Duration(4.0)
fjtg.trajectory.points.append(point)
for joint in fjtg.trajectory.joint_names: # Specifying high tolerances for the hand as they are slow compared to other hardware
goal_tol = JointTolerance()
goal_tol.name = joint
goal_tol.position = 5.0
goal_tol.velocity = 5.0
goal_tol.acceleration = 5.0
fjtg.goal_tolerance.append(goal_tol)
fjtg.goal_time_tolerance = rospy.Duration(3)
fjtg.trajectory.header.stamp = rospy.Time.now()
return fjtg
def __init__(self):
rospy.init_node('arm_simple_trajectory')
# Set to True to move back to the starting configurations
reset = rospy.get_param('~reset', False)
# Which joints define the arm?
arm_joints = ['joint_lift',
'joint_waist',
'joint_big_arm',
'joint_forearm',
'joint_wrist_pitching',
'joint_wrist_rotation']
if reset:
# Set the arm back to the resting position
arm_goal = [0.0, 0.0, 0.0, 0.0, 0.0, 0.0]
else:
# Set a goal configuration for the arm
arm_goal = [1.0, 1.0, 1.0, 1.0, 0.0, 0.0]
# Connect to the right arm trajectory action server
rospy.loginfo('Waiting for xm arm trajectory controller...')
arm_client = actionlib.SimpleActionClient('xm_arm_controller/follow_joint_trajectory',
FollowJointTrajectoryAction)
arm_client.wait_for_server()
rospy.loginfo('...connected.')
rospy.sleep(1)
# 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)
# Send the trajectory to the arm action server
rospy.loginfo('Moving the arm to goal position...')
rospy.sleep(1)
# 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)
# Send the goal to the action server
arm_client.send_goal(arm_goal)
rospy.loginfo('...done')
rospy.sleep(1)
def move_arm_joints(client, arm_joint_positions, duration=5.0):
trajectory = JointTrajectory()
trajectory.joint_names = ARM_JOINT_NAMES
trajectory.points.append(JointTrajectoryPoint())
trajectory.points[0].positions = 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(duration)
arm_goal = FollowJointTrajectoryGoal()
arm_goal.trajectory = trajectory
arm_goal.goal_time_tolerance = rospy.Duration(0.0)
print("Moving amr to joints: ", arm_joint_positions)
client.send_goal(arm_goal)
client.wait_for_result(rospy.Duration(duration+1.0))
rospy.loginfo("...done")
def __init__(self):
rospy.init_node('trajectory_demo')
# 是否需要回到初始位置
reset = rospy.get_param('~reset', False)
arm_joints = [
'joint1', 'joint2', 'joint3', 'joint4', 'joint5', 'joint6'
]
# 调整目标位置
if reset:
arm_goal = [0, 0, 0, 0, 0, 0]
else:
arm_goal = [-0.3, -1.0, 0.5, 0.8, 1.0, -0.7]
# 连接机械臂轨迹规划的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.')
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)
rospy.loginfo('Moving the arm to goal position...')
arm_goal = FollowJointTrajectoryGoal()
arm_goal.trajectory = arm_trajectory
arm_goal.goal_time_tolerance = rospy.Duration(0.0)
arm_client.send_goal(arm_goal)
arm_client.wait_for_result(rospy.Duration(5.0))
rospy.loginfo('...done')
def tiltWristGoal(beginPositions, endPositions):
trajectory = JointTrajectory()
trajectory.joint_names = armJointNames
trajectory.points.append(JointTrajectoryPoint())
trajectory.points[0].positions = beginPositions
trajectory.points[0].velocities = [0.0] * len(beginPositions)
trajectory.points[0].accelerations = [0.0] * len(beginPositions)
trajectory.points[0].time_from_start = rospy.Duration(0.0)
trajectory.points.append(JointTrajectoryPoint())
trajectory.points[1].positions = endPositions
trajectory.points[1].velocities = [0.0] * len(endPositions)
trajectory.points[1].accelerations = [0.0] * len(endPositions)
trajectory.points[1].time_from_start = rospy.Duration(2.0)
armGoal = FollowJointTrajectoryGoal()
armGoal.trajectory = trajectory
armGoal.goal_time_tolerance = rospy.Duration(0.0)
return armGoal
def publish_goal():
action_client = True
client = actionlib.SimpleActionClient(
'/revel_trajectory_controller/follow_joint_trajectory',
FollowJointTrajectoryAction)
pub = rospy.Publisher('/revel_trajectory_controller/command',
JointTrajectory,
queue_size=10)
rospy.init_node('trajectory_tester', anonymous=True)
traj = JointTrajectory()
traj.joint_names.append("joint_1")
traj.joint_names.append("joint_2")
traj.joint_names.append("joint_3")
traj.joint_names.append("joint_4")
traj.joint_names.append("joint_5")
traj.joint_names.append("joint_6")
jtp = JointTrajectoryPoint()
jtp.positions.append(0.0)
jtp.positions.append(0.0)
jtp.positions.append(0.0)
jtp.positions.append(0.0)
jtp.positions.append(0.0)
jtp.positions.append(0.0)
jtp.velocities.append(1.0)
jtp.velocities.append(1.0)
jtp.velocities.append(1.0)
jtp.velocities.append(1.0)
jtp.velocities.append(1.0)
jtp.velocities.append(1.0)
jtp.time_from_start = rospy.Duration(0.0)
traj.points.append(jtp)
traj.header.stamp = rospy.Time.now()
if not action_client:
pub.publish(traj)
else:
goal = FollowJointTrajectoryGoal()
goal.trajectory = traj
goal.goal_time_tolerance = rospy.Duration(4.0)
client.wait_for_server()
print 'Found server'
client.send_goal(goal)
def execute_trajectory(path):
#Creates the SimpleActionClient, passing the type of the action
client = actionlib.SimpleActionClient('joint_trajectory_action',
FollowJointTrajectoryAction)
#Waits until the action server has started up and started listening for goals.
client.wait_for_server()
#Creates a goal to send to the action server.
goal = FollowJointTrajectoryGoal()
goal.trajectory = path
goal.goal_time_tolerance = rospy.Duration(0.01)
#Sends the goal to the action server.
client.send_goal(goal)
#Waits for the server to finish performing the action.
client.wait_for_result()
#Returns out the result of executing the action
return client.get_result()
def brazo():
print("Initializing node... ")
#rospy.init_node("joint_trajectory_client_example")
rospy.sleep(1)
print("Running. Ctrl-c to quit")
positions = {
'larm_shoulder_p': -1.55,
'larm_shoulder_r': 1.55,
'larm_elbow_p': -1.0,
'larm_gripper': 0.0,
}
client = actionlib.SimpleActionClient(
'/fullbody_controller/follow_joint_trajectory',
FollowJointTrajectoryAction,
)
if not client.wait_for_server(timeout=rospy.Duration(10)):
rospy.logerr("Timed out waiting for Action Server")
rospy.signal_shutdown("Timed out waiting for Action Server")
sys.exit(1)
# init goal
goal = FollowJointTrajectoryGoal()
goal.goal_time_tolerance = rospy.Time(1)
goal.trajectory.joint_names = positions.keys()
# points
point = JointTrajectoryPoint()
goal.trajectory.joint_names = positions.keys()
point.positions = positions.values()
point.time_from_start = rospy.Duration(10)
goal.trajectory.points.append(point)
# send goal
goal.trajectory.header.stamp = rospy.Time.now()
client.send_goal(goal)
print(goal)
print("waiting...")
if not client.wait_for_result(timeout=rospy.Duration(20)):
rospy.logerr("Timed out waiting for JTA")
rospy.loginfo("Exitting...")
def test_scaled_trajectory(self):
"""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.client.send_goal(goal)
self.client.wait_for_result()
self.assertEqual(
self.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.client.send_goal(goal)
self.client.wait_for_result()
self.assertEqual(
self.client.get_result().error_code, FollowJointTrajectoryResult.GOAL_TOLERANCE_VIOLATED
)
rospy.loginfo("Received result GOAL_TOLERANCE_VIOLATED")
def setHeadTilt(self):
head_joint_names = ["head_pan_joint", "head_tilt_joint"]
head_joint_positions = [0.0, 0.4]
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 for _ in head_joint_positions]
trajectory.points[0].accelerations = [
0.0 for _ in head_joint_positions]
trajectory.points[0].time_from_start = rospy.Duration(0.0)
head_goal = FollowJointTrajectoryGoal()
head_goal.trajectory = trajectory
head_goal.goal_time_tolerance = rospy.Duration(0.0)
rospy.loginfo("Setting Head positions...")
self.head_client.send_goal(head_goal)
self.head_client.wait_for_result(rospy.Duration(10.0))
rospy.loginfo("...done")
def tuck_arm(self):
while not self.state_recv:
rospy.loginfo("Waiting for controllers to be up...")
rospy.sleep(0.1)
trajectory = JointTrajectory()
trajectory.joint_names = self.joint_names
trajectory.points.append(JointTrajectoryPoint())
trajectory.points[0].positions = self.tucked
trajectory.points[0].velocities = [0.0 for i in self.joint_names]
trajectory.points[0].accelerations = [0.0 for i in self.joint_names]
trajectory.points[0].time_from_start = rospy.Duration(5.0)
rospy.loginfo("Tucking arm...")
goal = FollowJointTrajectoryGoal()
goal.trajectory = trajectory
goal.goal_time_tolerance = rospy.Duration(0.0)
self.client.send_goal(goal)
self.client.wait_for_result(rospy.Duration(6.0))
rospy.loginfo("...done")
def test_joint_passthrough(self):
#### Power cycle the robot in order to make sure it is running correctly####
self.assertTrue(self.set_robot_to_mode(RobotMode.POWER_OFF))
rospy.sleep(0.5)
self.assertTrue(self.set_robot_to_mode(RobotMode.RUNNING))
rospy.sleep(0.5)
self.send_program_srv.call()
rospy.sleep(0.5) # TODO properly wait until the controller is running
self.switch_on_controller("forward_joint_traj_controller")
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, -1.57, -1.57, 0, 0, 0]]
position_list.append([0.2, -1.57, -1.57, 0, 0, 0])
position_list.append([-0.5, -1.57, -1.2, 0, 0, 0])
duration_list = [3.0, 7.0, 10.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)
for i, joint_name in enumerate(goal.trajectory.joint_names):
goal.goal_tolerance.append(
JointTolerance(joint_name, 0.2, 0.2, 0.2))
goal.goal_time_tolerance = rospy.Duration(0.6)
self.joint_passthrough_trajectory_client.send_goal(goal)
self.joint_passthrough_trajectory_client.wait_for_result()
self.assertEqual(
self.joint_passthrough_trajectory_client.get_result().error_code,
FollowJointTrajectoryResult.SUCCESSFUL)
rospy.loginfo("Received result SUCCESSFUL")
def execute_cb(self, goal):
"""
:type goal: FollowJointTrajectoryGoal
"""
rospy.loginfo("Got a goal!")
# Just resend the goal to the real as
# while checking for cancel goals
# Create goal
g = FollowJointTrajectoryGoal()
# We ignore path_tolerance and goal_tolerance... does not make sense in
# a gripper
g.goal_time_tolerance = goal.goal_time_tolerance
jt = self.substitute_trajectory(goal.trajectory)
g.trajectory = jt
if not self._ac.wait_for_server(rospy.Duration(5.0)):
rospy.logerr("Action server for gripper not found!")
self._as.set_aborted(FollowJointTrajectoryResult())
# Send goal subscribing to feedback to republish it
self._ac.send_goal(g, feedback_cb=self._feedback_cb)
# wait for goal to finish while checking if cancel is requested
while not self._ac.wait_for_result(rospy.Duration(0.1)):
rospy.loginfo("Waiting for goal to finish...")
# Deal with goal cancelled
if self._as.is_preempt_requested():
self._ac.cancel_all_goals()
self._as.set_preempted()
return
result = self._ac.get_result()
res = FollowJointTrajectoryResult()
if result:
res.error_code = FollowJointTrajectoryResult.SUCCESSFUL
self._as.set_succeeded(res)
else:
res.error_code = result
self._as.set_aborted(res)
def __init__(self):
rospy.init_node('trajectory_demo')
reset = rospy.get_param('~reset', False)
arm_joints = [
'joint_1', 'joint_2', 'joint_3', 'joint_4', 'joint_5', 'joint_6'
]
if reset:
arm_goal = [0, 0, 0, 0, 0, 0]
else:
arm_goal = [0.09, 0.26, 1.02, 0, -1.14, 3.24]
rospy.loginfo('Waiting for arm trajectory controller...')
arm_client = actionlib.SimpleActionClient('/joint_trajectory_action',
FollowJointTrajectoryAction)
arm_client.wait_for_server()
rospy.loginfo('...connected.')
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)
rospy.loginfo('Moving the arm to goal position...')
goal = FollowJointTrajectoryGoal()
goal.trajectory = arm_trajectory
goal.goal_time_tolerance = rospy.Duration(0.0)
arm_client.send_goal(goal)
arm_client.wait_for_result(rospy.Duration(5.0))
rospy.loginfo('...done')
def main(filename):
print("Initializing node... ")
#rospy.init_node("joint_trajectory_client_csv_example")
rospy.sleep(1)
print("Running. Ctrl-c to quit")
# init goal
goal = FollowJointTrajectoryGoal()
goal.goal_time_tolerance = rospy.Time(1)
with open(filename) as f:
reader = csv.reader(f, skipinitialspace=True)
first_row = True
for row in reader:
if first_row:
goal.trajectory.joint_names = row[1:]
first_row = False
else:
point = JointTrajectoryPoint()
point.positions = [float(n) for n in row[1:]]
point.time_from_start = rospy.Duration(float(row[0]))
goal.trajectory.points.append(point)
client = actionlib.SimpleActionClient(
'/fullbody_controller/follow_joint_trajectory',
FollowJointTrajectoryAction,
)
if not client.wait_for_server(timeout=rospy.Duration(10)):
rospy.logerr("Timed out waiting for Action Server")
rospy.signal_shutdown("Timed out waiting for Action Server")
sys.exit(1)
# send goal
goal.trajectory.header.stamp = rospy.Time.now()
client.send_goal(goal)
print("waiting...")
if not client.wait_for_result(timeout=rospy.Duration(60)):
rospy.logerr("Timed out waiting for JTA")
rospy.loginfo("Exitting...")
def move_arm_joints_v2(client, arm_joint_positions_list):
trajectory = JointTrajectory()
trajectory.joint_names = ARM_JOINT_NAMES
duration = 0.0
wait_time = np.random.uniform(3.0, 5.0)
for i, a_joint_pose in enumerate(arm_joint_positions_list):
duration += np.random.uniform(3.0, 5.0)
trajectory.points.append(JointTrajectoryPoint())
trajectory.points[i].positions = a_joint_pose
trajectory.points[i].velocities = [0.0] * len(a_joint_pose)
trajectory.points[i].accelerations = [0.0] * len(a_joint_pose)
trajectory.points[i].time_from_start = rospy.Duration(duration)
arm_goal = FollowJointTrajectoryGoal()
arm_goal.trajectory = trajectory
arm_goal.goal_time_tolerance = rospy.Duration(0.0)
print("Moving amr to joints: ", arm_joint_positions_list)
client.send_goal(arm_goal)
client.wait_for_result(rospy.Duration(len(arm_joint_positions_list) * wait_time))
rospy.loginfo("...done")
def execute(self,
trajectory,
done_cb=None,
active_cb=None,
feedback_cb=None):
"""
:type trajectory: trajectory_msgs/JointTrajectory
:type done_cb: function
:type active_cb: function
:type feedback_cb: function
"""
if not self._connected:
rospy.logwarn_throttle(
1,
"TrajectoryAsyncExecuter: Client failed to connect to the action server .. nothing will be executed!"
)
return False
goal = FollowJointTrajectoryGoal()
goal.goal_time_tolerance = self._goal_time_tolerance
goal.trajectory = trajectory
self._client.send_goal(goal, done_cb, active_cb, feedback_cb)
def __init__(self):
rospy.init_node('trajectory_demo')
reset = rospy.get_param('~reset', False)
arm_joints = [
'base_to_armA', 'armA_to_armB', 'armB_to_armC', 'armC_to_armD'
]
if reset:
arm_goal = [0, 0, 0, 0]
else:
arm_goal = [-0.3, -1.0, 0.5, 0.8]
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.')
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 for i in arm_joints]
arm_trajectory.points[0].accelerations = [0 for i in arm_joints]
arm_trajectory.points[0].time_from_start = rospy.Duration(3)
rospy.loginfo("Moving the arm to goal position..")
arm_goal = FollowJointTrajectoryGoal()
arm_goal.trajectory = arm_trajectory
arm_goal.goal_time_tolerance = rospy.Duration(0)
arm_client.send_goal(arm_goal)
arm_client.wait_for_result(rospy.Duration(5))
rospy.loginfo('Done.')
def enviarTrayectoria(self, arm_goal1):
# arm_goal = [0, 0, 0, 0, 0, 0]
# arm_goal = [-0.3, 0.5, -1.0, 1.8, 0.3, 0.6]
arm_joints = [
'joint_1', 'joint_2', 'joint_3', 'joint_4', 'joint_5', 'joint_6'
]
# Connect to the right arm trajectory action server
rospy.loginfo('Waiting for right arm trajectory controller...')
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_goal1
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(1.5)
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
self.arm_client.send_goal(arm_goal)
# Wait for up to 5 seconds for the motion to complete
self.arm_client.wait_for_result(rospy.Duration(5.0))
self.actualizar_IK(arm_goal1)
rospy.loginfo('...done')
def goal_gripper(self, pos):
if pos == "close":
self.a2 = -1.6
self.b2 = 1.6
else:
self.a2 = 0
self.b2 = 0
trajectory = JointTrajectory()
trajectory.joint_names = self.joint_names1
trajectory.points.append(JointTrajectoryPoint())
trajectory.points[0].positions = [self.a2, self.b2]
trajectory.points[0].time_from_start = rospy.Duration(0.001)
goal = FollowJointTrajectoryGoal()
goal.trajectory = trajectory
goal.goal_time_tolerance = rospy.Duration(0.0)
self.client1.send_goal(goal)
while True:
if abs(self.g_0 - self.a2) < 0.01 and abs(self.h_0 -
self.b2) < 0.01:
break
def main():
jta = actionlib.SimpleActionClient('/arm_1/arm_controller/follow_joint_trajectory', FollowJointTrajectoryAction)
jta.wait_for_server()
goal = FollowJointTrajectoryGoal()
goal.trajectory.joint_names = ['arm_joint_1', 'arm_joint_2','arm_joint_3','arm_joint_4', 'arm_joint_5']
goal.goal_time_tolerance = rospy.Time(0.3)
point = []
for i in range(0, 4):
point.append(JointTrajectoryPoint())
point[0].positions =[0.1, 0.1, -0.1, 0.1, 0.1]
point[0].velocities = [0,0,0,0,0]
point[0].accelerations = [0,0,0,0,0]
point[0].time_from_start = rospy.Duration(0.0)
goal.trajectory.points.append(point[0])
point[1].positions =[3.14, 1.57, -1.57, 1.57, 1.57]
point[1].velocities = [0,0,0,0,0]
point[1].accelerations = [0,0,0,0,0]
point[1].time_from_start = rospy.Duration(6)
goal.trajectory.points.append(point[1])
point[2].positions =[2, 2, -2, 2, 2]
point[2].velocities = [0,0,0,0,0]
point[2].accelerations = [0,0,0,0,0]
point[2].time_from_start = rospy.Duration(12.0)
goal.trajectory.points.append(point[2])
point[3].positions =[0.4, 0.4, -0.4, 0.4, 0.4]
point[3].velocities = [0,0,0,0,0]
point[3].accelerations = [0,0,0,0,0]
point[3].time_from_start = rospy.Duration(18.0)
goal.trajectory.points.append(point[3])
jta.send_goal_and_wait(goal)
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 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 receiverLoop(self):
while True:
print "self.conn.recv()"
input_msg = self.conn.recv()
print input_msg
goal = FollowJointTrajectoryGoal()
goal.goal_time_tolerance = input_msg.goal_time_tolerance
goal.goal_tolerance = input_msg.goal_tolerance
goal.path_tolerance = input_msg.path_tolerance
goal.trajectory.header = input_msg.trajectory.header
goal.trajectory.joint_names = input_msg.joint_names
for input_point in input_msg.trajectory.points:
p = JointTrajectoryPoint()
p.accelerations = input_point.accelerations
p.positions = input_point.positions
p.velocities = input_point.velocities
p.time_from_start = input_point.time_from_start
goal.trajectory.points.append(p)
print "Goal will be:"
print goal
#self.right_arm_as.send_goal(goal)
rospy.loginfo("Waiting for result")
self.right_arm_as.wait_for_result()
rospy.loginfo("Goal done")
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 gripper_final_action(self):
self.timer = 0
print "in gripper_final_action... "
service_result = self.joint_config_client(0, 0, 0.0, 1.0)
# print "sevice ", service_result
pose_difference = np.array([[1, 0, 0, 0], [0, 1, 0, 0],
[0, 0, 1, -0.22], [0, 0, 0, 1]])
final_pose_T = np.dot(self.base_target_T, pose_difference)
# block of interpolation between current_pose and final_pose of ee
print "final_pose_T : ", final_pose_T
final_q_ik = self.kdl_kin.inverse(
final_pose_T,
np.array(self.end_eff_current_pose) + 0.3)
# print "final_q_ik : ", np.shape(final_q_ik), type(final_q_ik)
if final_q_ik is None:
print "The target could not be reached!"
return
# actionlib commands
self.trajectory.points[0].positions = final_q_ik
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)
goal = FollowJointTrajectoryGoal()
goal.trajectory = self.trajectory
goal.goal_time_tolerance = rospy.Duration(0.0)
self.manipulator_client.send_goal(goal)
self.manipulator_client.wait_for_result()
service_result = self.joint_config_client(0, 0, 1.0, 1.0)
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)
trajectory.points.append(JointTrajectoryPoint())
trajectory.points[2].positions = arm_joint_positions
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 __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 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):
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):
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')
