def drive(gest):
if gest.data == 1: #FIST
turtlesimPub.publish("go back")
tsPub.publish(Twist(Vector3(-1.0, 0, 0), Vector3(0, 0, 0)))
elif gest.data == 2 and armState == 1: #WAVE_IN, RIGHT arm
turtlesimPub.publish("go left")
tsPub.publish(Twist(Vector3(0, 0, 0), Vector3(0, 0, 1.0)))
elif gest.data == 2 and armState == 2: #WAVE_IN, LEFT arm
turtlesimPub.publish("go right")
tsPub.publish(Twist(Vector3(0, 0, 0), Vector3(0, 0, -1.0)))
elif gest.data == 3 and armState == 1: #WAVE_OUT, RIGHT arm
turtlesimPub.publish("go right")
tsPub.publish(Twist(Vector3(0, 0, 0), Vector3(0, 0, -1.0)))
elif gest.data == 3 and armState == 2: #WAVE_OUT, LEFT arm
turtlesimPub.publish("go left")
tsPub.publish(Twist(Vector3(0, 0, 0), Vector3(0, 0, 1.0)))
elif gest.data == 4: #FINGERS_SPREAD
turtlesimPub.publish("go forward")
tsPub.publish(Twist(Vector3(1.0, 0, 0), Vector3(0, 0, 0)))
python类Vector3()的实例源码
def ControlListener():
rospy.init_node('robot_motion_control', anonymous=True)
rospy.Subscriber("robot1Com", controldata, callback1)
P1.pub = rospy.Publisher('/home1/command', Vector3, queue_size=10)
rospy.Subscriber("robot2Com", controldata, callback2)
P2.pub = rospy.Publisher('/home2/command', Vector3, queue_size=10)
while not rospy.is_shutdown():
rospy.spin()
return
# spin() simply keeps python from exiting until this node is stopped
#rospy.spin()
def fmt(obj, nest_level=0):
""" Format any common object """
if nest_level > 10:
return ""
if isinstance(obj, float):
return "{0:.3f}".format(obj)
if isinstance(obj, list):
return "(" + ",".join(map(lambda x: fmt(x, nest_level + 1), obj)) + ")"
if isinstance(obj, (numpy.ndarray, numpy.generic)):
return fmt(obj.tolist(), nest_level + 1)
if isinstance(obj, dict):
pairs = map(lambda x, y: "(" + fmt(x) + "," + fmt(y, nest_level + 1) + ")", obj.items())
return fmt(pairs)
if isinstance(obj, Vector3):
return "({},{},{})".format(fmt(obj.x), fmt(obj.y), fmt(obj.z))
if isinstance(obj, Quaternion):
return "({},{},{},{})".format(fmt(obj.x), fmt(obj.y), fmt(obj.z), fmt(obj.w))
# print " obj " + str(obj) + " is of type " + str(type(obj))
return str(obj)
def determine_hand_move_time(self, sidename, position, orientation, cur_transform):
move_time = self.HAND_TRAJECTORY_TIME
desired_q = msg_q_to_q(orientation)
cur_q = msg_q_to_q(cur_transform.rotation)
rotation_amount = quaternion_multiply(desired_q, conjugate_q(cur_q))
rot_w = rotation_amount[3]
if rot_w < 0: rot_w = -rot_w
add_time = 0
if rot_w < 0.85:
add_time = self.HAND_ROTATION_TIME
# if rot_w < 0.92
# add_time = self.HAND_ROTATION_TIME
# if rot_w < 0.75:
# add_time += self.HAND_ROTATION_TIME
if add_time > 0:
rospy.loginfo('Doing significant {} hand rotation, '
'adding rotation wait of {}.'.format(sidename, add_time))
move_time += add_time
return move_time
# Moves the hand center to an absolute world position (Vector3) and orientation (Quaternion).
def figure_target(self, zarj, previous):
""" Set the origin """
self.origin = previous.target
if abs(self.distance) > 0.001:
pelvis = zarj.transform.lookup_transform('world', 'pelvis',
rospy.Time()).transform
quaternion = [pelvis.rotation.w, pelvis.rotation.x,
pelvis.rotation.y, pelvis.rotation.z]
matrix = quaternion_matrix(quaternion)
point = np.matrix([0, self.distance, 0, 1], dtype='float32')
point.resize((4, 1))
rotated = matrix*point
self.target = Vector3(self.origin.x - rotated.item(1),
self.origin.y + rotated.item(2),
self.origin.z)
else:
self.target = self.origin
if self.turn is not None:
self.target_orientation = self.turn + \
previous.target_orientation
def walk_to(self, point):
""" Shuffle to a given point, point is given for the left foot """
msg = FootstepDataListRosMessage()
msg.default_transfer_time = self.DEFAULT_TRANSFER_TIME
msg.default_swing_time = self.DEFAULT_SWING_TIME
msg.execution_mode = msg.OVERRIDE # Override means replace others
msg.unique_id = self.zarj.uid
self.lookup_feet()
delta = Vector3(point.x - self.lf_start_position.x,
point.y - self.lf_start_position.y,
0)
start_foot = self._find_first_xy_foot(point)
msg.footstep_data_list = self.create_xy_steps(delta, start_foot, 0.15)
self.steps = 0
self.planned_steps = len(msg.footstep_data_list)
self.walk_failure = None
self.start_walk = rospy.get_time()
self.footstep_list_publisher.publish(msg)
log('FootstepDataList: uid ' + str(msg.unique_id))
def turn_body_to_pose(self, pose):
"""
Turn the torso to a given orientation
Angles smaller than 110 or larger than 250 are unstable
"""
log("Turn body to match pose {}".format(pose))
msg = ChestTrajectoryRosMessage()
msg.unique_id = self.zarj.uid
msg.execution_mode = msg.OVERRIDE
result = self.zarj.transform.tf_buffer.transform(pose, 'world')
point = SO3TrajectoryPointRosMessage()
point.time = 1.0
point.orientation = result.pose.orientation
point.angular_velocity = Vector3(0.0, 0.0, 0.0)
msg.taskspace_trajectory_points = [point]
self.chest_publisher.publish(msg)
def drive(gest):
global movingState
global zero
global speed
if gest.data == 1: #FIST
movingState -= 1
elif gest.data == 4 or gest.data == 2: #FINGERS_SPREAD
movingState += 1
elif gest.data == 3 :
zero = y
if movingState > 0 :
movingState = 1
turtlesimPub.publish("go forward")
speed = 1
# tsPub.publish(Twist(Vector3(1.0, 0, 0), Vector3(0, 0, 0)))
elif movingState < 0 :
movingState = -1
turtlesimPub.publish("go back")
speed = -1
# tsPub.publish(Twist(Vector3(-1.0, 0, 0), Vector3(0, 0, 0)))
else :
speed = 0
print (speed)
def strength(emgArr1):
emgArr=emgArr1.data
# Define proportional control constant:
K = 0.005
# Get the average muscle activation of the left, right, and all sides
aveRight=(emgArr[0]+emgArr[1]+emgArr[2]+emgArr[3])/4
aveLeft=(emgArr[4]+emgArr[5]+emgArr[6]+emgArr[7])/4
ave=(aveLeft+aveRight)/2
# If all muscles activated, drive forward exponentially
if ave > 500:
tsPub.publish(Twist(Vector3(0.1*math.exp(K*ave),0,0),Vector3(0,0,0)))
# If only left muscles activated, rotate proportionally
elif aveLeft > (aveRight + 200):
tsPub.publish(Twist(Vector3(0,0,0),Vector3(0,0,K*ave)))
# If only right muscles activated, rotate proportionally
elif aveRight > (aveLeft + 200):
tsPub.publish(Twist(Vector3(0,0,0),Vector3(0,0,-K*ave)))
# If not very activated, don't move (high-pass filter)
else:
tsPub.publish(Twist(Vector3(0,0,0),Vector3(0,0,0)))
def marker_from_circle(circle, index=0, linewidth=0.1, color=ColorRGBA(1, 0, 0, 1), z=0., lifetime=10.0):
marker = Marker()
marker.header = make_header("/map")
marker.ns = "Markers_NS"
marker.id = index
marker.type = Marker.CYLINDER
marker.action = 0 # action=0 add/modify object
# marker.color.r = 1.0
# marker.color.g = 0.0
# marker.color.b = 0.0
# marker.color.a = 0.4
marker.color = color
marker.lifetime = rospy.Duration.from_sec(lifetime)
marker.pose = Pose()
marker.pose.position.z = z
marker.pose.position.x = circle.center[0]
marker.pose.position.y = circle.center[1]
marker.scale = Vector3(circle.radius*2.0, circle.radius*2.0, 0)
return marker
def marker_from_point_radius(point, radius, index=0, linewidth=0.1, color=ColorRGBA(1, 0, 0, 1), z=0., lifetime=10.0):
marker = Marker()
marker.header = make_header("/map")
marker.ns = "Speed_NS"
marker.id = index
marker.type = Marker.CYLINDER
marker.action = 0 # action=0 add/modify object
# marker.color.r = 1.0
# marker.color.g = 0.0
# marker.color.b = 0.0
# marker.color.a = 0.4
marker.color = color
marker.lifetime = rospy.Duration.from_sec(lifetime)
marker.pose = Pose()
marker.pose.position.z = z
marker.pose.position.x = point[0]
marker.pose.position.y = point[1]
marker.scale = Vector3(radius*2.0, radius*2.0, 0.001)
return marker
def test_transform(self):
t = Transform(
translation=Vector3(1, 2, 3),
rotation=Quaternion(*transformations.quaternion_from_euler(np.pi, 0, 0))
)
t_mat = ros_numpy.numpify(t)
np.testing.assert_allclose(t_mat.dot([0, 0, 1, 1]), [1.0, 2.0, 2.0, 1.0])
msg = ros_numpy.msgify(Transform, t_mat)
np.testing.assert_allclose(msg.translation.x, t.translation.x)
np.testing.assert_allclose(msg.translation.y, t.translation.y)
np.testing.assert_allclose(msg.translation.z, t.translation.z)
np.testing.assert_allclose(msg.rotation.x, t.rotation.x)
np.testing.assert_allclose(msg.rotation.y, t.rotation.y)
np.testing.assert_allclose(msg.rotation.z, t.rotation.z)
np.testing.assert_allclose(msg.rotation.w, t.rotation.w)
def numpy_to_transform(arr):
from tf import transformations
shape, rest = arr.shape[:-2], arr.shape[-2:]
assert rest == (4,4)
if len(shape) == 0:
trans = transformations.translation_from_matrix(arr)
quat = transformations.quaternion_from_matrix(arr)
return Transform(
translation=Vector3(*trans),
rotation=Quaternion(*quat)
)
else:
res = np.empty(shape, dtype=np.object_)
for idx in np.ndindex(shape):
res[idx] = Transform(
translation=Vector3(*transformations.translation_from_matrix(arr[idx])),
rotation=Quaternion(*transformations.quaternion_from_matrix(arr[idx]))
)
def numpy_to_pose(arr):
from tf import transformations
shape, rest = arr.shape[:-2], arr.shape[-2:]
assert rest == (4,4)
if len(shape) == 0:
trans = transformations.translation_from_matrix(arr)
quat = transformations.quaternion_from_matrix(arr)
return Pose(
position=Vector3(*trans),
orientation=Quaternion(*quat)
)
else:
res = np.empty(shape, dtype=np.object_)
for idx in np.ndindex(shape):
res[idx] = Pose(
position=Vector3(*transformations.translation_from_matrix(arr[idx])),
orientation=Quaternion(*transformations.quaternion_from_matrix(arr[idx]))
)
def _tuple_to_msg_transform(tf_t):
"""
Converts a tf tuple into a geometry_msgs/Transform message
:type tf_t: ((float, float, float), (float, float, float, float))
:rtype: geometry_msgs.msg.Transform
"""
transl = Vector3(*tf_t[0])
rot = Quaternion(*tf_t[1])
return Transform(transl, rot)
def drive_arc(self, omega, travelTime, sign):
'''given the omega, travel time and direction, drive in the corresponding arc'''
# The third parameter (sign) represents whether the forward velocity of the twist will be positive or negative
now = rospy.Time.now()
while rospy.Time.now() - now <= travelTime:
self.twist = Twist(linear=Vector3(sign*SPEED,0,0), angular=Vector3(0,0,omega))
def __init__(self, name):
self.tf_buffer = None
self.tf_listener = None
self.robot_name = name
self.rate = rospy.Rate(10.0)
self.base_pose = None
self.pose_subscriber = None
self.world_transform = Vector3(0, 0, 0)
self.gazebo_subscriber = None
self.gazebo_model = None
def received_pose_calibration(self, data):
""" Receive a pose message for calibration """
if self.pose_subscriber is None:
return
self.base_pose = data
self.pose_subscriber.unregister()
self.pose_subscriber = None
self.world_transform = Vector3(
data.pose.pose.position.x - self.gazebo_model.x,
data.pose.pose.position.y - self.gazebo_model.y,
data.pose.pose.position.z - self.gazebo_model.z)
def v_to_msg_v(v):
return Vector3(v[0], v[1], v[2])
def add_msg_v(msg_v1, msg_v2):
return Vector3(msg_v1.x + msg_v2.x, msg_v1.y + msg_v2.y, msg_v1.z + msg_v2.z)
def move_hand_center_abs(self, sidename, position, orientation, move_time = None):
desired_q = msg_q_to_q(orientation)
# Use the hand trajectory message to move the hand
if move_time is None:
cur_transform = self.get_current_hand_center_transform(sidename)
move_time = self.determine_hand_move_time(sidename, position, orientation, cur_transform)
rospy.loginfo('Desired {} hand center orientation in world reference frame is {}.'.format(
sidename, fmt_q_as_ypr(desired_q)))
# Rotate from orientation based on x-axis being along hand to one where x-axis
# is perpendicular to palm.
if sidename == 'left':
to_palm_q = [0, 0, -self.SQRT_TWO / 2, self.SQRT_TWO/2]
elif sidename == 'right':
to_palm_q = [0, 0, self.SQRT_TWO / 2, self.SQRT_TWO/2]
else:
rospy.logerr("Unknown side {}".format(sidename))
return
perpendicular_to_palm_q = quaternion_multiply(desired_q, to_palm_q)
palm_orientation = q_to_msg_q(perpendicular_to_palm_q)
# rospy.loginfo('Palm quaternion desired: ' + str(perpendicular_to_palm_q))
msg = self.make_hand_trajectory(sidename, position, palm_orientation, self.HAND_TRAJECTORY_TIME)
self.hand_trajectory_publisher.publish(msg)
rospy.sleep(move_time + 0.5)
# Moves the hand by a relative displacement and to a named orientation. If no orientation
# is provided, then the orientation is not changed. The relative displacement is done
# within the reference frame of the torso so Vector3(1,0,0) will move the hand forward
# away from the front of the robot.
def create_xy_steps(self, movement, start_foot, stride=DEFAULT_STRIDE):
""" Create a series of xy direction footsteps. """
footsteps = []
current_step = Vector3(0, 0, 0)
foot = start_foot
while current_step.x != movement.x or current_step.y != movement.y:
if foot == start_foot:
full_step = False
d_x = movement.x - current_step.x
d_y = movement.y - current_step.y
if abs(d_x) > stride:
d_x = sign(d_x) * stride
full_step = True
if abs(d_y) > stride:
d_y = sign(d_y) * stride
full_step = True
current_step.x += d_x
current_step.y += d_y
footsteps.append(
self.create_one_footstep(foot, [current_step.x,
current_step.y, 0.0],
world=True))
if not full_step:
break
else:
footsteps.append(
self.create_one_footstep(foot, [current_step.x,
current_step.y, 0.0],
world=True))
foot = invert_foot(foot)
foot = invert_foot(foot)
footsteps.append(self.create_one_footstep(foot, [current_step.x,
current_step.y, 0.0],
world=True))
return footsteps
def get_lean_points(self, angle):
""" Return a set of trajectory points to accomplish a lean """
point = SO3TrajectoryPointRosMessage()
point.time = 1.0 # Just give it a second to get there
rotate = quaternion_about_axis(radians(angle), [0, 0, -1])
point.orientation = Quaternion(rotate[1], rotate[2], rotate[3], rotate[0])
point.angular_velocity = Vector3(0, 0, 0)
log('Lean to: {}'.format(point))
points = []
points.append(point)
return points
def lean_body(self, angle):
""" Lean forward or back a given angle """
chest_position = self.zarj.transform.lookup_transform(
'world', 'torso', rospy.Time()).transform
log("Lean body {} degrees".format(angle))
msg = ChestTrajectoryRosMessage()
msg.unique_id = self.zarj.uid
msg.execution_mode = msg.OVERRIDE
euler = euler_from_quaternion((chest_position.rotation.w,
chest_position.rotation.x,
chest_position.rotation.y,
chest_position.rotation.z))
point = SO3TrajectoryPointRosMessage()
point.time = 1.0
point.orientation = chest_position.rotation
qua = quaternion_from_euler(euler[0], euler[1] + radians(angle),
euler[2])
point.orientation = Quaternion(qua[1], qua[2], qua[3], qua[0])
point.angular_velocity = Vector3(0.0, 0.0, 0.0)
msg.taskspace_trajectory_points = [point]
self.chest_publisher.publish(msg)
def turn_body(self, angle):
"""
Turn the torso by a given angle
Angles smaller than 110 or larger than 250 are unstable
"""
log("Turn body {} degrees".format(angle))
chest_position = self.zarj.transform.lookup_transform(
'world', 'torso', rospy.Time()).transform
msg = ChestTrajectoryRosMessage()
msg.unique_id = self.zarj.uid
msg.execution_mode = msg.OVERRIDE
euler = euler_from_quaternion((chest_position.rotation.w,
chest_position.rotation.x,
chest_position.rotation.y,
chest_position.rotation.z))
point = SO3TrajectoryPointRosMessage()
point.time = 1.0
qua = quaternion_from_euler(euler[0] + radians(angle), euler[1],
euler[2])
point.orientation = Quaternion(qua[1], qua[2], qua[3], qua[0])
point.angular_velocity = Vector3(0.0, 0.0, 0.0)
msg.taskspace_trajectory_points = [point]
self.chest_publisher.publish(msg)
def turn_body_to(self, angle, wait=True):
"""
Turn the torso to a given angle as related to the pelvis
Angles smaller than 110 or larger than 250 are unstable
"""
log("Turn body to {} degrees".format(angle))
chest_position = self.zarj.transform.lookup_transform(
'pelvis', 'torso', rospy.Time()).transform
msg = ChestTrajectoryRosMessage()
msg.unique_id = self.zarj.uid
msg.execution_mode = msg.OVERRIDE
euler = euler_from_quaternion((chest_position.rotation.w,
chest_position.rotation.x,
chest_position.rotation.y,
chest_position.rotation.z))
qua = quaternion_from_euler(radians(-angle-180), euler[1], euler[2])
pose = PoseStamped()
pose.pose.orientation = Quaternion(qua[1], qua[2], qua[3], qua[0])
pose.header.frame_id = 'pelvis'
pose.header.stamp = rospy.Time()
result = self.zarj.transform.tf_buffer.transform(pose, 'world')
point = SO3TrajectoryPointRosMessage()
point.time = 1.0
point.orientation = result.pose.orientation
point.angular_velocity = Vector3(0.0, 0.0, 0.0)
msg.taskspace_trajectory_points = [point]
self.chest_publisher.publish(msg)
if wait:
rospy.sleep(point.time + 0.1)
def eyes_found_red(name, center):
global found_button
# Button pressed position gathered from Gazebo analysis
buttonPos = Vector3(3.35, -0.73, 1.25)
offset = Vector3(center.point.x - buttonPos.x,
center.point.y - buttonPos.y,
center.point.z - buttonPos.z)
zarj_eyes.remove_detection_request(name)
zarj_tf.set_world_transform(offset)
found_button = True
def _do_repair(self, a, b):
""" do repair """
if LEAK_SIDE:
point_vector = Vector3(a, 0, b)
else:
point_vector = Vector3(0, a, b)
print 'Move by', point_vector
self.fc.zarj.hands.move_hand_center_rel('right', point_vector,
move_time=0.5)
# TODO: What are we going to do to ensure we are touching the wall?
rospy.sleep(2.0)
return False
baxter_continuous_scan.py 文件源码
项目：cu-perception-manipulation-stack
作者: correlllab
项目源码
文件源码
阅读 21
收藏 0
点赞 0
评论 0
def _vector_to(self, vector, to='base'):
h = Header()
h.stamp = rospy.Time(0)
h.frame_id = '{}_gripper'.format(self.limb_name)
v = Vector3(*vector)
v_base = self.tl.transformVector3(to,
Vector3Stamped(h, v)).vector
v_cartesian = [v_base.x, v_base.y, v_base.z, 0, 0, 0]
return v_cartesian
stacking_blocks.py 文件源码
项目：cu-perception-manipulation-stack
作者: correlllab
项目源码
文件源码
阅读 18
收藏 0
点赞 0
评论 0
def _vector_to(self, vector, to='base'):
h = Header()
h.stamp = rospy.Time(0)
h.frame_id = '{}_gripper'.format(self.limb_name)
v = Vector3(*vector)
v_base = self.tl.transformVector3(to,
Vector3Stamped(h, v)).vector
v_cartesian = [v_base.x, v_base.y, v_base.z, 0, 0, 0]
return v_cartesian
