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_to(self, result_trajectory, duration=5.0):
trajectory = result_trajectory
follow_goal = FollowJointTrajectoryGoal()
follow_goal.trajectory = trajectory
self.client.send_goal(follow_goal)
self.client.wait_for_result()
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_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))
rospy.loginfo('...done')
def execute_trajectory(self, trajectory, duration=5.0):
follow_goal = FollowJointTrajectoryGoal()
follow_goal.trajectory = trajectory
self.client.send_goal(follow_goal)
#rospy.sleep(1.5)
#self.client.cancel_goal()
self.client.wait_for_result()
self.finish_time = rospy.Time.now()
self.believed_state = trajectory.points[-1].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)
# Which joints define the arm?
arm_joints = ['link_1_joint',
'servo_2_joint',
'link_3_joint',
'link_4_joint',
'link_5_joint']
if reset:
# Set the arm back to the resting position
arm_goal = [0, 0, 0, 0, 0]
else:
# Set a goal configuration for the arm
arm_goal = [-0.525487, 0.904972, 0.480017, -1.331166, 0.731413]
# 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.')
# 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)
rospy.loginfo('...done')
def move_to(self, positions, duration=5.0):
print "============= Moving"
trajectory = JointTrajectory()
trajectory.joint_names = self.joint_names
trajectory.points.append(JointTrajectoryPoint())
trajectory.points[0].positions = positions
trajectory.points[0].velocities = [0.0 for _ in positions]
trajectory.points[0].accelerations = [0.0 for _ in positions]
trajectory.points[0].time_from_start = rospy.Duration(duration)
follow_goal = FollowJointTrajectoryGoal()
follow_goal.trajectory = trajectory
def build_follow_trajectory(self, traj):
# Build 'Follow Joint Trajectory' goal from trajectory (includes tolerances for error)
traj_goal = FollowJointTrajectoryGoal()
traj_goal.trajectory = traj
tolerance = JointTolerance()
tolerance.position = 0.05
tolerance.velocity = 0.1
traj_goal.path_tolerance = [tolerance for i in range(len(traj.points))]
traj_goal.goal_tolerance = [tolerance for i in range(len(traj.points))]
traj_goal.goal_time_tolerance = rospy.Duration(3)
return traj_goal
def btnPlay(self):
if not self.ui.chkAllMotors.isChecked():
print("all motors should have torque")
return
if self.ui.spinSetFrame.value()>self.ui.spinPlayTo.value() or self.ui.spinSetFrame.value()<1 or self.ui.spinPlayTo.value()>self.totalFrames:
print("\nwrong range in play\n")
return
trajectories=[]
for cat in self.motorTrajectoryTopics:
jointTraj=JointTrajectory()
jts_names=[]
for name in self.names:
#if self.motorNamesAndCat[name]==cat and name!='l_hip_yaw':
if self.motorNamesAndCat[name]==cat:
jointTraj.joint_names.append(name+"_joint")
jts_names.append(name)
fcount=self.ui.spinPlayTo.value()-self.ui.spinSetFrame.value()+1
totalDelay=0.0
for n in range(0,fcount):
pos_arr=JointTrajectoryPoint()
readLoc=n+self.ui.spinSetFrame.value()
delaySecs=float(self.TimeDelayFromPrevious[readLoc-1])/1000.0
totalDelay=totalDelay+delaySecs
pos_arr.time_from_start=rospy.Duration().from_sec(totalDelay)
for name in jts_names:
pos_arr.positions.append(self.TrajectoryInfo[name][readLoc-1])
print("\n "+name+": "+str(self.TrajectoryInfo[name][readLoc-1]))
velocity=1.0#2*pi rad/sec
if n!=0:
velocity=float(self.TrajectoryInfo[name][readLoc-1]-self.TrajectoryInfo[name][readLoc-2])/delaySecs
##pos_arr.velocities.append(velocity)
jointTraj.points.append(pos_arr)
jointTraj.header.stamp=rospy.Time.now()
trajectories.append(jointTraj)
trajClients=[]
mm=0
for cat in self.motorTrajectoryTopics:
trajClients.append(actionlib.SimpleActionClient(self.motorTrajectoryTopics[cat],FollowJointTrajectoryAction))
trajClients[mm].wait_for_server()
mm=mm+1
#trajectories[nn] .. how to send this goal
nn=0
for traj in trajClients:
goal=FollowJointTrajectoryGoal()
#goal.trajectory.points=trajectories[nn].points
goal.trajectory=trajectories[nn]
traj.send_goal(goal)
nn=nn+1
print ("\nDone Sending Play Info...\n")
def execute(self, trajectory, test=None, epsilon=0.1):
"""
Safely executes a trajectory in joint space on the robot or simulate through RViz and its Moveit plugin (File moveit.rviz must be loaded into RViz)
This method is BLOCKING until the command succeeds or failure occurs i.e. the user interacted with the cuff or collision has been detected
Non-blocking needs should deal with the JointTrajectory action server
:param trajectory: either a RobotTrajectory or a JointTrajectory
:param test: pointer to a function that returns True if execution must stop now. /!\ Should be fast, it will be called at 100Hz!
:param epsilon: distance threshold on the first point. If distance with first point of the trajectory is greater than espilon execute a controlled trajectory to the first point. Put float(inf) value to bypass this functionality
:return: True if execution ended successfully, False otherwise
"""
def distance_to_first_point(point):
joint_pos = np.array(point.positions)
return np.linalg.norm(current_array - joint_pos)
self.display(trajectory)
with self._stop_lock:
self._stop_reason = ''
if isinstance(trajectory, RobotTrajectory):
trajectory = trajectory.joint_trajectory
elif not isinstance(trajectory, JointTrajectory):
raise TypeError("Execute only accept RobotTrajectory or JointTrajectory")
ftg = FollowJointTrajectoryGoal()
ftg.trajectory = trajectory
# check if it is necessary to move to the first point
current = self.get_current_state()
current_array = np.array([current.joint_state.position[current.joint_state.name.index(joint)] for joint in trajectory.joint_names])
if distance_to_first_point(trajectory.points[0]) > epsilon:
# convert first point to robot state
rs = RobotState()
rs.joint_state.name = trajectory.joint_names
rs.joint_state.position = trajectory.points[0].positions
# move to the first point
self.move_to_controlled(rs)
# execute the input trajectory
self.client.send_goal(ftg)
# Blocking part, wait for the callback or a collision or a user manipulation to stop the trajectory
while self.client.simple_state != SimpleGoalState.DONE:
if callable(test) and test():
self.client.cancel_goal()
return True
if self._stop_reason!='':
self.client.cancel_goal()
return False
self._rate.sleep()
return True
def __init__(self):
rospy.init_node("gripper_trajectory_demo")
# Set to True to move back to the starting configurations
reset = rospy.get_param("~reset", False)
# Set the duration of the trajectory in seconds
duration = rospy.get_param("~duration", 2.0)
# Which joints define the head?
gripper_joints = ["right_gripper_finger_joint"]
# Set a goal configuration for the head
if reset:
gripper_goal = [0]
else:
gripper_goal = [0.25]
# Connect to the head trajectory action server
rospy.loginfo("Waiting for gripper trajectory controller...")
gripper_client = actionlib.SimpleActionClient(
"right_gripper_controller/follow_joint_trajectory", FollowJointTrajectoryAction
)
gripper_client.wait_for_server()
rospy.loginfo("...connected.")
# Create a head trajectory with a single end point using the gripper_goal positions
trajectory = JointTrajectory()
trajectory.joint_names = gripper_joints
trajectory.points.append(JointTrajectoryPoint())
trajectory.points[0].positions = gripper_goal
trajectory.points[0].velocities = [0.0 for i in gripper_joints]
trajectory.points[0].accelerations = [0.0 for i in gripper_joints]
trajectory.points[0].time_from_start = rospy.Duration(duration)
# Send the trajectory to the head action server
rospy.loginfo("Moving the gripper to goal position...")
goal = FollowJointTrajectoryGoal()
goal.trajectory = trajectory
goal.goal_time_tolerance = rospy.Duration(0.0)
# Send the goal
gripper_client.send_goal(goal)
# Wait for up to 5 seconds for the motion to complete
gripper_client.wait_for_result(rospy.Duration(5.0))
rospy.loginfo("...done")
def move_to_start(self):
self.moving_mode = True
# Look down
goal = PointHeadGoal()
goal.target.header.stamp = rospy.Time.now()
goal.target.header.frame_id = '/base_link'
goal.target.point.x = 1.5
goal.target.point.y = 0.0
goal.target.point.z = -0.2
goal.min_duration = rospy.Duration(0.5)
if 0: logger.info('Point head to %s...', goal);
self.head_point_client.send_goal(goal)
if 0: logger.info('Point head sent')
goal = GripperCommandGoal()
goal.command.max_effort = 60
goal.command.position = 0.1
self.gripper_client.send_goal(goal)
joints = [
'shoulder_pan_joint',
'shoulder_lift_joint',
'upperarm_roll_joint',
'elbow_flex_joint',
'forearm_roll_joint',
'wrist_flex_joint',
'wrist_roll_joint',
]
pose = [0.0, +0.8, 0.0, -0.8, 0.0, 0.0, 0.0]
result = self.arm_move_group.moveToJointPosition(joints, pose, plan_only=True)
if result.error_code.val == MoveItErrorCodes.SUCCESS:
if 0: logger.info('Got trajectory %s', result.planned_trajectory)
follow_goal = FollowJointTrajectoryGoal()
follow_goal.trajectory = result.planned_trajectory.joint_trajectory
logger.info('sending trajectory to arm...')
self.arm_trajectory_client.send_goal(follow_goal)
result = self.arm_trajectory_client.wait_for_result()
logger.info('arm followed trajectory %s', result)
else:
logger.warn('moveToJointPosition returned %s', result)
return
result = self.head_point_client.wait_for_result()
logger.info('head followed trajectory %s', result)
logger.info('sending empty arm goal')
empty_goal = FollowJointTrajectoryGoal()
self.arm_trajectory_client.send_goal(empty_goal)
logger.info('Point head done')
self.moving_mode = False
def get_hand_goal(goal_positions):
grasp_msg = FollowJointTrajectoryGoal()
point1 = JointTrajectoryPoint(
positions=goal_positions,
velocities=[0.0, 0.0, 0.0],
time_from_start = rospy.Duration(1))
grasp_msg.trajectory = JointTrajectory(
joint_names = ['hand_right_thumb_joint', 'hand_right_index_1_joint', 'hand_right_middle_1_joint'],
points = [point1])
return grasp_msg
def execute_traj_moveit(self, waypoints):
# Cycle through waypoints
for point in waypoints:
plannedTraj = self.arm_planner.plan_jointTargetInput(point)
if plannedTraj == None or len(plannedTraj.joint_trajectory.points) < 1:
print "Error: no plan found"
return -1
else:
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()
return 1
def __init__(self):
rospy.init_node('head_trajectory_demo')
# Set to True to move back to the starting configurations
reset = rospy.get_param('~reset', False)
# Set the duration of the trajectory in seconds
duration = rospy.get_param('~duration', 3.0)
# Which joints define the head?
head_joints = ['head_pan_joint', 'head_tilt_joint']
# Set a goal configuration for the head
if reset:
head_goal = [0, 0]
else:
head_goal = [-1.3, -0.3]
# 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 head trajectory with a single end point using the head_goal positions
trajectory = JointTrajectory()
trajectory.joint_names = head_joints
trajectory.points.append(JointTrajectoryPoint())
trajectory.points[0].positions = head_goal
trajectory.points[0].velocities = [0.0 for i in head_joints]
trajectory.points[0].accelerations = [0.0 for i in head_joints]
trajectory.points[0].time_from_start = rospy.Duration(duration)
# Send the trajectory to the head action server
rospy.loginfo('Moving the head to goal position...')
goal = FollowJointTrajectoryGoal()
goal.trajectory = trajectory
goal.goal_time_tolerance = rospy.Duration(0.0)
# Send the goal
head_client.send_goal(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 on_enter(self, userdata):
self._failed = False
# create goal
goal = FollowJointTrajectoryGoal()
goal.trajectory = userdata.joint_trajectory
goal.trajectory.header.stamp = rospy.Time.now() + rospy.Duration(0.2)
# execute the motion
try:
self._client.send_goal(self._action_topic, goal)
except Exception as e:
Logger.logwarn('Was unable to execute joint trajectory:\n%s' % str(e))
self._failed = True
def move_to(self, positions, duration=5.0):
if len(self.joint_names) != len(positions):
print("Invalid trajectory position")
return False
trajectory = JointTrajectory()
trajectory.joint_names = self.joint_names
trajectory.points.append(JointTrajectoryPoint())
trajectory.points[0].positions = positions
trajectory.points[0].velocities = [0.0 for _ in positions]
trajectory.points[0].accelerations = [0.0 for _ in positions]
trajectory.points[0].time_from_start = rospy.Duration(duration)
follow_goal = FollowJointTrajectoryGoal()
follow_goal.trajectory = trajectory
self.client.send_goal(follow_goal)
self.client.wait_for_result()
def move_to_initial(self, positions, duration):
assert(len(self.joint_names) == len(positions))
trajectory = JointTrajectory()
trajectory.joint_names = self.joint_names
p = JointTrajectoryPoint()
p.positions = positions
p.time_from_start = rospy.Duration(duration)
trajectory.points.append(p)
follow_goal = FollowJointTrajectoryGoal()
follow_goal.trajectory = trajectory
self.client.send_goal(follow_goal)
self.client.wait_for_result()
def __init__(self):
rospy.init_node('trajectory_demo')
# Which joints define the arm?
arm_joints = ['x_joint']
# Set the arm back to the resting position
arm_goal = [2]
# Connect to the right arm trajectory action server
rospy.loginfo('Waiting for right arm trajectory controller...')
arm_client = actionlib.SimpleActionClient('follow_joint_trajectory', FollowJointTrajectoryAction)
arm_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]
arm_trajectory.points[0].accelerations = [0.0]
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)
# Wait for up to 5 seconds for the motion to complete
arm_client.wait_for_result(rospy.Duration(5.0))
rospy.loginfo('...done')
def move_to(self, positions, velocities, accelerations, delta_t):
assert(len(self.joint_names) == positions.shape[1])
trajectory = JointTrajectory()
trajectory.joint_names = self.joint_names
for i in range(positions.shape[0]): # for each row i
p = JointTrajectoryPoint()
p.positions = list(positions[i])
p.velocities = list(velocities[i])
p.accelerations = list(accelerations[i])
p.time_from_start = rospy.Duration(i * delta_t)
trajectory.points.append(p)
follow_goal = FollowJointTrajectoryGoal()
follow_goal.trajectory = trajectory
self.client.send_goal(follow_goal)
self.client.wait_for_result()
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 execute(self, traj_msg):
""" Execute a JointTrajectory message and return a TrajectoryFuture.
@param traj_msg: requested trajectory
@type traj_msg: trajectory_msgs.msg.JointTrajectory
@return future representing the execution of the trajectory
@rtype TrajectoryFuture
"""
from control_msgs.msg import FollowJointTrajectoryGoal
goal_msg = FollowJointTrajectoryGoal()
goal_msg.trajectory = traj_msg
traj_future = TrajectoryFuture(traj_msg)
traj_future._handle = self._client.send_goal(
goal_msg,
transition_cb=traj_future.on_transition,
feedback_cb=traj_future.on_feedback
)
return traj_future
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 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 update(self):
points = []
tick = 0
self.phase = np.random.uniform(np.pi)
while tick <= self.time:
x = 2*np.pi*self.freq*tick + self.phase
positions = [np.sin(x)]*len(self.joint_names)
velocities = [np.cos(x)]*len(self.joint_names)
accelerations = [-np.sin(x)]*len(self.joint_names)
points.append(JointTrajectoryPoint(positions=positions,
velocities=velocities,
accelerations=accelerations,
time_from_start=rospy.Time.from_sec(tick)))
tick += self.step
msg = JointTrajectory(joint_names=self.joint_names, points=points)
msg.header.stamp = rospy.Time.now()
self.pub.publish(msg)
# joint_trajectroy_action
goal = FollowJointTrajectoryGoal()
goal.trajectory = msg
self.client.send_goal(goal)
def move_to_joint(end_pos, duration):
print end_pos
traj = build_traj(get_cur_pos(), end_pos, duration)
# wait for subscribers to connect
# pub = rospy.Publisher('joint_path_command', JointTrajectory)
# if not wait_for_subs(pub, 1, 0.5, 2.0):
# rospy.logwarn('Timeout while waiting for subscribers. Publishing trajectory anyway.')
# pub.publish(traj)
_left_client = actionlib.SimpleActionClient(
'/joint_trajectory_action',
FollowJointTrajectoryAction,
)
print "Waiting..."
_left_client.wait_for_server();
print "Done waiting"
goal = FollowJointTrajectoryGoal()
goal.trajectory = traj
goal.trajectory.header.stamp = rospy.Time.now()
_left_client.send_goal(goal)
print "Sent goal"
rate = rospy.Rate(60) # hz
counter = 0
while counter < duration*70:
rospy.is_shutdown()
rate.sleep()
counter = counter+1
print "Received Result: "
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')
torso_client = actionlib.SimpleActionClient(
"torso_controller/follow_joint_trajectory",
FollowJointTrajectoryAction)
torso_client.wait_for_server()
rospy.loginfo("...connected.")
trajectory = JointTrajectory()
trajectory.points.append(JointTrajectoryPoint())
trajectory.joint_names = torso_joint_names
trajectory.points[0].positions = torso_joint_positions
trajectory.points[0].velocities = [0.0] * len(torso_joint_positions)
trajectory.points[0].accelerations = [0.0] * len(torso_joint_positions)
trajectory.points[0].time_from_start = rospy.Duration(5.0)
torso_goal = FollowJointTrajectoryGoal()
torso_goal.trajectory = trajectory
torso_goal.goal_time_tolerance = rospy.Duration(0.0)
torso_client.send_goal(torso_goal)
torso_client.wait_for_result(rospy.Duration(5.0))
"""
rospy.loginfo("Waiting for head_controller...")
head_client = actionlib.SimpleActionClient("head_controller/follow_joint_trajectory", FollowJointTrajectoryAction)
head_client.wait_for_server()
rospy.loginfo("...connected.")
rospy.loginfo("Waiting for arm_controller...")
arm_client = actionlib.SimpleActionClient("arm_controller/follow_joint_trajectory", FollowJointTrajectoryAction)
arm_client.wait_for_server()
rospy.loginfo("...connected.")
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 follow_trajectory(self, trajectory):
follow_goal = FollowJointTrajectoryGoal()
follow_goal.trajectory = trajectory
self.client.send_goal(follow_goal)
self.client.wait_for_result()
def get_default_pose():
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_intermediate_positions = [1.32, 0, -1.4, 1.72, 0.0, 1.66, 0.0]
arm_intermediate_positions = [0.0, -0.62, 0, 0, 0.0, 0.62, 0.0]
# arm_joint_positions = [1.32, 1.40, -0.2, 1.72, 0.0, 1.66, 0.0]
# arm_joint_positions = [1.1, -0.64, -1.83, 0.96, 1.13, -.96, 0.0]
arm_joint_positions = [1.32, 0.7, 0.0, -2.0, 0.0, -0.57, 0.0]
head_joint_names = ["head_pan_joint", "head_tilt_joint"]
head_joint_positions = [0.0, 0.0]
#rospy.init_node("prepare_simulated_robot_pick_place")
# Check robot serial number, we never want to run this on a real robot!
if rospy.get_param("robot/serial") != "ABCDEFGHIJKLMNOPQRSTUVWX":
rospy.logerr("This script should not be run on a real robot")
sys.exit(-1)
# rospy.loginfo("Waiting for move_base...")
# move_base = actionlib.SimpleActionClient("move_base", MoveBaseAction)
# move_base.wait_for_server()
# rospy.loginfo("...connected.")
rospy.loginfo("Waiting for head_controller...")
head_client = actionlib.SimpleActionClient(
"head_controller/follow_joint_trajectory", FollowJointTrajectoryAction)
head_client.wait_for_server()
rospy.loginfo("...connected.")
rospy.loginfo("Waiting for arm_controller...")
arm_client = actionlib.SimpleActionClient(
"arm_controller/follow_joint_trajectory", FollowJointTrajectoryAction)
arm_client.wait_for_server()
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.")
# move_goal = MoveBaseGoal()
# move_goal.target_pose.pose.position.x = x
# move_goal.target_pose.pose.position.y = y
# move_goal.target_pose.pose.orientation.z = sin(theta/2.0)
# move_goal.target_pose.pose.orientation.w = cos(theta/2.0)
# move_goal.target_pose.header.frame_id = frame
# move_goal.target_pose.header.stamp = rospy.Time.now()
# move_base.send_goal(move_goal)
# move_base.wait_for_result()
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)
# duration = 5.0
# trajectory.points[0].time_from_start = rospy.Duration(duration)
#
# 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)
# duration += 5.0
# trajectory.points[1].time_from_start = rospy.Duration(duration)
#
# trajectory.points.append(JointTrajectoryPoint())
# trajectory.points[2].positions = arm_joint_positions
# trajectory.points[2].velocities = [0.0] * len(arm_joint_positions)
# trajectory.points[2].accelerations = [0.0] * len(arm_joint_positions)
# duration += 5.0
# trajectory.points[2].time_from_start = rospy.Duration(duration)
i = 0
duration = 5.0
trajectory.points.append(JointTrajectoryPoint())
arm_joint_positions = [0.0, -1.22, 0.0, 1.20, 0.0, 1.6,
0.0] # above the block, READY TO PICK
trajectory.points[i].positions = arm_joint_positions
trajectory.points[i].velocities = [0.0] * len(arm_joint_positions)
trajectory.points[i].accelerations = [0.0] * len(arm_joint_positions)
trajectory.points[i].time_from_start = rospy.Duration(duration)
i += 1
duration += 5.0
trajectory.points.append(JointTrajectoryPoint())
arm_joint_positions = [0.0, -1.20, 0.0, 1.45, 0.0, 1.3,
0.0] # PERFECT - PICK POSE
trajectory.points[i].positions = arm_joint_positions
trajectory.points[i].velocities = [0.0] * len(arm_joint_positions)
trajectory.points[i].accelerations = [0.0] * len(arm_joint_positions)
trajectory.points[i].time_from_start = rospy.Duration(duration)
# i += 1
# duration += 5.0
# trajectory.points.append(JointTrajectoryPoint())
# arm_joint_positions = [0.0, -1.22, 0.0, 1.20, 0.0, 1.6, 0.0] # above the block, READY TO PICK
# trajectory.points[i].positions = arm_joint_positions
# trajectory.points[i].velocities = [0.0] * len(arm_joint_positions)
# trajectory.points[i].accelerations = [0.0] * len(arm_joint_positions)
# 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)
gripper_goal = GripperCommandGoal()
gripper_goal.command.max_effort = 10.0
gripper_goal.command.position = 0.1
rospy.loginfo("Setting positions...")
head_client.send_goal(head_goal)
arm_client.send_goal(arm_goal)
gripper_client.send_goal(gripper_goal)
gripper_client.wait_for_result(rospy.Duration(5.0))
arm_client.wait_for_result(rospy.Duration(6.0))
head_client.wait_for_result(rospy.Duration(6.0))
rospy.loginfo("...done")
gripper_goal = GripperCommandGoal()
gripper_goal.command.max_effort = 10.0
gripper_goal.command.position = 0.0
#gripper_client.send_goal(gripper_goal)
time.sleep(5.0)
gripper_client.send_goal_and_wait(gripper_goal, rospy.Duration(5.0))
#gripper_client.wait_for_result(rospy.Duration(100.0))
rospy.loginfo("...done")
trajectory = JointTrajectory()
trajectory.joint_names = arm_joint_names
i = 0
duration = 5.0
trajectory.points.append(JointTrajectoryPoint())
arm_joint_positions = [0.0, -1.22, 0.0, 1.20, 0.0, 1.6,
0.0] # above the block, READY TO PICK
trajectory.points[i].positions = arm_joint_positions
trajectory.points[i].velocities = [0.0] * len(arm_joint_positions)
trajectory.points[i].accelerations = [0.0] * len(arm_joint_positions)
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)
arm_client.send_goal(arm_goal)
arm_client.wait_for_result(rospy.Duration(6.0))
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')
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[
0].positions = arm_joint_positions5 #Asignamos la trayectoria a ejecutar
trajectory.points[0].velocities = [0.0] * len(
arm_joint_positions0) #Asignamos los valores por default = 0
trajectory.points[0].accelerations = [0.0] * len(
arm_joint_positions0) #Asignamos los valores por default = 0
trajectory.points[0].time_from_start = rospy.Duration(
1.0) #Asignamos la duración de 1seg para iniciar
arm_goal = FollowJointTrajectoryGoal() #Creamos una variable de tipo Goal
arm_goal.trajectory = trajectory #Asignamos nuestra trayectoria hacia la trayectoria de nuestro objetivo(goal)
arm_goal.goal_time_tolerance = rospy.Duration(
0.0
) #Asignamos la duración de 0seg como tiempo de tolerancia de nuestro objetivo
rospy.loginfo("Setting positions...") #Imprimimos en pantalla
arm_client.send_goal(
arm_goal
) #Enviamos nuestro objetivo hacia el Acción Server de nuestro robot Fetch
arm_client.wait_for_result(
rospy.Duration(6.0)) #Esperamos 6 segundos para obtener los resultados
rospy.loginfo(
arm_client.wait_for_server()
rospy.loginfo("...connected.")
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
trajectory.points[2].velocities = [0.0] * len(arm_joint_positions)
trajectory.points[2].accelerations = [0.0] * len(arm_joint_positions)
trajectory.points[2].time_from_start = rospy.Duration(7.5)
arm_goal = FollowJointTrajectoryGoal()
arm_goal.trajectory = trajectory
arm_goal.goal_time_tolerance = rospy.Duration(0.0)
rospy.loginfo("Setting positions...")
arm_client.send_goal(arm_goal)
arm_client.wait_for_result(rospy.Duration(6.0))
rospy.loginfo("...done")
def execute(self, component_name, as_name, target="", blocking=True):
ah = ActionHandle("move_joint_trajectory", component_name, target,
blocking)
rospy.loginfo("Move <<%s>> to <<%s>>", component_name, target)
# get joint_names from parameter server
param_string = self.ns_global_prefix + "/" + component_name + "/joint_names"
if not rospy.has_param(param_string):
rospy.logerr(
"parameter %s does not exist on ROS Parameter Server, aborting...",
param_string)
ah.set_failed(2)
return ah
joint_names = rospy.get_param(param_string)
# check joint_names parameter
if not type(joint_names) is list: # check list
rospy.logerr(
"no valid joint_names for %s: not a list, aborting...",
component_name)
print "joint_names are:", joint_names
ah.set_failed(3)
return ah
else:
for i in joint_names:
#print i,"type1 = ", type(i)
if not type(i) is str: # check string
rospy.logerr(
"no valid joint_names for %s: not a list of strings, aborting...",
component_name)
print "joint_names are:", param
ah.set_failed(3)
return ah
else:
rospy.logdebug("accepted joint_names for component %s",
component_name)
# get joint values from parameter server
if type(target) is str:
if not rospy.has_param(self.ns_global_prefix + "/" +
component_name + "/" + target):
rospy.logerr(
"parameter %s does not exist on ROS Parameter Server, aborting...",
self.ns_global_prefix + "/" + component_name + "/" +
target)
ah.set_failed(2)
return ah
param = rospy.get_param(self.ns_global_prefix + "/" +
component_name + "/" + target)
else:
param = target
# check trajectory parameters
if not type(param) is list: # check outer list
rospy.logerr("no valid parameter for %s: not a list, aborting...",
component_name)
print "parameter is:", param
ah.set_failed(3)
return ah
traj = []
for point in param:
#print point,"type1 = ", type(point)
if type(point) is str:
if not rospy.has_param(self.ns_global_prefix + "/" +
component_name + "/" + point):
rospy.logerr(
"parameter %s does not exist on ROS Parameter Server, aborting...",
self.ns_global_prefix + "/" + component_name + "/" +
point)
ah.set_failed(2)
return ah
point = rospy.get_param(self.ns_global_prefix + "/" +
component_name + "/" + point)
point = point[
0] # \todo TODO: hack because only first point is used, no support for trajectories inside trajectories
#print point
elif type(point) is list:
rospy.logdebug("point is a list")
else:
rospy.logerr(
"no valid parameter for %s: not a list of lists or strings, aborting...",
component_name)
print "parameter is:", param
ah.set_failed(3)
return ah
# here: point should be list of floats/ints
#print point
if not len(point) == len(joint_names): # check dimension
rospy.logerr(
"no valid parameter for %s: dimension should be %d and is %d, aborting...",
component_name, len(joint_names), len(point))
print "parameter is:", param
ah.set_failed(3)
return ah
for value in point:
#print value,"type2 = ", type(value)
if not ((type(value) is float) or
(type(value) is int)): # check type
#print type(value)
rospy.logerr(
"no valid parameter for %s: not a list of float or int, aborting...",
component_name)
print "parameter is:", param
ah.set_failed(3)
return ah
rospy.logdebug("accepted value %f for %s", value,
component_name)
traj.append(point)
rospy.logdebug("accepted trajectory for %s", component_name)
# convert to ROS trajectory message
traj_msg = JointTrajectory()
traj_msg.header.stamp = rospy.Time.now() + rospy.Duration(0.5)
traj_msg.joint_names = joint_names
point_nr = 0
for point in traj:
point_nr = point_nr + 1
point_msg = JointTrajectoryPoint()
point_msg.positions = point
point_msg.velocities = [0] * len(joint_names)
point_msg.time_from_start = rospy.Duration(
3 * point_nr
) # this value is set to 3 sec per point. \todo TODO: read from parameter
traj_msg.points.append(point_msg)
# call action server
action_server_name = "/" + component_name + '_controller/follow_joint_trajectory'
rospy.logdebug("calling %s action server", action_server_name)
client = actionlib.SimpleActionClient(action_server_name,
FollowJointTrajectoryAction)
# trying to connect to server
rospy.logdebug("waiting for %s action server to start",
action_server_name)
if not client.wait_for_server(rospy.Duration(5)):
# error: server did not respond
rospy.logerr(
"%s action server not ready within timeout, aborting...",
action_server_name)
ah.set_failed(4)
return ah
else:
rospy.logdebug("%s action server ready", action_server_name)
# sending goal
client_goal = FollowJointTrajectoryGoal()
client_goal.trajectory = traj_msg
#print client_goal
client.send_goal(client_goal)
ah.set_client(client)
ah.wait_inside()
return ah
def movel(self, destino, orientation=None, speed=None):
"""
Move to destination following a line
args:
destino: a list of xyz defining a point in space
orientation: a quaternion defining the orientation
"""
self.client.cancel_all_goals()
# to create uniform movements lets define speed to calculate time
if speed is None:
speed = 0.4 # m/s
cur_pose = self.fwd_kin(self.joints_state, i_unit='r', o_unit='p')
if orientation is None:
orientation = cur_pose.orientation
if type(destino) == type(Pose()):
desired_position = np.array([
destino.position.x, destino.position.y, destino.position.z,
destino.orientation.x, destino.orientation.y,
destino.orientation.z, destino.orientation.w
])
tmp1 = np.array([
cur_pose.position.x, cur_pose.position.y, cur_pose.position.z
])
tmp2 = np.array(
[destino.position.x, destino.position.y, destino.position.z])
else:
desired_position = np.array([
*destino, orientation.x, orientation.y, orientation.z,
orientation.w
])
tmp1 = np.array([
cur_pose.position.x, cur_pose.position.y, cur_pose.position.z
])
tmp2 = np.array(destino)
distance = np.linalg.norm(tmp1 - tmp2)
if distance <= 0:
nanoseconds = 1_000_000
else:
nanoseconds = 1_000_000_000 * distance / speed
# check if it is possible to reach
if len(self.check_working_space([tmp2.tolist()])) == 0:
rospy.logerr('Position %s out of reach' %
np.array_str(tmp2, precision=2, suppress_small=True))
return False
# cur_pose = np.array([cur_pose.position.x,cur_pose.position.y,cur_pose.position.z])
lista = self.trajectory(
desired_position, cur_pose
) # returns a list of np array with position and orientation
pose = Pose()
g = FollowJointTrajectoryGoal()
g.trajectory = JointTrajectory()
g.trajectory.joint_names = self.JOINT_NAMES
joints_inv_kine = self.joints_state
g.trajectory.points = []
n = len(lista)
if self.debug:
os.chdir('/home/ubuntu/Documents/debug')
new_lista = []
for item in lista:
new_lista.append(
np.array_str(item, precision=2, suppress_small=True))
with open(time.strftime("%Y%m%d-%H%M%S") + ' lista.yaml',
'w') as file:
yaml.dump(new_lista, file)
for i, point in enumerate(lista):
pose.position.x = point[0]
pose.position.y = point[1]
pose.position.z = point[2]
pose.orientation.x = point[3]
pose.orientation.y = point[4]
pose.orientation.z = point[5]
pose.orientation.w = point[6]
a = self.inv_kine(self.ros2np(pose))
a = a.transpose().tolist()
joints_inv_kine = self.select(a, joints_inv_kine)
t = int(i * nanoseconds / n)
g.trajectory.points.append(
JointTrajectoryPoint(positions=joints_inv_kine,
velocities=[0] * 6,
time_from_start=rospy.Duration(0, t)))
# Callback goal
self.done = False
self.client.send_goal(g, done_cb=self.done_callback)
start_time = time.time()
while not self.done and time.time(
) - start_time < 3: # 3 seconds timeout
time.sleep(0.05)
if not self.done:
rospy.logerr('Canceling movement')
self.client.cancel_all_goals()
return self.done
def joint_traj_jtas(self, j_traj):
self.client.wait_for_server()
goal = FollowJointTrajectoryGoal()
goal.trajectory = j_traj
self.client.send_goal(goal)
def __init__(self):
x = loadmat('cuadrupedo_andar.mat')
# print x
piernas_der = x['estruc_patas_rec']
piernaLF = piernas_der[0, 0]
piernaLR = piernas_der[0, 1]
piernaRF = piernas_der[0, 2]
piernaRR = piernas_der[0, 3]
rospy.init_node('trajectory_cua')
# 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'
]
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.')
rr_trajectory1 = JointTrajectory()
rr_trajectory1.joint_names = piernas_joints_RR
rf_trajectory1 = JointTrajectory()
rf_trajectory1.joint_names = piernas_joints_RF
lr_trajectory1 = JointTrajectory()
lr_trajectory1.joint_names = piernas_joints_LR
lf_trajectory1 = JointTrajectory()
lf_trajectory1.joint_names = piernas_joints_LF
# piernas_goalF0 = [0, -0.3, -0.4, 0.8, -0.3, -0.4,
# 0, -0.4, -0.3, 0.6, +0.4, -0.3]
# arm_trajectory.points.append(JointTrajectoryPoint())
# arm_trajectory.points[0].positions = piernas_goalF0
# arm_trajectory.points[0].velocities = [0.0 for k in piernas_goalF0]
# arm_trajectory.points[0].accelerations = [0.0 for k in piernas_goalF0]
# arm_trajectory.points[0].time_from_start = rospy.Duration(2)
piernasLF_goalF01 = [
0, piernaLF['nuevo_femur'][0, 0], -piernaLF['nuevo_tibia'][0, 0],
-piernaLF['nuevo_tarsus'][0, 0]
]
lf_trajectory1.points.append(JointTrajectoryPoint())
lf_trajectory1.points[0].positions = piernasLF_goalF01
lf_trajectory1.points[0].time_from_start = rospy.Duration(1)
piernasLR_goalF01 = [
0, piernaLR['nuevo_femur'][0, 0], -piernaLR['nuevo_tibia'][0, 0],
-piernaLR['nuevo_tarsus'][0, 0]
]
lr_trajectory1.points.append(JointTrajectoryPoint())
lr_trajectory1.points[0].positions = piernasLR_goalF01
lr_trajectory1.points[0].time_from_start = rospy.Duration(1)
piernasRF_goalF01 = [
0, -piernaRF['nuevo_femur'][0, 0], piernaRF['nuevo_tibia'][0, 0],
piernaRF['nuevo_tarsus'][0, 0]
]
rf_trajectory1.points.append(JointTrajectoryPoint())
rf_trajectory1.points[0].positions = piernasRF_goalF01
rf_trajectory1.points[0].time_from_start = rospy.Duration(1)
piernasRR_goalF01 = [
0, -piernaRR['nuevo_femur'][0, 0], piernaRR['nuevo_tibia'][0, 0],
piernaRR['nuevo_tarsus'][0, 0]
]
rr_trajectory1.points.append(JointTrajectoryPoint())
rr_trajectory1.points[0].positions = piernasRR_goalF01
rr_trajectory1.points[0].time_from_start = rospy.Duration(1)
ind = 1
salto = 5
for i in range(1, int(piernaRR['nuevo_tarsus'].shape[0] * 0.3), salto):
piernasLF_goalF01 = [
0, piernaLF['nuevo_femur'][i, 0],
piernaLF['nuevo_tibia'][i, 0], piernaLF['nuevo_tarsus'][i, 0]
]
lf_trajectory1.points.append(JointTrajectoryPoint())
lf_trajectory1.points[ind].positions = piernasLF_goalF01
# lf_trajectory1.points[ind].velocities = [0.0 for k in piernasLF_goalF01]
# lf_trajectory1.points[ind].accelerations = [0.0 for k in piernasLF_goalF01]
piernasLR_goalF01 = [
0, piernaLR['nuevo_femur'][i, 0],
piernaLR['nuevo_tibia'][i, 0], piernaLR['nuevo_tarsus'][i, 0]
]
lr_trajectory1.points.append(JointTrajectoryPoint())
lr_trajectory1.points[ind].positions = piernasLR_goalF01
# lr_trajectory1.points[ind].velocities = [0.0 for k in piernasLF_goalF01]
# lr_trajectory1.points[ind].accelerations = [0.0 for k in piernasLF_goalF01]
piernasRF_goalF01 = [
0, -piernaRF['nuevo_femur'][i, 0],
piernaRF['nuevo_tibia'][i, 0], piernaRF['nuevo_tarsus'][i, 0]
]
rf_trajectory1.points.append(JointTrajectoryPoint())
rf_trajectory1.points[ind].positions = piernasRF_goalF01
# rf_trajectory1.points[ind].velocities = [0.0 for k in piernasLF_goalF01]
# rf_trajectory1.points[ind].accelerations = [0.0 for k in piernasLF_goalF01]
piernasRR_goalF01 = [
0, -piernaRR['nuevo_femur'][i, 0],
piernaRR['nuevo_tibia'][i, 0], piernaRR['nuevo_tarsus'][i, 0]
]
rr_trajectory1.points.append(JointTrajectoryPoint())
rr_trajectory1.points[ind].positions = piernasRR_goalF01
# rr_trajectory1.points[ind].velocities = [0.0 for k in piernasLF_goalF01]
# rr_trajectory1.points[ind].accelerations = [0.0 for k in piernasLF_goalF01]
# print ind
# print int(art1_der['senialCompleta'].shape[1]*0.2)
# arm_trajectory.points[ind].velocities = [0.0 for j in piernas_goalF]
# arm_trajectory.points[ind].accelerations = [0.0 for j in piernas_goalF]
# art1_der['tiempo_completo'][0, i]
# 0.01 * (ind + 1)
lf_trajectory1.points[ind].time_from_start = rospy.Duration(
1 +
(piernaLF['tiempo'][i, 0] - piernaLF['tiempo'][0, 0]) * 2.8)
lr_trajectory1.points[ind].time_from_start = rospy.Duration(
1 +
(piernaLR['tiempo'][i, 0] - piernaLR['tiempo'][0, 0]) * 2.8)
rf_trajectory1.points[ind].time_from_start = rospy.Duration(
1 +
(piernaRF['tiempo'][i, 0] - piernaRF['tiempo'][0, 0]) * 2.8)
rr_trajectory1.points[ind].time_from_start = rospy.Duration(
1 +
(piernaRR['tiempo'][i, 0] - piernaRR['tiempo'][0, 0]) * 2.8)
# print 1+art1_der['tiempo_completo'][0, i]
ind += 1
# print art1_der['senialCompleta'][0, i]
print int(piernaRR['nuevo_tarsus'].shape[0])
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
rospy.loginfo('Moving the arm to goal position...')
# 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.01)
lm_goal.goal_time_tolerance = rospy.Duration(0.01)
rf_goal.goal_time_tolerance = rospy.Duration(0.01)
lr_goal.goal_time_tolerance = rospy.Duration(0.01)
rm_goal.goal_time_tolerance = rospy.Duration(0.01)
lf_goal.goal_time_tolerance = rospy.Duration(0.01)
# Send the goal to the action server
rr_client.send_goal(rr_goal)
rf_client.send_goal(rf_goal)
lr_client.send_goal(lr_goal)
lf_client.send_goal(lf_goal)
# rr_client.wait_for_result(rospy.Duration(5.0))
rr_client.wait_for_result()
'''rr_client.send_goal(rr_goal)
lm_client.send_goal(lm_goal)
rf_client.send_goal(rf_goal)
lr_client.send_goal(lr_goal)
rm_client.send_goal(rm_goal)
lf_client.send_goal(lf_goal)
# rr_client.wait_for_result(rospy.Duration(5.0))
rr_client.wait_for_result()
lr_client.send_goal(lr_goal)
rm_client.send_goal(rm_goal)
lf_client.send_goal(lf_goal)
# rr_client.wait_for_result(rospy.Duration(5.0))
rr_client.wait_for_result()
rr_client.send_goal(rr_goal)
lm_client.send_goal(lm_goal)
rf_client.send_goal(rf_goal)
lr_client.send_goal(lr_goal)
rm_client.send_goal(rm_goal)
lf_client.send_goal(lf_goal)
# rr_client.wait_for_result(rospy.Duration(5.0))
rr_client.wait_for_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 marker_inf_cb(self, data):
# in order to filter the inamissible (0.0, 0.0, 0.0, -inf) detections
if data.data[0:3] == (0.0, 0.0, 0.0):
# print "inadmissible input!"
return
vec3 = Vector3()
vec3.x = data.data[0]
vec3.y = data.data[1]
vec3.z = data.data[2]
ball_pose = np.array([vec3.x, vec3.y, vec3.z])
for i in range(3):
if np.isnan(ball_pose[i]):
return
cam_object_T = self.set_cam_object_T(ball_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)
# in order to assign object the same orientation as target frame
base_object_T[0:3, 0:3] = self.base_target_T[0:3, 0:3]
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.3
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 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 __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):
x = loadmat('hex_wave.mat'
) #hex_transversal hex_lateral2 hex_lateral_r hex_wave
# print x
pierna1 = x['pierna1']
pierna2 = x['pierna2']
pierna3 = x['pierna3']
pierna4 = x['pierna4']
pierna5 = x['pierna5']
pierna6 = x['pierna6']
rospy.init_node('trajectory_hex')
# 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_LM = [
'coxa_joint_LM', 'femur_joint_LM', 'tibia_joint_LM',
'tarsus_joint_LM'
]
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_RM = [
'coxa_joint_RM', 'femur_joint_RM', 'tibia_joint_RM',
'tarsus_joint_RM'
]
piernas_joints_LF = [
'coxa_joint_LF', 'femur_joint_LF', 'tibia_joint_LF',
'tarsus_joint_LF'
]
rospy.loginfo('Waiting for right arm trajectory controller...')
rr_client = actionlib.SimpleActionClient(
'/hexapodo/pata_rr/follow_joint_trajectory',
FollowJointTrajectoryAction)
rr_client.wait_for_server()
lm_client = actionlib.SimpleActionClient(
'/hexapodo/pata_lm/follow_joint_trajectory',
FollowJointTrajectoryAction)
lm_client.wait_for_server()
rf_client = actionlib.SimpleActionClient(
'/hexapodo/pata_rf/follow_joint_trajectory',
FollowJointTrajectoryAction)
rf_client.wait_for_server()
lr_client = actionlib.SimpleActionClient(
'/hexapodo/pata_lr/follow_joint_trajectory',
FollowJointTrajectoryAction)
lr_client.wait_for_server()
rm_client = actionlib.SimpleActionClient(
'/hexapodo/pata_rm/follow_joint_trajectory',
FollowJointTrajectoryAction)
rm_client.wait_for_server()
lf_client = actionlib.SimpleActionClient(
'/hexapodo/pata_lf/follow_joint_trajectory',
FollowJointTrajectoryAction)
lf_client.wait_for_server()
rospy.loginfo('...connected.')
rr_trajectory1 = JointTrajectory()
rr_trajectory1.joint_names = piernas_joints_RR
lm_trajectory1 = JointTrajectory()
lm_trajectory1.joint_names = piernas_joints_LM
rf_trajectory1 = JointTrajectory()
rf_trajectory1.joint_names = piernas_joints_RF
lr_trajectory1 = JointTrajectory()
lr_trajectory1.joint_names = piernas_joints_LR
rm_trajectory1 = JointTrajectory()
rm_trajectory1.joint_names = piernas_joints_RM
lf_trajectory1 = JointTrajectory()
lf_trajectory1.joint_names = piernas_joints_LF
# piernas_goalF0 = [0, -0.3, -0.4, 0.8, -0.3, -0.4,
# 0, -0.4, -0.3, 0.6, +0.4, -0.3]
# arm_trajectory.points.append(JointTrajectoryPoint())
# arm_trajectory.points[0].positions = piernas_goalF0
# arm_trajectory.points[0].velocities = [0.0 for k in piernas_goalF0]
# arm_trajectory.points[0].accelerations = [0.0 for k in piernas_goalF0]
# arm_trajectory.points[0].time_from_start = rospy.Duration(2)
piernasLF_goalF01 = [
pierna6[0, 0], pierna6[1, 0], pierna6[2, 0], -0.4
] #0.35
lf_trajectory1.points.append(JointTrajectoryPoint())
lf_trajectory1.points[0].positions = piernasLF_goalF01
lf_trajectory1.points[0].time_from_start = rospy.Duration(1)
piernasLM_goalF01 = [
pierna5[0, 0], pierna5[1, 0], pierna5[2, 0], -0.4
] #0.35
lm_trajectory1.points.append(JointTrajectoryPoint())
lm_trajectory1.points[0].positions = piernasLM_goalF01
lm_trajectory1.points[0].time_from_start = rospy.Duration(1)
piernasLR_goalF01 = [
pierna4[0, 0], pierna4[1, 0], pierna4[2, 0], -0.4
] #0.35
lr_trajectory1.points.append(JointTrajectoryPoint())
lr_trajectory1.points[0].positions = piernasLR_goalF01
lr_trajectory1.points[0].time_from_start = rospy.Duration(1)
piernasRF_goalF01 = [
pierna3[0, 0], -pierna3[1, 0], -pierna3[2, 0], 0.4
] #0.35
rf_trajectory1.points.append(JointTrajectoryPoint())
rf_trajectory1.points[0].positions = piernasRF_goalF01
rf_trajectory1.points[0].time_from_start = rospy.Duration(1)
piernasRM_goalF01 = [
pierna2[0, 0], -pierna2[1, 0], -pierna2[2, 0], 0.4
] #0.35
rm_trajectory1.points.append(JointTrajectoryPoint())
rm_trajectory1.points[0].positions = piernasRM_goalF01
rm_trajectory1.points[0].time_from_start = rospy.Duration(1)
piernasRR_goalF01 = [
pierna1[0, 0], -pierna1[1, 0], -pierna1[2, 0], 0.4
] #0.35
rr_trajectory1.points.append(JointTrajectoryPoint())
rr_trajectory1.points[0].positions = piernasRR_goalF01
rr_trajectory1.points[0].time_from_start = rospy.Duration(1)
ind = 1
salto = 5
t_actual = 1
for i in range(1, int(pierna1.shape[1] * 0.25), salto):
piernasLF_goalF01 = [
pierna6[0, i], pierna6[1, i], pierna6[2, i], -0.4
] #0.35
lf_trajectory1.points.append(JointTrajectoryPoint())
lf_trajectory1.points[ind].positions = piernasLF_goalF01
lf_trajectory1.points[0].velocities = [
0.0 for k in piernasLF_goalF01
]
lf_trajectory1.points[0].accelerations = [
0.0 for k in piernasLF_goalF01
]
piernasLM_goalF01 = [
pierna5[0, i], pierna5[1, i], pierna5[2, i], -0.4
] #0.35
lm_trajectory1.points.append(JointTrajectoryPoint())
lm_trajectory1.points[ind].positions = piernasLM_goalF01
lm_trajectory1.points[0].velocities = [
0.0 for k in piernasLM_goalF01
]
lm_trajectory1.points[0].accelerations = [
0.0 for k in piernasLM_goalF01
]
piernasLR_goalF01 = [
pierna4[0, i], pierna4[1, i], pierna4[2, i], -0.4
] #0.35
lr_trajectory1.points.append(JointTrajectoryPoint())
lr_trajectory1.points[ind].positions = piernasLR_goalF01
lr_trajectory1.points[0].velocities = [
0.0 for k in piernasLR_goalF01
]
lr_trajectory1.points[0].accelerations = [
0.0 for k in piernasLR_goalF01
]
piernasRF_goalF01 = [
pierna3[0, i], -pierna3[1, i], -pierna3[2, i], 0.4
] #0.35
rf_trajectory1.points.append(JointTrajectoryPoint())
rf_trajectory1.points[ind].positions = piernasRF_goalF01
rf_trajectory1.points[0].velocities = [
0.0 for k in piernasRF_goalF01
]
rf_trajectory1.points[0].accelerations = [
0.0 for k in piernasRF_goalF01
]
piernasRM_goalF01 = [
pierna2[0, i], -pierna2[1, 0], -pierna2[2, i], 0.4
] #0.35
rm_trajectory1.points.append(JointTrajectoryPoint())
rm_trajectory1.points[ind].positions = piernasRM_goalF01
rm_trajectory1.points[0].velocities = [
0.0 for k in piernasRM_goalF01
]
rm_trajectory1.points[0].accelerations = [
0.0 for k in piernasRM_goalF01
]
piernasRR_goalF01 = [
pierna1[0, i], -pierna1[1, 0], -pierna1[2, i], 0.4
] #0.35
rr_trajectory1.points.append(JointTrajectoryPoint())
rr_trajectory1.points[ind].positions = piernasRR_goalF01
rr_trajectory1.points[0].velocities = [
0.0 for k in piernasRR_goalF01
]
rr_trajectory1.points[0].accelerations = [
0.0 for k in piernasRR_goalF01
]
# print ind
# print int(art1_der['senialCompleta'].shape[1]*0.2)
# arm_trajectory.points[ind].velocities = [0.0 for j in piernas_goalF]
# arm_trajectory.points[ind].accelerations = [0.0 for j in piernas_goalF]
# art1_der['tiempo_completo'][0, i]
# 0.01 * (ind + 1)
t_actual += 0.01
lf_trajectory1.points[ind].time_from_start = rospy.Duration(
t_actual)
lm_trajectory1.points[ind].time_from_start = rospy.Duration(
t_actual)
lr_trajectory1.points[ind].time_from_start = rospy.Duration(
t_actual)
rf_trajectory1.points[ind].time_from_start = rospy.Duration(
t_actual)
rm_trajectory1.points[ind].time_from_start = rospy.Duration(
t_actual)
rr_trajectory1.points[ind].time_from_start = rospy.Duration(
t_actual)
# print 1+art1_der['tiempo_completo'][0, i]
ind += 1
# print art1_der['senialCompleta'][0, i]
# Create an empty trajectory goal
rospy.loginfo('Moving the arm to goal position...')
print pierna1.shape[1]
# 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
lm_goal.trajectory = lm_trajectory1
rf_goal.trajectory = rf_trajectory1
lr_goal.trajectory = lr_trajectory1
rm_goal.trajectory = rm_trajectory1
lf_goal.trajectory = lf_trajectory1
# Specify zero tolerance for the execution time
rr_goal.goal_time_tolerance = rospy.Duration(0.01)
lm_goal.goal_time_tolerance = rospy.Duration(0.01)
rf_goal.goal_time_tolerance = rospy.Duration(0.01)
lr_goal.goal_time_tolerance = rospy.Duration(0.01)
rm_goal.goal_time_tolerance = rospy.Duration(0.01)
lf_goal.goal_time_tolerance = rospy.Duration(0.01)
# Send the goal to the action server
rr_client.send_goal(rr_goal)
lm_client.send_goal(lm_goal)
rf_client.send_goal(rf_goal)
lr_client.send_goal(lr_goal)
rm_client.send_goal(rm_goal)
lf_client.send_goal(lf_goal)
# rr_client.wait_for_result(rospy.Duration(5.0))
rr_client.wait_for_result()
# rr_client.send_goal(rr_goal)
# lm_client.send_goal(lm_goal)
# rf_client.send_goal(rf_goal)
#
# lr_client.send_goal(lr_goal)
# rm_client.send_goal(rm_goal)
# lf_client.send_goal(lf_goal)
# # rr_client.wait_for_result(rospy.Duration(5.0))
# rr_client.wait_for_result()
#
# lr_client.send_goal(lr_goal)
# rm_client.send_goal(rm_goal)
# lf_client.send_goal(lf_goal)
# # rr_client.wait_for_result(rospy.Duration(5.0))
# rr_client.wait_for_result()
#
# rr_client.send_goal(rr_goal)
# lm_client.send_goal(lm_goal)
# rf_client.send_goal(rf_goal)
#
# lr_client.send_goal(lr_goal)
# rm_client.send_goal(rm_goal)
# lf_client.send_goal(lf_goal)
# # rr_client.wait_for_result(rospy.Duration(5.0))
# rr_client.wait_for_result()
#'''
rospy.loginfo('...done')
def gspline_to_follow_joint_trajectory_goal(_trj, _joint_names, _sample_rate):
trj_msg = gspline_to_joint_trajectory_message(_trj, _joint_names,
_sample_rate)
msg = FollowJointTrajectoryGoal()
msg.trajectory = trj_msg
return msg
def __init__(self):
filename = '/home/kaiser/WS_ROS/catkin_ws/src/urdf_serp/urdf/urdf_exportado3.urdf'
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
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
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
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()
fksolver_izq_up_down.JntToCart(posicionInicial_izq_up_down,
finalFrame_izq_up_down)
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'
]
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 = [0.0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0]
print "Inverse Kinematics"
q_init = posicionInicial_izq_up_down # initial angles
#desiredFrame = PyKDL.Frame(PyKDL.Vector(0.5, 0.4, 1))
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 = PyKDL.JntArray(6)
ik_izq_up_down.CartToJnt(q_init, desiredFrame, q_out)
print "Output angles in rads: ", q_out
#arm_goal[0] = q_out[0]
#arm_goal[1] = q_out[1]
#arm_goal[2] = q_out[2]
#arm_goal[3] = q_out[3]
#arm_goal[4] = q_out[4]
#arm_goal[5] = q_out[5]
# 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)
arm_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 = piernas_joints
arm_trajectory.points.append(JointTrajectoryPoint())
arm_trajectory.points[0].positions = arm_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)
# 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))
rospy.loginfo('...done')
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())
def __init__(self):
pkl_file = open('datos_articulaciones.pkl', 'rb')
# data1 = pickle.load(pkl_file)
# pprint.pprint(data1)
# data2 = pickle.load(pkl_file)
# pprint.pprint(data2)
# print data1['b'][0]
# for i in data2:
# print i
datos = []
try:
while True: # loop indefinitely
print "LEYENDO"
#print pickle.load(pkl_file)
datos.append(pickle.load(
pkl_file)) # add each item from the file to a list
except EOFError: # the exception is used to break the loop
pass # we don't need to do anything special
pkl_file.close()
print len(datos)
# print datos_trans
for i in datos:
pass
# print i
filename = '/home/kaiser/WS_ROS/catkin_ws/src/urdf_serp/urdf/urdf_exportado4.urdf'
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
# 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.')
print "Los datos son", len(datos)
# 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 = datos[0]
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 = datos[1]
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 = datos[2]
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 = datos[3]
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 = datos[4]
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 = datos[5]
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 = datos[6]
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')
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 = torso_joint_names
trajectory.points.append(JointTrajectoryPoint())
trajectory.points[0].positions = torso_joint_positions
trajectory.points[0].velocities = [0.0] * len(torso_joint_positions)
trajectory.points[0].accelerations = [0.0] * len(torso_joint_positions)
trajectory.points[0].time_from_start = rospy.Duration(5.0)
torso_goal = FollowJointTrajectoryGoal()
torso_goal.trajectory = trajectory
torso_goal.goal_time_tolerance = rospy.Duration(0.0)
trajectory = JointTrajectory()
import actionlib
import math
from control_msgs.msg import FollowJointTrajectoryAction
client = actionlib.SimpleActionClient('/panda_arm_controller/follow_joint_trajectory', FollowJointTrajectoryAction)
client.wait_for_server()
from control_msgs.msg import FollowJointTrajectoryGoal
goal = FollowJointTrajectoryGoal()
from trajectory_msgs.msg import JointTrajectory
joint_traj = JointTrajectory()
joint_traj.joint_names = ["panda_joint1", "panda_joint2", "panda_joint3", "panda_joint4", "panda_joint5", "panda_joint6", "panda_joint7"]
from trajectory_msgs.msg import JointTrajectoryPoint
point1 = JointTrajectoryPoint()
point2 = JointTrajectoryPoint()
point1.positions = [0.33877080806309046, -0.3623406480725775, -1.2750252749223279, -1.8702679826187074, 2.4926642728445163, 2.873639813867795, 0.06015034910227879]
point1.velocities = [0., 0., 0., 0., 0., 0., 0.]
#point2.positions = [0.33877080806309046, -0.3623406480725775, -1.2750252749223279, -1.8702679826187074, 2.4926642728445163, 2.873639813867795 - math.pi/2, 0.06015034910227879]
point2.positions = [0.33877080806309046, -1.5, -1.2750252749223279, -1.8702679826187074, 2.4926642728445163, 2.873639813867795 - math.pi/2, 0.06015034910227879]
point2.velocities = [0., 0., 0., 0., 0., 0., 0.]
point1.time_from_start = rospy.Duration(2.)
point2.time_from_start = rospy.Duration(4.)
from copy import deepcopy
joint_traj.points = [point1, point2, deepcopy(point1), deepcopy(point2)]
joint_traj.points[2].time_from_start += rospy.Duration(4.)
joint_traj.points[3].time_from_start += rospy.Duration(4.)
goal.trajectory = joint_traj
joint_traj.header.stamp = rospy.Time.now()+rospy.Duration(1.0)
client.send_goal_and_wait(goal)
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 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):
pkl_file = open('datos_articulacionespkl.pkl', 'rb')
# data1 = pickle.load(pkl_file)
# pprint.pprint(data1)
# data2 = pickle.load(pkl_file)
# pprint.pprint(data2)
# print data1['b'][0]
# for i in data2:
# print i
datos = []
try:
while True: # loop indefinitely
print "LEYENDO"
# print pickle.load(pkl_file)
datos.append(pickle.load(
pkl_file)) # add each item from the file to a list
except EOFError: # the exception is used to break the loop
pass # we don't need to do anything special
pkl_file.close()
print len(datos)
# print datos_trans
for i in datos:
pass
# print i
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_blue_box1_der_joint', 'cilinder_blue1_box2_der_joint',
'cilinder_blue_box2_der_joint', 'cilinder_blue_box4_der_joint',
'cilinder_blue1_box6_der_joint', 'cilinder_blue_box6_der_joint',
'cilinder_blue_box1_izq_joint', 'cilinder_blue1_box2_izq_joint',
'cilinder_blue_box2_izq_joint', 'cilinder_blue_box4_izq_joint',
'cilinder_blue1_box6_izq_joint', 'cilinder_blue_box6_izq_joint'
]
# 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 = datos[0]
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(1.0)
arm_trajectory.points.append(JointTrajectoryPoint())
arm_trajectory.points[1].positions = datos[1]
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 = datos[2]
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 = datos[3]
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 = datos[4]
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 = datos[5]
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 = datos[6]
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 = datos[1]
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 = datos[2]
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 = datos[3]
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 = datos[4]
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 = datos[5]
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 = datos[6]
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()
arm_client.send_goal(piernas_goal)
print arm_client.get_result()
rospy.loginfo('...done')
def __init__(self):
filename = '/home/kaiser/WS_ROS/catkin_ws/src/urdf_serp/urdf/urdf_exportado4.urdf'
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' ]
if reset:
# Set the arm back to the resting position
arm_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]
else:
# Set a goal configuration for the arm
arm_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 = PyKDL.Frame(PyKDL.Vector(0.5, 0.4, 1))
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
arm_goal[6] = q_out_izq_up_down[0]
arm_goal[7] = q_out_izq_up_down[1]
arm_goal[8] = q_out_izq_up_down[2]
arm_goal[9] = q_out_izq_up_down[3]
arm_goal[10] = q_out_izq_up_down[4]
arm_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]-0.06
desiredFrame.p[2] = finalFrame_der_up_down.p[2]+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
arm_goal[0] = q_out_der_up_down[0]
arm_goal[1] = q_out_der_up_down[1]
arm_goal[2] = q_out_der_up_down[2]
arm_goal[3] = q_out_der_up_down[3]
arm_goal[4] = q_out_der_up_down[4]
arm_goal[5] = q_out_der_up_down[5]
# 222222222222222222222222222222222222222222
arm_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"
q_init_izq_down_up = posicionInicial_izq_down_up # initial angles
# desiredFrame = PyKDL.Frame(PyKDL.Vector(0.5, 0.4, 1))
desiredFrame2 = finalFrame_izq_down_up
desiredFrame2.p[0] = finalFrame_izq_down_up.p[0] - 0.05
desiredFrame2.p[1] = finalFrame_izq_down_up.p[1] - 0.06
desiredFrame2.p[2] = finalFrame_izq_down_up.p[2] - 0.02
print "Desired Position: ", desiredFrame2.p
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 rads: ", q_out_izq_down_up
arm_goal2[6] = q_out_izq_down_up[5]
arm_goal2[7] = q_out_izq_down_up[4]
arm_goal2[8] = q_out_izq_down_up[3]
arm_goal2[9] = q_out_izq_down_up[2]
arm_goal2[10] = q_out_izq_down_up[1]
arm_goal2[11] = q_out_izq_down_up[0]
print "Inverse Kinematics"
q_init_der_down_up = q_out_der_up_down # initial angles
q_init_der_down_up[0] = q_out_der_up_down[5]
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]
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.06
desiredFrame3.p[2] = finalFrame_der_down_up.p[2] - 0.02
print "Desired Position: ", desiredFrame3.p
q_out_der_down_up = PyKDL.JntArray(cadena_der_up_down.getNrOfJoints())
ik_der_down_up.CartToJnt(q_init_der_down_up, desiredFrame3, q_out_der_down_up)
print "Output angles in rads: ", q_out_der_down_up
arm_goal2[0] = q_out_der_down_up[5]
arm_goal2[1] = q_out_der_down_up[4]
arm_goal2[2] = q_out_der_down_up[3]
arm_goal2[3] = q_out_der_down_up[2]
arm_goal2[4] = q_out_der_down_up[1]
arm_goal2[5] = q_out_der_down_up[0]
# 333333333333333333333333333333333333333333333333
arm_goal3 = [0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0]
print "Inverse Kinematics"
fksolver_izq_down_up.JntToCart(q_out_izq_down_up,finalFrame_izq_down_up)
# desiredFrame = PyKDL.Frame(PyKDL.Vector(0.5, 0.4, 1))
desiredFrame3 = finalFrame_izq_down_up
desiredFrame3.p[0] = finalFrame_izq_down_up.p[0]
desiredFrame3.p[1] = finalFrame_izq_down_up.p[1] - 0.02
desiredFrame3.p[2] = finalFrame_izq_down_up.p[2]
ik_izq_down_up.CartToJnt(q_out_izq_down_up, desiredFrame3, 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]
fksolver_izq_up_down.JntToCart(q_init_izq_up_down,finalFrame_izq_up_down)
desiredFrame4 = finalFrame_izq_up_down
desiredFrame4.p[0] = finalFrame_izq_up_down.p[0]
desiredFrame4.p[1] = finalFrame_izq_up_down.p[1] - 0.13
desiredFrame4.p[2] = finalFrame_izq_up_down.p[2] + 0.012
print "Desired Position: ", desiredFrame4.p
q_out_izq_up_down2 = PyKDL.JntArray(cadena_izq_up_down.getNrOfJoints())
ik_izq_up_down.CartToJnt(q_init_izq_up_down, desiredFrame4, q_out_izq_up_down2)
print "Output angles in rads: ", q_out_izq_up_down2
arm_goal3[6] = q_out_izq_up_down2[0]
arm_goal3[7] = q_out_izq_up_down2[1]
arm_goal3[8] = q_out_izq_up_down2[2]
arm_goal3[9] = q_out_izq_up_down2[3]
arm_goal3[10] = q_out_izq_up_down2[4]
arm_goal3[11] = q_out_izq_up_down2[5]
print "Inverse Kinematics"
arm_goal3[0] = -0.4
arm_goal3[1] = -0.25
arm_goal3[2] = 0
arm_goal3[3] = 0.5
arm_goal3[4] = 0.4
arm_goal3[5] = -0.25
# 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)
arm_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 = piernas_joints
arm_trajectory.points.append(JointTrajectoryPoint())
arm_trajectory.points[0].positions = arm_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(1.0)
#arm_trajectory.points.append(JointTrajectoryPoint())
#arm_trajectory.points[1].positions = arm_goal2
#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(3.0)
#arm_trajectory.points.append(JointTrajectoryPoint())
#arm_trajectory.points[2].positions = arm_goal3
#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(4.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.01)
# 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))
rospy.loginfo('...done')
def wipe(self, start, finish):
dist = int(round(
200 *
self.calc_dist(start, finish))) # 1 point per cm of separation
print "Steps: %s" % dist
x_step = finish.pose.position.x - start.pose.position.x
y_step = finish.pose.position.y - start.pose.position.y
z_step = finish.pose.position.z - start.pose.position.z
#print "Increments: %s,%s,%s" %(x_step, y_step, z_step)
qs = [
start.pose.orientation.x, start.pose.orientation.y,
start.pose.orientation.z, start.pose.orientation.w
]
qf = [
finish.pose.orientation.x, finish.pose.orientation.y,
finish.pose.orientation.z, finish.pose.orientation.w
]
steps = []
#print "Start: %s" %start
#print "Finish: %s" %finish
for i in range(dist):
frac = float(i) / float(dist)
steps.append(PoseStamped())
steps[i].header.stamp = rospy.Time.now()
steps[i].header.frame_id = start.header.frame_id
steps[i].pose.position.x = start.pose.position.x + x_step * frac
steps[i].pose.position.y = start.pose.position.y + y_step * frac
steps[i].pose.position.z = start.pose.position.z + z_step * frac
new_q = transformations.quaternion_slerp(qs, qf, frac)
steps[i].pose.orientation.x = new_q[0]
steps[i].pose.orientation.y = new_q[1]
steps[i].pose.orientation.z = new_q[2]
steps[i].pose.orientation.w = new_q[3]
steps.append(finish)
#print "Steps:"
#print steps
#raw_input("Press Enter to continue")
rospy.loginfo("Planning straight-line path, please wait")
self.wt_log_out.publish(
data="Planning straight-line path, please wait")
rospy.loginfo("Initiating wipe action")
self.blind_move(finish)
self.r_arm_traj_client.wait_for_result(rospy.Duration(20))
rospy.loginfo("At beginning of path")
pts = []
for i, p in enumerate(steps):
frac = float(i) / float(len(steps))
request = self.form_ik_request(p)
if not i == 0:
request.ik_request.ik_seed_state.joint_state.position = seed
ik_goal = self.ik_pose_proxy(request)
pts.append(ik_goal.solution.joint_state.position)
seed = pts[i]
points = []
for i in range(len(pts) - 1):
angs1 = pts[i]
angs2 = pts[i + 1]
increm = np.subtract(angs2, angs1)
for j in range(10):
points.append(np.add(angs1, np.multiply(0.1 * j, increm)))
#points = np.unwrap(points,1)
p1 = points
traj = JointTrajectory()
traj.header.frame_id = steps[0].header.frame_id
traj.joint_names = self.ik_info.kinematic_solver_info.joint_names
times = []
for i in range(len(points)):
frac = float(i) / float(len(points))
traj.points.append(JointTrajectoryPoint())
traj.points[i].positions = points[i]
traj.points[i].velocities = [0] * 7
times.append(rospy.Duration(frac * dist * 0.2))
traj_goal = FollowJointTrajectoryGoal()
traj_goal.trajectory = traj
tolerance = JointTolerance()
tolerance.position = 0.05
tolerance.velocity = 0.1
traj_goal.path_tolerance = [tolerance for i in range(len(traj.points))]
traj_goal.goal_tolerance = [tolerance for i in range(len(traj.points))]
traj_goal.goal_time_tolerance = rospy.Duration(3)
#print "Steps: %s" %steps
count = 0
while count < 6:
traj_goal.trajectory.points.reverse()
for i in range(len(times)):
traj_goal.trajectory.points[i].time_from_start = times[i]
if count == 0:
print traj_goal.trajectory.points
raw_input("Review Trajectory Goal")
traj_goal.trajectory.header.stamp = rospy.Time.now()
self.r_arm_follow_traj_client.send_goal(traj_goal)
self.r_arm_follow_traj_client.wait_for_result(rospy.Duration(20))
rospy.sleep(0.5)
count += 1
#traj_goal = JointTrajectoryGoal()
#traj_goal.trajectory = traj
#print "Steps: %s" %steps
#count = 0
#
#while count < 6:
#traj_goal.trajectory.points.reverse()
#for i in range(len(times)):
#traj_goal.trajectory.points[i].time_from_start = times[i]
#print traj_goal
#raw_input("Review Trajectory Goal")
##print "Traj goal start:"
##print traj_goal.trajectory.points[0].positions
##print "Traj goal end:"
##print traj_goal.trajectory.points[-1].positions
#traj_goal.trajectory.header.stamp = rospy.Time.now()
#self.r_arm_traj_client.send_goal(traj_goal)
#self.r_arm_traj_client.wait_for_result(rospy.Duration(20))
#rospy.sleep(0.5)
#count += 1
rospy.loginfo("Done Wiping")
self.wt_log_out.publish(data="Done Wiping")
self.linear_move(Float32(-0.15))
randoms = []
max_value = max - min
for i in range(n):
randoms.append((random.random() * max_value) + min) # this scales the value between min and max
return randoms
if __name__ == '__main__':
rospy.init_node('random_left_leg_goals')
ctl_as = SimpleActionClient(CONTROLLER_AS, FollowJointTrajectoryAction)
rospy.loginfo("Connecting to " + CONTROLLER_AS)
ctl_as.wait_for_server()
rospy.loginfo("Connected.")
while not rospy.is_shutdown():
fjtg = FollowJointTrajectoryGoal()
jt = JointTrajectory()
#jt.joint_names = ['leg_left_1_joint', 'leg_left_2_joint', 'leg_left_3_joint', 'leg_left_4_joint', 'leg_left_5_joint', 'leg_left_6_joint']
jt.joint_names = JOINT_NAMES
jtp = JointTrajectoryPoint()
jtp.positions = get_n_randoms(len(JOINT_NAMES), -4.0, 4.0)
jtp.velocities = [0.0] * len(JOINT_NAMES)
#jtp.accelerations = [0.0] * len(JOINT_NAMES)
#jtp.effort = [0.0] * len(JOINT_NAMES)
jtp.time_from_start = rospy.Duration(random.random() + 0.5) # 0.5s to 1.5s
jt.points.append(jtp)
fjtg.trajectory = jt
ctl_as.send_goal_and_wait(fjtg)
rospy.loginfo("Sent: " + str(jt))
rospy.sleep(jtp.time_from_start)
def run(self, pan_amount=0.0, tilt_amount=0.0, ignore_errors=True):
"""
The run function for this step
Args:
pan_amount (float) : the amount in radians to pan the head by. left
(+ve) and right (-ve)
tilt_amount (float) : the amount in radians to tilt the head by.
down (+ve) and up (-ve)
ignore_errors (bool) : if ``True``, ignore the trajectory time limit
exceeded errors that sometimes pop up from the
:const:`HEAD_ACTION_SERVER`
.. seealso::
:meth:`task_executor.abstract_step.AbstractStep.run`
"""
rospy.loginfo("Action {}: Pan by amount {}, Tilt by amount {}".format(
self.name, pan_amount, tilt_amount))
# Update the desired pan and tilt positions based on the joint limits
desired_pan = max(LookPanTiltAction.HEAD_PAN_MIN,
self._current_pan + pan_amount)
desired_pan = min(LookPanTiltAction.HEAD_PAN_MAX, desired_pan)
desired_tilt = max(LookPanTiltAction.HEAD_TILT_MIN,
self._current_tilt + tilt_amount)
desired_tilt = min(LookPanTiltAction.HEAD_TILT_MAX, desired_tilt)
# Create and send the goal height
trajectory = JointTrajectory()
trajectory.joint_names = self._joint_names
trajectory.points.append(JointTrajectoryPoint())
trajectory.points[0].positions = [desired_pan, desired_tilt]
trajectory.points[0].velocities = [0.0, 0.0]
trajectory.points[0].accelerations = [0.0, 0.0]
trajectory.points[0].time_from_start = rospy.Duration(self._duration)
follow_goal = FollowJointTrajectoryGoal()
follow_goal.trajectory = trajectory
self._head_client.send_goal(follow_goal)
self.notify_action_send_goal(LookPanTiltAction.HEAD_ACTION_SERVER,
follow_goal)
# Yield an empty dict while we're executing
while self._head_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._head_client.get_state()
self._head_client.wait_for_result()
result = self._head_client.get_result()
self.notify_action_recv_result(LookPanTiltAction.HEAD_ACTION_SERVER,
status, result)
if status == GoalStatus.PREEMPTED:
yield self.set_preempted(action=self.name,
status=status,
goal=[desired_pan, desired_tilt],
result=result)
elif status == GoalStatus.SUCCEEDED or ignore_errors:
yield self.set_succeeded()
else:
yield self.set_aborted(action=self.name,
status=status,
goal=[desired_pan, desired_tilt],
result=result)
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')
mpr.allowed_planning_time = 3.0 # [s]
# print mpr
try:
req = rospy.ServiceProxy('/plan_kinematic_path', GetMotionPlan)
res = req(mpr)
traj = res.motion_plan_response.trajectory
except rospy.ServiceException, e:
print "Service call failed: %s" % e
client = actionlib.SimpleActionClient(self.joint_traj_action_topic,
FollowJointTrajectoryAction)
client.wait_for_server()
# http://docs.ros.org/diamondback/api/control_msgs/html/index-msg.html
goal = FollowJointTrajectoryGoal()
goal.trajectory = traj.joint_trajectory
client.send_goal(goal)
client.wait_for_result(rospy.Duration.from_sec(15.0))
# http://docs.ros.org/api/actionlib/html/classactionlib_1_1simple__action__client_1_1SimpleActionClient.html
# print client.get_state()
def move_from_pregrasp_to_grasp(self, second):
if self.verbose:
print 'Moving from pregrasp to grasp'
# Cartesian path
header = Header()
header.frame_id = "/" + self.reference_frame
start_state = RobotState()
start_state.joint_state.header.frame_id = "/" + self.reference_frame
names = []
vals = []
def __init__(self):
joint_state_topic = ['joint_states:=/iiwa/joint_states']
moveit_commander.roscpp_initialize(joint_state_topic)
rospy.init_node('moveit_ik_demo')
arm = moveit_commander.MoveGroupCommander('arm')
gripper = moveit_commander.MoveGroupCommander('gripper')
end_effector_link = arm.get_end_effector_link()
reference_frame = 'iiwa_link_0'
arm.set_pose_reference_frame(reference_frame)
gripper_joints = ['finger_joint']
arm.allow_replanning(True)
# ----set gripper action client----
gripper_client = actionlib.SimpleActionClient(
'iiwa/gripper_controller/follow_joint_trajectory',
FollowJointTrajectoryAction)
gripper_client.wait_for_server()
rospy.loginfo('...connected.')
# ----set accuracy----
arm.set_goal_position_tolerance(0.01)
arm.set_goal_orientation_tolerance(0.01)
gripper.set_goal_position_tolerance(0.01)
gripper.set_goal_orientation_tolerance(0.05)
# ----set arm poses----
# pose1
target_pose1 = PoseStamped()
target_pose1.header.frame_id = reference_frame
target_pose1.header.stamp = rospy.Time.now()
target_pose1.pose.position.x = 0.6
target_pose1.pose.position.y = 0.0
target_pose1.pose.position.z = 0.3
target_pose1.pose.orientation.x = 0
target_pose1.pose.orientation.y = 1
target_pose1.pose.orientation.z = 0
target_pose1.pose.orientation.w = 0
# pose1
target_pose2 = PoseStamped()
target_pose2.header.frame_id = reference_frame
target_pose2.header.stamp = rospy.Time.now()
target_pose2.pose.position.x = 0.0
target_pose2.pose.position.y = 0.6
target_pose2.pose.position.z = 0.3
target_pose2.pose.orientation.x = 0
target_pose2.pose.orientation.y = 1
target_pose2.pose.orientation.z = 0
target_pose2.pose.orientation.w = 0
# ----set gripepr poses----
gripper_open = [0.05]
gripper_close = [0.5]
gripper_traj_open = JointTrajectory()
gripper_traj_open.joint_names = gripper_joints
gripper_traj_open.points.append(JointTrajectoryPoint())
gripper_traj_open.points[0].positions = gripper_open
gripper_traj_open.points[0].velocities = [0.0 for i in gripper_joints]
gripper_traj_open.points[0].accelerations = [
0.0 for i in gripper_joints
]
gripper_traj_open.points[0].time_from_start = rospy.Duration(3.0)
gripper_traj_close = JointTrajectory()
gripper_traj_close.joint_names = gripper_joints
gripper_traj_close.points.append(JointTrajectoryPoint())
gripper_traj_close.points[0].positions = gripper_close
gripper_traj_close.points[0].velocities = [0.0 for i in gripper_joints]
gripper_traj_close.points[0].accelerations = [
0.0 for i in gripper_joints
]
gripper_traj_close.points[0].time_from_start = rospy.Duration(3.0)
gripper_goal = FollowJointTrajectoryGoal()
gripper_goal.trajectory = gripper_traj_open
gripper_goal.goal_time_tolerance = rospy.Duration(0.0)
gripper_client.send_goal(gripper_goal)
gripper_client.wait_for_result(rospy.Duration(5.0))
# other setting
shift_value = -0.05
while (True):
arm.set_named_target('home')
arm.go()
rospy.sleep(2)
# arm pose1
arm.set_start_state_to_current_state()
arm.set_pose_target(target_pose1, end_effector_link)
traj = arm.plan()
arm.execute(traj)
rospy.sleep(1)
# move close to target
arm.shift_pose_target(2, shift_value, end_effector_link)
arm.go()
rospy.sleep(1)
# close griper
gripper_goal = FollowJointTrajectoryGoal()
gripper_goal.trajectory = gripper_traj_close
gripper_goal.goal_time_tolerance = rospy.Duration(0.0)
gripper_client.send_goal(gripper_goal)
gripper_client.wait_for_result(rospy.Duration(5.0))
# arm pose2
arm.set_start_state_to_current_state()
arm.set_pose_target(target_pose2, end_effector_link)
traj = arm.plan()
arm.execute(traj)
rospy.sleep(1)
# move close to target
arm.shift_pose_target(2, shift_value, end_effector_link)
arm.go()
rospy.sleep(1)
# open griper
gripper_goal = FollowJointTrajectoryGoal()
gripper_goal.trajectory = gripper_traj_open
gripper_goal.goal_time_tolerance = rospy.Duration(0.0)
gripper_client.send_goal(gripper_goal)
gripper_client.wait_for_result(rospy.Duration(5.0))
arm.set_named_target('home')
arm.go()
rospy.sleep(2)
"""
arm.shift_pose_target(1, -0.05, end_effector_link)
arm.go()
rospy.sleep(1)
arm.shift_pose_target(3, -1.57, end_effector_link)
arm.go()
rospy.sleep(1)
arm.set_named_target('home')
arm.go()
"""
moveit_commander.roscpp_shutdown()
moveit_commander.os._exit(0)
print("Generating client...")
client = actionlib.SimpleActionClient('left_arm_controller/follow_joint_trajectory', FollowJointTrajectoryAction)
print("Waiting for server...")
client.wait_for_server()
# create goal
goal = FollowJointTrajectoryGoal()
# define goal trajectory
waypoint1 = JointTrajectoryPoint()
waypoint1.positions = [0, 1, .75, 0.5]
waypoint1.time_from_start = Duration(0.0)
traj1 = JointTrajectory()
traj1.joint_names = ["l_shoulder_y", "l_shoulder_x", "l_arm_z", "l_elbow_y"]
traj1.points = [waypoint1]
# set goal trajectory
goal.trajectory = traj1
# send goal
client.send_goal(goal, feedback_cb=feedback_cb)
print("Sent goal")
client.wait_for_result()
print('[Result] State: %d'%(client.get_state()))
time.sleep(15)
