Esempio n. 1
0
    def stare_chain_thread(self, index):
        index = int(index)
        rate = rospy.Rate(10)
        while self.stare_landmark_init:
            # Compute Transformation
            angle = euler_from_quaternion([
                self.map.landmark[index].pose.pose.orientation.x,
                self.map.landmark[index].pose.pose.orientation.y,
                self.map.landmark[index].pose.pose.orientation.z,
                self.map.landmark[index].pose.pose.orientation.w
            ])
            O = PyKDL.Rotation.RPY(angle[0], angle[1], angle[2])
            t = PyKDL.Vector(self.map.landmark[index].pose.pose.position.x,
                             self.map.landmark[index].pose.pose.position.y,
                             self.map.landmark[index].pose.pose.position.z)
            wTl = PyKDL.Frame(O, t)

            wTo = wTl * self.offset_transformation
            orientation = wTo.M.GetRPY()
            position = wTo.p
            wwr = WorldWaypointReq()
            wwr.header.stamp = rospy.Time().now()
            wwr.goal = GoalDescriptor(self.name, 1,
                                      GoalDescriptor.PRIORITY_NORMAL)

            wwr.altitude_mode = False
            wwr.position.north = position[0]
            wwr.position.east = position[1]
            wwr.position.depth = position[2]
            wwr.altitude = 5.0
            wwr.orientation.roll = orientation[0]
            wwr.orientation.pitch = orientation[1]
            wwr.orientation.yaw = orientation[2]

            wwr.disable_axis.x = False
            wwr.disable_axis.y = False
            wwr.disable_axis.z = True
            wwr.disable_axis.roll = True
            wwr.disable_axis.pitch = True
            wwr.disable_axis.yaw = False

            self.pub_world_waypoint_req.publish(wwr)
            print 'publish:\n', wwr
            if not self.keep_pose:
                self.check_tolerance(wwr)
            rate.sleep()
Esempio n. 2
0
    def test_transform(self):
        b = tf2_ros.Buffer()
        t = TransformStamped()
        t.transform.translation.x = 1
        t.transform.rotation.x = 1
        t.header.stamp = rospy.Time(2.0)
        t.header.frame_id = 'a'
        t.child_frame_id = 'b'
        b.set_transform(t, 'eitan_rocks')
        out = b.lookup_transform('a','b', rospy.Time(2.0), rospy.Duration(2.0))
        self.assertEqual(out.transform.translation.x, 1)
        self.assertEqual(out.transform.rotation.x, 1)
        self.assertEqual(out.header.frame_id, 'a')
        self.assertEqual(out.child_frame_id, 'b')

        v = PyKDL.Vector(1,2,3)
        out = b.transform(tf2_ros.Stamped(v, rospy.Time(2), 'a'), 'b')
        self.assertEqual(out.x(), 0)
        self.assertEqual(out.y(), -2)
        self.assertEqual(out.z(), -3)

        f = PyKDL.Frame(PyKDL.Rotation.RPY(1,2,3), PyKDL.Vector(1,2,3))
        out = b.transform(tf2_ros.Stamped(f, rospy.Time(2), 'a'), 'b')
        print out
        self.assertEqual(out.p.x(), 0)
        self.assertEqual(out.p.y(), -2)
        self.assertEqual(out.p.z(), -3)
        # TODO(tfoote) check values of rotation

        t = PyKDL.Twist(PyKDL.Vector(1,2,3), PyKDL.Vector(4,5,6))
        out = b.transform(tf2_ros.Stamped(t, rospy.Time(2), 'a'), 'b')
        self.assertEqual(out.vel.x(), 1)
        self.assertEqual(out.vel.y(), -8)
        self.assertEqual(out.vel.z(), 2)
        self.assertEqual(out.rot.x(), 4)
        self.assertEqual(out.rot.y(), -5)
        self.assertEqual(out.rot.z(), -6)

        w = PyKDL.Wrench(PyKDL.Vector(1,2,3), PyKDL.Vector(4,5,6))
        out = b.transform(tf2_ros.Stamped(w, rospy.Time(2), 'a'), 'b')
        self.assertEqual(out.force.x(), 1)
        self.assertEqual(out.force.y(), -2)
        self.assertEqual(out.force.z(), -3)
        self.assertEqual(out.torque.x(), 4)
        self.assertEqual(out.torque.y(), -8)
        self.assertEqual(out.torque.z(), -4)
Esempio n. 3
0
 def _set_waypoint(self, intersection_point, person_position):
     """ Puts the goal as a waypoint in ED
     :param intersection_point: geometry_msgs PointStamped of the last raytrace intersection (in map)
     :param person_position: geometry_msgs PointStamped of the last measured person position (in map)
     """
     yaw = math.atan2(person_position.point.y - intersection_point.point.y,
                      person_position.point.x - intersection_point.point.x)
     position = kdl.Vector(intersection_point.point.x,
                           intersection_point.point.y, 0.0)
     orientation = kdl.Rotation.RPY(0.0, 0.0, yaw)
     waypoint = FrameStamped(frame=kdl.Frame(orientation, position),
                             frame_id="/map")
     self.robot.ed.update_entity(id=self.waypoint_id,
                                 type="waypoint",
                                 frame_stamped=waypoint)
     # import ipdb;ipdb.set_trace()
     return
Esempio n. 4
0
 def resolve(self):
     return Entity(
         identifier="foo",
         object_type=None,
         frame_id="/map",
         pose=kdl.Frame(),
         shape=None,
         volumes={
             "on_top_of":
             BoxVolume(
                 kdl.Vector(-1.0, -1.0, -1.0),
                 kdl.Vector(1.0, 1.0, 1.0),
             )
         },
         super_types=[],
         last_update_time=None,
     )
	def move_psm3_to(self,new_position):
		'''
		new position vector is given with respect to world coordinates
		'''
		if self.tf_world_psm3b is None:
			print("no transformation to move the robot")
			exit(0)

		#add the offset 
		new_position = new_position 
		#Transform to psmb
		new_position = self.tf_world_psm3b.Inverse() * new_position
		
		# print("moving to new location:")
		# self.print_vect(new_position)

		self.api_psm3_arm.move(PyKDL.Frame(self.fix_orientation, new_position))
Esempio n. 6
0
def test_matrix(arm, speed=0.05, frame=PyKDL.Frame()):
    r = 0.05  #amplitude
    theta = np.repeat(np.linspace(0, np.pi / 2, 3),
                      3)  #must multiply to equal each other.
    phi = np.tile(np.linspace(0, np.pi / 2, 3), 3)

    #calculating cartesian motions
    x = r * np.sin(theta) * np.cos(phi)
    y = r * np.sin(theta) * np.sin(phi)
    z = r * np.cos(theta)
    z_home = -0.1135
    for a, b, c in zip(x[2:], y[2:], z[2:]):
        print(a, b, c)
        vmove(arm, PyKDL.Vector(a, b, c + z_home), speed, frame)
        rospy.sleep(1)
        vmove(arm, PyKDL.Vector(0, 0, z_home), speed, frame)
        rospy.sleep(1)
Esempio n. 7
0
def transform(frontier_tf):
    global current_submap_poses

    while len(current_submap_poses) == 0:
        pass

    for frontier in frontier_tf:  #sada je svaki frontier oblik FrontierTF ima submapIndex, trajectory_id i transform
        if (frontier.trajectory_id,
                frontier.submapIndex) in current_submap_poses:
            pomocni = posemath.toMsg(
                current_submap_poses[frontier.trajectory_id,
                                     frontier.submapIndex] *
                PyKDL.Frame(
                    PyKDL.Rotation.Identity(),
                    PyKDL.Vector(frontier.transform.x, frontier.transform.y,
                                 0.0)))
            frontier.projected = pomocni.position
Esempio n. 8
0
def inverseKinematics(robotChain,
                      pos,
                      rot,
                      qGuess=None,
                      minJoints=None,
                      maxJoints=None):
    """
    Perform inverse kinematics
    Args:
        robotChain: robot's chain object
        pos: 1 x 3 or 3 x 1 array of the end effector position.
        rot: 3 x 3 array of the end effector rotation
        qGuess: guess values for the joint positions
        minJoints: minimum value of the position for each joint
        maxJoints: maximum value of the position for each joint
    Returns:
        list of joint positions or None (no solution)
    """
    posKdl = kdl.Vector(pos[0], pos[1], pos[2])
    rotKdl = kdl.Rotation(rot[0, 0], rot[0, 1], rot[0, 2], rot[1, 0],
                          rot[1, 1], rot[1, 2], rot[2, 0], rot[2, 1], rot[2,
                                                                          2])
    frameKdl = kdl.Frame(rotKdl, posKdl)
    numJoints = robotChain.getNrOfJoints()
    minJoints = -np.pi * np.ones(numJoints) if minJoints is None else minJoints
    maxJoints = np.pi * np.ones(numJoints) if maxJoints is None else maxJoints
    minsKdl = jointListToKdl(minJoints)
    maxsKdl = jointListToKdl(maxJoints)
    fkKdl = kdl.ChainFkSolverPos_recursive(robotChain)
    ikVKdl = kdl.ChainIkSolverVel_pinv(robotChain)
    ikPKdl = kdl.ChainIkSolverPos_NR_JL(robotChain, minsKdl, maxsKdl, fkKdl,
                                        ikVKdl)

    if qGuess is None:
        # use the midpoint of the joint limits as the guess
        lowerLim = np.where(np.isfinite(minJoints), minJoints, 0.)
        upperLim = np.where(np.isfinite(maxJoints), maxJoints, 0.)
        qGuess = (lowerLim + upperLim) / 2.0
        qGuess = np.where(np.isnan(qGuess), [0.] * len(qGuess), qGuess)

    qKdl = kdl.JntArray(numJoints)
    qGuessKdl = jointListToKdl(qGuess)
    if ikPKdl.CartToJnt(qGuessKdl, frameKdl, qKdl) >= 0:
        return jointKdlToList(qKdl)
    else:
        return None
Esempio n. 9
0
def callback(msg):

    if (msg.num_people_detected == 1):

        #Only publish when exactly one person is detected
        print("Exactly one person detected")

        global q_solved

        ############
        # Solve IK #
        ############

        #initial joint positions
        q_init = kdl.JntArray(chain_RHand.getNrOfJoints())

        for i in q_solved:
            q_init[i] = i

        q_solved = kdl.JntArray(chain_RHand.getNrOfJoints())

        if (msg.right_wrist.x == "nan"):
            print("No right wrist detected")

        else:
            print(msg.person_ID)
            x, y, z, = msg.right_wrist.x, msg.right_wrist.y, msg.right_wrist.z  #Assign x, y and z with the coordinates from right_wrist

        #desired final cartesian position
        posVec = kdl.Vector(x, y, z)
        F_des = kdl.Frame(posVec)
        IK_RHand.CartToJnt(q_init, F_des, q_solved)

        #convert to np (for purposes of printing)
        q_solved = np.array([q_solved[i] for i in range(q_solved.rows())])

        #publish and print desired final joint positions
        publisher.send({jnt: q_solved[i] for i, jnt in enumerate(Rarm_joints)})
        print(q_solved)

    elif (msg.num_people_detected == 0):
        print("No person detected")

    else:
        print("Too many people detected!", msg.num_people_detected)
Esempio n. 10
0
def move_end_effector_to_cartesian_position_and_angle_and_wrench(
        which_hand, angleX, angleY, angleZ, x, y, z, imp_list, imp_times,
        max_wrench, path_tool_tolerance, time):
    global velma
    T_hand = PyKDL.Frame(PyKDL.Rotation.RPY(angleX, angleY, angleZ),
                         PyKDL.Vector(x, y, z))
    if which_hand == RIGHT_HAND:
        if not velma.moveCartImpRight([T_hand],
                                      time,
                                      None,
                                      None,
                                      imp_list,
                                      imp_times,
                                      max_wrench,
                                      start_time=0.5,
                                      path_tol=path_tool_tolerance):
            exitError(13)
        error = velma.waitForEffectorRight()
        if error != 0:
            if error == PATH_TOLERANCE_VIOLATED_ERROR:
                return PATH_TOLERANCE_VIOLATED_ERROR
    elif which_hand == LEFT_HAND:
        if not velma.moveCartImpLeft([T_hand], [3.0],
                                     None,
                                     None,
                                     imp_list,
                                     imp_times,
                                     max_wrench,
                                     start_time=0.5,
                                     path_tol=path_tool_tolerance):
            exitError(13)
        error = velma.waitForEffectorLeft()
        if error != 0:
            if error == PATH_TOLERANCE_VIOLATED_ERROR:
                return PATH_TOLERANCE_VIOLATED_ERROR
    else:
        print "Error - none hand has been specified!"
    rospy.sleep(0.2)
    # Check precision
    if which_hand == RIGHT_HAND:
        T_hand_diff = PyKDL.diff(T_hand, velma.getTf("B", "Tr"), 1.0)
        if T_hand_diff.vel.Norm() > 0.05 or T_hand_diff.rot.Norm() > 0.05:
            print "Precision for right hand is not perfect!"
    elif which_hand == LEFT_HAND:
        print "d 1.13 - check precision for the left hand - later"
    def cartesian_goal(self, goal):
        rospy.loginfo(rospy.get_caller_id() + '-> starting cartesian goal')
        self.prepare_cartesian()

        cart_goal = PyKDL.Frame()
        cart_goal.p = self.arm.get_desired_position().p
        cart_goal.M = self.arm.get_desired_position().M

        # Translation motion
        cart_goal.p[0] = goal.pose.position.x
        cart_goal.p[1] = goal.pose.position.y

        if goal.pose.position.z > -0.120:
            cart_goal.p[2] = -0.121
            rospy.loginfo("Defaulting to -0.121 z value")
        else:
            cart_goal.p[2] = goal.pose.position.z

        # # Rotation motion
        # cart_goal.M = PyKDL.Rotation.Quaternion(
        #     goal.pose.orientation.x,
        #     goal.pose.orientation.y,
        #     goal.pose.orientation.z, goal.pose.orientation.w)

        # r, p, yaw = cart_goal.M.GetRPY()
        # x, y, z, w = cart_goal.M.GetQuaternion()
        # rospy.loginfo(x)
        # rospy.loginfo("roll angle: " + str(r * 180 / 3.1415926))
        # rospy.loginfo("pitch: " + str (p * 180 / 3.1415926))
        # rospy.loginfo("Yaw angle: " + str(yaw * 180 / 3.1415926))
        # feature_points_right = rospy.wait_for_message(
        # "/featurization/right/feature_points", FeaturePoints, timeout=None)

        # self.joint

        if (self.check_cartesian_error(cart_goal) < self.position_threshold):
            rospy.loginfo(
                'Position is within position threshold, no goal will be sent.')
        else:
            # Send goal
            self.arm.move(cart_goal.p)

            error = self.check_cartesian_error(cart_goal)
            rospy.loginfo('Inverse kinematic error in position: ' + str(error))
            rospy.loginfo('Cartesian goal complete')
Esempio n. 12
0
def main(screen):
	
	screen.nodelay(1)
		
	# this sets the increment for our displacement
	delta = 0.001
	
	total_translation = 0.0
	
	translation = PyKDL.Vector(0.0,0.0,delta)
	
	'''define the waypoint positions for the PSMs for this load test'''
	cart1 = PyKDL.Vector(0.116,-0.090,-0.083)
	rot1 = PyKDL.Rotation()
	rot1 = rot1.Quaternion(0.694,0.0113,0.719,-0.005)
	pos1 = PyKDL.Frame(rot1,cart1)
	

	p2 = dvrk.psm('PSM2')
	c = dvrk.console()
	
	# set our rate to 30hz
	rate = rospy.Rate(30)

	print 'homing to position...'
	c.teleop_stop()
	#p2.open_jaw()
	#p2.move(pos1)
	p2.close_jaw()
	
	while True:
		
		if screen.getch() == ord('w'):
			p2.dmove(translation)
			total_translation = total_translation + delta		

		if screen.getch() == ord('d'):
			print 'displacement is: '
			print total_translation
		
		if screen.getch() == ord('q'):
			print 'quitting'
			break
			
	c.teleop_start()
Esempio n. 13
0
    def test_transform(self):
        b = tf2_ros.Buffer()
        t = TransformStamped()
        t.transform.translation.x = 1.0
        t.transform.rotation = Quaternion(w=0, x=1, y=0, z=0)
        t.header.stamp = builtin_interfaces.msg.Time(sec=2)
        t.header.frame_id = 'a'
        t.child_frame_id = 'b'
        b.set_transform(t, 'eitan_rocks')
        out = b.lookup_transform('a','b', rclpy.time.Time(seconds=2),  rclpy.time.Duration(seconds=2))
        self.assertEqual(out.transform.translation.x, 1)
        self.assertEqual(out.transform.rotation.x, 1)
        self.assertEqual(out.header.frame_id, 'a')
        self.assertEqual(out.child_frame_id, 'b')

        v = PyKDL.Vector(1,2,3)
        out = b.transform(tf2_ros.Stamped(v, builtin_interfaces.msg.Time(sec=2), 'a'), 'b')
        self.assertEqual(out.x(), 0)
        self.assertEqual(out.y(), -2)
        self.assertEqual(out.z(), -3)

        f = PyKDL.Frame(PyKDL.Rotation.RPY(1,2,3), PyKDL.Vector(1,2,3))
        out = b.transform(tf2_ros.Stamped(f, builtin_interfaces.msg.Time(sec=2), 'a'), 'b')
        self.assertEqual(out.p.x(), 0)
        self.assertEqual(out.p.y(), -2)
        self.assertEqual(out.p.z(), -3)
        # TODO(tfoote) check values of rotation

        t = PyKDL.Twist(PyKDL.Vector(1,2,3), PyKDL.Vector(4,5,6))
        out = b.transform(tf2_ros.Stamped(t, builtin_interfaces.msg.Time(sec=2), 'a'), 'b')
        self.assertEqual(out.vel.x(), 1)
        self.assertEqual(out.vel.y(), -8)
        self.assertEqual(out.vel.z(), 2)
        self.assertEqual(out.rot.x(), 4)
        self.assertEqual(out.rot.y(), -5)
        self.assertEqual(out.rot.z(), -6)

        w = PyKDL.Wrench(PyKDL.Vector(1,2,3), PyKDL.Vector(4,5,6))
        out = b.transform(tf2_ros.Stamped(w, builtin_interfaces.msg.Time(sec=2), 'a'), 'b')
        self.assertEqual(out.force.x(), 1)
        self.assertEqual(out.force.y(), -2)
        self.assertEqual(out.force.z(), -3)
        self.assertEqual(out.torque.x(), 4)
        self.assertEqual(out.torque.y(), -8)
        self.assertEqual(out.torque.z(), -4)
Esempio n. 14
0
    def move_tip(self, x=0., y=0., z=0., roll=0., pitch=0., yaw=0.):
        """
        Moves the tooltip in the world frame by the given x,y,z / roll,pitch,yaw.
        @return True on success
        """
        transform = PyKDL.Frame(PyKDL.Rotation.RPY(pitch, roll, yaw),
                                PyKDL.Vector(-x, -y, -z))

        tip_pose = self.get_tip_pose()
        tip_pose_kdl = posemath.fromMsg(tip_pose)
        final_pose = toMsg(tip_pose_kdl * transform)

        self.arm_commander.set_start_state_to_current_state()
        self.arm_commander.set_pose_targets([final_pose])
        plan = self.arm_commander.plan()
        if not self.arm_commander.execute(plan):
            return False
        return True
def calcNextSquarePoint():
	"""
	Wyznacza nastepny punkt ruchu po kwadracie
	na podstawie aktualnej pozycji cuRoPo
	
	Returns
	-------
	Position()
	
	"""
	vecRo = PyKDL.Vector(cuRoPo.x, cuRoPo.y, 0)
	twiRo = PyKDL.Rotation.RPY(0, 0, cuRoPo.theta)
	mxJednoRo = PyKDL.Frame(twiRo, vecRo)
	vecDestRel = PyKDL.Vector(0, (lefty*2-1)*setRoPo.s, 0) # kwadrat w lewo gdy lewo==True
	vecDest = mxJednoRo * vecDestRel
	nextSqPnt = Position(vecDest[0], vecDest[1], 0)
	print "nextSqPnt.x:", nextSqPnt.x, "  nextSqPnt.y:", nextSqPnt.y
	return nextSqPnt
    def query_plan(self):
        #to disable the interpolator
        #return(self.dst_pose)

        for time, pose in self.pose_vector:
            if time > rospy.Time.now():
                #Found the current setpoint
                #message = "time " + str(time)
                #rospy.logwarn(message)
                self.last_dst_pose = kdl.Frame(pose)  #save as last pose
                #print "Found frame:"
                #print pose
                return (pose)

        #If we get here, found no unreached setpoint, so just return the last goal frame
        #rospy.loginfo("returning dst pose")
        #return(self.last_dst_pose)
        return (None)  #None marks end of the interpolation
Esempio n. 17
0
    def Pos(self, t):
        if self.loop is True:
            _lambda = t / self.duration
            while _lambda > 2.0:
                _lambda /= 2.0
            if _lambda > 1.0:
                _lambda = 2.0 - _lambda
        else:
            _lambda = np.min((t / self.duration, 1.0))
        d = kdl.Frame()
        d.p = (1-_lambda)*self.initial_pose.p + _lambda*self.desired_pose.p
        R = self.initial_pose.M.Inverse()*self.desired_pose.M
        #v = R.GetRot()
        (alpha,v) = R.GetRotAngle()
        dR = kdl.Rotation.Rot(v,_lambda*alpha)
        d.M = self.initial_pose.M * dR

        return d
Esempio n. 18
0
    def test_inverse_frame(self, x, y, z, q):
        f = quaternion_matrix(q)
        f[0, 3] = x
        f[1, 3] = y
        f[2, 3] = z

        r2 = PyKDL.Frame()
        r2.M = PyKDL.Rotation.Quaternion(q[0], q[1], q[2], q[3])
        r2.p[0] = x
        r2.p[1] = y
        r2.p[2] = z
        r2 = r2.Inverse()
        r2 = pykdl_frame_to_numpy(r2)
        self.assertTrue(
            np.isclose(w.compile_and_execute(w.inverse_frame, [f]),
                       r2,
                       atol=1.e-4,
                       rtol=1.e-4).all())
Esempio n. 19
0
def main():
    filename = None
    if (sys.argv > 1):
        filename = 'uthai_description/urdf/uthai.urdf'
    (ok, tree) = kdl_parser_py.urdf.treeFromFile(filename)

    chain = tree.getChain("base_footprint","r_foot_ft_link")
    fksolver = kdl.ChainFkSolverPos_recursive(chain)
    ikvelsolver = kdl.ChainIkSolverVel_pinv(chain)
    iksolver = kdl.ChainIkSolverPos_NR(chain,fksolver,ikvelsolver)

    q_init = kdl.JntArray(6)
    q = kdl.JntArray(6)
    F_dest = kdl.Frame(kdl.Rotation.RPY(0,0,0),kdl.Vector(0.0,0,0))

    status = iksolver.CartToJnt(q_init,F_dest,q)
    print("Status : ",status)
    print(q)
Esempio n. 20
0
def create_frame_from_points(p0, p1):
    # type: (kdl.Vector, kdl.Vector) -> kdl.Frame
    """
    Creates a frame from two points. The origin of the frame is the first point. The x-direction points into the
    direction of the next point
    :param p0: Origin of the frame
    :type p0: kdl.Vector
    :param p1: x-direction of the frame will point to this vector
    :type p1: kdl.Vector
    :return: Created frame
    :rtype: kdl.Frame
    """
    unit_x = p1 - p0  # difference
    unit_x.Normalize()  # normalize it so that Norm is 1.0
    unit_y = kdl.Rotation.RPY(0.0, 0.0, 0.5 * math.pi) * unit_x
    unit_z = unit_x * unit_y  # cross-product
    rotation = kdl.Rotation(unit_x, unit_y, unit_z)
    return kdl.Frame(rotation, p0)
Esempio n. 21
0
 def test_initialize(self):
     # initialize is run in setUp, but robot.base.get_location has been replaced, so running it again
     self.robot.base.get_location = lambda: FrameStamped(
         kdl.Frame(kdl.Rotation().Identity(), kdl.Vector(3.5, 3.5, 0)),
         "/map")
     self.tour_guide.initialize()
     self.assertListEqual(self.tour_guide._passed_room_ids,
                          [self._kitchen.id])
     self.assertListEqual(self.tour_guide._passed_furniture_ids,
                          [self._cabinet.id])
     self.assertListEqual(sorted(self.tour_guide._furniture_entities),
                          sorted([self._cabinet, self._bookcase]))
     self.assertListEqual(sorted(self.tour_guide._room_entities),
                          sorted([self._kitchen, self._bedroom]))
     self.assertDictEqual(self.tour_guide._furniture_entities_room, {
         self._kitchen: [self._cabinet],
         self._bedroom: [self._bookcase]
     })
Esempio n. 22
0
 def getImagePoints(self,
                    options=VirtualObjectOptions(),
                    camera_frame=PyKDL.Frame(),
                    camera=None):
     if camera is None:
         return []
     obj_points = self.getObjectPoints(camera_frame=camera_frame,
                                       options=options)
     object_to_camera = camera_frame.Inverse() * self
     cRvec, cTvec = transformations.KDLToCv(object_to_camera)
     img_points, _ = cv2.projectPoints(
         obj_points, cRvec, cTvec, np.array(camera.camera_matrix),
         np.array(camera.distortion_coefficients))
     points = []
     for i in range(0, len(img_points)):
         points.append(
             (int(img_points.item(i, 0, 0)), int(img_points.item(i, 0, 1))))
     return points
    def createReleaseMarker(self):
        marker = InteractiveMarker()
        marker.header.frame_id = self._frameID
        marker.name = self._releaseMarkerName
        # marker.scale = self._torusMeshScalar
        marker.scale = 0.9
        wTc = getFrameFromPose(self._LargeWheelMarker.pose)
        # put orientation of
        ### need to rotate this in z
        cV1 = kdl.Vector(0.0, 0.0, 0.0)
        cR1 = kdl.Rotation.RPY(0, 0, self._turnRotation)
        frame = wTc * kdl.Frame(cR1, cV1)
        marker.pose = getPoseFromFrame(frame)
        marker.controls.append(CreateTransRotControl("TranslateX"))
        self.addInteractiveMarker(marker, self.releaseCallback)
        self.server.applyChanges()

        return marker
Esempio n. 24
0
        def calc_R(xa):
            ret = []
            t = PyKDL.Frame(PyKDL.Rotation.Rot(axis, xa))

            for i in range(0, len(l2)):
                pos2rot_list[i] = t * pos2_list[i]
                n2rot_list[i] = t * n2_list[i]

            for i in range(0, len(l1)):
                score = getScore(pos1_list[i], pos2rot_list[i], n1_list[i],
                                 n2rot_list[i])
                ret.append(score)

#            for i in range(0, len(l2)):
#                score = getMinScore(pos2rot_list[i], pos1_list, n2rot_list[i], n1_list)
#                ret.append(score)

            return np.array(ret)
Esempio n. 25
0
def pose_msg_to_kdl_frame(pose):
    """
    Convert a geometry_msgs.msg.Pose message to a PyKDL.Frame object
    :param pose: Pose to be converted
    :type pose: geometry_msgs.msg.Pose
    :rtype: PyKDL.Frame

    >>> pose = gm.Pose(gm.Point(1, 2, 3), gm.Quaternion(1, 0, 0, 0))
    >>> frame = pose_msg_to_kdl_frame(pose)
    >>> frame.p
    [           1,           2,           3]
    >>> frame.M.GetQuaternion()
    (1.0, 0.0, 0.0, 0.0)
    """
    rot = kdl.Rotation.Quaternion(pose.orientation.x, pose.orientation.y,
                                  pose.orientation.z, pose.orientation.w)
    trans = kdl.Vector(pose.position.x, pose.position.y, pose.position.z)
    return kdl.Frame(rot, trans)
Esempio n. 26
0
    def get_flange_pose(self, q):
        """Returns the flange pose for a given joint configuration.

        Arguments:
        q -- joint configuration
        """
        joint_arr = kdl.JntArray(self._num_of_joints)
        for joint_num in range(self._num_of_joints):
            joint_arr[joint_num] = q[joint_num]
        frame = kdl.Frame()
        if self._tool_segment is None:
            self._fk_solver_pos.JntToCart(
                joint_arr, frame, self._complete_chain.getNrOfSegments())
        else:
            self._fk_solver_pos.JntToCart(
                joint_arr, frame,
                self._complete_chain.getNrOfSegments() - 1)
        return kdl_frame_to_m3d_transform(frame)
Esempio n. 27
0
    def kdl_inverse_kinematics(self, pos, quat, seed=None):
        pos = PyKDL.Vector(pos[0], pos[1], pos[2])
        rot = PyKDL.Rotation()
        rot = rot.Quaternion(quat[0], quat[1], quat[2], quat[3])
        goal_pose = PyKDL.Frame(rot, pos)

        seed_array = joint_list_to_kdl(self._default_seed)
        if seed != None:
            seed_array = joint_list_to_kdl(seed)

        result_angles = PyKDL.JntArray(self._num_jnts)
        if self._ik_p_kdl.CartToJnt(seed_array, goal_pose, result_angles) >= 0:
            result = np.array(list(result_angles))
            # return result
            return True
        else:
            # return None
            return False
Esempio n. 28
0
def get_obstacle_pose(
        cap_front,
        cap_rear,
        obs_front,
        obs_rear,
        obs_gps_to_centroid,
        velodyne_to_front,
        cap_yaw_error_rad=0,
        cap_pitch_error_rad=0):

    # calculate capture yaw in ENU frame and setup correction rotation
    cap_front_v = dict_to_vect(cap_front)
    cap_rear_v = dict_to_vect(cap_rear)
    cap_yaw = get_yaw(cap_front_v, cap_rear_v)
    cap_yaw += cap_yaw_error_rad
    rot_cap = kd.Rotation.EulerZYX(-cap_yaw, -cap_pitch_error_rad, 0)

    obs_rear_v = dict_to_vect(obs_rear)
    if obs_front:
        obs_front_v = dict_to_vect(obs_front)
        obs_yaw = get_yaw(obs_front_v, obs_rear_v)
        # use the front gps as the obstacle reference point if it exists as it's closers
        # to the centroid and mounting metadata seems more reliable
        cap_to_obs = obs_front_v - cap_front_v
    else:
        cap_to_obs = obs_rear_v - cap_front_v

    # transform capture car to obstacle vector into capture car velodyne lidar frame
    res = rot_cap * cap_to_obs
    res += list_to_vect(velodyne_to_front)

    # obs_gps_to_centroid is offset for front gps if it exists, otherwise rear
    obs_gps_to_centroid_v = list_to_vect(obs_gps_to_centroid)
    if obs_front:
        # if we have both front + rear RTK calculate an obstacle yaw and use it for centroid offset
        obs_rot_z = kd.Rotation.RotZ(obs_yaw - cap_yaw)
        centroid_offset = obs_rot_z * obs_gps_to_centroid_v
    else:
        # if no obstacle yaw calculation possible, treat rear RTK as centroid and offset in Z only
        obs_rot_z = kd.Rotation()
        centroid_offset = kd.Vector(0, 0, obs_gps_to_centroid_v[2])
    res += centroid_offset

    return frame_to_dict(kd.Frame(obs_rot_z, res))
Esempio n. 29
0
def read_data(bag, arm):
    '''
    Returns tuple (time, theta, theta_ref, angular_vel, angular_vel_ref,
    angular_acc, angular_acc_ref, s, gripper_pos_command, gripper_vel_command, gripper_pos)
    '''
    global alpha
    time = np.array([])
    frame = PyKDL.Frame(PyKDL.Rotation.RPY(0.0, 0.0, 1.22),
                        PyKDL.Vector(0.0, 0.0, 0.0))
    force_x = np.array([])
    force_x_t = np.array([])
    force_y = np.array([])
    force_y_t = np.array([])
    force_z = np.array([])
    torque_x = np.array([])
    torque_x_t = np.array([])
    torque_y = np.array([])
    torque_y_t = np.array([])
    torque_z = np.array([])

    for topic, msg, t in bag.read_messages(
            topics=['/ft/' + arm + '_gripper_motor']):
        time = np.append(time, msg.header.stamp.to_sec())
        force_x = np.append(force_x, msg.wrench.force.x)
        force_y = np.append(force_y, msg.wrench.force.y)
        #aligned_Fx = msg.wrench.force.x*math.cos(alpha) - msg.wrench.force.y*math.sin(alpha)
        #aligned_Fy = msg.wrench.force.x*math.sin(alpha) + msg.wrench.force.y*math.cos(alpha)
        #force_x_t = np.append(force_x_t, aligned_Fx)
        #force_y_t = np.append(force_y_t, aligned_Fy)

        #force_z = np.append(force_z, msg.wrench.force.z)


##        torque_x = np.append(torque_x, msg.wrench.torque.x)
##        torque_y = np.append(torque_y, msg.wrench.torque.y)
##        aligned_Tx = msg.wrench.torque.y*math.sin(alpha) + msg.wrench.torque.x*math.cos(alpha)
##        aligned_Ty = msg.wrench.torque.y*math.cos(alpha) - msg.wrench.torque.x*math.sin(alpha)
##        torque_x_t = np.append(torque_x_t, aligned_Tx)
##        torque_y_t = np.append(torque_y_t, aligned_Ty)

#torque_z = np.append(torque_z, msg.wrench.torque.z)

#return time, force_x, force_y, force_z, torque_x, torque_y, torque_z
    return time, force_x, force_y,  #force_x_t, force_y_t, torque_x, torque_y, torque_x_t, torque_y_t
Esempio n. 30
0
def get_completion(partial_np_pc, transform):

    goal = graspit_shape_completion.msg.CompleteMeshGoal()
    assert partial_np_pc.shape[0] > 3
    assert partial_np_pc.shape[1] == 3
    for i in range(partial_np_pc.shape[0]):
        point = partial_np_pc[i, :]
        goal.partial_mesh.vertices.append(geometry_msgs.msg.Point(*point))

    client = actionlib.SimpleActionClient('complete_mesh', graspit_shape_completion.msg.CompleteMeshAction)

    client.wait_for_server()
    client.send_goal(goal)
    client.wait_for_result()
    result = client.get_result()

    data = np.ones((len(result.completed_mesh.vertices), 4))
    for i, point in enumerate(result.completed_mesh.vertices):
        x = point.x
        y = point.y
        z = point.z
        data[i] = np.array((x, y, z, 1))

    data_of = data[:]
    trans_frame = PyKDL.Frame(PyKDL.Rotation.RPY(0, 0, 0),
                              PyKDL.Vector(0, 0, -1))
    trans_matrix = tf_conversions.posemath.toMatrix(trans_frame)

    data = np.dot(trans_matrix, data.T).T
    data_of = np.dot(transform, data_of.T).T

    data = data[:, 0:3]
    data_of = data_of[:, 0:3]
    
    completed_pcd_cf = pcl.PointCloud(np.array(data, np.float32))
    completed_pcd_of = pcl.PointCloud(np.array(data_of, np.float32))

    vg = np.array(result.voxel_grid).reshape((PATCH_SIZE, PATCH_SIZE, PATCH_SIZE))
    vox = binvox_rw.Voxels(vg,
                           (PATCH_SIZE, PATCH_SIZE, PATCH_SIZE),
                           (result.voxel_offset_x, result.voxel_offset_y, result.voxel_offset_z -1 ),
                           result.voxel_resolution * PATCH_SIZE,
                           "xyz")
    return completed_pcd_cf, completed_pcd_of, vox